search

matthias-mayr / Cartesian-Impedance-Controller

A C++ implementation of Cartesian impedance control for torque-controlled manipulators with ROS bindings.
https://matthias-mayr.github.io/Cartesian-Impedance-Controller/
BSD 3-Clause "New" or "Revised" License
212 stars 32 forks source link

Cartesian Stiffness Reference Frame #24

Open torydebra opened 2 weeks ago

torydebra commented 2 weeks ago

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 commented 2 weeks ago

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 commented 2 weeks ago

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: https://github.com/matthias-mayr/Cartesian-Impedance-Controller/blob/3c6b61697b67366b05dea9661527310c4bd972ef/src/cartesian_impedance_controller.cpp#L268

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

torydebra commented 1 week ago

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!