Solving issues with large rotational limits

[francofusco]

As found out by others as well, there is a small issue in some controllers when a revolute joints has "large" limits, see eg #365

The problem is that some position controllers call the function angles::shortest_angular_distance_with_limits, which assumes that the total allowed range of rotation is lower than 2*PI. Therefore, if one has as limits such as [-PI-eps, PI+eps), angles will assume that the valid range has size 2*eps instead of 2*(PI+eps). To allow dealing with larger rotations, the PR ros/angles#16 introduced the function shortest_angular_distance_with_large_limits.

I updated three controllers to use such function in case upper_limit-lower_limit is greater than or equal to 2*PI. I tested it and the oscillations that could be experienced before now disappear.

[bmagyar]

The changes look good to me on first read. Could you please add a test that fails without these changes? Could reuse the same code for all controllers.

[francofusco]

Glad to! However, how should I proceed? The issue can be seen "visually" without much problems, however I am not so sure about what strategy to follow to analyze it in a test - I believe you want a test that can be checked by continuous integration, isn't it? I think the easier way to spot the problem in the code is to check the return value of angles::shortest_angular_distance_with_limits (it will likely return false at some point), but I am not sure that it will always do so... I will think about it and try to come up with a satisfactory solution!

[matthew-reynolds]

Looks good to me, I agree that tests would be great. Perhaps we could check the sign of the commanded vel/effort for a specific set of limits and current position to see if it is using the correct limits? I don't know what angle shortest_angular_distance_with_limits returns if the limits are violated, so maybe this wouldn't work.

[bmagyar]

Ping

[francofusco]

Sorry for being inactive for such a long time.

I removed the check lower < upper, I will now work on a test to make sure things work as intended.

[francofusco]

I added a simple test for effort_controllers::JointPositionController: it involves a simple robot with a single revolute joint with limits [-6.3, 6.3]. I created a supersimple RobotHW to fake a real robot, and the test simply tries to move the single joint to the positions 1.0, -1.0 and 0.0 in succession. Without the fix, the joint oscillates around the initial position and thus never reaches the targets - teh tset should fail due to the 50s timeout set in [effort_position_controller.test](effort_controllers/test/effort_position_controller.test). With the fix, the position is reached and the test should run just fine. To "see" the test, you can launch the test manually:

roslaunch effort_controllers simple_bot_hw.launch
rosrun effort_controllers effort_position_controller_test

If you start RViz, set the fixed frame as world and add a RobotModel plugin you should be able to see the robot (nothing more than a rotating beam). Recompile with and without the fix to see the difference.

Is this good enough? The test is quite simple, but I could not think of any better solution... If you think it is ok, I can then add a similar test also for velocity_controllers/JointPositionController.

Note: the test is failing during the CI due to the timeout. Since on my PC it was working well, I think it might be due to a lower processing power of the hosting virtual machine (?). I will try to solve the issue by increasing both the control period (it is currently 1ms) and the timeout. Sorry if this adds unnecessary commits...

[francofusco]

I was finally able to make the tests succeed. Again, I apologize for the long commit list... I can rebase and squash all test-related commits into one, let me know!

[francofusco]

Thanks for the approval! Before merging, do you want me to rebase the commit history and/or add the same test also for velocity_controllers/JointPositionController?

[matthew-reynolds]

This looks great, thanks! I think we just need to touch up the CMakeLists & package.xml to get the dependencies sorted out, and then I have a handful of small requests.

Can you confirm that these tests fail with the old angles::shortest_angular_distance_with_limits?

[bmagyar]

I'm happy with these, can merge as soon as @matthew-reynolds is happy too :)

[francofusco]

@matthew-reynolds I addressed your comments, let me know if anything needs to be updated 😃

Can you confirm that these tests fail with the old angles::shortest_angular_distance_with_limits?

Yes, if I change the angles call to the old one, the joint does not rotate (actually, it keeps oscillating around zero).

Update: I uploaded two videos, showing the difference between the proposed changes and the old behavior.

[francofusco]

Just found a stupid mistake in the simple_bot_hw.cpp: the variable cmd_ was not initialized in the constructor, I believe it was causing troubles in the tests...

[matthew-reynolds]

Awesome, thanks for the videos, this is great.

I'm good as soon as @bmagyar gives the license a peek, and once we swap to EXPECT.

[francofusco]

I updated the code according to the last comments. I think all points from your reviews have been addressed, but let me know if I forgot something instead!

[bmagyar]

Thanks very much for sticking with us through this review! Merging...