Refactor Servo velocity bounds enforcement #2471

jliukkonen · 2021-01-05T02:18:27Z

The enforceVelLimits function struck me as bit odd so I figured I make this PR to start a conversation about what I think it is doing and I why I think it looks like it might not be correct.

Prior to my changes the function would calculate joint angle velocity for each joint based on the delta theta (joint angle change) and would determined the smallest scaling factor for the velocities based on the biggest out-of-bounds velocity value. So if one would have joint velocities of, say, [1, 1, 1, 2, 3, 100], and each joint was bounded to [-1, 1] range, then the scaling factor would be 0.01 . That's at least how I interpreted it. This scaling factor was then applied to the delta thetas meaning if one joint received a command that would translate into very large velocity, the other joints would slow to a crawl.

I felt it would make more sense and be more correct to just apply the velocity limits as they are used elsewhere in MoveIt, that is, make them upper and lower bounds for the joint velocities by clamping the velocities to [min_vel, max_vel] range.

One thing I don't currently understand is how the delta thetas and joint bounds are matched together. Is it guaranteed that the bounds are in correct order with respect to the delta_theta vector and just applying them in order is enough?

…y limits correctly.

codecov · 2021-01-05T04:19:50Z

Codecov Report

Merging #2471 (fb5b233) into master (59a3724) will decrease coverage by 0.03%.
The diff coverage is 100.00%.

@@            Coverage Diff             @@
##           master    #2471      +/-   ##
==========================================
- Coverage   60.26%   60.24%   -0.02%     
==========================================
  Files         351      351              
  Lines       26477    26472       -5     
==========================================
- Hits        15955    15946       -9     
- Misses      10522    10526       +4

Impacted Files	Coverage Δ
moveit_ros/moveit_servo/src/servo_calcs.cpp	`62.67% <100.00%> (-0.41%)`	⬇️
...dl_kinematics_plugin/src/kdl_kinematics_plugin.cpp	`74.78% <0.00%> (-1.32%)`	⬇️
...nning_scene_monitor/src/planning_scene_monitor.cpp	`67.68% <0.00%> (-0.47%)`	⬇️
moveit_ros/moveit_servo/src/pose_tracking.cpp	`46.11% <0.00%> (ø)`
.../moveit_servo/include/moveit_servo/pose_tracking.h	`100.00% <0.00%> (ø)`
.../collision_detection_fcl/src/collision_env_fcl.cpp	`88.42% <0.00%> (ø)`
...sion_detection_bullet/src/collision_env_bullet.cpp	`78.38% <0.00%> (ø)`
...industrial_motion_planner/joint_limits_container.h	`100.00% <0.00%> (ø)`
...industrial_motion_planner/joint_limits_extension.h	`100.00% <0.00%> (ø)`
... and 2 more

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 59a3724...fb5b233. Read the comment docs.

AndyZe · 2021-01-05T14:13:45Z

The reason it scales velocities down uniformly is so the robot follows a straight line. Thus, I'm not a huge fan of this change.

If you can show it's really important from a performance standpoint, we could make your new method optional.

moveit_ros/moveit_servo/src/servo_calcs.cpp

AndyZe · 2021-01-22T16:01:41Z

@jliukkonen I hope you can finish this PR! I know you're pretty busy but it's a nice simplification.

jschleicher · 2021-01-22T17:45:40Z

The reason it scales velocities down uniformly is so the robot follows a straight line. Thus, I'm not a huge fan of this change.

Definitely! According to our previous tests it is essential, that you keep the direction, when scaling the velocity vector. Nevertheless I suspect, there is some problem hidden in enforceVelLimits() leading to artifacts as described in #2096.

AndyZe

I added some commits for unit testing. Looks good to me, now. Thanks for a nice simplification.

moveit_ros/moveit_servo/src/servo_calcs.cpp

jliukkonen · 2021-02-02T02:09:16Z

Thanks for fixing some of the bugs and adding unit tests @AndyZe! I think I managed to nail all the remaining issues. I added some comments to the code, please check them out when re-reviewing.

jliukkonen · 2021-02-02T02:12:25Z

moveit_ros/moveit_servo/src/servo_calcs.cpp

