实现两吸盘间零件转移：添加双浮动关节报错的技术问询

问题背景 #

已实现单个吸盘通过添加QuaternionFloatingJoint将零件吸附至末端执行器的功能，核心代码如下：

def add_suction_joint(plant: MultibodyPlant) -> QuaternionFloatingJoint:
    suction_body = plant.GetBodyByName("combo_end_effector_link")

    object_id = plant.GetModelInstanceByName("part1")
    object_body = plant.GetBodyByName("part1_link", object_id)
    suction_joint = plant.AddJoint(
        QuaternionFloatingJoint(
            "suction_joint",
            suction_body.body_frame(),
            object_body.body_frame(),
        ),
    )

    translation = [0.03, 0, 0.34]
    rotation = RollPitchYaw(
        0,
        np.pi / 2,
        0,
    ).ToQuaternion()  # 调整位姿使抓手对齐关节轴
    suction_to_object = RigidTransform(rotation, translation)

    suction_joint.SetDefaultPose(suction_to_object)

但为第二个吸盘添加同类关节时触发报错，报错代码：

plant.AddJoint(
        QuaternionFloatingJoint(
            "gripper_2_joint",
            plant.GetBodyByName("gripper_2_link").body_frame(),
            object_body.body_frame(),
        ),
    )

错误信息：

RuntimeError: When creating a model, an attempt was made to add two inboard joints to the same rigid body; this is not allowed. One possible cause might be attempting to weld a robot to World somewhere other than its base rigid body; see Drake issue #17429 for discussion and work-arounds, e.g., reversing some joint parent/child directions. Another possible cause might be attempting to form a kinematic loop using joints; to create a loop, consider using a LinearBushingRollPitchYaw instead of a joint.

报错原因 #

Drake的多体约束规则明确：单个刚体不能同时拥有两个输入关节，因此无法同时用两个关节绑定同一个零件，报错逻辑合理。

可行解决方案 #

方案1：动态切换关节（刚性吸附首选） #

核心逻辑：同一时间仅保留一个绑定零件的吸盘关节，转移时先解绑第一个吸盘，再绑定第二个。
操作步骤：

1. 吸附阶段：为当前吸盘与零件添加QuaternionFloatingJoint，通过位姿变换固定相对位置。
2. 转移触发：
   • 调用plant.RemoveJoint()移除第一个吸盘的关节，让零件暂时处于自由状态（可配合SetFreeBodyPose临时固定位姿）。
   • 控制第二个吸盘移动到零件的目标吸附位置。
   • 为第二个吸盘与零件添加新的QuaternionFloatingJoint，设置正确的位姿变换。
     示例代码：

# 移除第一个吸盘关节
plant.RemoveJoint(plant.GetJointByName("suction_joint"))

# 此处添加第二个吸盘的运动控制代码，将其移动到零件位置

# 添加第二个吸盘的绑定关节
def add_second_suction_joint(plant: MultibodyPlant):
    gripper_2_body = plant.GetBodyByName("gripper_2_link")
    object_id = plant.GetModelInstanceByName("part1")
    object_body = plant.GetBodyByName("part1_link", object_id)
    
    gripper_2_joint = plant.AddJoint(
        QuaternionFloatingJoint(
            "gripper_2_joint",
            gripper_2_body.body_frame(),
            object_body.body_frame(),
        ),
    )
    
    # 根据第二个吸盘的实际位置调整位姿参数
    translation = [0.03, 0, 0.34]
    rotation = RollPitchYaw(0, np.pi/2, 0).ToQuaternion()
    gripper_to_object = RigidTransform(rotation, translation)
    
    gripper_2_joint.SetDefaultPose(gripper_to_object)

注意：Drake支持在仿真运行过程中动态添加/移除关节，需确保操作在仿真步间隙执行。

方案2：临时禁用物理属性+固定位姿 #

若不想动态操作关节，可通过禁用零件物理效果实现临时固定：

1. 第一个吸盘释放：
   • 调用plant.SetFreeBodyPose()将零件设置到第二个吸盘的目标位姿。
   • 禁用零件物理作用：plant.SetBodyActive(object_body, False)，此时零件不受重力、碰撞影响，保持固定位姿。
2. 第二个吸盘吸附：
   • 重新启用零件物理效果：plant.SetBodyActive(object_body, True)。
   • 添加第二个吸盘与零件的绑定关节，完成转移。
     该方案适合静态转移场景，若需要零件在转移过程中保持物理交互则不适用。

方案3：使用线性衬套替代关节（柔性吸附场景） #

如果不需要完全刚性的吸附，可使用LinearBushingRollPitchYaw替代关节，它允许微小位姿偏差，且不会触发多体约束冲突：

# 用线性衬套替代关节模拟吸附
bushing = plant.AddJoint(
    LinearBushingRollPitchYaw(
        "gripper_2_bushing",
        plant.GetBodyByName("gripper_2_link").body_frame(),
        plant.GetBodyByName("part1_link", plant.GetModelInstanceByName("part1")).body_frame(),
        RigidTransform(),  # 初始位姿变换
        # 刚度系数：数值越大，吸附越接近刚性
        kp=[1000, 1000, 1000, 100, 100, 100],
        # 阻尼系数：调整吸附的响应速度
        kd=[100, 100, 100, 10, 10, 10]
    )
)

这种方式可同时存在多个衬套，通过调整刚度参数模拟不同的吸附力度，适合需要柔性交互的场景。

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