Fix: TOTG can return vels/accels greater than the limits by AndyZe · Pull Request #2084 · moveit/moveit2

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

  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

  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 merged 2 commits intomoveit:mainfrom
AndyZe:andyz/totg_jump

AndyZe commented Apr 5, 2023 •

edited

Loading

Uh oh!

codecov bot commented Apr 5, 2023 •

edited

Loading

Uh oh!

sjahr left a comment •

edited

Loading

Uh oh!

AndyZe commented Apr 6, 2023

Uh oh!

AndyZe Apr 6, 2023

Uh oh!

sjahr Apr 6, 2023

Uh oh!

sjahr Apr 6, 2023

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

Conversation

AndyZe commented Apr 5, 2023 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Description

Uh oh!

codecov bot commented Apr 5, 2023 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

sjahr left a comment • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

AndyZe commented Apr 6, 2023

Uh oh!

AndyZe Apr 6, 2023

Choose a reason for hiding this comment

Uh oh!

sjahr Apr 6, 2023

Choose a reason for hiding this comment

Uh oh!

sjahr Apr 6, 2023

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

AndyZe commented Apr 5, 2023 •

edited

Loading

codecov bot commented Apr 5, 2023 •

edited

Loading

sjahr left a comment •

edited

Loading