Fix a bug when checking a pose is empty and TOTG corner case #1274
Conversation
…waypoint than the input last waypoint
| @@ -1096,6 +1096,10 @@ bool TimeOptimalTrajectoryGeneration::doTimeParameterizationCalculations(robot_t | |||
|
|
|||
| if (diverse_point) | |||
| points.push_back(new_point); | |||
| // If the last point is not a diverse_point we replace the last added point with it to make sure to always have the | |||
There was a problem hiding this comment.
The fix looks reasonable. But why is the variable called diverse_point when it seems to mean, based on the comments, simply "unique waypoint", which would be easier to understand?
There was a problem hiding this comment.
I don't really understand the fix. If it's not a diverse_point, that means it's identical to the previous point, right? So it shouldn't matter if you use the previous point or if you use this one
Slightly agree with Jere that unique_point is more readable than diverse_point but I think that should be a separate PR.
There was a problem hiding this comment.
Maybe it's a matter of tolerances.
There was a problem hiding this comment.
I don't really understand the fix. If it's not a diverse_point, that means it's identical to the previous point, right?
It's identical with some tolerance min_angle_change_, it was causing MTC to fail because the last waypoint after parameterizing the trajectory isn't the expected goal waypoint
There was a problem hiding this comment.
Should I rename the variable in this PR or in a separate one?
| @@ -1096,6 +1096,10 @@ bool TimeOptimalTrajectoryGeneration::doTimeParameterizationCalculations(robot_t | |||
|
|
|||
| if (diverse_point) | |||
| points.push_back(new_point); | |||
| // If the last point is not a diverse_point we replace the last added point with it to make sure to always have the | |||
There was a problem hiding this comment.
I don't really understand the fix. If it's not a diverse_point, that means it's identical to the previous point, right? So it shouldn't matter if you use the previous point or if you use this one
Slightly agree with Jere that unique_point is more readable than diverse_point but I think that should be a separate PR.
| @@ -1096,6 +1096,10 @@ bool TimeOptimalTrajectoryGeneration::doTimeParameterizationCalculations(robot_t | |||
|
|
|||
| if (diverse_point) | |||
| points.push_back(new_point); | |||
| // If the last point is not a diverse_point we replace the last added point with it to make sure to always have the | |||
There was a problem hiding this comment.
Maybe it's a matter of tolerances.
Co-authored-by: AndyZe <andyz@utexas.edu>
Codecov Report
@@ Coverage Diff @@
## main #1274 +/- ##
==========================================
+ Coverage 61.44% 61.47% +0.04%
==========================================
Files 274 274
Lines 24936 24940 +4
==========================================
+ Hits 15319 15329 +10
+ Misses 9617 9611 -6
Continue to review full report at Codecov.
|
…1274) * Fix having empty object pose would use the shape pose as the object pose * TOTG: Fix parameterizing a trajectory would produce a different last waypoint than the input last waypoint * Update moveit_core/planning_scene/test/test_planning_scene.cpp Co-authored-by: AndyZe <andyz@utexas.edu> Co-authored-by: AndyZe <andyz@utexas.edu>
Description
Fix: moveit/moveit_task_constructor#350
The first commit fix
isEmptyfor ROS2 sincew == 1The second commit fix a corner case for TOTG where it might not use the last input waypoint when parameterizing the trajectory
To test run: