0

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

zisangsang
  • 81
  • 6

1 Answers1

1

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)
Hongkai Dai
  • 2,546
  • 11
  • 12
  • 1
    Thinks!! Ithink the `AddConstraint` need change to `ik.prog().AddConstraint(com_position_constraint, np.concatenate((ik.q(), r)))` and it works well. – zisangsang Aug 31 '21 at 03:11