如何为Pydrake逆运动学求解器的q添加临时关节位置约束？

我正在使用PydrakeInverseKinematics求解逆运动学，希望为某次特定的IK求解调用临时设置关节位置约束，而非修改全局的机器人关节限制。理想中可以调用类似ik_solver.AddConfigurationConstraint(lower_limits=[...], upper_limits=[...])的方法。

基础关节约束来自机器人plant，但plant初始化后无法修改关节限制。我知道可以每次重新初始化带不同关节约束的URDF，但这种方式太繁琐。

解决方案 #

无需重新初始化plant，直接通过IK求解器关联的MathematicalProgram为关节变量q添加临时边界约束即可，这种约束仅作用于当前的IK求解器实例，不会影响全局plant的关节限制设置。

具体实现步骤：

• 获取IK求解器对应的MathematicalProgram实例
• 通过ik.q()获取关节变量向量q
• 调用prog.AddBoundingBoxConstraint()为q添加自定义上下界约束

修改后的完整代码示例 #

self.ik = PydrakeInverseKinematics(self.plant, self.plant_context)  # type: ignore  # noqa: PGH003

self.ee_frame = self.plant.GetFrameByName(frame)

self.p_WG_lower = goal_pose.translation() - np.repeat(self.position_tolerance, 3)
self.p_WG_higher = goal_pose.translation() + np.repeat(self.position_tolerance, 3)

q0_node = initial_guess
q0 = np.array(q0_node)

self.ik.AddPositionConstraint(
    self.ee_frame,
    np.zeros(3),
    self.plant.world_frame(),
    self.p_WG_lower,
    self.p_WG_higher,
)

self.ik.AddOrientationConstraint(
    self.ee_frame,
    RotationMatrix(),
    self.plant.world_frame(),
    goal_pose.rotation(),
    self.orientation_tolerance,
)

prog = self.ik.get_mutable_prog()
q = self.ik.q()
prog.AddQuadraticErrorCost(np.identity(len(q)), q0, q)

# 添加临时关节位置约束（仅对本次IK求解生效）
# 示例：为前3个关节设置自定义上下界
custom_lower = np.array([-1.0, -0.5, -0.8])
custom_upper = np.array([1.0, 0.5, 0.8])
# 如果需要覆盖所有关节，确保custom_lower和custom_upper长度与q一致
prog.AddBoundingBoxConstraint(custom_lower, custom_upper, q[:3])

# if add_collision_constraint:
# self.ik.AddMinimumDistanceLowerBoundConstraint(0.00000001, 0.01)

prog.SetInitialGuess(q, q0)
options = SolverOptions()

result = NloptSolver().Solve(prog, solver_options=options)  # pyright: ignore[reportCallIssue]

说明 #

• AddBoundingBoxConstraint是Drake中用于添加变量上下界约束的通用方法，完全满足需求
• 可以针对部分关节（如示例中的q[:3]）或全部关节设置约束，灵活度高
• 该约束仅存在于当前PydrakeInverseKinematics实例对应的优化问题中，不会修改plant的全局关节限制，每次创建新的IK求解器实例时可重新定义不同约束

内容的提问来源于stack exchange，提问作者Jasmine Cheng