# Imports import numpy as np import altair as alt import pydot from IPython.display import display, SVG, clear_output import plotly.express as px from pydrake.all import ( AddMultibodyPlantSceneGraph, AngleAxis, BasicVector, DiagramBuilder, FindResourceOrThrow, Integrator, JacobianWrtVariable, LeafSystem, MeshcatVisualizerCpp, MultibodyPlant, MultibodyPositionToGeometryPose, Parser, PiecewisePose, PiecewisePolynomial, Quaternion, RigidTransform, RollPitchYaw, RotationMatrix, SceneGraph, Simulator, TrajectorySource ) from pydrake.examples.manipulation_station import ManipulationStation from manipulation import running_as_notebook from manipulation.meshcat_cpp_utils import ( StartMeshcat, MeshcatJointSlidersThatPublish) from manipulation.scenarios import AddMultibodyTriad, SetColor # TODO(russt): Move this to drake (adding the element name support to the base class). import pandas as pd def dataframe(trajectory, times, names): assert trajectory.rows() == len(names) values = trajectory.vector_values(times) data = {'t': times } for i in range(len(names)): data[names[i]] = values[i,:] return pd.DataFrame(data)

# Start the visualizer. meshcat = StartMeshcat()

def kinematic_tree_example(): plant = MultibodyPlant(time_step=0.0) parser = Parser(plant) parser.AddModelFromFile(FindResourceOrThrow( "drake/manipulation/models/allegro_hand_description/sdf/allegro_hand_description_right.sdf")) parser.AddModelFromFile(FindResourceOrThrow( "drake/examples/manipulation_station/models/061_foam_brick.sdf")) plant.Finalize() # TODO(russt): Add floating base connections # TODO(russt): Consider a more interactive javascript rendering? display(SVG(pydot.graph_from_dot_data(plant.GetTopologyGraphvizString())[0].create_svg())) kinematic_tree_example()

def gripper_forward_kinematics_example(): builder = DiagramBuilder() # TODO: Replace this with a simple model directive of iiwa+wsg (no clutter bins) station = builder.AddSystem(ManipulationStation()) station.SetupClutterClearingStation() station.Finalize() plant = station.get_multibody_plant() scene_graph = station.get_scene_graph() # 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() visualizer = MeshcatVisualizerCpp.AddToBuilder( builder, station.GetOutputPort("query_object"), meshcat) diagram = builder.Build() context = diagram.CreateDefaultContext() gripper = plant.GetBodyByName("body") def pose_callback(context): pose = plant.EvalBodyPoseInWorld(context, gripper) ## This is the important line 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)})) sliders = MeshcatJointSlidersThatPublish( meshcat, plant, visualizer, context) sliders.Run(pose_callback) gripper_forward_kinematics_example()

meshcat.DeleteAddedControls()

def num_positions_velocities_example(): plant = MultibodyPlant(time_step = 0.0) Parser(plant).AddModelFromFile(FindResourceOrThrow( "drake/examples/manipulation_station/models/061_foam_brick.sdf")) plant.Finalize() context = plant.CreateDefaultContext() print(context) print(f"plant.num_positions() = {plant.num_positions()}") print(f"plant.num_velocities() = {plant.num_velocities()}") num_positions_velocities_example()

def pick_and_place_jacobians_example(): builder = DiagramBuilder() # TODO: Replace this with a simple model directive of iiwa+wsg (no clutter bins) station = builder.AddSystem(ManipulationStation()) station.SetupClutterClearingStation() station.Finalize() meshcat.Delete() visualizer = MeshcatVisualizerCpp.AddToBuilder( builder, station.GetOutputPort("query_object"), meshcat) diagram = builder.Build() context = diagram.CreateDefaultContext() plant = station.get_multibody_plant() G = plant.GetBodyByName("body").body_frame() W = plant.world_frame() def pose_callback(context): J_G = plant.CalcJacobianSpatialVelocity(context, JacobianWrtVariable.kQDot, G, [0,0,0], W, W) ## This is the important line clear_output(wait=True) print("J_G:") print(np.array2string(J_G, formatter={ 'float': lambda x: "{:5.1f}".format(x)})) print( f"smallest singular value(J_G): {np.min(np.linalg.svd(J_G, compute_uv=False))}") # If you want to set the initial positions manually, use this: #plant.SetPositions(plant.GetMyContextFromRoot(context), # plant.GetModelInstanceByName("iiwa"), # [0, 0, 0, 0, 0, 0, 0]) sliders = MeshcatJointSlidersThatPublish( meshcat, plant, visualizer, context) sliders.Run(pose_callback) pick_and_place_jacobians_example()

