How to use KinematicTrajectoryOptimization in a context which has free body objects in it?

Question

We are trying to use KinematicTrajectoryOptimization to optimize our trajectory for basic pick and place. We are using 8th notebook provided by Russ Tedrake as a reference Deepnote . Normally, they do not have a free body object in the examples. But, we need an object on the space as well. Hence, we added an urdf model to use as a brick. However, after adding that box to the plant, obviously, the context dimensions change. With that change, it raises the following exception:

SystemExit: Failure at systems/trajectory_optimization/kinematic_trajectory_optimization.cc:351 in AddVelocityBounds(): condition 'lb.size() == num_positions()' failed.


/shared-libs/python3.8/py-core/lib/python3.8/site-packages/IPython/core/interactiveshell.py:3386: UserWarning: To exit: use 'exit', 'quit', or Ctrl-D.


warn("To exit: use 'exit', 'quit', or Ctrl-D.", stacklevel=1)

Is there a solution for that problem? It is probably because of additional quarternion.

Thanks in advance.

Answer 1

I agree with your analysis; if you have the extra body that num_positions() will increase by 7 (for x,y,z + quaternion). You can adjust add elements to your lower and upper bounds accordingly when calling Add*Bounds() -- infinite limits are fine.

As the documentation says, the velocities in the optimization (eg for AddVelocityBounds) will correspond to qdot, not v. The multibodyplant, on the other hand will use angular velocity in the state in place of qdot for the quaternion. You'll have to map between them yourself (eg using MapVelocityToQdot ).