Refactor out velocity limit enforcement with test #540
Conversation
Codecov Report
@@ Coverage Diff @@
## main #540 +/- ##
==========================================
+ Coverage 46.73% 46.78% +0.06%
==========================================
Files 184 185 +1
Lines 19686 19689 +3
==========================================
+ Hits 9198 9210 +12
+ Misses 10488 10479 -9
Continue to review full report at Codecov.
|
|
I can see that this gets rid of the |
| @@ -154,9 +154,6 @@ class ServoCalcs | |||
| void suddenHalt(trajectory_msgs::msg::JointTrajectory& joint_trajectory) const; | |||
| void suddenHalt(sensor_msgs::msg::JointState& joint_state) const; | |||
|
|
|||
| /** \brief Scale the delta theta to match joint velocity/acceleration limits */ | |||
| void enforceVelLimits(Eigen::ArrayXd& delta_theta); | |||
|
|
|||
| /** \brief Avoid overshooting joint limits */ | |||
| bool enforcePositionLimits(sensor_msgs::msg::JointState& joint_state) const; | |||
There was a problem hiding this comment.
Seems like this should be moved to the new enforce_limits.cpp too
There was a problem hiding this comment.
I agree. This was more about converting the test to a unit test instead of a ros test (as those take forever and are flaky on foxy).
Long term I think it would be nice to separate all the math/logic from the ros code so it could be easily tested.
There was a problem hiding this comment.
I like this, and agree we should move as much as possible of the logic to more testable places. Probably outside the scope of this PR, but good candidates are probably:
calculateWorstCaseStopTimecheckValidCommandscaleCartesianCommandscaleJointCommand- Maybe
velocityScalingFactorForSingularity composeJointTrajMessageinsertRedundantPointsIntoTrajectoryremoveDimensionremoveDriftDimensionsenforceControlDimensions
I think a lot of unit tests for those are already written, but use a friend class. So transferring the tests should not be too bad
| Eigen::ArrayXd velocity = delta_theta / publish_period; | ||
|
|
||
| // Get the velocity scaling factor | ||
| double velocity_scaling_factor = getVelocityScalingFactor(joint_model_group, velocity); |
There was a problem hiding this comment.
I like splitting this out into another function as well
Co-authored-by: AdamPettinger <adam.pettinger@utexas.edu>
tylerjw
force-pushed
the
functional_velocity_limits
branch
from
4bc17b6
to
2ca21cc
Compare
|
@AndyZe I rebased this on main. Would you mind reviewing this again and let me know if you like this change. |
* Refactor out velocity limit enforcement with test * Update moveit_ros/moveit_servo/test/enforce_limits_tests.cpp Co-authored-by: AdamPettinger <adam.pettinger@utexas.edu> Co-authored-by: AdamPettinger <adam.pettinger@utexas.edu>
* Refactor out velocity limit enforcement with test * Update moveit_ros/moveit_servo/test/enforce_limits_tests.cpp Co-authored-by: AdamPettinger <adam.pettinger@utexas.edu> Co-authored-by: AdamPettinger <adam.pettinger@utexas.edu>
Description
While working on my parameters refactor I pulled this function (and it's test) out of the code that depends on ros so it could be tested by unit tests.
Checklist