meshcat.DeleteAddedControls()

# We can write a new System by deriving from the LeafSystem class. # There is a little bit of boiler plate, but hopefully this example makes sense. class PseudoInverseController(LeafSystem): def __init__(self, plant): LeafSystem.__init__(self) self._plant = plant self._plant_context = plant.CreateDefaultContext() self._iiwa = plant.GetModelInstanceByName("iiwa") self._G = plant.GetBodyByName("body").body_frame() self._W = plant.world_frame() self.DeclareVectorInputPort("iiwa_position", BasicVector(7)) self.DeclareVectorOutputPort("iiwa_velocity", BasicVector(7), self.CalcOutput) def CalcOutput(self, context, output): q = self.get_input_port().Eval(context) self._plant.SetPositions(self._plant_context, self._iiwa, q) J_G = self._plant.CalcJacobianSpatialVelocity( self._plant_context, JacobianWrtVariable.kQDot, self._G, [0,0,0], self._W, self._W) J_G = J_G[:,0:7] # Ignore gripper terms V_G_desired = np.array([0, # rotation about x -.1, # rotation about y 0, # rotation about z 0, # x -.05, # y -.1]) # z v = np.linalg.pinv(J_G).dot(V_G_desired) output.SetFromVector(v) def jacobian_controller_example(): builder = DiagramBuilder() station = builder.AddSystem(ManipulationStation()) station.SetupClutterClearingStation() station.Finalize() controller = builder.AddSystem(PseudoInverseController( station.get_multibody_plant())) integrator = builder.AddSystem(Integrator(7)) builder.Connect(controller.get_output_port(), integrator.get_input_port()) builder.Connect(integrator.get_output_port(), station.GetInputPort("iiwa_position")) builder.Connect(station.GetOutputPort("iiwa_position_measured"), controller.get_input_port()) meshcat.Delete() visualizer = MeshcatVisualizerCpp.AddToBuilder( builder, station.GetOutputPort("query_object"), meshcat) diagram = builder.Build() simulator = Simulator(diagram) station_context = station.GetMyContextFromRoot(simulator.get_mutable_context()) station.GetInputPort("iiwa_feedforward_torque").FixValue(station_context, np.zeros((7,1))) station.GetInputPort("wsg_position").FixValue(station_context, [0.1]) integrator.set_integral_value( integrator.GetMyContextFromRoot(simulator.get_mutable_context()), station.GetIiwaPosition(station_context)) simulator.set_target_realtime_rate(1.0) simulator.AdvanceTo(0.01); return simulator simulator = jacobian_controller_example()

# Wait for the model to load in the visualizer, then run this cell to see the interesting part... simulator.AdvanceTo(5.0 if running_as_notebook else 0.1);

