Add admittance controller #173

destogl · 2021-04-21T21:06:22Z

Add stock admittance controller.

Description - TBD

AndyZe · 2021-05-02T14:23:29Z

I think you should squash my commits down to 1 👍

admittance_controller/src/incremental_ik_calculator.cpp

AndyZe · 2021-05-06T20:05:31Z

admittance_controller/include/admittance_controller/admittance_rule.hpp

+  std::string sensor_frame_;
+
+  // Admittance parameters
+  //   bool unified_mode_ = false;


No idea what unified mode means. Needs a comment

AndyZe · 2021-05-06T20:08:09Z

admittance_controller/include/admittance_controller/admittance_rule.hpp

+  geometry_msgs::msg::WrenchStamped measured_force_;
+  geometry_msgs::msg::WrenchStamped measured_force_control_frame_;
+
+  geometry_msgs::msg::PoseStamped current_origin_;


Current origin of what? Needs a comment

AndyZe · 2021-05-06T20:10:36Z

admittance_controller/src/admittance_controller.cpp

+  auto input_pose_cmd = input_pose_command_.readFromRT();

-  // Get current robot joint angles
+  // Position has to always there


^ sentence doesn't make sense

AndyZe · 2021-05-06T20:13:04Z

doc/admittance_controller.rst

+
+The paramters are adjustable using ``dynamic_reconfigure`` functionality and rqt-plugin.
+
+The controller has following parameters:


Really appreciate this documentation!

admittance_controller/src/admittance_rule.cpp

admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp

AndyZe · 2021-05-06T20:28:49Z

admittance_controller/src/admittance_rule.cpp

    }
  }

+  // TODO: Add here desired vector at the IK tip and not tool!!!!


I'm going to add notes for several lines that aren't reachable from this PR.

Line 213 (above) seems unnecessary. Please double-check

L227:

TODO: this formula could cause a "gimbal lock" if the angular difference is large

Above L239, add comment:

// Acceleration is regarded as the control input. It is integrated to calculate velocity and acceleration.

AndyZe · 2021-05-06T20:45:55Z

admittance_controller/src/admittance_rule.cpp

  tf2::doTransform(current_pose_control_frame_, desired_pose_, relative_desired_pose_);

  // Use Jacobian-based IK
  // Calculate desired Cartesian displacement of the robot


Update the comment (prob my mistake):

// Convert desired position deltas to joint deltas

codecov-commenter · 2021-05-07T23:30:53Z

Codecov Report

Merging #173 (4de5d79) into master (7187083) will increase coverage by 0.00%.
The diff coverage is n/a.

@@            Coverage Diff             @@
##           master     #173      +/-   ##
==========================================
  Coverage   35.27%   35.28%              
==========================================
  Files         286       26     -260     
  Lines       24816     2256   -22560     
  Branches    16082     1462   -14620     
==========================================
- Hits         8755      796    -7959     
+ Misses       1452      132    -1320     
+ Partials    14609     1328   -13281

Flag	Coverage Δ
unittests	`35.28% <ø> (+<0.01%)`	⬆️

Flags with carried forward coverage won't be shown. Click here to find out more.

Impacted Files	Coverage Δ
...nt_state_controller/src/joint_state_controller.cpp
...include/joint_trajectory_controller/trajectory.hpp
..._broadcaster/test/test_joint_state_broadcaster.cpp
...ller/test/test_load_forward_command_controller.cpp
...include/joint_trajectory_controller/trajectory.hpp
...ller/test/test_load_forward_command_controller.cpp
...nt_state_controller/src/joint_state_controller.cpp
...include/joint_trajectory_controller/trajectory.hpp
...int_trajectory_controller/test/test_trajectory.cpp
...ontrollers/src/joint_group_velocity_controller.cpp
... and 302 more

Outline of a class for IK calculations More outline of incremental IK Begin hooking up with MoveIt

…re is not output.

* Rename incremental_ik_calculator -> incremental_kinematics * Use Eigen double types. Rename _trafo transform variables * Fix typo in transformation matrix * Implement incremental forward kinematics

…el to 0 in ff mode.

…ion weirdness.

…ulation of the refrence pose from the reference deltas.

…nsion5) branch Co-authored-by: Nathan Brooks<[email protected]> Co-authored-by: AndyZe<[email protected]>

…admittance-controller-development branch.

…not read properly.

…date_without_reactivation parameter.

AndyZe · 2022-01-21T18:21:01Z

admittance_controller/src/admittance_controller.cpp

