Constraints on the initial condition

[patil-apurva]

In the cuRobo report it is mentioned that the first and last three states of the trajectory are implicitly made the same. If this is the case the velocity and acceleration of the first time step should be exactly zero. But we observe that the initial velocities are close to zero; not exactly zero. And the initial accelerations are not close to zero. Can you please explain the reason?

Also, if we want to start from some given non-zero velocities and accelerations, how can we make sure that the initial velocities and accelerations of the trajectory are exactly the same as the the given values?

[balakumar-s]

cuRobo takes into account the initial velocity and acceleration by computing previous positions (before t = 0) through backward integration here:

curobo/src/curobo/curobolib/cpp/tensor_step_kernel.cu

Line 254 in 3690d28

in_pos[0] = st_pos - dt * (st_vel - (0.5 * st_acc * dt)); // start -1, start, u0, u1

But we observe that the initial velocities are close to zero; not exactly zero. And the initial accelerations are not close to zero

Since we are doing this backward integration, there will be numerical precision issues causing the velocity and acceleration to be not exactly zero. How far away is it from zero?

Also, if we want to start from some given non-zero velocities and accelerations, how can we make sure that the initial velocities and accelerations of the trajectory are exactly the same as the the given values?

You can do this by setting optimize_dt to False: https://curobo.org/_api/curobo.wrap.reacher.motion_gen.html#curobo.wrap.reacher.motion_gen.MotionGenConfig.optimize_dt

This is available as an input argument to https://curobo.org/_api/curobo.wrap.reacher.motion_gen.html#curobo.wrap.reacher.motion_gen.MotionGenConfig.load_from_robot_config

[patil-apurva]

Thank you for your reply.

The initial velocities are pretty close to zero. So initial velocities is not a concern. But the initial accelerations are far off from zero. The values of initial acceleration we get for the Kinova robot are as follows:
2.31, 2.73, 0.99, -1.56, -0.30, 0.56, 1.11.

As you mentioned before, the first and last three states of the trajectory are implicitly made the same. How is this achieved? Are equality constraints placed on these states, or is there an associated cost? Could you please direct me to the specific code block where these constraints or costs are set?

[balakumar-s]

The first and last three positions are implicitly made same by:

1. using start_position as the value of position for first three time steps
2. using the same index of u_position for last three time steps:
   a.

   curobo/src/curobo/curobolib/cpp/tensor_step_kernel.cu

   Line 292 in 3690d28

   in_pos[3] = u_position[b_addrs + (h_idx - 1) * dof + d_idx];

   
   b.

   curobo/src/curobo/curobolib/cpp/tensor_step_kernel.cu

   Line 306 in 3690d28

   out_pos = in_pos[3];

[patil-apurva]

When I set optimize_dtto False, I get the following error:

Traceback (most recent call last): File "/pkgs/curobo/examples/motion_gen_example_test_goal_change.py", line 180, in <module> demo_motion_gen() File "/pkgs/curobo/examples/motion_gen_example_test_goal_change.py", line 57, in demo_motion_gen motion_gen.warmup(parallel_finetune=True) File "/usr/local/lib/python3.10/dist-packages/curobo/wrap/reacher/motion_gen.py", line 1869, in warmup self.plan_single_js( File "/usr/local/lib/python3.10/dist-packages/curobo/wrap/reacher/motion_gen.py", line 2081, in plan_single_js result = self._plan_js_from_solve_state( File "/usr/local/lib/python3.10/dist-packages/curobo/wrap/reacher/motion_gen.py", line 3701, in _plan_js_from_solve_state opt_dt = torch.min(opt_dt[traj_result.success]) IndexError: too many indices for tensor of dimension 0

I haven't tried to debug it myself. Is there a quick fix to remove this error?
Also, can you please explain the reason behind setting optimize_dt to False in order to start from non-zero velocities and accelerations?

[balakumar-s]

Is it possible to use Cartesian Pose target for planning? Joint space planning does not work with optimize_dt=False.

[balakumar-s]

We have a fix for this in the next two days. Also note that the convergence from non-static initial state will be worse as we haven't tried to improve it.