def grasp_poses_example(): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step = 0.0) parser = Parser(plant, scene_graph) grasp = parser.AddModelFromFile(FindResourceOrThrow( "drake/manipulation/models/wsg_50_description/sdf/schunk_wsg_50_no_tip.sdf"), "grasp") pregrasp = parser.AddModelFromFile(FindResourceOrThrow( "drake/manipulation/models/wsg_50_description/sdf/schunk_wsg_50_no_tip.sdf"), "pregrasp") brick = parser.AddModelFromFile(FindResourceOrThrow( "drake/examples/manipulation_station/models/061_foam_brick.sdf")) plant.Finalize() B_O = plant.GetBodyByName("base_link", brick) B_Ggrasp = plant.GetBodyByName("body", grasp) B_Gpregrasp = plant.GetBodyByName("body", pregrasp) # Set the pregrasp to be green and slightly transparent. inspector = scene_graph.model_inspector() for body_index in plant.GetBodyIndices(pregrasp): SetColor( scene_graph, [0, .6, 0, .5], plant.get_source_id(), inspector.GetGeometries(plant.GetBodyFrameIdOrThrow(body_index))) meshcat.Delete() meshcat.SetProperty("/Background", "visible", False) visualizer = MeshcatVisualizerCpp.AddToBuilder( builder, scene_graph, meshcat) diagram = builder.Build() context = diagram.CreateDefaultContext() plant_context = plant.GetMyContextFromRoot(context) # TODO(russt): Set a random pose of the object. # Get the current object, O, pose X_WO = plant.EvalBodyPoseInWorld(plant_context, B_O) p_GgraspO = [0, 0.12, 0] R_GgraspO = RotationMatrix.MakeXRotation(np.pi/2.0).multiply( RotationMatrix.MakeZRotation(np.pi/2.0)) X_GgraspO = RigidTransform(R_GgraspO, p_GgraspO) X_OGgrasp = X_GgraspO.inverse() X_WGgrasp = X_WO.multiply(X_OGgrasp) # pregrasp is negative y in the gripper frame (see the figure!). X_GgraspGpregrasp = RigidTransform([0, -0.08, 0]) X_WGpregrasp = X_WGgrasp @ X_GgraspGpregrasp plant.SetFreeBodyPose(plant_context, B_Ggrasp, X_WGgrasp) # Open the fingers, too. plant.GetJointByName("left_finger_sliding_joint", grasp).set_translation(plant_context, -0.054) plant.GetJointByName("right_finger_sliding_joint", grasp).set_translation(plant_context, 0.054) plant.SetFreeBodyPose(plant_context, B_Gpregrasp, X_WGpregrasp) # Open the fingers, too. plant.GetJointByName("left_finger_sliding_joint", pregrasp).set_translation(plant_context, -0.054) plant.GetJointByName("right_finger_sliding_joint", pregrasp).set_translation(plant_context, 0.054) diagram.Publish(context) grasp_poses_example()

def make_gripper_frames(X_G, X_O): """ Takes a partial specification with X_G["initial"] and X_O["initial"] and X_0["goal"], and returns a X_G and times with all of the pick and place frames populated. """ # Define (again) the gripper pose relative to the object when in grasp. p_GgraspO = [0, 0.12, 0] R_GgraspO = RotationMatrix.MakeXRotation(np.pi/2.0).multiply( RotationMatrix.MakeZRotation(np.pi/2.0)) X_GgraspO = RigidTransform(R_GgraspO, p_GgraspO) X_OGgrasp = X_GgraspO.inverse() # pregrasp is negative y in the gripper frame (see the figure!). X_GgraspGpregrasp = RigidTransform([0, -0.08, 0]) X_G["pick"] = X_O["initial"].multiply(X_OGgrasp) X_G["prepick"] = X_G["pick"].multiply(X_GgraspGpregrasp) X_G["place"] = X_O["goal"].multiply(X_OGgrasp) X_G["preplace"] = X_G["place"].multiply(X_GgraspGpregrasp) # I'll interpolate a halfway orientation by converting to axis angle and halving the angle. X_GprepickGpreplace = X_G["prepick"].inverse().multiply(X_G["preplace"]) angle_axis = X_GprepickGpreplace.rotation().ToAngleAxis() X_GprepickGclearance = RigidTransform(AngleAxis(angle=angle_axis.angle()/2.0, axis=angle_axis.axis()), X_GprepickGpreplace.translation()/2.0 + np.array([0, -0.3, 0])) X_G["clearance"] = X_G["prepick"].multiply(X_GprepickGclearance) # Now let's set the timing times = {"initial": 0} X_GinitialGprepick = X_G["initial"].inverse().multiply(X_G["prepick"]) times["prepick"] = times["initial"] + 10.0*np.linalg.norm(X_GinitialGprepick.translation()) # Allow some time for the gripper to close. times["pick_start"] = times["prepick"] + 2.0 times["pick_end"] = times["pick_start"] + 2.0 X_G["pick_start"] = X_G["pick"] X_G["pick_end"] = X_G["pick"] times["postpick"] = times["pick_end"] + 2.0 X_G["postpick"] = X_G["prepick"] time_to_from_clearance = 10.0*np.linalg.norm(X_GprepickGclearance.translation()) times["clearance"] = times["postpick"] + time_to_from_clearance times["preplace"] = times["clearance"] + time_to_from_clearance times["place_start"] = times["preplace"] + 2.0 times["place_end"] = times["place_start"] + 2.0 X_G["place_start"] = X_G["place"] X_G["place_end"] = X_G["place"] times["postplace"] = times["place_end"] + 2.0 X_G["postplace"] = X_G["preplace"] return X_G, times X_G = {"initial": RigidTransform(RotationMatrix.MakeXRotation(-np.pi/2.0), [0, -0.25, 0.25])} X_O = {"initial": RigidTransform(RotationMatrix.MakeZRotation(np.pi/2.0), [-.2, -.75, 0.025]), "goal": RigidTransform(RotationMatrix.MakeZRotation(np.pi),[.75, 0, 0.025])} X_G, times = make_gripper_frames(X_G, X_O) print(f"Sanity check: The entire maneuver will take {times['postplace']} seconds to execute.")

def visualize_gripper_frames(X_G, X_O): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step = 0.0) parser = Parser(plant, scene_graph) gripper = FindResourceOrThrow( "drake/manipulation/models/wsg_50_description/sdf/schunk_wsg_50_no_tip.sdf") brick = FindResourceOrThrow( "drake/examples/manipulation_station/models/061_foam_brick.sdf") for key, pose in X_G.items(): g = parser.AddModelFromFile(gripper, f"gripper_{key}") plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("body", g), pose) for key, pose in X_O.items(): o = parser.AddModelFromFile(brick, f"object_{key}") plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("base_link", o), pose) plant.Finalize() meshcat.Delete() visualizer = MeshcatVisualizerCpp.AddToBuilder( builder, scene_graph, meshcat) diagram = builder.Build() context = diagram.CreateDefaultContext() diagram.Publish(context) visualize_gripper_frames(X_G, X_O)

def make_gripper_trajectory(X_G, times): """ Constructs a gripper position trajectory from the plan "sketch". """ sample_times = [] poses = [] for name in ["initial", "prepick", "pick_start", "pick_end", "postpick", "clearance", "preplace", "place_start", "place_end", "postplace"]: sample_times.append(times[name]) poses.append(X_G[name]) return PiecewisePose.MakeLinear(sample_times, poses) traj = make_gripper_trajectory(X_G, times)

traj_p_G = traj.get_position_trajectory() data = dataframe(traj_p_G, traj_p_G.get_segment_times(), ['x','y','z']) alt.vconcat( alt.Chart(data).mark_line().encode(x='t', y='x').properties(height=80), alt.Chart(data).mark_line().encode(x='t', y='y').properties(height=80), alt.Chart(data).mark_line().encode(x='t', y='z').properties(height=80), )

data = dataframe(traj_p_G, np.linspace(traj_p_G.start_time(), traj_p_G.end_time(), 50), ['x','y','z']) fig = px.line_3d(data, x='x', y='y', z='z') fig.update_traces(mode='lines+markers', marker=dict(size=3, color="rgb(153, 51, 51)"), line=dict(color="rgb(153, 51, 51)")) if running_as_notebook: fig.show()

traj_R_G = traj.get_orientation_trajectory() data = dataframe(traj_R_G, traj_R_G.get_segment_times(), ['x','y','z','w']) alt.vconcat( alt.Chart(data).mark_line().encode(x='t', y='x').properties(height=80), alt.Chart(data).mark_line().encode(x='t', y='y').properties(height=80), alt.Chart(data).mark_line().encode(x='t', y='z').properties(height=80), alt.Chart(data).mark_line().encode(x='t', y='w').properties(height=80) )

opened = np.array([0.107]); closed = np.array([0.0]); def make_wsg_command_trajectory(times): traj_wsg_command = PiecewisePolynomial.FirstOrderHold( [times["initial"], times["pick_start"]], np.hstack([[opened], [opened]])) traj_wsg_command.AppendFirstOrderSegment(times["pick_end"], closed) traj_wsg_command.AppendFirstOrderSegment(times["place_start"], closed) traj_wsg_command.AppendFirstOrderSegment(times["place_end"], opened) traj_wsg_command.AppendFirstOrderSegment(times["postplace"], opened) return traj_wsg_command traj_wsg_command = make_wsg_command_trajectory(times)

data = dataframe(traj_wsg_command, traj_wsg_command.get_segment_times(), ['wsg']) alt.Chart(data).mark_line().encode(x='t', y='wsg').properties(height=80)

