Joint Trajectory Controller Improvements

[f371xx]

Hi,

TLDR: Improved the Joint Trajectory Controller to actually be usable. Also smaller fixes in adjacent files.

I attempted to make use of the MoveIt Plugin and noticed a few major issues with the execution, at least on the j2s7s300.

This mostly stems from a seemingly unfinished Joint Trajectory Controller class, which had a lot of debug outputs and unused code (especially in the header file).

The result was, that MoveIt-generated trajectories were not followed to the last pose, as the previous joint trajectory controller work from a principle to simply set the velocities of the next pose to be reached. As the last pose of a trajectory generally has a velocity of zero, the robot stops one pose short.
I fixed this by calculating a remainder of time to continue motion during the approach of the final pose. Tests now show a remarkable difference, as the planned poses are now reached almost perfectly.

In the same run, I fixed some inconsistencies in the Joint Trajectory Action Service.

[felixmaisonneuve]

Hi @f371xx,

Thank you very much for your time and effort. I am pretty sure this would solve #384 and #398.
This is something I wanted to investigate for a long time but just didn't have the time to do so (It kept being pushed to later because of other priorities).

I will try to review and test all of this next week.

@Tzxtoiny and @WH-0501 will probably be interested in this fix as well

[robertbeach]

Hi, did you ever test this change and/or integrate it into the source tree?

[felixmaisonneuve]

Not me, but a coworker did and she said it was still causing issues.

She said, before, it would systematically undershoot the goal position. Now, with your fix, it would sometimes undershoot, sometimes overshoot and sometimes it is exactly at the correct position.

It would undershoot/overshoot for big distances, but if you send the trjectory again, it would stop exactly at the correct position.

She also mentionned your implementation won't work for very small movements (<5cm)

I still want to investigate further this PR, because this has some very good potential and you did a good cleanup of the code, but I just don't manage to find the ime to do so.