Fix: TOTG can return vels/accels greater than the limits #2084

AndyZe · 2023-04-05T15:44:13Z

Description

This is a port of the MoveIt1 PR: moveit/moveit#3394

The trajectory in the new test looks like this:

Position:

Velocity:

Acceleration:

codecov · 2023-04-05T17:53:35Z

Codecov Report

Patch coverage has no change and project coverage change: +0.01 🎉

Comparison is base (127d483) 50.90% compared to head (1929dd8) 50.91%.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #2084      +/-   ##
==========================================
+ Coverage   50.90%   50.91%   +0.01%     
==========================================
  Files         391      391              
  Lines       32128    32126       -2     
==========================================
+ Hits        16353    16354       +1     
+ Misses      15775    15772       -3

Impacted Files	Coverage Δ
...ry_processing/time_optimal_trajectory_generation.h	`100.00% <ø> (ø)`
...cessing/src/time_optimal_trajectory_generation.cpp	`86.51% <ø> (-0.04%)`	⬇️

... and 1 file with indirect coverage changes

Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here.

☔ View full report in Codecov by Sentry.
📢 Do you have feedback about the report comment? Let us know in this issue.

sjahr

Can you elaborate a bit more on what this fix does? Which effect does removing time_step = time - previous->time_; have? The test makes sense, although I am wondering what the vel/accel profile for a single-point trajectory looks like if we apply the constraint that both need to be 0 at the end. Does TOTG insert additional points to "interpolate" between the two points?

AndyZe · 2023-04-06T13:13:07Z

Sure. There was a comment on the original MoveIt1 PR but I'll expand it here.

although I am wondering what the vel/accel profile for a single-point trajectory looks like if we apply the constraint that both need to be 0 at the end. Does TOTG insert additional points to "interpolate" between the two points?

Yes it does! I added a plot of the test trajectory to the description

AndyZe · 2023-04-06T13:17:30Z

moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp

@@ -849,7 +848,6 @@ Eigen::VectorXd Trajectory::getAcceleration(double time) const
  const double acceleration =
      2.0 * (it->path_pos_ - previous->path_pos_ - time_step * previous->path_vel_) / (time_step * time_step);

-  time_step = time - previous->time_;


Notice that a different time_step is used previously at L848. The time_step at L848 is exactly at one of the waypoints that TOTG used in calculation so vel/accel limits aren't exceeded.

The time_step at L852 is not exactly on one of the waypoints that TOTG used in calculation and that's why it exceeds the vel/accel limits.

There will be a very small loss in accuracy by removing this but it should be fine if TOTG is run with a small delta_t. Since the default delta_t is 0.001sec, that's certainly true.

Thanks for the explanation!

sjahr · 2023-04-06T13:35:47Z

moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp

@@ -849,7 +848,6 @@ Eigen::VectorXd Trajectory::getAcceleration(double time) const
  const double acceleration =
      2.0 * (it->path_pos_ - previous->path_pos_ - time_step * previous->path_vel_) / (time_step * time_step);

-  time_step = time - previous->time_;


Thanks for the explanation!

