在Drake中使用C-IRIS时忽略<mimic>冗余自由度并保留从动指碰撞检测的技术咨询

问题背景回顾 #

你之前通过SDF的<mimic>标签实现了夹爪两指的对称运动，但该方式并未减少MultibodyPlant的配置空间维度（num_positions()仍为2）。现在使用C-IRIS生成凸多面体时，需要将冗余自由度排除出配置空间，同时确保从动指的碰撞仍被正确检测。

以下是针对Pydrake 1.35.0的具体解决方案，分为冗余DOF处理和碰撞检测保障两部分：

解决方案步骤 #

1. 移除SDF中的<mimic>标签，改用程序级位置约束

SDF的<mimic>标签仅添加运动学约束，不会减少配置空间维度。我们需要在代码中主动关联主动指和从动指的位置，为后续C-IRIS的简化配置空间做准备：

import numpy as np
from pydrake.multibody.plant import AddMultibodyPlantSceneGraph
from pydrake.multibody.parsing import Parser
from pydrake.systems.framework import DiagramBuilder

# 构建包含夹爪的MultibodyPlant和SceneGraph
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
parser = Parser(plant)
# 加载无<mimic>标签的夹爪模型
gripper_model = parser.AddModelFromFile("your_gripper_model.sdf")
plant.Finalize()

# 获取主动指（左指）和从动指（右指）的关节
left_joint = plant.GetJointByName("left_finger_joint")
right_joint = plant.GetJointByName("right_finger_joint")

# 添加对称位置约束：从动指位置 = 主动指位置的对称值（根据实际运动方向调整符号）
# 示例：右指位置 = -左指位置（假设夹爪是向中心对称开合）
plant.AddPositionConstraint(
    joint=right_joint,
    joint_position_start=0,
    dependent_joint=left_joint,
    dependent_joint_position_start=0,
    position_function=lambda q_left: -q_left,
    velocity_function=lambda q_left: -1.0  # 速度约束（可选，保证运动同步）
)

2. 配置C-IRIS使用简化的1-DOF配置空间

C-IRIS的CirisCollisionChecker支持通过上下文映射函数将简化配置（仅主动指DOF）映射到完整的机器人上下文，从而忽略冗余DOF。同时，该函数会自动同步从动指的位置，确保碰撞检测包含从动指：

from pydrake.planning import CirisCollisionChecker, RobotDiagram
from pydrake.multibody.tree import BodyIndex

# 构建RobotDiagram（C-IRIS需要的封装结构）
diagram = builder.Build()
robot_diagram = RobotDiagram(diagram)

# 定义简化配置到完整上下文的映射函数
def map_simplified_to_full(simplified_q, context):
    # simplified_q是1维数组，仅包含主动指的位置
    full_context = diagram.GetMyContextFromRoot(context)
    plant_context = plant.GetMyContextFromRoot(full_context)
    
    # 获取当前完整配置
    full_q = plant.GetPositions(plant_context).copy()
    # 设置主动指位置
    full_q[left_joint.position_start()] = simplified_q[0]
    # 设置从动指为对称位置
    full_q[right_joint.position_start()] = -simplified_q[0]
    # 更新上下文
    plant.SetPositions(plant_context, full_q)
    return full_context

# 定义需要检查的碰撞对（必须包含从动指与所有障碍物的组合）
def get_collision_pairs(plant):
    collision_pairs = []
    # 获取夹爪的左右指Body
    left_finger = plant.GetBodyByName("left_finger_body")
    right_finger = plant.GetBodyByName("right_finger_body")
    # 获取所有障碍物的Body（假设障碍物在名为"obstacles"的模型实例中）
    obstacle_model = parser.GetModelInstanceByName("obstacles")
    obstacle_bodies = [plant.GetBodyByIndex(i) for i in plant.GetBodyIndices(obstacle_model)]
    
    # 添加左指与障碍物的碰撞对
    for obs_body in obstacle_bodies:
        collision_pairs.append((left_finger.index(), obs_body.index()))
    # 添加右指与障碍物的碰撞对
    for obs_body in obstacle_bodies:
        collision_pairs.append((right_finger.index(), obs_body.index()))
    # 可选：添加左右指之间的碰撞对
    collision_pairs.append((left_finger.index(), right_finger.index()))
    return collision_pairs

# 创建C-IRIS碰撞检查器
collision_checker = CirisCollisionChecker(
    robot_diagram=robot_diagram,
    configuration_space_dimension=1,  # 仅保留1个主动DOF
    context_mapper=map_simplified_to_full,
    collision_pairs=get_collision_pairs(plant)
)

3. 验证配置与碰撞检测

通过以下代码验证配置空间维度和碰撞检测的正确性：

# 验证C-IRIS的配置空间维度（应为1）
print(f"C-IRIS配置空间维度: {collision_checker.configuration_space_dimension()}")

# 测试主动指移动时的碰撞检测
test_q = np.array([0.1])  # 主动指移动0.1单位
is_colliding = collision_checker.CheckCollision(test_q)
print(f"当前配置是否碰撞: {is_colliding}")

关键原理说明 #

• 冗余DOF处理：通过context_mapper函数，C-IRIS仅接收主动指的1-DOF配置，无需感知从动指的冗余DOF，从而将配置空间维度从2降至1。
• 从动指碰撞保障：每次碰撞检测前，context_mapper会自动将从动指的位置同步为主动指的对称值，再执行碰撞检查，确保从动指的几何与障碍物的碰撞被正确检测。
• 替代方案提示：若你需要更深度的配置空间简化，可以考虑使用Drake的MultibodyPlant的冗余关节移除功能，但该功能在Python中实现复杂，上述方法更适合快速落地。

备注：内容来源于stack exchange，提问作者Julia López Gómez