+      for (auto index = 0u; index < num_joints; ++index) {
+        // TODO(destogl): ATTENTION: This does not work properly, deltas are getting neutralized and robot is not moving on external forces
+        // TODO(anyone): Is here OK to use shortest_angular_distance?
+        joint_deltas[index] = angles::shortest_angular_distance(current_joint_states.positions[index], (*input_joint_cmd)->points[0].positions[index]);


First time looking at this in awhile!

I just wanted to note, this should probably use shortest_angular_distance_with_large_limits().

https://github.com/ros/angles/blob/811da0230f13348fd40337f63219c3b1520088f3/angles/include/angles/angles.h#L222

destogl marked this pull request as draft April 21, 2021 21:06

olivier-stasse reviewed May 6, 2021

View reviewed changes

admittance_controller/src/incremental_ik_calculator.cpp Outdated Show resolved Hide resolved

AndyZe reviewed May 6, 2021

View reviewed changes

destogl mentioned this pull request May 7, 2021

add addmittance controller state message ros-controls/control_msgs#56

Closed

AndyZe force-pushed the add-admittance-controller branch 2 times, most recently from 73d1b24 to e788915 Compare May 18, 2021 14:48

AndyZe deleted the add-admittance-controller branch May 26, 2021 15:34

AndyZe restored the add-admittance-controller branch May 26, 2021 15:34

destogl force-pushed the add-admittance-controller branch from a257737 to 1739024 Compare October 21, 2021 13:55

destogl and others added 18 commits October 26, 2021 16:25

Add shell of the controller with a test

0112fa5

Outline the admittance control pipeline

732d744

Outline of a class for IK calculations More outline of incremental IK Begin hooking up with MoveIt

Hook up with MoveIt

e096b4f

Link against Eigen

42dec45

Do the pseudoinverse calculation

4b61b50

Set up transform buffer stuff

f98a3d0

Transform the incoming wrench

663fc77

Correct the wrench transformation

6e77403

Minor renaming to be technically correct

c120eca

Testable version of Admittance Controller. IK not yet enables, so the…

42ffb92

…re is not output.

Try to integrate IK

3d3692f

Finish the method for forward kinematics (#4)

fd8995c

* Rename incremental_ik_calculator -> incremental_kinematics * Use Eigen double types. Rename _trafo transform variables * Fix typo in transformation matrix * Implement incremental forward kinematics

Version that could work

6c05bb3

Corrected subscriptions types and debugging

38a3f5b

Corrected some tests and extended testing

3aed12f

Debugging admittance controller

46ca459

Wait for valid wrench data

0c281cd

[WIP] Debug current joint states

5723950

destogl and others added 21 commits October 26, 2021 16:26

Adjust formating of moveit kinematics file.

bfac9b9

Rename desired to reference and interal variables with prefix admittance

3c2d8ba

Seems working. Neede one more round of checks.

a828a93

Cleaning of the not used feedforward variables.

48518f6

Change approach of calculating absolute desired pose.

c730ff1

Use logic to force stable behavior when no external forces are measured.

3dd8741

Rename the wrench variable (ik_base, not control frame)

d1b6e35

Use simple forward diff formula. Delete unused vars. Set admittance v…

6588004

…el to 0 in ff mode.

Different "pose error" calculation for open-loop mode. Prevents rotat…

1314082

…ion weirdness.

Resort calculation of admittance displacement to have it in one place

e79f8bd

Do not initialize memory in update joint-deltas method. Simplify calc…

329a93d

…ulation of the refrence pose from the reference deltas.

Comment gravity comp parameter parsing, for now

029f613

Delete unused EPSILON variables

793379e

Add admittance controller compile options

e562fb4

Add filter_chain_ definition

c327cd4

Add filter chain to admittance controller

aa53eef

Enalbe filters by moving admittance_rule impl to a header file.

3cf78c3

Remove protection logic since it is not needed anymore

58bd473

Remove unused code

c386a6e

Add Admittance Controller from 79a2dfe (destogl:ros2-controllers-exte…

58c6ac9

…nsion5) branch Co-authored-by: Nathan Brooks<[email protected]> Co-authored-by: AndyZe<[email protected]>

Compilable with changed controllers' update() api -> against destogl:…

d99ebed

…admittance-controller-development branch.

destogl force-pushed the add-admittance-controller branch from 5b885cb to d99ebed Compare October 26, 2021 14:26

destogl added 4 commits October 26, 2021 20:18

Add debuging output to the AdmittanceController to know when SRDF is …

e9d3cb3

…not read properly.

Add debuging output to the AdmittanceController to know when SRDF is …

9c1afc9

…not read properly.

Update to new version of ControlParameters and add enable_paramter_up…

c3b19c7

…date_without_reactivation parameter.

Use updated ParameterHandler from control_toolbox.

afe216f

AndyZe reviewed Jan 21, 2022

View reviewed changes

pac48 mentioned this pull request Jun 15, 2022

Chainable Admittance Controller moveit/moveit2#1371

Closed

destogl mentioned this pull request Jul 11, 2022

Add admittance controller with full changes overview #382

Closed

destogl closed this Jul 11, 2022


		The paramters are adjustable using ``dynamic_reconfigure`` functionality and rqt-plugin.

		The controller has following parameters:

Add admittance controller #173

Add admittance controller #173

Uh oh!

Conversation

destogl commented Apr 21, 2021

Uh oh!

AndyZe commented May 2, 2021

Uh oh!

Uh oh!

AndyZe May 6, 2021 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

codecov-commenter commented May 7, 2021 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

5 participants

AndyZe May 6, 2021 •

edited

Loading

codecov-commenter commented May 7, 2021 •

edited

Loading