Increase priority for constrained planning state space (moveit#1300) * Change priority for the constrained planning state space * Fix constrained planning tests * Use PRM instead of RRTConnect --------- Co-authored-by: Sebastian Jahr <sebastian.jahr@picknik.ai> Remove "new" from smart pointer instantiation (moveit#2019) Temporarily disable TestPathConstraints with the Panda robot (moveit#2016) This test has become flaky since it was modified to use the OMPL constrained state space (moveit#2015). Fix mimic joints with TOTG (moveit#1989) Fix include install destination (moveit#2008) Co-authored-by: Henning Kayser <henningkayser@picknik.ai> Co-authored-by: Tyler Weaver <maybe@tylerjw.dev> Ruckig-smoothing : reduce number of duration extensions (moveit#1990) * extend duration only for failed segment * update comment * Remove trajectory reset before extension * readability improvement * Remove call to RobotState update --------- Co-authored-by: ibrahiminfinite <ibrahimjkd@@gmail.com> Co-authored-by: AndyZe <andyz@utexas.edu> Add stale GHA (moveit#2022) * Issues and PRs are labeled as stale after 45 days. * Stale issues are closed after another 45 days. Enable workflow_dispatch for stale GHA Remove invalid description field in GHA Add callback for velocity scaling override + fix params namespace not being set (moveit#2021) Fix python tests (moveit#1979) * ensure joint models in robot_model submodule * add build tests Upgrade apt dependencies --with-new-pkgs (moveit#2039) 2.7.1 🛠️ Bump actions/stale from 7 to 8 (moveit#2037) Allow ci-testing Dockerfile to update the ROS_DISTRO (moveit#2035) Move displaced launch file into planning_component_tools (moveit#2044) Delete the Ruckig "batches" option, deprecated by moveit#1990 (moveit#2028) Added set_robot_trajectory_msg to python bindings (moveit#2050) Use $DISPLAY rather than assuming :0 (moveit#2049) * Use $DISPLAY rather than assuming : * Double quotes --------- Co-authored-by: Sebastian Jahr <sebastian.jahr@picknik.ai> Optionally mitigate Ruckig overshoot (moveit#2051) * Optionally mitigate Ruckig overshoot * Cleanup Update description of moveit_ros_planning_interface (moveit#2045) * Update description of moveit_ros_planning_interface * Update moveit_ros/planning_interface/package.xml Co-authored-by: Henning Kayser <henningkayser@picknik.ai> --------- Co-authored-by: Henning Kayser <henningkayser@picknik.ai> Add URDF Loader Exceptions and Fix Infinite While-Loop when URDF file isn't in a ROS Package (moveit#2032) * Fixed infinite while loop in utilities.cpp and added some exception handling to start screen widget * Fix trailing whitespace, fix getSharePath exception catch on empty request * Fix clang tidy suggestion and error message updates based on pr comments [hybrid planning] improve planning scene monitoring (moveit#1090) * Create new PSM in local constraint solver. Different type of collision checking. * Small boolean logic fixup * Don't configure planning scene monitor twice and pass scene as const * Remove not required call of updateSceneWithCurrentState() * Revert PlanningSceneMonitorConstPtr to PlanningSceneMonitorPtr * Set planning_scene_monitor update rate * Decrease planning scene update rate * Use `updateSceneWithCurrentState()` in psm * Revert the manner of collision checking --------- Co-authored-by: Sebastian Jahr <sebastian.jahr@picknik.ai> Document how collision checking includes descendent links (moveit#2058) Move stateless PlanningScene helper functions out of the class (moveit#2025) Readability: kinematic_state -> robot_state (moveit#2078) moveit_py citation (moveit#2029) Extract parallel planning from moveit cpp (moveit#2043) * Add parallel_planning_interface * Add parallel planning interface * Rename package to pipeline_planning_interface * Move plan_responses_container into own header + source file * Add plan_responses_contrainer source file * Add solution selection and stopping criterion function files * Remove parallel planning from moveit_cpp * Move parallel planning into planning package * Update moveit_cpp * Drop planning_interface changes * Add documentation * Update other moveit packages * Remove removed header * Address CI complains * Address clang-tidy complains * Address clang-tidy complains 2 * Address clang-tidy complains 3 * Extract planning pipeline map creation function from moveit_cpp * Cleanup comment * Use const moveit::core::RobotModelConstPtr& * Formatting * Add header descriptions * Remove superfluous TODOs * Cleanup PILZ: Throw if IK solver doesn't exist (moveit#2082) * Throw if IK solver doesn't exist * Format enabled -wformat (moveit#2065) Co-authored-by: Sebastian Jahr <sebastian.jahr@picknik.ai> Add test and debug issue where TOTG returns accels > limit (moveit#2084) lint fix lint fix 1 lint fix 2 lint fix 3

Add test and debug issue where TOTG returns accels > limit

a1e90ac

AndyZe force-pushed the andyz/totg_jump branch 2 times, most recently from a7362c9 to a1e90ac Compare April 5, 2023 17:29

sjahr reviewed Apr 6, 2023

View reviewed changes

Merge branch 'main' into andyz/totg_jump

1929dd8

AndyZe commented Apr 6, 2023

View reviewed changes

sjahr approved these changes Apr 6, 2023

View reviewed changes

AndyZe merged commit 4d3420c into moveit:main Apr 6, 2023

AndyZe deleted the andyz/totg_jump branch April 6, 2023 15:01

mechwiz pushed a commit to mechwiz/moveit2 that referenced this pull request May 18, 2023

Add test and debug issue where TOTG returns accels > limit (moveit#2084)

9c523c7

uavster mentioned this pull request Oct 25, 2023

Exit loop when position can't change. (backport #2307) #2309

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Fix: TOTG can return vels/accels greater than the limits #2084

Fix: TOTG can return vels/accels greater than the limits #2084

AndyZe commented Apr 5, 2023 •

edited

Loading

codecov bot commented Apr 5, 2023 •

edited

Loading

sjahr left a comment •

edited

Loading

AndyZe commented Apr 6, 2023

AndyZe Apr 6, 2023

sjahr Apr 6, 2023

sjahr Apr 6, 2023

Fix: TOTG can return vels/accels greater than the limits #2084

Fix: TOTG can return vels/accels greater than the limits #2084

Conversation

AndyZe commented Apr 5, 2023 • edited Loading

Description

codecov bot commented Apr 5, 2023 • edited Loading

Codecov Report

sjahr left a comment • edited Loading

Choose a reason for hiding this comment

AndyZe commented Apr 6, 2023

AndyZe Apr 6, 2023

Choose a reason for hiding this comment

sjahr Apr 6, 2023

Choose a reason for hiding this comment

sjahr Apr 6, 2023

Choose a reason for hiding this comment

AndyZe commented Apr 5, 2023 •

edited

Loading

codecov bot commented Apr 5, 2023 •

edited

Loading

sjahr left a comment •

edited

Loading