Add velocity impedance controller #94

buschbapti · 2021-04-06T18:20:02Z

This PR adds the controller used in CITRIFIED based on velocity integration. I believe @eeberhard it should behave exactly as the one you have over there, although it should be tested more thoroughly.

domire8 · 2021-04-07T06:35:17Z

source/controllers/src/impedance/VelocityImpedance.cpp

+  CartesianState integrated_desired_state = desired_state;
+  integrated_desired_state.set_pose(desired_pose.data());
+  // compute the impedance control law normally
+  return this->Impedance<CartesianState>::compute_command(integrated_desired_state, feedback_state);


In this compute_command, there is a call this->get_inertia(). What happens there? As I cannot find a default initialization

See my comment below. As desired is simply a velocity state, but integrated, the controller can only act in position and velocity. Hence to remove the effect of the acceleration, inertia is set to 0 in the constructor. This is the reason why the constructor takes only two matrices, stiffness and damping.

aaah now I found it haha

domire8 · 2021-04-07T06:49:38Z

source/controllers/test/tests/test_velocity_impedance.cpp

+  // expect some non null data
+  EXPECT_GT(command.data().norm(), 0.);
+  // as opposed to the normal impedance controller, even if the two velocities are the same
+  // we still expect a command as the desired pose is non null


Where is the desired pose non null?

In the computation of the controller actually. The velocity is integrated and added to the current feedback pose. So even if the feedback pose is 0, if the desired velocity is non 0, desired pose will be non 0 as well. Feedback can be a full state but desired should be a velocity state. Unfortunately I can't restrict it due to the templating.

domire8

Looks good to me!

buschbapti · 2021-04-09T07:48:31Z

source/controllers/src/impedance/VelocityImpedance.cpp

