Start Joint State with Non-Zero Vel/Acc/Jerk for TrajOpt Solver

[gaokai15]

Hi! I'm trying to do trajectory optimization with start and goal joint states. The start and goal joint states could be with non-zero vel/acc/jerk. I use the last joint state of the previous trajectory as the start state for the next trajectory, but result.success =False. I checked the values of the state. None of them exceeds the maximum vel (indicated in urdf) or maximum acc/jerk (indicated in robot config file). Do you know what would be the potential issue? And in the config file, the "max_jerk" and "max_acceleration" are the acc/jerk bounds, right?

[balakumar-s]

We added optimizing from non-zero start velocity and acceleration. You can look at

curobo/examples/isaac_sim/motion_gen_reacher.py

Line 174 in 256227b

if args.reactive:

for how to change the motion gen configuration parameters to support non-static start state.

We do not yet support non-zero velocity, acceleration for the final state. This could be done by changing the tensor_step function which currently sets the last three time steps to the same action. Do you want to have a target joint velocity/acceleration as well?

Regarding max_jerk and max_acceleration, they are the bounds as you said.

[gaokai15]

Thank you for the reply! Which motion gen config parameters are for non-static start state? I see "trim_steps = [1, None]" may potentially output a solution with non-static start, but the start velocity is not related to the velocity I specified in start JointState, right?

On our side, we are looking for a way to do motion planning from a specific non-static start joint state to a target non-static goal joint state. Any insights or suggestions from you would be very helpful! Thank you for making curobo available.

[balakumar-s]

These are the parameters you need to change:

Parameter	Value	Why?
optimize_dt	False	Do not try to optimize over dt as changing dt will change the magnitude of initial velocity & acceleration
trajopt_dt	0.05	Reduce initial dt to get more optimal solution as we disabled optimization over dt
trajopt_tsteps	36	Since we are optimizing over smaller dt, we might need more timesteps to optimize.
trim_steps	[1, None]	Output trajectory will contain trajectory starting from [0-1,0,... T]. We hence trim the first timestep.

Regarding optimizing for target joint state, does that mean you want to have a specific target velocity and acceleration for the joints at the final timestep? This is something that can be exposed in a later release.

[gaokai15]

Thank you for the suggestions! Yes, having a specific target velocity and acceleration for the joints at the final timestep would be useful! Thanks for your work!