-      }
+      const double unbounded_velocity = velocity(joint_delta_index);
+      // Clamp each joint velocity to a joint specific [min_velocity, max_velocity] range.
+      const auto bounded_velocity = std::min(std::max(unbounded_velocity, bounds.min_velocity_), bounds.max_velocity_);


This was wrong in my previous version. The velocities were bounded before applying velocity scaling factor so velocity was possibly scaled down twice depending on the velocity values and joint limits. First by applying velocity bounds and then by applying scaling factor to the already bounded velocities. Now the velocity bounds are only used to determine the scaling factor.

jliukkonen · 2021-02-02T02:33:14Z

moveit_ros/moveit_servo/test/basic_servo_tests.cpp

+  enforceVelLimits(delta_theta);
+
+  // From Panda arm MoveIt joint_limits.yaml. The largest velocity limits for a joint.
+  const double panda_max_joint_vel = 2.610;  // rad/s


Since joint_limits.yaml take precedence over the URDF the value is actually 2.61 and not 2.871. Took me a while to understand that that was the source of errors. :)

good catch there

AndyZe · 2021-02-02T14:09:33Z

moveit_ros/moveit_servo/test/basic_servo_tests.cpp

+  enforceVelLimits(delta_theta);
+
+  // From Panda arm MoveIt joint_limits.yaml. The largest velocity limits for a joint.
+  const double panda_max_joint_vel = 2.610;  // rad/s


good catch there

AndyZe · 2021-02-02T14:11:23Z

moveit_ros/moveit_servo/test/basic_servo_tests.cpp

+  const double publish_period = parameters.publish_period;
+
+  // Request joint angle changes that are too fast, given the control period in servo settings YAML file.
+  Eigen::ArrayXd delta_theta(7);


i see you made this a 7-vector because the joint group has 7 joints. Good catch.

AndyZe · 2021-02-03T14:16:04Z

Small numerical errors are throwing off the unit test, for example:

Expected equality of these values:
 orig_delta_theta(i)
 Which is: -0.012999999999999999
 delta_theta(i)
 Which is: -0.012293478260869564"

Prob need to use EXPECT_NEAR (expected, actual, absolute_range)

AndyZe · 2021-02-03T14:26:49Z

I'll push a commit to do that with, say, 1e-3 tolderance

henningkayser

This looks much cleaner than the previous implementation.

* Refactor/fix Servo enforceVelLimits function to utilize joint velocity limits correctly. * Use uniform scaling factor instead of joint specific velocity bounds. * Move joint increment outside of if() * Add friend test for velocity limits * Clean up debug code, clang format * Test for negative velocities, too * Apply velocity scaling correctly and skip clamping zero velocities. * Improve enforceVelLimits unit tests. * Use EXPECT_NEAR in the new unit test Co-authored-by: AndyZe <andyz@utexas.edu>

See moveit/moveit2#526 for MoveIt2 patch. moveit#2471 was not sufficiently reviewed.

…it#2471)

Refactor/fix Servo enforceVelLimits function to utilize joint velocit…

c39fd2b

…y limits correctly.

jliukkonen requested review from rhaschke and v4hn as code owners January 5, 2021 02:18

jliukkonen requested review from AndyZe and henningkayser January 5, 2021 02:18

Use uniform scaling factor instead of joint specific velocity bounds.

69bf56c

jliukkonen changed the title ~~Refactor/fix Servo velocity bounds enforcement~~ [WIP] Refactor Servo velocity bounds enforcement Jan 6, 2021

AndyZe reviewed Jan 6, 2021

View reviewed changes

moveit_ros/moveit_servo/src/servo_calcs.cpp Show resolved Hide resolved

moveit_ros/moveit_servo/src/servo_calcs.cpp Outdated Show resolved Hide resolved

AndyZe added 4 commits January 31, 2021 21:31

Move joint increment outside of if()

186b45a

Add friend test for velocity limits

04ada12

Clean up debug code, clang format

79c51da

Test for negative velocities, too

f5667ac

AndyZe approved these changes Feb 1, 2021

View reviewed changes

jliukkonen commented Feb 1, 2021

View reviewed changes

moveit_ros/moveit_servo/src/servo_calcs.cpp Outdated Show resolved Hide resolved

jliukkonen added 2 commits February 1, 2021 19:06

Apply velocity scaling correctly and skip clamping zero velocities.

