ROS 2 Migration

[mrceki]

Hi,
This PR includes my initial work on migrating the cartesian_impedance_controller to ROS 2. I've tried to follow the existing structure and maintain consistency with the original implementation.

Please feel free to create a new branch and update the base branch of the PR.

I'd also appreciate any guidance on how you usually test this controller, particularly in ROS 2. I've done some preliminary testing in sim and on real robot, but I'd like to hear how should I test it properly.

[matthias-mayr]

Thanks a lot for this PR. A ROS 2 port is long awaited and appreciated - I have just been too busy to do it myself.

I will have a look at it and probably put it in a ros2 branch.

[qleonardolp]

Hi @matthias-mayr and @mrceki. I'm a PhD student working on impedance control benchmarking. I'm very interested in this ROS2 migration.

Testing the controller with a 6-Dof manipulator in my workspace, an Eigen related error crashed the controller when I activate it. Just to give some context, in my launcher the controller_manager load the cartesian_impedance_controller inactive. It was intentional, and up to the on_configure stage, everything was fine.

If it is of your interesting to make the source code suitable to work with 6-Dof manipulators, then I would like to suggest the following. (I'm open to support on reviewing this PR btw 😃 )

As the identity matrix dimensions are hard-coded when computing the nullspace torque vector, see cartesian_impedance_controller.cpp#L277, my suggestion is to use the n_joints_ class member to set this matrix dimensions:

tau_nullspace << (Eigen::MatrixXd::Identity(this->n_joints_, this->n_joints_) - this->jacobian_.transpose() * jacobian_transpose_pinv) * (this->nullspace_stiffness_ * (this->q_d_nullspace_ - this->q_) - this->nullspace_damping_ * this->dq_);

After this change, the controller could activate without problems.

[mrceki]

Hi @qleonardolp , thanks a lot!

I am not sure if there is any 6-Dof torque controlled robot that you can use with this robot. I am assuming you are trying the controller UR5 in simulation but in real life this controller wont work, because universal robots are position controlled. Also to have a nullspace configuration, robot should have at least 7-Dof, so I would like to hear @matthias-mayr 's opinion on that.

I would be happy If you can help to review PR, especially because of this is my first controller implementation.

[qleonardolp]

I am not sure if there is any 6-Dof torque controlled robot that you can use with this robot. I am assuming you are trying the controller UR5 in simulation but in real life this controller wont work, because universal robots are position controlled. Also to have a nullspace configuration, robot should have at least 7-Dof

Interesting, I didn't know that UR5 is position-controlled. But indeed I was testing with this model.

About the nullspace feature, I agree that it only makes sense for redundant manipulators. Otherwise it would be only necessary to deal with Jacobian singularities.

I'm kind of a ros2_control enthusiast. I'm using this framework both on my full time job and PhD. Reviewing this PR will be a win-win game. The only limitation I have is the robot hardware. I will not be able to test it on a real robot, just in Gazebo.