Search code examples
drake

How to use ComPositionConstraint to do inverse kinematics?

There are AddPositionConstraint for PositionConstraint in the InverseKinematics class. Is there similar API for ComPositionConstraint? Thanks!

Solution

  • No we don't have AddComPositionConstraint yet. You can instantiate a ComPositionConstraint, and then add it to InverseKinematics. One example is as follows

    InverseKinematics ik(plant, plant_context)
com_position_constraint = ComPositionConstraint(plant, None, plant.world_frame(), plant_context)
# r is the variable for CoM position.
r = ik.prog().NewContinuousVariables(3)
ik.prog().AddConstraint(com_position_constraint, np.concatenate([ik.q(), r]))
# Set the desired Com position as r_des
r_des = np.array([0, 0, 1])
ik.prog().AddBoundingBoxConstraint(r_des, r_des, r)
# Set the initial guess for IK program.
q_init = ...
r_init = ...
ik.prog().SetInitialGuess(ik.q(), q_init)
ik.prog().SetInitialGuess(r, r_init)
result = Solve(ik.prog(), initial_guess)