# Imports import numpy as np import pydot from IPython.display import display, HTML, SVG from pydrake.all import ( AddMultibodyPlantSceneGraph, DiagramBuilder, FindResourceOrThrow, GenerateHtml, InverseDynamicsController, MultibodyPlant, Parser, Simulator, MeshcatVisualizerCpp) from manipulation import running_as_notebook from manipulation.meshcat_cpp_utils import ( StartMeshcat, MeshcatJointSlidersThatPublish)

# Start the visualizer. meshcat = StartMeshcat()

# First pick your robot by un-commenting one of these: robot = "Kuka LBR iiwa 7" #robot = "Kuka LBR iiwa 14" #robot = "Kinova Jaco Gen2 (7 DoF)" #robot = "Franka Emika Panda" def get_model_file(description): # Note: I could download remote model resources here if necessary. if description == "Kuka LBR iiwa 7": return FindResourceOrThrow("drake/manipulation/models/iiwa_description/iiwa7/iiwa7_no_collision.sdf") elif description == "Kuka LBR iiwa 14": return FindResourceOrThrow("drake/manipulation/models/iiwa_description/sdf/iiwa14_no_collision.sdf") elif description == "Kinova Jaco Gen2 (7 DoF)": return FindResourceOrThrow("drake/manipulation/models/jaco_description/urdf/j2s7s300.urdf") elif description == "Franka Emika Panda": return FindResourceOrThrow("drake/manipulation/models/franka_description/urdf/panda_arm_hand.urdf") raise Exception("Unknown model") def joint_slider_demo(robot): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=1e-4) model = Parser(plant, scene_graph).AddModelFromFile(get_model_file(robot)) plant.WeldFrames(plant.world_frame(), plant.get_body( plant.GetBodyIndices(model)[0]).body_frame()) plant.Finalize() meshcat.Delete() meshcat.DeleteAddedControls() visualizer = MeshcatVisualizerCpp.AddToBuilder(builder, scene_graph, meshcat) diagram = builder.Build() context = diagram.CreateDefaultContext() sliders = MeshcatJointSlidersThatPublish(meshcat, plant, visualizer, context) sliders.Run() joint_slider_demo(robot)

meshcat.Delete() meshcat.DeleteAddedControls()

plant = MultibodyPlant(time_step=1e-4) Parser(plant).AddModelFromFile( FindResourceOrThrow("drake/manipulation/models/iiwa_description/sdf/iiwa14_no_collision.sdf")) plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("iiwa_link_0")) plant.Finalize()

context = plant.CreateDefaultContext() print(context)

# Set all of the joint positions at once in a single vector. plant.SetPositions(context, [-1.57, 0.1, 0, 0, 0, 1.6, 0]) # You can also set them by referencing particular joints. plant.GetJointByName("iiwa_joint_4").set_angle(context, -1.2) print(context)

plant.get_actuation_input_port().FixValue(context, np.zeros(7));

simulator = Simulator(plant, context) simulator.AdvanceTo(5.0) print(context)

builder = DiagramBuilder() # Adds both MultibodyPlant and the SceneGraph, and wires them together. plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=1e-4) # Note that we parse into both the plant and the scene_graph here. Parser(plant, scene_graph).AddModelFromFile( FindResourceOrThrow("drake/manipulation/models/iiwa_description/sdf/iiwa14_no_collision.sdf")) plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("iiwa_link_0")) plant.Finalize() # Adds the MeshcatVisualizer and wires it to the SceneGraph. visualizer = MeshcatVisualizerCpp.AddToBuilder(builder, scene_graph, meshcat) diagram = builder.Build()

context = diagram.CreateDefaultContext() diagram.Publish(context)

print(context)

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

plant_context = plant.GetMyMutableContextFromRoot(context) plant.SetPositions(plant_context, [-1.57, 0.1, 0, -1.2, 0, 1.6, 0]) plant.get_actuation_input_port().FixValue(plant_context, np.zeros(7)) print(context)