class GripperTrajectoriesToPosition(LeafSystem): def __init__(self, plant, traj_p_G, traj_R_G, traj_wsg_command): LeafSystem.__init__(self) self.plant = plant self.gripper_body = plant.GetBodyByName("body") self.left_finger_joint = plant.GetJointByName("left_finger_sliding_joint") self.right_finger_joint = plant.GetJointByName("right_finger_sliding_joint") self.traj_p_G = traj_p_G self.traj_R_G = traj_R_G self.traj_wsg_command = traj_wsg_command self.plant_context = plant.CreateDefaultContext() self.DeclareVectorOutputPort("position", BasicVector(plant.num_positions()), self.CalcPositionOutput) def CalcPositionOutput(self, context, output): t = context.get_time() X_G = RigidTransform(Quaternion(self.traj_R_G.value(t)), self.traj_p_G.value(t)) self.plant.SetFreeBodyPose(self.plant_context, self.gripper_body, X_G) wsg = self.traj_wsg_command.value(t) self.left_finger_joint.set_translation(self.plant_context, -wsg/2.0) self.right_finger_joint.set_translation(self.plant_context, wsg/2.0) output.SetFromVector(self.plant.GetPositions(self.plant_context)) def visualize_pick_and_place_trajectory(traj_p_G, traj_R_G, traj_wsg_command, X_O): builder = DiagramBuilder() # Note: Don't use AddMultibodyPlantSceneGraph because we are only using # MultibodyPlant for parsing, then wiring directly to SceneGraph. scene_graph = builder.AddSystem(SceneGraph()) plant = MultibodyPlant(time_step=0.0) plant.RegisterAsSourceForSceneGraph(scene_graph) parser = Parser(plant, scene_graph) gripper = parser.AddModelFromFile(FindResourceOrThrow( "drake/manipulation/models/wsg_50_description/sdf/schunk_wsg_50_no_tip.sdf"), "gripper") brick = FindResourceOrThrow( "drake/examples/manipulation_station/models/061_foam_brick.sdf") for key, pose in X_O.items(): o = parser.AddModelFromFile(brick, f"object_{key}") plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("base_link", o), pose) plant.Finalize() to_pose = builder.AddSystem(MultibodyPositionToGeometryPose(plant)) builder.Connect(to_pose.get_output_port(), scene_graph.get_source_pose_port(plant.get_source_id())) traj_to_position = builder.AddSystem(GripperTrajectoriesToPosition(plant, traj_p_G, traj_R_G, traj_wsg_command)) builder.Connect(traj_to_position.get_output_port(), to_pose.get_input_port()) meshcat.Delete() visualizer = MeshcatVisualizerCpp.AddToBuilder( builder, scene_graph, meshcat) diagram = builder.Build() simulator = Simulator(diagram) # TODO(russt): get recording working again with new Meshcat (and remove realtime rate) #meshcat.start_recording() if running_as_notebook: simulator.set_target_realtime_rate(1.0) simulator.AdvanceTo(traj_p_G.end_time() if running_as_notebook else 0.1) #meshcat.publish_recording() visualize_pick_and_place_trajectory(traj_p_G, traj_R_G, traj_wsg_command, X_O)

traj_v_G = traj_p_G.MakeDerivative() traj_w_G = traj_R_G.MakeDerivative() data = dataframe(traj_v_G, np.linspace(traj_v_G.start_time(), traj_v_G.end_time()), ['x','y','z']) alt.vconcat( alt.Chart(data).mark_line().encode(x='t', y='x').properties(height=80), alt.Chart(data).mark_line().encode(x='t', y='y').properties(height=80), alt.Chart(data).mark_line().encode(x='t', y='z').properties(height=80), )

# We can write a new System by deriving from the LeafSystem class. # There is a little bit of boiler plate, but hopefully this example makes sense. class PseudoInverseController(LeafSystem): def __init__(self, plant): LeafSystem.__init__(self) self._plant = plant self._plant_context = plant.CreateDefaultContext() self._iiwa = plant.GetModelInstanceByName("iiwa") self._G = plant.GetBodyByName("body").body_frame() self._W = plant.world_frame() self.w_G_port = self.DeclareVectorInputPort("omega_WG", BasicVector(3)) self.v_G_port = self.DeclareVectorInputPort("v_WG", BasicVector(3)) self.q_port = self.DeclareVectorInputPort("iiwa_position", BasicVector(7)) self.DeclareVectorOutputPort("iiwa_velocity", BasicVector(7), self.CalcOutput) # TODO(russt): Add missing binding #joint_indices = plant.GetJointIndices(self._iiwa) #self.position_start = plant.get_joint(joint_indices[0]).position_start() #self.position_end = plant.get_joint(joint_indices[-1]).position_start() self.iiwa_start = plant.GetJointByName("iiwa_joint_1").velocity_start() self.iiwa_end = plant.GetJointByName("iiwa_joint_7").velocity_start() def CalcOutput(self, context, output): w_G = self.w_G_port.Eval(context) v_G = self.v_G_port.Eval(context) V_G = np.hstack([w_G, v_G]) q = self.q_port.Eval(context) self._plant.SetPositions(self._plant_context, self._iiwa, q) J_G = self._plant.CalcJacobianSpatialVelocity( self._plant_context, JacobianWrtVariable.kV, self._G, [0,0,0], self._W, self._W) J_G = J_G[:,self.iiwa_start:self.iiwa_end+1] # Only iiwa terms. v = np.linalg.pinv(J_G).dot(V_G) output.SetFromVector(v)

