Motion generation with multiple intermediate target waypoints in world space

[uvikas]

Hello! I have an arm setup with a pre-recorded end-effector trajectory. I would like to generate a motion plan for the arm to follow this pre-recorded trajectory. I was wondering if this package supports generating a single motion plan with multiple intermediate target waypoint poses in world space using MotionGen.

One way of doing this is to generate separate motion plans between each intermediate target pose. But since non-zero vel/accel/jerk for final state is not supported, this doesn't yield a single smooth motion through waypoints. Would it be possible to directly define all intermediate target waypoints in a single MotionGen?

[balakumar-s]

We currently do not have a cost implementation that can track a pose per timestep. However, you can add it here:

curobo/src/curobo/rollout/arm_reacher.py

Line 239 in c09d949

with profiler.record_function("cost/pose"):

[balakumar-s]

We recently added constrained planning which would allow for tracking linear trajectories: https://curobo.org/source/advanced_examples/3_constrained_planning.html

This still does not account for waypoint following.

[blooop]

Thanks for adding the constrained planning feature!

I have created an example based off the constrained planning example to match my needs but would like some clarification on how specify the constraints.

I have 3 points in a right angle triangle.
start: (0,0,0)
p1: (1,0,0)
p2: (1,1,0)

to move from start -> p1 I have the robot constrained in orientation, z and move along the x direction and use hold_vec_weight= [1,1,1,1,0,1]
to move from p1 ->p2 I constrain in orientation, z and move along y with hold_vec_weight= [1,1,1,0,1,1]

I would like to move back to the start point but can't find the correct hold_vec_weight to do this.

I have tried:
[1, 1, 1, 0.5, 0.5, 1]
[1, 1, 1, 1.4142, 1.4142, 0]
[1, 1, 1, 1.4142, 1.4142, 1]
[1, 1, 1, 0.70710, 0.70710, 0]
[1, 1, 1, 0.70710, 0.70710, 1]

How would I go about constraining the pose of the end effector in a non axis aligned way?

Thanks