Skip to content

Use adjustable waypoint batch sizes for Ruckig#1719

Merged
AndyZe merged 10 commits intomoveit:mainfrom
AndyZe:andyz/ruckig_waypoint_batch
Dec 20, 2022
Merged

Use adjustable waypoint batch sizes for Ruckig#1719
AndyZe merged 10 commits intomoveit:mainfrom
AndyZe:andyz/ruckig_waypoint_batch

Conversation

@AndyZe
Copy link
Copy Markdown
Member

@AndyZe AndyZe commented Nov 16, 2022 •
edited
Loading

Addresses #1439.

This feeds waypoints into Ruckig in batches of min(100 waypoints, 10% of waypoints).

  • If you feed in too many waypoints at once, the output trajectory will have a longer duration than it needs to.
  • If you feed in too few waypoints at once, and the trajectory is very long (like 100,000 waypoints), the runtime will be very slow.

Hopefully this is a happy medium. @swiz23 are you able to test this?

This can be tested easily by adding this line to moveit_resources/panda/ompl_planning.yaml, then run any of the motion planning MoveIt2 tutorials:

request_adapters: >-
default_planner_request_adapters/AddRuckigTrajectorySmoothing
default_planner_request_adapters/AddTimeOptimalParameterization
...

@AndyZe AndyZe force-pushed the andyz/ruckig_waypoint_batch branch from 71be35c to 8a099a6 Compare November 16, 2022 17:07
@moveit moveit deleted a comment from codecov bot Nov 16, 2022
@codecov
Copy link
Copy Markdown

codecov bot commented Nov 18, 2022 •
edited
Loading

Codecov Report

Base: 50.28% // Head: 50.31% // Increases project coverage by +0.04% 🎉

Coverage data is based on head (b43248a) compared to base (9f7d6df).
Patch coverage: 95.13% of modified lines in pull request are covered.

Additional details and impacted files 
@@            Coverage Diff             @@
##             main    #1719      +/-   ##
==========================================
+ Coverage   50.28%   50.31%   +0.04%     
==========================================
  Files         374      374              
  Lines       31286    31325      +39     
==========================================
+ Hits        15728    15759      +31     
- Misses      15558    15566       +8
Impacted Files Coverage Δ
...rajectory_processing/src/ruckig_traj_smoothing.cpp 91.58% <94.60%> (+0.62%) ⬆️
...ics_plugin_loader/src/kinematics_plugin_loader.cpp 83.34% <100.00%> (+0.58%) ⬆️
...nning_scene_monitor/src/planning_scene_monitor.cpp 45.26% <0.00%> (-0.43%) ⬇️
moveit_core/robot_model/src/joint_model_group.cpp 65.36% <0.00%> (-0.24%) ⬇️
...e/collision_detection_fcl/src/collision_common.cpp 73.76% <0.00%> (-0.22%) ⬇️

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 at Codecov.
📢 Do you have feedback about the report comment? Let us know in this issue.

@AndyZe AndyZe force-pushed the andyz/ruckig_waypoint_batch branch from 378777c to ea605da Compare December 2, 2022 14:13
@AndyZe AndyZe requested a review from stephanie-eng December 2, 2022 14:13
@AndyZe AndyZe force-pushed the andyz/ruckig_waypoint_batch branch from ea605da to 0705c0a Compare December 8, 2022 16:57
ChrisThrasher
ChrisThrasher suggested changes Dec 8, 2022
View reviewed changes
@AndyZe AndyZe force-pushed the andyz/ruckig_waypoint_batch branch 2 times, most recently from fe19f77 to f5dca42 Compare December 15, 2022 14:38
sjahr
sjahr reviewed Dec 15, 2022
View reviewed changes
Copy link
Copy Markdown
Contributor

@sjahr sjahr left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Looks good, just a couple of minor comments

@AndyZe AndyZe force-pushed the andyz/ruckig_waypoint_batch branch 2 times, most recently from 6ca7695 to b7d2ad0 Compare December 15, 2022 15:35
sjahr
sjahr reviewed Dec 15, 2022
View reviewed changes
@AndyZe AndyZe force-pushed the andyz/ruckig_waypoint_batch branch 2 times, most recently from 4d52527 to 43179d8 Compare December 15, 2022 17:53
henningkayser
henningkayser reviewed Dec 16, 2022
View reviewed changes
moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
size_t first_new_waypoint = first_point_previously_smoothed ? 1 : 0;

// Add smoothed waypoints to the output
for (size_t waypoint_idx = first_new_waypoint; waypoint_idx < sub_trajectory.getWayPointCount(); ++waypoint_idx)
Copy link
Copy Markdown
Member

@henningkayser henningkayser Dec 16, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Looks like this could use RobotTrajectory::append(). Same for the loop further up.

Copy link
Copy Markdown
Member Author

@AndyZe AndyZe Dec 16, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Good idea but I don't think that will work because of the dt argument. We might not have a constant dt.

  RobotTrajectory& append(const RobotTrajectory& source, double dt, size_t start_index = 0,
                          size_t end_index = std::numeric_limits<std::size_t>::max());

Copy link
Copy Markdown
Member

@henningkayser henningkayser Dec 16, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

it's only the dt between the two trajectories. All other steps remain the same

Copy link
Copy Markdown
Member Author

@AndyZe AndyZe Dec 16, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

👍 I replaced the second loop. The first loop --> [move_group-4] [WARN] [1671207429.602316971] [RobotTrajectory]: First and only waypoint has a duration of 0.

Copy link
Copy Markdown
Member Author

@AndyZe AndyZe Dec 16, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Sigh, I think there may be a bug in append()

Copy link
Copy Markdown
Member Author

@AndyZe AndyZe Dec 16, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Bug report here: #1813

Copy link
Copy Markdown
Member Author

@AndyZe AndyZe Dec 16, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Can we merge this in the meantime?

@AndyZe AndyZe force-pushed the andyz/ruckig_waypoint_batch branch from b1d06df to 196963d Compare December 16, 2022 14:46
@AndyZe AndyZe force-pushed the andyz/ruckig_waypoint_batch branch from 55f9ead to 9601311 Compare December 20, 2022 14:30
@AndyZe AndyZe merged commit 2e610aa into moveit:main Dec 20, 2022
@AndyZe AndyZe deleted the andyz/ruckig_waypoint_batch branch December 20, 2022 16:56
This was referenced Feb 19, 2023
Closed
Closed
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Reviewers

@sjahr sjahr sjahr left review comments

@henningkayser henningkayser henningkayser approved these changes

@stephanie-eng stephanie-eng stephanie-eng approved these changes

+1 more reviewer

@ChrisThrasher ChrisThrasher ChrisThrasher requested changes

Reviewers whose approvals may not affect merge requirements

Assignees

No one assigned

Labels

None yet

Projects

None yet

Milestone

No milestone

Development

Successfully merging this pull request may close these issues.

5 participants

@AndyZe @henningkayser @stephanie-eng @ChrisThrasher @sjahr