8a1f13f

Improve enforceVelLimits unit tests.

aac5099

jliukkonen requested a review from AndyZe February 2, 2021 02:09

jliukkonen commented Feb 2, 2021

View reviewed changes

jliukkonen changed the title ~~[WIP] Refactor Servo velocity bounds enforcement~~ Refactor Servo velocity bounds enforcement Feb 2, 2021

jliukkonen commented Feb 2, 2021

View reviewed changes

AndyZe approved these changes Feb 2, 2021

View reviewed changes

AdamPettinger mentioned this pull request Feb 2, 2021

[jog_arm] cartesian jog one linear axis adds rotation as well #2096

Open

Use EXPECT_NEAR in the new unit test

fb5b233

AndyZe force-pushed the pr-refactor-servo-velocity-bounds branch from 8e58e45 to fb5b233 Compare February 3, 2021 20:46

henningkayser approved these changes Feb 4, 2021

View reviewed changes

AndyZe merged commit 8878682 into moveit:master Feb 4, 2021

tylerjw mentioned this pull request Apr 9, 2021

Noetic Release 1.1.2 #2593

Closed

AndyZe mentioned this pull request Apr 15, 2021

Re-re-enable servo tests. moveit/moveit2#423

Merged

vatanaksoytezer mentioned this pull request Apr 19, 2021

Refactor Servo velocity bounds enforcement. Disable flaky unit tests. moveit/moveit2#428

Merged

tylerjw mentioned this pull request Apr 29, 2021

Melodic Backports #2630

Merged

v4hn added a commit to v4hn/moveit that referenced this pull request Jun 30, 2021

Remove faulty gtest include

7337bce

See moveit/moveit2#526 for MoveIt2 patch. moveit#2471 was not sufficiently reviewed.

v4hn mentioned this pull request Jun 30, 2021

Remove faulty gtest include #2747

Merged

sjahr pushed a commit to sjahr/moveit that referenced this pull request Jun 21, 2024

Fix docstring spacing in newly added trajectory Python bindings (move…

19c58b8

…it#2471)

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Refactor Servo velocity bounds enforcement #2471

Refactor Servo velocity bounds enforcement #2471

jliukkonen commented Jan 5, 2021 •

edited

Loading

codecov bot commented Jan 5, 2021 •

edited

Loading

AndyZe commented Jan 5, 2021 •

edited

Loading

AndyZe commented Jan 22, 2021

jschleicher commented Jan 22, 2021

AndyZe left a comment

jliukkonen commented Feb 2, 2021

jliukkonen Feb 2, 2021 •

edited

Loading

jliukkonen Feb 2, 2021

AndyZe Feb 2, 2021

AndyZe Feb 2, 2021

AndyZe Feb 2, 2021

AndyZe commented Feb 3, 2021 •

edited

Loading

AndyZe commented Feb 3, 2021

henningkayser left a comment

Refactor Servo velocity bounds enforcement #2471

Refactor Servo velocity bounds enforcement #2471

Conversation

jliukkonen commented Jan 5, 2021 • edited Loading

codecov bot commented Jan 5, 2021 • edited Loading

Codecov Report

AndyZe commented Jan 5, 2021 • edited Loading

AndyZe commented Jan 22, 2021

jschleicher commented Jan 22, 2021

AndyZe left a comment

Choose a reason for hiding this comment

jliukkonen commented Feb 2, 2021

jliukkonen Feb 2, 2021 • edited Loading

Choose a reason for hiding this comment

jliukkonen Feb 2, 2021

Choose a reason for hiding this comment

AndyZe Feb 2, 2021

Choose a reason for hiding this comment

AndyZe Feb 2, 2021

Choose a reason for hiding this comment

AndyZe Feb 2, 2021

Choose a reason for hiding this comment

AndyZe commented Feb 3, 2021 • edited Loading

AndyZe commented Feb 3, 2021

henningkayser left a comment

Choose a reason for hiding this comment

jliukkonen commented Jan 5, 2021 •

edited

Loading

codecov bot commented Jan 5, 2021 •

edited

Loading

AndyZe commented Jan 5, 2021 •

edited

Loading

jliukkonen Feb 2, 2021 •

edited

Loading

AndyZe commented Feb 3, 2021 •

edited

Loading