Pydrake正运动学示例：如何获取滑块最终关节位置？

问题描述 #

使用Drake的JointSliders调整机械臂关节后，点击Stop按钮，返回的关节位置q_Throw始终是初始值，无法获取滑块调整后的最终值；虽然夹具位姿X_WThrow能反映最终状态，但通过自定义LeafSystem的Publish事件提取的方式过于繁琐。

原代码 #

import numpy as np
from IPython.display import clear_output, display
from pydrake.all import (AbstractValue, AddMultibodyPlantSceneGraph,
                         DiagramBuilder, JointSliders, LeafSystem,
                         MeshcatVisualizer, Parser, RigidTransform,
                         RollPitchYaw, StartMeshcat)

from manipulation import FindResource, running_as_notebook
from manipulation.scenarios import AddMultibodyTriad, AddPackagePaths

meshcat = StartMeshcat()


def gripper_forward_kinematics_example():
    builder = DiagramBuilder()

    plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0)
    parser = Parser(plant)
    AddPackagePaths(parser)
    parser.AddAllModelsFromFile(FindResource("models/iiwa_and_wsg.dmd.yaml"))
    plant.Finalize()

    # Draw the frames
    for body_name in ["iiwa_link_1", "iiwa_link_2", "iiwa_link_3", "iiwa_link_4", "iiwa_link_5", "iiwa_link_6", "iiwa_link_7", "body"]:
        AddMultibodyTriad(plant.GetFrameByName(body_name), scene_graph)

    meshcat.Delete()
    meshcat.DeleteAddedControls()
    visualizer = MeshcatVisualizer.AddToBuilder(
        builder, scene_graph.get_query_output_port(), meshcat)

    wsg = plant.GetModelInstanceByName("wsg")
    gripper = plant.GetBodyByName("body", wsg)

    X_WResult = None
    my_context = None
    class PrintPose(LeafSystem):
        def __init__(self, body_index):
            LeafSystem.__init__(self)
            self._body_index = body_index
            self.DeclareAbstractInputPort("body_poses",
                                        AbstractValue.Make([RigidTransform()]))
            self.DeclareForcedPublishEvent(self.Publish)

        def Publish(self, context):
            pose = self.get_input_port().Eval(context)[self._body_index]
            clear_output(wait=True)
            print("gripper position (m): " + np.array2string(
                pose.translation(), formatter={
                    'float': lambda x: "{:3.2f}".format(x)}))
            print("gripper roll-pitch-yaw (rad):" + np.array2string(
                RollPitchYaw(pose.rotation()).vector(),
                            formatter={'float': lambda x: "{:3.2f}".format(x)}))
            nonlocal X_WResult
            X_WResult = pose

    print_pose = builder.AddSystem(PrintPose(gripper.index()))
    builder.Connect(plant.get_body_poses_output_port(),
                    print_pose.get_input_port())

    default_interactive_timeout = None if running_as_notebook else 1.0
    sliders = builder.AddSystem(JointSliders(meshcat, plant))
    diagram = builder.Build()
    
    sliders.Run(diagram, default_interactive_timeout)
    context = diagram.CreateDefaultContext()
    plant_context = plant.GetMyContextFromRoot(context)
    meshcat.DeleteAddedControls()
    return plant.GetPositions(plant_context), X_WResult

q_Throw, X_WThrow = gripper_forward_kinematics_example()
print(f"q_Throw: {q_Throw}")
print(f"X_WThrow: {X_WThrow}")

原输出 #

gripper position (m): [0.00 -0.50 0.05]
gripper roll-pitch-yaw (rad):[-0.65 -0.00 0.00]
q_Throw: [-1.57  0.1   0.   -1.2   0.    1.6   0.    0.    0.  ]
X_WThrow: RigidTransform(
  R=RotationMatrix([
    [0.9999996829318354, -0.0006327896728353677, -0.0004834392000864399],
    [0.0007963267107333013, 0.794635497803698, 0.6070863130511505],
    [6.159035539635457e-17, -0.6070865055389548, 0.7946357497573973],
  ]),
  p=[0.00039543349390139975, -0.4965717753679821, 0.04892577743118429],
)

解决方案 #

1. 获取最终关节位置 #

问题核心是sliders.Run()后创建了全新的默认上下文，而非使用滑块运行后已更新的上下文。需从sliders的上下文提取最终关节值。

2. 简化夹具位姿提取 #

无需自定义LeafSystem存储位姿，直接用Drake内置API计算夹具的世界位姿，逻辑更简洁。

修改后的完整代码 #

import numpy as np
from IPython.display import clear_output
from pydrake.all import (AddMultibodyPlantSceneGraph,
                         DiagramBuilder, JointSliders,
                         MeshcatVisualizer, Parser, RigidTransform,
                         RollPitchYaw, StartMeshcat)

from manipulation import FindResource, running_as_notebook
from manipulation.scenarios import AddMultibodyTriad, AddPackagePaths

meshcat = StartMeshcat()


def gripper_forward_kinematics_example():
    builder = DiagramBuilder()

    plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0)
    parser = Parser(plant)
    AddPackagePaths(parser)
    parser.AddAllModelsFromFile(FindResource("models/iiwa_and_wsg.dmd.yaml"))
    plant.Finalize()

    # Draw the frames
    for body_name in ["iiwa_link_1", "iiwa_link_2", "iiwa_link_3", "iiwa_link_4", "iiwa_link_5", "iiwa_link_6", "iiwa_link_7", "body"]:
        AddMultibodyTriad(plant.GetFrameByName(body_name), scene_graph)

    meshcat.Delete()
    meshcat.DeleteAddedControls()
    visualizer = MeshcatVisualizer.AddToBuilder(
        builder, scene_graph.get_query_output_port(), meshcat)

    wsg = plant.GetModelInstanceByName("wsg")
    gripper = plant.GetBodyByName("body", wsg)

    default_interactive_timeout = None if running_as_notebook else 1.0
    sliders = builder.AddSystem(JointSliders(meshcat, plant))
    diagram = builder.Build()
    
    # 运行滑块交互，完成后diagram上下文已更新为最终状态
    sliders.Run(diagram, default_interactive_timeout)
    
    # 获取更新后的上下文
    diagram_context = diagram.GetMyContextFromRoot(sliders.context())
    plant_context = plant.GetMyContextFromRoot(diagram_context)
    
    # 提取最终关节位置
    q_final = plant.GetPositions(plant_context)
    
    # 直接计算夹具世界位姿
    X_W_gripper = plant.CalcBodyPoseInWorld(plant_context, gripper)
    
    # 打印位姿信息（可选）
    clear_output(wait=True)
    print("gripper position (m): " + np.array2string(
        X_W_gripper.translation(), formatter={
            'float': lambda x: "{:3.2f}".format(x)}))
    print("gripper roll-pitch-yaw (rad):" + np.array2string(
        RollPitchYaw(X_W_gripper.rotation()).vector(),
                    formatter={'float': lambda x: "{:3.2f}".format(x)}))
    
    meshcat.DeleteAddedControls()
    return q_final, X_W_gripper

q_Throw, X_WThrow = gripper_forward_kinematics_example()
print(f"q_Throw: {q_Throw}")
print(f"X_WThrow: {X_WThrow}")

修改说明 #

• 关节位置获取：通过sliders.context()获取滑块运行后的上下文，从中提取plant的上下文，确保拿到调整后的最终关节值。
• 位姿提取简化：使用plant.CalcBodyPoseInWorld()直接计算夹具的世界位姿，移除了自定义系统和全局变量，逻辑更清晰。

内容的提问来源于stack exchange，提问作者Shao Yuan Chew Chia