物理仿真中直接控制工具6DOF位姿的最简方法？现有方案遇阻

我希望在物理仿真过程中直接控制工具的6DOF位姿，不用依赖机器人驱动。最直观的思路是给plant加3个移动关节（PrismaticJoint）和3个转动关节（RevoluteJoint）对应6个自由度，但没成功。

尝试的代码（仅x、Rx自由度，其余自由度操作类似） #

screwdriver_x_joint = plant.AddJoint(
    PrismaticJoint(
        "screwdriver_x_joint",
        plant.world_frame(),
        plant.GetFrameByName("screwdriver_joint_frame"),
        axis=[1,0,0],
    )
)
plant.AddJointActuator("screwdriver_x_joint", screwdriver_x_joint)

screwdriver_rx_joint = plant.AddJoint(
    RevoluteJoint(
        "screwdriver_rx_joint",
        plant.world_frame(),
        plant.GetFrameByName("screwdriver_joint_frame"),
        axis=[1,0,0],
    )
)
plant.AddJointActuator("screwdriver_rx_joint", screwdriver_rx_joint)

遇到的错误 #

This MultibodyGraph already has a joint 'screwdriver_x_joint' connecting 'world' to 'screwdriver'. Therefore adding joint 'screwdriver_rx_joint' connecting 'world' to 'screwdriver' is not allowed.

我看到过一种方案是加额外虚拟刚体再挂载驱动器，但太繁琐，有没有更优的方法？

直接用**6自由度自由关节（FreeJoint）**就能解决问题，这是最简洁的方案，不需要额外虚拟刚体。

FreeJoint本身就集成了3个移动自由度和3个转动自由度，正好匹配工具的6DOF位姿控制需求。你只需要添加一个FreeJoint连接世界坐标系和工具的关节框架，再给这个关节添加驱动器即可。

示例代码如下：

# 添加6自由度自由关节
screwdriver_free_joint = plant.AddJoint(
    FreeJoint(
        "screwdriver_free_joint",
        plant.world_frame(),
        plant.GetFrameByName("screwdriver_joint_frame"),
    )
)
# 给自由关节添加驱动器，控制所有6个自由度
plant.AddJointActuator("screwdriver_free_joint", screwdriver_free_joint)

这样就能直接控制工具的6DOF位姿，既避开了同一刚体对间不能添加多个关节的限制，又省去了虚拟刚体的繁琐操作。如果需要单独控制某个自由度，也可以通过设置驱动器参数来指定激活的自由度。

内容的提问来源于stack exchange，提问作者Antonia Bronars