Constraints for Direct Collocation method in pydrake

user_19079619 0
Viewed 58

I am looking for a way to describe the constraints of the Direct Collocation method in pydrake.
I got the robot model from my own URDF by using FindResource as this(l11-16).
Then, I tried to make some functions which calculate the positions of the joints as swing_foot_height(q) of this.
However there is a problem. It is maybe a type error.
I defined q as following

robot = MultibodyPlant(time_step=0.0)
scene_graph = SceneGraph()
robot.RegisterAsSourceForSceneGraph(scene_graph)
file_name = FindResource("models/robot.urdf")
Parser(robot).AddModelFromFile(file_name)
robot.Finalize()
context = robot.CreateDefaultContext()

dircol = DirectCollocation(
    robot,
    context,
    ...(Omission)...
    input_port_index=robot.get_actuation_input_port().get_index())

x = dircol.state()
nq = biped_robot.num_positions()
q = x[0:nq]

Then, I used this q for the function like swing_foot_height(q). The error is like

SetPositions(): incompatible function arguments. The following argument types are supported:
...
q: numpy.ndarray[numpy.float64[m, 1]]
...
Invoked with:
...
array([Variable('x(0)', Continuous), ... Variable('x(9)', Continuous)],dtype=object)

Are there some way to avoid this error?

drake
2 Answers

As far as I understand this, trajectory optimization with DirectCollocation converts the data type of the decision variables (in your case, x and q) to AutoDiffXd type. That is the type you're seeing here in the "Invoked with" error message. This is the type used for automatic differentiation which is used for finding the gradients for the optimization solver.

You'll need to convert back to float to use the SetPositions() function.

user_145075020

Right. In the compass gait notebook that you cited, there was an important line:

# overwrite MultibodyPlant with its autodiff copy
compass_gait = compass_gait.ToAutoDiffXd()

so that multibody plant that was being used in the constraint is actually an AutoDiffXd version of the plant.

The littledog notebook has more examples of this, with a more robust implementation that works for both float and autodiff constraint evaluations.

user_95100200
Related