Global trajectory optimization through N ordered waypoints

[jarkenau]

I have an ordered sequence of N end-effector pose targets, known up-front, and want a single joint-space trajectory that smoothly flows through all of them (non-zero velocity at intermediate waypoints and cross-segment optimization). Instead of chaining N independent plan_pose calls.

As far as I can tell, the public API doesn't expose a way to do this. The trajopt horizon is fixed at 1 in the YAML config:

curobo/curobo/content/configs/task/trajopt/transition_bspline_trajopt.yml

Line 9 in ac5f931

horizon: 1

Is this a use case you'd be interested in supporting, and is it something that could land on the roadmap?

[balakumar-s]

This is a useful use case, but full support is a fairly hard problem. To get global feasibility, we would also need waypoint support in the geometric planner, not just in the trajectory optimizer.

There are two intermediate steps we have been thinking about supporting:

1. Track pose waypoints with a local optimizer, for example using a Jacobian pseudoinverse method. This would not do collision checking, but it could be a useful implementation for free-space or lightly constrained use cases.
2. Add waypoint support to trajectory optimization and seed it with the locally optimized trajectories. This would include collision costs and may be sufficient for many applications, though it would still not be the same as full geometric planning with global feasibility across all waypoints.

Could you share a bit more about the use case you have in mind? In particular:

• is this for a specific task or application?
• roughly how many waypoints do you expect in a typical sequence?
• do you need collision checking / obstacle avoidance along the trajectory, or is this mostly for free-space motion through known pose waypoints?