Fix jog application of joint limits

[machinekoder]

This PR fixes some problems when the robot arm hit joint limits. Previously, it was possible to overdrive the joint limits in some instance. Additionally, jogging was halted when a velocity limit was hit. This resulted in unsmooth movement, even when the target position was correct.

To fix this problem, I applied the joint velocity limits when a limit is hit. When the position limit is hit, the jogging is stopped.

Additionally, now the calculation is stopped when no jog input is received.

Furthermore, I fixed some bugs:

• 2 params where not correctly initialized
• warning topic did not work
• pthread return values were not checked
• xbox_to_twist launch file was incorrect and node name as well

Moreover, I applied code style and quality fix where applicable.

Note: I still encounter a problem when stopping jogging, I will fix this tomorrow, I just wanted to get this PR out in case anyone else is working these problems as well, to prevent double work.

[machinekoder]

I would be happy to add some unit tests where applicable.

[machinekoder]

I found the problem or at least a way to circumvent it. Instead of publishing from the main thread, the publishing function is now executed from the jog calc thread. The jog calc function is not executed at publish_period. If that's a problem (maybe we want to check for collision more frequently?) I suggest adding another parameter that specifies the jog calculation period.

[AndyZe]

Wow, this looks much smoother in simulation and still decent on the real robot. You've definitely done something good, but it will take me a while to process.

[AndyZe]

Thank you! Like I said, there's a very noticeable improvement in my Gazebo simulation (position-controlled) and it still works quite well on the hardware UR5 (velocity-controlled).

[AndyZe]

Also, would you like to be listed as an author in package.xml? If so, what name/handle should I put?

[machinekoder]

@AndyZe I have another patch coming up for joint jogging, but I didn't find time to push it yet. And yes, feel free to add me to the author's list in the package.xml. Just use Alexander Rössler or machinekoder, email alex@machinekoder.com

[machinekoder]

@AndyZe The tweak in the CMakeLists.txt is necessary for Qt Creator and CLion (basically any IDEs that use the CMake code model) to detect the header files as part of the source code. I found this tweak in these guidelines https://github.com/ethz-asl/programming_guidelines/wiki/QT-creator

[AndyZe]

On my hardware robot (UR5), I am still overshooting the joints limits at times.

[AndyZe]

I think the problem is here:

for (std::size_t i = 0; i < jtstate.velocity.size(); ++i) { jt_traj.points[0].positions[i] = origjts.position[i]; jt_traj.points[0].velocities[i] = 0.; }

Restoring it to previous joints works fine for position-controlled robots, but setting the velocities to zero does not work. The robot drifts across the joint's boundary and it stops there. So let me know if you have any ideas how to handle that.

[machinekoder]

@AndyZe I have a patch prepared that will fix this problem. Basically, we save and set the last joint position value at the end of a jog move.