Pydrake: How to return joint positions in forward kinematics example?

Question

With reference to https://manipulation.csail.mit.edu/pick.html#example3 Example 3.3 Forward Kinematics for the gripper frame Deepnote Notebook

I'm trying to assign the values of the joint positions as well as the spatial pose of the gripper to variables after clicking the Stop JointSliders button but the joint positions that are being returned are the initial joint positions and not the final value of the sliders.

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}")

After opening meshcat and adjusting the sliders, the current output is:

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],
)

Where X_WThrow reflects the final pose, but q_Throw reflects the initial joint angles.

After manually adjusting the sliders and pressing stop button I'd like q_Throw to be set to the final joint positions. Additionally, it would be great to know what the best way to extract the value of X_WThrow as well as my current method is really hacky...

Thank you so much for your help!

Answer 1

As captured in https://github.com/RussTedrake/manipulation/issues/220, you captured one error in my use of clear_output... the text that was displayed was effectively delayed by one UI event.

It's true that the current implementation of JointSliders in Drake creates a new Context when you call Run, and does not return the values. And I agree that having it return the most recent joint positions when the method exits would be very useful and a strict improvement. I will open a PR to enable that.