如何使用 ComPositionConstraint 做逆向运动学？

Question

InverseKinematics class 中有 AddPositionConstraint for PositionConstraint。 ComPositionConstraint 有类似的 API 吗？谢谢！

Answer 1

不，我们还没有 AddComPositionConstraint。您可以实例化一个 ComPositionConstraint，然后将其添加到 InverseKinematics。一个例子如下

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)

如何使用 ComPositionConstraint 做逆向运动学？

How to use ComPositionConstraint to do inverse kinematics?

drake