CODEWITHSUNDEEP
pythonoptimizationdrake
saitcanbaskol
saitcanbaskol

Reputation: 15

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

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.

Upvotes: 1

Views: 118

Answers (1)

Russ Tedrake
Russ Tedrake

Reputation: 5533

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 (e.g. 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 (e.g. using MapVelocityToQdot).

Upvotes: 1

Related Questions