X_O = {"initial": RigidTransform(RotationMatrix.MakeZRotation(np.pi/2.0), [-.2, -.65, 0.09]), "goal": RigidTransform(RotationMatrix.MakeZRotation(np.pi),[.5, 0, 0.09])} builder = DiagramBuilder() station = builder.AddSystem(ManipulationStation()) station.SetupClutterClearingStation() station.AddManipulandFromFile( "drake/examples/manipulation_station/models/061_foam_brick.sdf", X_O["initial"]) station.Finalize() # Find the initial pose of the gripper (as set in the default Context) temp_context = station.CreateDefaultContext() plant = station.get_multibody_plant() temp_plant_context = plant.GetMyContextFromRoot(temp_context) X_G = {"initial": plant.EvalBodyPoseInWorld(temp_plant_context, plant.GetBodyByName("body"))} X_G, times = make_gripper_frames(X_G, X_O) print(f"Sanity check: The entire maneuver will take {times['postplace']} seconds to execute.") # Make the trajectories traj = make_gripper_trajectory(X_G, times) # TODO(russt): Offer PiecewisePose get_velocity_trajectory(). traj_v_G = traj.get_position_trajectory().MakeDerivative() traj_w_G = traj.get_orientation_trajectory().MakeDerivative() v_G_source = builder.AddSystem(TrajectorySource(traj_v_G)) v_G_source.set_name("v_WG") w_G_source = builder.AddSystem(TrajectorySource(traj_w_G)) w_G_source.set_name("omega_WG") controller = builder.AddSystem(PseudoInverseController(plant)) controller.set_name("PseudoInverseController") builder.Connect(v_G_source.get_output_port(), controller.GetInputPort("v_WG")) builder.Connect(w_G_source.get_output_port(), controller.GetInputPort("omega_WG")) integrator = builder.AddSystem(Integrator(7)) integrator.set_name("integrator") builder.Connect(controller.get_output_port(), integrator.get_input_port()) builder.Connect(integrator.get_output_port(), station.GetInputPort("iiwa_position")) builder.Connect(station.GetOutputPort("iiwa_position_measured"), controller.GetInputPort("iiwa_position")) traj_wsg_command = make_wsg_command_trajectory(times) wsg_source = builder.AddSystem(TrajectorySource(traj_wsg_command)) wsg_source.set_name("wsg_command") builder.Connect(wsg_source.get_output_port(), station.GetInputPort("wsg_position")) meshcat.Delete() visualizer = MeshcatVisualizerCpp.AddToBuilder( builder, station.GetOutputPort("query_object"), meshcat) diagram = builder.Build() diagram.set_name("pick_and_place") simulator = Simulator(diagram) station_context = station.GetMyContextFromRoot(simulator.get_mutable_context()) # TODO(russt): Add this missing python binding #integrator.set_integral_value( # integrator.GetMyContextFromRoot(simulator.get_mutable_context()), # station.GetIiwaPosition(station_context)) integrator.GetMyContextFromRoot(simulator.get_mutable_context()).get_mutable_continuous_state_vector().SetFromVector(station.GetIiwaPosition(station_context)) if running_as_notebook: simulator.set_target_realtime_rate(1.0) #meshcat.start_recording() simulator.AdvanceTo(traj_p_G.end_time() if running_as_notebook else 0.1) #meshcat.stop_recording() #meshcat.publish_recording()

SVG(pydot.graph_from_dot_data(diagram.GetGraphvizString(max_depth=1))[0].create_svg())