Hybrid Motion Planning

[RoBeau]

I’m trying to use the Hybrid Planning architecture for a simple demonstration using real hardware.

The goal is for the system to replan a trajectory that becomes invalid and splice the new trajectory into the current one without the arm stopping.

This seems like fairly basic functionality, the implementation I had in mind was

• Start executing the global trajectory
• Local planning loop
  • Check if remaining trajectory is invalid
  • if valid
    • Do nothing
  • If invalid
    • Return a waypoint some time in the future but before the collision to the hybrid manager
    • Get new motion plan from global planning use the pre-collision waypoint as the starting pose
    • Publish new trajectory to ros control with the correct time stamp to allow the trajectory substitution to splice the new solution into the current trajectory.

What I’m struggling with is how to implement this using the current hybrid planning architecture with the

• Trajectory Operator Plugin (TOP)
• Constraint Solver Plugin (TSP)
• Logic Plugin

The example ones are just checking and publishing the next waypoint in the reference trajectory which ignores any time parametrization and couples the arm speed with the local planner frequency. I would think to make this usable in real life you would need to send smaller sub sections of the global solution to ros control and let’s do its normal thing.

I have been testing an implementation that attempts this but I’m getting this jerky saw tooth behavior which I think is from ros2 control trajectory substitution trying to splice each new local solution that is published.

There doesn’t seem to be many people using this hybrid planning system yet, just wondering what the vision was with the plugins and if they actually make sense as a way to implement different types of behavior.

[github-actions]

This issue is being labeled as stale because it has been open 45 days with no activity. It will be automatically closed after another 45 days without follow-ups.

[github-actions]

This issue was closed because it has been stalled for 45 days with no activity.