+                                                                  const CartesianState& feedback_state) {
+  using namespace std::chrono_literals;
+  // compute the displacement by multiplying the desired twist by the unit time period and add it to the current pose
+  CartesianPose desired_pose= static_cast<CartesianPose>(feedback_state) + 1s * static_cast<CartesianTwist>(desired_state);


When doing PR #96 I realized that to converge this line should be CartesianPose desired_pose = 1s * static_cast<CartesianTwist>(desired_state) + static_cast<CartesianPose>(feedback_state);. It is related to issue #27

The main difference of this to the CITRIFIED twist controller is that the current pose is used here.

If we do the following, and explicitly set the feedback_state pose to Identity, then as far as I can tell the behaviour will be the same as the controller I have been using.

Suggested change

CartesianPose desired_pose= static_cast<CartesianPose>(feedback_state) + 1s * static_cast<CartesianTwist>(desired_state);

CartesianPose desired_pose = 1s * static_cast<CartesianTwist>(desired_state);

For position, this "relative offset" doesn't matter anyway. For orientation, I already forgot the exact justification but I think it has to do with the angular displacement being in the "local frame" (from a null orientation) such that the vector part of the quaternion corresponds to the angular velocity we want

Why does this relative offset not matter? The integrated_desired_state and the feedback_state could then have positions far away from each other, leading to a high control input in the compute_command no?

buschbapti · 2021-04-14T14:24:39Z

After the discussions and the merging of PR #100 and #101, the issues regarding this controller should now be addressed. This is ready for testing on the real robot.

A VelocityImpedanceController is an ImpedanceController that only accepts a Velocity input and integrate the desired part for position error. Effectively, this controller is at equilibrium only when both desired and real velocities are 0

…se/positions

eeberhard

Finally tested it on the robot, and it gives exactly the same behaviour as the currently employed strategy for orientation control.

The test branch I used on CITRIFIED was test/velocity_impedance_ctrl, with the corresponding change on this line:
https://github.com/epfl-lasa/CITRIFIED/blob/4d5de7a69e1d039b878891f5691f9f8dedbc043e/control/source/controllers/TwistController.cpp#L54

domire8 · 2021-04-22T13:36:25Z

Finally tested it on the robot, and it gives exactly the same behaviour as the currently employed strategy for orientation control.

The test branch I used on CITRIFIED was test/velocity_impedance_ctrl, with the corresponding change on this line:
https://github.com/epfl-lasa/CITRIFIED/blob/4d5de7a69e1d039b878891f5691f9f8dedbc043e/control/source/controllers/TwistController.cpp#L54

What do you mean with the corresponding change ?
And also, did you just use it for the orientation control because it was less change in the CITRIFIED code? Doesn't the velocity impedance controller now completely replace our TwistController?

eeberhard · 2021-04-22T14:25:59Z

What do you mean with the corresponding change ?

Sorry, I was originally going to link a "compare to development", but that had some other changes as well. Best just to look at the diff in the test commit:
epfl-lasa/CITRIFIED@4d5de7a

The main thing that line / file shows is that the command can be added to the previous CartesianWrench command from the linear dissipative controller.

And also, did you just use it for the orientation control because it was less change in the CITRIFIED code? Doesn't the velocity impedance controller now completely replace our TwistController?

No, it absolutely only applies to orientation in this case to get the behaviour we want. It is a kind of "feedforward" velocity controller since it is also integrating the velocity scaled by the stiffness. For linear space we want to use the "passive DS" equivalent dissipative controller which specifically only has a damping matrix, only that the matrix is "rotated" in the direction of the desired velocity.

When this VelocityImpedance controller goes through, I would still want a concrete wrapper class that combines the linear and angular parts, just as the TwistController does in CITRIFIED

buschbapti requested review from eeberhard and domire8 April 6, 2021 18:20

domire8 reviewed Apr 7, 2021

View reviewed changes

domire8 previously approved these changes Apr 7, 2021

View reviewed changes

buschbapti commented Apr 9, 2021

View reviewed changes

buschbapti dismissed domire8’s stale review via 6dd5741 April 14, 2021 14:23

buschbapti force-pushed the feature/velocity_impedance branch from 18bfdc2 to 6dd5741 Compare April 14, 2021 14:23

buschbapti added 7 commits April 15, 2021 08:59

feat: add a VelocityImpedanceController

ab0269e

A VelocityImpedanceController is an ImpedanceController that only accepts a Velocity input and integrate the desired part for position error. Effectively, this controller is at equilibrium only when both desired and real velocities are 0

fix: update test according to new standard

d00aee1

fix: update to namespace change

93ae160

fix: update to namespace change

d8494ab

fix: correct controller by adding the displacement to the feedback po…

3a0b809

…se/positions

fix: use the new copy constructors to ensure only velocity feedback

1416ed4

fix: change the order of the include

d66e97e

domire8 force-pushed the feature/velocity_impedance branch from be7ed87 to d66e97e Compare April 15, 2021 06:59

eeberhard approved these changes Apr 22, 2021

View reviewed changes

domire8 approved these changes Apr 22, 2021

View reviewed changes

buschbapti merged commit 58a4ec3 into develop Apr 22, 2021

buschbapti deleted the feature/velocity_impedance branch April 22, 2021 15:45

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add velocity impedance controller #94

Add velocity impedance controller #94

buschbapti commented Apr 6, 2021

domire8 Apr 7, 2021

buschbapti Apr 7, 2021 •

edited

Loading

domire8 Apr 7, 2021

domire8 Apr 7, 2021

buschbapti Apr 7, 2021

domire8 left a comment

buschbapti Apr 9, 2021

eeberhard Apr 12, 2021

domire8 Apr 12, 2021

buschbapti commented Apr 14, 2021

eeberhard left a comment

domire8 commented Apr 22, 2021 •

edited

Loading

eeberhard commented Apr 22, 2021 •

edited

Loading

	CartesianPose desired_pose= static_cast<CartesianPose>(feedback_state) + 1s * static_cast<CartesianTwist>(desired_state);
	CartesianPose desired_pose = 1s * static_cast<CartesianTwist>(desired_state);

Add velocity impedance controller #94

Add velocity impedance controller #94

Conversation

buschbapti commented Apr 6, 2021

domire8 Apr 7, 2021

Choose a reason for hiding this comment

buschbapti Apr 7, 2021 • edited Loading

Choose a reason for hiding this comment

domire8 Apr 7, 2021

Choose a reason for hiding this comment

domire8 Apr 7, 2021

Choose a reason for hiding this comment

buschbapti Apr 7, 2021

Choose a reason for hiding this comment

domire8 left a comment

Choose a reason for hiding this comment

buschbapti Apr 9, 2021

Choose a reason for hiding this comment

eeberhard Apr 12, 2021

Choose a reason for hiding this comment

domire8 Apr 12, 2021

Choose a reason for hiding this comment

buschbapti commented Apr 14, 2021

eeberhard left a comment

Choose a reason for hiding this comment

domire8 commented Apr 22, 2021 • edited Loading

eeberhard commented Apr 22, 2021 • edited Loading

buschbapti Apr 7, 2021 •

edited

Loading

domire8 commented Apr 22, 2021 •

edited

Loading

eeberhard commented Apr 22, 2021 •

edited

Loading