如何在Drake中将2DOF两指夹爪改为1DOF对称联动结构

问题描述 #

我需要在Drake中添加一个类WSG夹爪的SDF模型，要求将原本的两个移动关节（左、右手指各一个）改为1自由度的对称联动结构：

• 仅保留1个独立自由度，plant.num_positions()返回1
• 修改左手指关节值时，右手指自动产生该值-1倍的位移

尝试过在SDF中使用<mimic>标签，但plant.num_positions()仍统计两个关节；考虑过焊接右手指关节，但无法实现联动。

当前SDF中的关节定义如下：

<joint name="left_finger_sliding_joint" type="prismatic">
  <parent>body</parent>
  <child>left_finger</child>
  <axis>
    <xyz>-1 0 0</xyz>
    <!-- Drake attaches an actuator to all and only joints with a nonzero
         effort limit. -->
    <limit>
      <lower>-0.055</lower>
      <upper>0</upper>
      <effort>80</effort>
      <stiffness>15000</stiffness>
    </limit>
    <dynamics>
      <spring_reference>0</spring_reference>
      <spring_stiffness>0</spring_stiffness>
      <damping>0</damping>
      <friction>0</friction>
    </dynamics>
  </axis>
</joint>

<joint name="right_finger_sliding_joint" type="prismatic">
  <parent>body</parent>
  <child>right_finger</child>
  <axis>
    <xyz>1 0 0</xyz>
    <!-- Drake attaches an actuator to all and only joints with a nonzero
         effort limit. -->
    <limit>
      <lower>0</lower>
      <upper>0.055</upper>
      <effort>80</effort>
      <stiffness>15000</stiffness>
    </limit>
    <!-- Mimic the left finger sliding joint. This will make the right
         finger move in the opposite direction of the left finger. 
         This will reduce the dimensionality of the gripper's configuration -->
    <mimic joint="left_finger_sliding_joint">
      <multiplier>-1.</multiplier>
      <offset>0.</offset>
      <reference>0.</reference>
    </mimic>
    <dynamics>
      <spring_reference>0</spring_reference>
      <spring_stiffness>0</spring_stiffness>
      <damping>0</damping>
      <friction>0</friction>
    </dynamics>
  </axis>
</joint>

解决方案 #

核心原因 #

Drake的SDF解析器不会因为<mimic>标签移除被模仿关节的自由度，只会在运动学层面添加约束，因此plant.num_positions()仍会统计两个关节。要实现真正的1自由度，需要结合SDF定义和Drake编程接口处理。

步骤1：调整SDF文件 #

保留左手指关节作为主动关节，修改右手指关节配置：

• 移除右手指关节的<effort>项（避免Drake为其创建执行器）
• 保留<mimic>标签维持运动学关联
• 同步右手指关节的<limit>范围与左手指，避免约束冲突

修改后的右手指关节示例：

<joint name="right_finger_sliding_joint" type="prismatic">
  <parent>body</parent>
  <child>right_finger</child>
  <axis>
    <xyz>1 0 0</xyz>
    <limit>
      <lower>-0.055</lower>
      <upper>0</upper>
      <stiffness>15000</stiffness>
      <!-- 移除effort，不生成执行器 -->
    </limit>
    <mimic joint="left_finger_sliding_joint">
      <multiplier>-1.</multiplier>
      <offset>0.</offset>
      <reference>0.</reference>
    </mimic>
    <dynamics>
      <spring_reference>0</spring_reference>
      <spring_stiffness>0</spring_stiffness>
      <damping>0</damping>
      <friction>0</friction>
    </dynamics>
  </axis>
</joint>

步骤2：通过Drake API合并自由度 #

加载模型后，使用AddMimicJointConstraint强制将右手指关节的自由度标记为冗余，最终num_positions()会只统计主动关节的数量。

Python示例代码

from pydrake.multibody.plant import MultibodyPlant
from pydrake.multibody.parsing import Parser

# 创建MultibodyPlant实例
plant = MultibodyPlant(time_step=0.0)
parser = Parser(plant)
# 加载调整后的SDF模型
parser.AddModelFromFile("gripper.sdf")

# 获取左右手指关节
left_joint = plant.GetJointByName("left_finger_sliding_joint")
right_joint = plant.GetJointByName("right_finger_sliding_joint")

# 添加模仿约束，将右关节的运动绑定到左关节
plant.AddMimicJointConstraint(
    joint=right_joint,
    mimic_joint=left_joint,
    multiplier=-1.0,
    offset=0.0
)

# 初始化plant
plant.Finalize()

# 验证自由度数量，此时应返回1
print(f"Number of positions: {plant.num_positions()}")

C++示例代码

#include <drake/multibody/plant/multibody_plant.h>
#include <drake/multibody/parsing/parser.h>
#include <iostream>

int main() {
  // 创建MultibodyPlant实例
  auto plant = std::make_unique<drake::multibody::MultibodyPlant<double>>(0.0);
  drake::multibody::Parser parser(plant.get());
  // 加载调整后的SDF模型
  parser.AddModelFromFile("gripper.sdf");

  // 获取左右手指关节
  const auto& left_joint = plant->GetJointByName("left_finger_sliding_joint");
  const auto& right_joint = plant->GetJointByName("right_finger_sliding_joint");

  // 添加模仿约束
  plant->AddMimicJointConstraint(right_joint, left_joint, -1.0, 0.0);

  // 初始化plant
  plant->Finalize();

  // 验证自由度数量
  std::cout << "Number of positions: " << plant->num_positions() << std::endl;
  return 0;
}

验证效果 #

完成上述操作后：

• 修改左手指关节的位置值，右手指会自动产生-1倍的位移
• plant.num_positions()返回1，符合1自由度的需求

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