Reference frame handling for cartesian compliance control

[matteodv99tn]

I am looking at the code, and I think there's an issue in how you handle reference frames

https://github.com/lucabeber/effort_controller/blob/25213244d5cf555498ff02a56f78fcd18e1fd8e6/cartesian_impedance_controller/src/cartesian_impedance_controller.cpp#L234

In this line of code you have the motion_error which appears to be computed in the base reference frame of the robot, while the m_cartesian_stiffness is defined w.r.t. the compliance link (the same goes for the damping).

I think that what you want to do is something like this, where stiffness/damping are projected from the compliance link into base. From what I understand, this step is missing in your code (so implicitly, you always use the base link as compliance frame).

Let me know if this remark is correct or I am missing something.

[matteodv99tn]

@Hydran00 maybe you should also get a look at that

[Hydran00]

@lucabeber maybe you should also get a look at that