How can Trajopt plan a trajectory that does not just point-to-point but has a lot of via points(cartesian space)

[saiyao233]

How can Trajopt plan a trajectory that does not just point-to-point but has a lot of via points(cartesian space)

[balakumar-s]

This is not implemented. You can add a cost term that adds a pose cost for every time step.

[saiyao233]

Do you have plans to implement this function soon?

[vonHartz]

Thanks for the reply @balakumar-s. I do not see how this could be solved with a PoseCost, as the pose penalty would be different for each point in the trajectory and the PoseCost just seems to be applied for each time step. Do you see any way to implement this in curobo?

@saiyao233 What did you end up doing?

[changhaonan]

Yeah. This can be a very useful function.

[balakumar-s]

You can write a custom cost function in pytorch to do this:
Example for tracking positions:

1. Current position: shape [batch, horizon, 3]
2. Goal position : shape [1, horizon, 3]

Add this before this line:

curobo/src/curobo/rollout/arm_reacher.py

Line 326 in 2fbffc3

with profiler.record_function("cat_sum"):

new code:

new_cost = 1000.0 *  torch.linalg.norm(goal_position - current_position, dim=-3)
cost_list.append(new_cost)

[changhaonan]

Thanks a lot for your suggestion. I will try about it!

[vonHartz]

Thanks for the suggestion, @balakumar-s .
I have tried for a while, but got some issues.

Here's a minimal example I've created: https://gist.github.com/vonHartz/a5a910694ca1e4bd5fa3856f785753be
Note that it uses the URDF from: https://github.com/stepjam/RLBench/tree/master

Here are the issues I've run into:

• The MotionGen does not seem to always pass a full trajectory, but sometimes only a batch of goal poses, resulting in a cost of shape [800, 1]. In these iterations https://gist.github.com/vonHartz/a5a910694ca1e4bd5fa3856f785753be#file-curobo-py-L147 fails, because my cost has shape [800, 188], where 188 is the traj duration.
• To solve the issue, I only apply the new_cost in iterations when goal_cost is not applied.
• However, as a result, the trajectory seems to completely ignore the trajectory cost. The first subplot shows the original trajectory (dotted) and the output of MotionGen (solid) as x,y,z coordinates (red, green and blue).
  
• Digging a bit deeper, trying to find out where the 800 comes from, it seems to come from the Particle Optimizer and is later reshaped to [32, 25, 1]. 32 is the number of batches of IK seeds, and 25 the number of particles. Ie. there is no time dimension. So can I even combine it with the traj cost?

Any idea what might be wrong?

Alternatively, can the waypoints be incorporated as constraints instead of costs?

Thanks!