Added check to ensure that the targets are reachable within the robot…

[urmahp]

…s limits

This will break the test_joint_trajectory_position_interface test in the ROS Driver as part of the test is designed to activate the speed scaling by sending unreachable targets. These changes therefore require that we update the test in the ROS Driver.

[codecov[bot]]

Codecov Report

All modified and coverable lines are covered by tests :white_check_mark:

Project coverage is 72.02%. Comparing base (d21a402) to head (2c2ed96). Report is 19 commits behind head on master.

:exclamation: Current head 2c2ed96 differs from pull request most recent head 92ee816

Please upload reports for the commit 92ee816 to get more accurate results.

Additional details and impacted files

@@            Coverage Diff             @@
##           master     #184      +/-   ##
==========================================
- Coverage   72.05%   72.02%   -0.04%     
==========================================
  Files          71       71              
  Lines        2652     2652              
  Branches      337      337              
==========================================
- Hits         1911     1910       -1     
- Misses        555      557       +2     
+ Partials      186      185       -1     

:umbrella: View full report in Codecov by Sentry.
:loudspeaker: Have feedback on the report? Share it here.

[fmauch]

While this is in a state where everything seems to do its job as intended, it's actually not as simple as that. Since we use servoj for executing trajectories in the position-based case, there is some delay between the moment where we call servoj and servoj actually starting the motion. Thus, simply looking at the current positions and the ones coming in as new targets will not necessarily reflect the actual velocities. I'll try to add a test showing this.

[fmauch]

I've updated this so that it doesn't terminate the program but instead ignores the commands as long as they are infeasible. With this, when I start this with a driver that always commands [0,0,0,0,0,0], the robot doesn't move until we send a target that is close to its current position.

I used a non-blocking popup as we discussed, but thinking about this, a blocking popup might actually be more feasible, don't you agree @urrsk ?

[fmauch]

@urrsk Now everything is where I think I would like it to be and the tests are in line, as well. If you could take another look, that would be awesome. Or @VinDp.

[fmauch]

@urrsk I implemented the things discussed. I was a bit puzzled as I found out that when ignoring the first trajectory point, apparently the read-from-socket call times out. That's why I increased the timeout in this PR. However, that doesn't seem right to me. I checked that sending out the trajectory points only took a few microseconds in the test program, so the timeout could either result from

• The network delaying the package
• Delay between socket send command and package actually entering the network layer
• Socket read in the thread being at lower priority?

I quickly modified the trajectoryThread() in order to just print the points to a textmsg instead of executing them and that showed the same behavior. The first trajectory point was printed fine, the others timed out.

[urrsk]

@urrsk I implemented the things discussed. I was a bit puzzled as I found out that when ignoring the first trajectory point, apparently the read-from-socket call times out. That's why I increased the timeout in this PR. However, that doesn't seem right to me. I checked that sending out the trajectory points only took a few microseconds in the test program, so the timeout could either result from

• The network delaying the package
• Delay between socket send command and package actually entering the network layer
• Socket read in the thread being at lower priority?

I quickly modified the trajectoryThread() in order to just print the points to a textmsg instead of executing them and that showed the same behavior. The first trajectory point was printed fine, the others timed out.

Interesting, it seems we need to do a bit more investigation on this

[fmauch]

@urrsk as discussed I use the higher timeout when the robot isn't moving, yet only. Could you please have another look?

[fmauch]

I'll investigate why the noetic tests fail. A quick look didn't make it clear to me.

[fmauch]

I'll investigate why the noetic tests fail. A quick look didn't make it clear to me.

Found it. The actual test wasn't working properly and this got uncovered by this PR. Necessary changes are being implemented in https://github.com/UniversalRobots/Universal_Robots_ROS_Driver/pull/721