Cartesian Stiffness Reference Frame

[torydebra]

Hi, thanks for your efforts in releasing the package.

I was wondering if the Cartesian stiffness can be set accordingly to a different frame other than the robot base frame (this->rbdyn_wrapper_.root_link();)

Partially related to this, can also be the pose reference frames changed from the this->rbdyn_wrapper_.root_link();, or is it a sort of constraint of rbdyn?

Thanks!

[matthias-mayr]

Thank you for your interest.

I don't see why this shouldn't be possible in principle to use other frames. Since the calculations are also done in the root frame, the limitation is that in other frames these matrices need to be updated. These transformation updates should happen at every time step, which was one of the reasons why I didn't tackle it yet.

For the pose references it would be similar, but I only allowed the root frame since the controller should be able to quickly process updates without any TF lookups. Similar reasoning for the wrench references. Right now it can be given in an arbitrary frame, but they are immediately transformed into the root frame and not updated, since that would require continuous updates.

One of the things to consider is that some robots like the Franka Emika Robot strictly require frequent control commands at 1kHz. Nevertheless, my take is that it's possible, even with the 1 ms time limit and would just need to be done.
If you want to make a PR, I am happy to review and merge it.

[torydebra]

Hi thanks for the answer.
Indeed I would like to try it on a real franka ;)

I was looking at the code, to see what should be changed (for now regarding the different reference for the Cartesian Stiffness).

The control is here:

Cartesian-Impedance-Controller/src/cartesian_impedance_controller.cpp

Line 268 in 3c6b616

tau_task << this->jacobian_.transpose() * (-this->cartesian_stiffness_ * this->error_ - this->cartesian_damping_ * (this->jacobian_ * this->dq_));

Hence, is it here that should I transform the error_ and the _jacobian ?
Any other clues to which should I take care?

[torydebra]

Hi, I just opened a PR solving both my questions. I tried also on a real franka and things still work.

I rotated some matrices according to a new frame control_frame settable as rosparam.
The pose reference subscriber now considers the frame_id field of the message

You can check my modifications if you like them.

Best!