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.

[matthias-mayr]

@mrceki summer was a bit busy, but here we are finally.
@qleonardolp If you have some time, we can look into improving this package.

I've visited the robotlab at Lund university these days and we looked into running the controller on a KUKA iiwa arm.
I'll have a look at the PR and the package in general now.
@mrceki: I'll squash and rebase some of your changes as a start to make them easier to go through. It looks like you gave me write access to your ros2 branch & to honor your work, I'll use it to do changes, so we can keep this PR. Your initial commits are in ros2-pr-init in this repo if you're interested, since the rebasing will change the history.

Branches:

• ROS 1 will get ros1
• ROS 2 will be the new main branch of this repo due to the ROS 1 EOL

Nullspace & DOF:

I'd need to reach out to the automatic control people. I'm more familiar with the coding & robotics-side. One thing that pops up in my head though is that also robots with less than 7 DOF can reach some poses with more than one joint configuration, in which case at least intuitively a nullspace configuration would make sense.
Independent of that, this controller shall function with all possible amounts of DOF

This controller & Robot interfaces

@qleonardolp I see that many people stumble questions about the compatibility of compliant controllers & different robots. That's why I submitted a talk about that topic to ROSCon DE this year and in two weeks time this repo will have content about it: https://github.com/matthias-mayr/ROS2-Compliant-Arm-Control