simulator = Simulator(diagram, context) simulator.set_target_realtime_rate(1.0) simulator.AdvanceTo(5.0 if running_as_notebook else 0.1)

def animation_demo(): builder = DiagramBuilder() # Adds both MultibodyPlant and the SceneGraph, and wires them together. plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=1e-4) # Note that we parse into both the plant and the scene_graph here. Parser(plant, scene_graph).AddModelFromFile( FindResourceOrThrow("drake/manipulation/models/iiwa_description/sdf/iiwa14_no_collision.sdf")) plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("iiwa_link_0")) plant.Finalize() # Adds the MeshcatVisualizer and wires it to the SceneGraph. visualizer = MeshcatVisualizerCpp.AddToBuilder(builder, scene_graph, meshcat) diagram = builder.Build() context = diagram.CreateDefaultContext() plant_context = plant.GetMyMutableContextFromRoot(context) plant.SetPositions(plant_context, [-1.57, 0.1, 0, -1.2, 0, 1.6, 0]) plant.get_actuation_input_port().FixValue(plant_context, np.zeros(7)) simulator = Simulator(diagram, context) simulator.set_target_realtime_rate(1.0) meshcat.start_recording() simulator.AdvanceTo(5.0 if running_as_notebook else 0.1) meshcat.stop_recording() meshcat.publish_recording() # TODO(russt): Need to re-implemented meshcat recording in the new (deepnote-friendly) meshcat. # animation_demo()

builder = DiagramBuilder() # Adds both MultibodyPlant and the SceneGraph, and wires them together. plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=1e-4) # Note that we parse into both the plant and the scene_graph here. iiwa_model = Parser(plant, scene_graph).AddModelFromFile( FindResourceOrThrow("drake/manipulation/models/iiwa_description/sdf/iiwa14_no_collision.sdf")) plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("iiwa_link_0")) plant.Finalize() # Adds the MeshcatVisualizer and wires it to the SceneGraph. visualizer = MeshcatVisualizerCpp.AddToBuilder(builder, scene_graph, meshcat) # Adds an approximation of the iiwa controller. # TODO(russt): replace this with the joint impedance controller. Kp = np.full(7, 100) Ki = 2 * np.sqrt(Kp) Kd = np.full(7, 1) iiwa_controller = builder.AddSystem(InverseDynamicsController(plant, Kp, Ki, Kd, False)) iiwa_controller.set_name("iiwa_controller"); builder.Connect(plant.get_state_output_port(iiwa_model), iiwa_controller.get_input_port_estimated_state()) builder.Connect(iiwa_controller.get_output_port_control(), plant.get_actuation_input_port()) diagram = builder.Build()

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

diagram.set_name("diagram") HTML('<script src="https://unpkg.com/gojs/release/go.js"></script>' + GenerateHtml(diagram))

context = diagram.CreateDefaultContext() plant_context = plant.GetMyMutableContextFromRoot(context) q0 = np.array([-1.57, 0.1, 0, -1.2, 0, 1.6, 0]) x0 = np.hstack((q0, 0*q0)) plant.SetPositions(plant_context, q0) iiwa_controller.GetInputPort('desired_state').FixValue( iiwa_controller.GetMyMutableContextFromRoot(context), x0) print(context)

simulator = Simulator(diagram, context) simulator.set_target_realtime_rate(1.0); simulator.AdvanceTo(5.0 if running_as_notebook else 0.1);

def hand_sliders(): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0) allegro_file = FindResourceOrThrow("drake/manipulation/models/allegro_hand_description/sdf/allegro_hand_description_right.sdf") Parser(plant, scene_graph).AddModelFromFile(allegro_file) plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("hand_root")) plant.Finalize() meshcat.Delete() visualizer = MeshcatVisualizerCpp.AddToBuilder(builder, scene_graph, meshcat) diagram = builder.Build() context = diagram.CreateDefaultContext() sliders = MeshcatJointSlidersThatPublish(meshcat, plant, visualizer, context) sliders.Run() # Runs until you press "Stop JointSliders" in Meshcat hand_sliders()