def finalize_func(): builder = self._builder builder.Connect(gripper_controller.get_generalized_force_output_port(), self._mbp.get_actuation_input_port(gripper_model_id)) # Add Gripper ports if arm_end_frame is None: # Gripper is not welded # State goes [_, r, p, y, x, y, z, g1, g2, dr, dp, dy, dx, dy, dz, dg1, dg2] # Gripper state demux = self._builder.AddSystem(Demultiplexer([1, 6, 2, 6, 2])) mux = self._builder.AddSystem(Multiplexer(input_sizes=[2, 2])) builder.Connect(self._mbp.get_state_output_port(gripper_model_id), demux.get_input_port(0)) builder.Connect(demux.get_output_port(2), mux.get_input_port(0)) builder.Connect(demux.get_output_port(4), mux.get_input_port(1)) builder.Connect(mux.get_output_port(0), gripper_controller.get_state_input_port()) builder.Connect(mux.get_output_port(0), mbp_to_gripper_state.get_input_port(0)) else: builder.Connect(self._mbp.get_state_output_port(gripper_model_id), gripper_controller.get_state_input_port()) builder.Connect(self._mbp.get_state_output_port(gripper_model_id), mbp_to_gripper_state.get_input_port(0)) gripper_position_name = gripper_name + "_position" gripper_force_limit_name = gripper_name + "_force_limit" gripper_state_name = gripper_name + "_state_measured" gripper_force_meas_name = gripper_name + "_force_measured" gripper_gen_force_meas_name = gripper_name + "_gen_force_measured" gripper_full_state = gripper_name + "_full_state" self._port_names.extend([gripper_position_name, gripper_force_limit_name, gripper_state_name, gripper_force_meas_name, gripper_gen_force_meas_name, gripper_full_state]) builder.ExportInput(gripper_controller.get_desired_position_input_port(), gripper_position_name) builder.ExportInput(gripper_controller.get_force_limit_input_port(), gripper_force_limit_name) builder.ExportOutput(mbp_to_gripper_state.get_output_port(0), gripper_state_name) builder.ExportOutput(gripper_controller.get_grip_force_output_port(), gripper_force_meas_name) builder.ExportOutput(gripper_controller.get_generalized_force_output_port(), gripper_gen_force_meas_name) builder.ExportOutput(self._mbp.get_state_output_port(gripper_model_id), gripper_full_state) if arm_end_frame is None: gripper_spatial_position = gripper_name + "_spatial_position" gripper_spatial_velocity = gripper_name + "_spatial_velocity" self._port_names.extend([gripper_spatial_position, gripper_spatial_velocity]) builder.ExportOutput(demux.get_output_port(1), gripper_spatial_position) builder.ExportOutput(demux.get_output_port(3), gripper_spatial_velocity)
def test_demultiplexer(self): # Test demultiplexer with scalar outputs. demux = Demultiplexer(size=4) context = demux.CreateDefaultContext() self.assertEqual(demux.get_num_input_ports(), 1) self.assertEqual(demux.get_num_output_ports(), 4) input_vec = np.array([1., 2., 3., 4.]) context.FixInputPort(0, BasicVector(input_vec)) output = demux.AllocateOutput() demux.CalcOutput(context, output) for i in range(4): self.assertTrue( np.allclose(output.get_vector_data(i).get_value(), input_vec[i])) # Test demultiplexer with vector outputs. demux = Demultiplexer(size=4, output_ports_sizes=2) context = demux.CreateDefaultContext() self.assertEqual(demux.get_num_input_ports(), 1) self.assertEqual(demux.get_num_output_ports(), 2) context.FixInputPort(0, BasicVector(input_vec)) output = demux.AllocateOutput() demux.CalcOutput(context, output) for i in range(2): self.assertTrue( np.allclose(output.get_vector_data(i).get_value(), input_vec[2*i:2*i+2]))
def connect_plan_runner(builder, station, plan): from manipulation_station.manipulation_station_plan_runner import ManipStationPlanRunner plan_list, gripper_setpoints = plan # TODO: disable the move home trajectories at the beginning of the plan # Add plan runner. plan_runner = ManipStationPlanRunner(station, plan_list, gripper_setpoints) builder.AddSystem(plan_runner) builder.Connect(plan_runner.hand_setpoint_output_port, station.GetInputPort("wsg_position")) builder.Connect(plan_runner.gripper_force_limit_output_port, station.GetInputPort("wsg_force_limit")) demux = builder.AddSystem(Demultiplexer(14, 7)) builder.Connect( plan_runner.GetOutputPort("iiwa_position_and_torque_command"), demux.get_input_port(0)) builder.Connect(demux.get_output_port(0), station.GetInputPort("iiwa_position")) builder.Connect(demux.get_output_port(1), station.GetInputPort("iiwa_feedforward_torque")) builder.Connect(station.GetOutputPort("iiwa_position_measured"), plan_runner.iiwa_position_input_port) builder.Connect(station.GetOutputPort("iiwa_velocity_estimated"), plan_runner.iiwa_velocity_input_port) # Add logger iiwa_position_command_log = LogOutput(demux.get_output_port(0), builder) iiwa_position_command_log._DeclarePeriodicPublish(0.005) iiwa_external_torque_log = LogOutput( station.GetOutputPort("iiwa_torque_external"), builder) iiwa_external_torque_log._DeclarePeriodicPublish(0.005) iiwa_position_measured_log = LogOutput( station.GetOutputPort("iiwa_position_measured"), builder) iiwa_position_measured_log._DeclarePeriodicPublish(0.005) wsg_state_log = LogOutput(station.GetOutputPort("wsg_state_measured"), builder) wsg_state_log._DeclarePeriodicPublish(0.1) wsg_command_log = LogOutput(plan_runner.hand_setpoint_output_port, builder) wsg_command_log._DeclarePeriodicPublish(0.1) return plan_runner
def connect_plan_runner(builder, station, plan): from plan_runner.manipulation_station_plan_runner import ManipStationPlanRunner plan_list, gripper_setpoints = plan # Add plan runner. plan_runner = ManipStationPlanRunner(plan_list, gripper_setpoints) builder.AddSystem(plan_runner) builder.Connect(plan_runner.hand_setpoint_output_port, station.GetInputPort("wsg_position")) builder.Connect(plan_runner.gripper_force_limit_output_port, station.GetInputPort("wsg_force_limit")) demux = builder.AddSystem(Demultiplexer(14, 7)) builder.Connect(plan_runner.GetOutputPort("gripper_setpoint"), station.GetInputPort("wsg_position")) builder.Connect(plan_runner.GetOutputPort("force_limit"), station.GetInputPort("wsg_force_limit")) builder.Connect(plan_runner.GetOutputPort("iiwa_position_command"), station.GetInputPort("iiwa_position")) builder.Connect(plan_runner.GetOutputPort("iiwa_torque_command"), station.GetInputPort("iiwa_feedforward_torque")) # Add logger iiwa_position_command_log = LogOutput( plan_runner.GetOutputPort("iiwa_position_command"), builder) iiwa_position_command_log._DeclarePeriodicPublish(0.005) iiwa_external_torque_log = LogOutput( station.GetOutputPort("iiwa_torque_external"), builder) iiwa_external_torque_log._DeclarePeriodicPublish(0.005) iiwa_position_measured_log = LogOutput( station.GetOutputPort("iiwa_position_measured"), builder) iiwa_position_measured_log._DeclarePeriodicPublish(0.005) wsg_state_log = LogOutput(station.GetOutputPort("wsg_state_measured"), builder) wsg_state_log._DeclarePeriodicPublish(0.1) wsg_command_log = LogOutput(plan_runner.hand_setpoint_output_port, builder) wsg_command_log._DeclarePeriodicPublish(0.1) return plan_runner
def test_demultiplexer(self): # Test demultiplexer with scalar outputs. demux = Demultiplexer(size=4) context = demux.CreateDefaultContext() self.assertEqual(demux.get_num_input_ports(), 1) self.assertEqual(demux.get_num_output_ports(), 4) input_vec = np.array([1., 2., 3., 4.]) context.FixInputPort(0, BasicVector(input_vec)) output = demux.AllocateOutput() demux.CalcOutput(context, output) for i in range(4): self.assertTrue( np.allclose( output.get_vector_data(i).get_value(), input_vec[i])) # Test demultiplexer with vector outputs. demux = Demultiplexer(size=4, output_ports_sizes=2) context = demux.CreateDefaultContext() self.assertEqual(demux.get_num_input_ports(), 1) self.assertEqual(demux.get_num_output_ports(), 2) context.FixInputPort(0, BasicVector(input_vec)) output = demux.AllocateOutput() demux.CalcOutput(context, output) for i in range(2): self.assertTrue( np.allclose( output.get_vector_data(i).get_value(), input_vec[2 * i:2 * i + 2]))
def test_demultiplexer(self): # Test demultiplexer with scalar outputs. demux = Demultiplexer(size=4) context = demux.CreateDefaultContext() self.assertEqual(demux.num_input_ports(), 1) self.assertEqual(demux.num_output_ports(), 4) numpy_compare.assert_equal(demux.get_output_ports_sizes(), [1, 1, 1, 1]) input_vec = np.array([1., 2., 3., 4.]) demux.get_input_port(0).FixValue(context, input_vec) output = demux.AllocateOutput() demux.CalcOutput(context, output) for i in range(4): self.assertTrue( np.allclose( output.get_vector_data(i).get_value(), input_vec[i])) # Test demultiplexer with vector outputs. demux = Demultiplexer(size=4, output_ports_size=2) context = demux.CreateDefaultContext() self.assertEqual(demux.num_input_ports(), 1) self.assertEqual(demux.num_output_ports(), 2) numpy_compare.assert_equal(demux.get_output_ports_sizes(), [2, 2]) demux.get_input_port(0).FixValue(context, input_vec) output = demux.AllocateOutput() demux.CalcOutput(context, output) for i in range(2): self.assertTrue( np.allclose( output.get_vector_data(i).get_value(), input_vec[2 * i:2 * i + 2])) # Test demultiplexer with different output port sizes. output_ports_sizes = np.array([1, 2, 1]) num_output_ports = output_ports_sizes.size input_vec = np.array([1., 2., 3., 4.]) demux = Demultiplexer(output_ports_sizes=output_ports_sizes) context = demux.CreateDefaultContext() self.assertEqual(demux.num_input_ports(), 1) self.assertEqual(demux.num_output_ports(), num_output_ports) numpy_compare.assert_equal(demux.get_output_ports_sizes(), output_ports_sizes) demux.get_input_port(0).FixValue(context, input_vec) output = demux.AllocateOutput() demux.CalcOutput(context, output) output_port_start = 0 for i in range(num_output_ports): output_port_size = output.get_vector_data(i).size() self.assertTrue( np.allclose( output.get_vector_data(i).get_value(), input_vec[output_port_start:output_port_start + output_port_size])) output_port_start += output_port_size
def build_manipulation_station(station): from underactuated.meshcat_visualizer import MeshcatVisualizer from .manipulation_station.manipulation_station_plan_runner import ManipStationPlanRunner builder = DiagramBuilder() builder.AddSystem(station) # Add plan runner. plan_runner = ManipStationPlanRunner(station=station) builder.AddSystem(plan_runner) builder.Connect(plan_runner.hand_setpoint_output_port, station.GetInputPort("wsg_position")) builder.Connect(plan_runner.gripper_force_limit_output_port, station.GetInputPort("wsg_force_limit")) demux = builder.AddSystem(Demultiplexer(14, 7)) builder.Connect( plan_runner.GetOutputPort("iiwa_position_and_torque_command"), demux.get_input_port(0)) builder.Connect(demux.get_output_port(0), station.GetInputPort("iiwa_position")) builder.Connect(demux.get_output_port(1), station.GetInputPort("iiwa_feedforward_torque")) builder.Connect(station.GetOutputPort("iiwa_position_measured"), plan_runner.iiwa_position_input_port) builder.Connect(station.GetOutputPort("iiwa_velocity_estimated"), plan_runner.iiwa_velocity_input_port) # Add meshcat visualizer plant = station.get_mutable_multibody_plant() scene_graph = station.get_mutable_scene_graph() viz = MeshcatVisualizer(scene_graph, is_drawing_contact_force=True, plant=plant) builder.AddSystem(viz) builder.Connect(station.GetOutputPort("pose_bundle"), viz.GetInputPort("lcm_visualization")) builder.Connect(station.GetOutputPort("contact_results"), viz.GetInputPort("contact_results")) # Add logger iiwa_position_command_log = LogOutput(demux.get_output_port(0), builder) iiwa_position_command_log._DeclarePeriodicPublish(0.005) iiwa_external_torque_log = LogOutput( station.GetOutputPort("iiwa_torque_external"), builder) iiwa_external_torque_log._DeclarePeriodicPublish(0.005) iiwa_position_measured_log = LogOutput( station.GetOutputPort("iiwa_position_measured"), builder) iiwa_position_measured_log._DeclarePeriodicPublish(0.005) wsg_state_log = LogOutput(station.GetOutputPort("wsg_state_measured"), builder) wsg_state_log._DeclarePeriodicPublish(0.1) wsg_command_log = LogOutput(plan_runner.hand_setpoint_output_port, builder) wsg_command_log._DeclarePeriodicPublish(0.1) # build diagram diagram = builder.Build() viz.load() #time.sleep(2.0) #RenderSystemWithGraphviz(diagram) return diagram, plan_runner
def __init__(self): #import pdb; pdb.set_trace() # Create a block diagram containing our system. builder = DiagramBuilder() # add the two decoupled plants (x(s)=u/s^2; y(s)=u/s^2) plant_x = builder.AddSystem(DoubleIntegrator()) plant_y = builder.AddSystem(DoubleIntegrator()) plant_x.set_name("double_integrator_x") plant_y.set_name("double_integrator_y") # add the controller, lead compensator for now just to stablize it controller_x = builder.AddSystem( Controller(tau_num=2., tau_den=.2, k=1.)) controller_y = builder.AddSystem( Controller(tau_num=2., tau_den=.2, k=1.)) controller_x.set_name("controller_x") controller_y.set_name("controller_y") # the perception's "Beam" model with the four sources of noise x_meas_fb = builder.AddSystem(Adder(num_inputs=4, size=1)) x_meas_fb.set_name("x_fb") y_meas_fb = builder.AddSystem(Adder(num_inputs=4, size=1)) y_meas_fb.set_name("y_fb") y_perception = builder.AddSystem(Adder(num_inputs=2, size=1)) y_perception.set_name("range_measurment_y") neg_y_meas = builder.AddSystem(Gain(k=-1., size=1)) neg_y_meas.set_name("neg_y_meas") wall_position = builder.AddSystem(ConstantVectorSource([Y_wall])) p_hit = builder.AddSystem(RandomSource(distribution=RandomDistribution.kGaussian, num_outputs=2,\ sampling_interval_sec=0.01)) p_hit.set_name("GaussionNoise(0,1)") p_short = builder.AddSystem(RandomSource(distribution=RandomDistribution.kExponential, num_outputs=2,\ sampling_interval_sec=0.01)) p_short.set_name("ExponentialNoise(1)") #p_max = builder.AddSystem(RandomSource(distribution=RandomDistribution.kUniform, num_outputs=1,\ # sampling_interval_sec=0.01)) p_rand = builder.AddSystem(RandomSource(distribution=RandomDistribution.kUniform, num_outputs=2,\ sampling_interval_sec=0.01)) p_rand.set_name("UniformNoise(0,1)") #import pdb; pdb.set_trace() p_hit_Dx = builder.AddSystem(Demultiplexer(size=2)) p_hit_Dx.set_name('Dmux1') p_short_Dx = builder.AddSystem(Demultiplexer(size=2)) p_short_Dx.set_name('Dmux2') p_rand_Dx = builder.AddSystem(Demultiplexer(size=2)) p_rand_Dx.set_name('Dmux3') normgain_x = builder.AddSystem(Gain(k=normal_coeff, size=1)) normgain_x.set_name("Sigma_x") normgain_y = builder.AddSystem(Gain(k=normal_coeff, size=1)) normgain_y.set_name("Sigma_y") expgain_x = builder.AddSystem(Gain(k=exp_coeff, size=1)) expgain_x.set_name("Exp_x") expgain_y = builder.AddSystem(Gain(k=exp_coeff, size=1)) expgain_y.set_name("Exp_y") randgain_x = builder.AddSystem(Gain(k=rand_coeff, size=1)) randgain_x.set_name("Rand_x") randgain_y = builder.AddSystem(Gain(k=rand_coeff, size=1)) randgain_y.set_name("Rand_y") #maxgain_x = builder.AddSystem(Adder(num_inputs=2, size=1)) #maxgain_x.set_name("Max_x") #maxgain_y = builder.AddSystem(Adder(num_inputs=2, size=1)) #maxgain_y.set_name("Max_y") # the summation to get the error (closing the loop) summ_x = builder.AddSystem(Adder(num_inputs=2, size=1)) summ_y = builder.AddSystem(Adder(num_inputs=2, size=1)) summ_x.set_name("summation_x") summ_y.set_name("summation_y") neg_x = builder.AddSystem(Gain(k=-1., size=1)) neg_y = builder.AddSystem(Gain(k=-1., size=1)) neg_uy = builder.AddSystem(Gain(k=-1., size=1)) neg_x.set_name("neg_x") neg_y.set_name("neg_y") neg_uy.set_name("neg_uy") # wire up all the blocks (summation to the controller to the plant ...) builder.Connect(summ_x.get_output_port(0), controller_x.get_input_port(0)) # e_x builder.Connect(summ_y.get_output_port(0), controller_y.get_input_port(0)) # e_y builder.Connect(controller_x.get_output_port(0), plant_x.get_input_port(0)) # u_x builder.Connect( controller_y.get_output_port(0), neg_uy.get_input_port(0)) # u_y (to deal with directions) builder.Connect(neg_uy.get_output_port(0), plant_y.get_input_port(0)) # u_y builder.Connect( plant_x.get_output_port(0), x_meas_fb.get_input_port(0)) # perception, nominal state meas builder.Connect(wall_position.get_output_port(0), y_perception.get_input_port(0)) # perception builder.Connect(plant_y.get_output_port(0), neg_y_meas.get_input_port(0)) # perception builder.Connect(neg_y_meas.get_output_port(0), y_perception.get_input_port(1)) # perception, z meas builder.Connect( y_perception.get_output_port(0), y_meas_fb.get_input_port(0)) # perception, nominal state meas builder.Connect(p_hit.get_output_port(0), p_hit_Dx.get_input_port(0)) # demux the noise builder.Connect(p_hit_Dx.get_output_port(0), normgain_x.get_input_port(0)) # normalize Normal dist builder.Connect(normgain_x.get_output_port(0), x_meas_fb.get_input_port(1)) # Normal dist builder.Connect(p_hit_Dx.get_output_port(1), normgain_y.get_input_port(0)) # normalize Normal dist builder.Connect(normgain_y.get_output_port(0), y_meas_fb.get_input_port(1)) # Normal dist builder.Connect(p_short.get_output_port(0), p_short_Dx.get_input_port(0)) # demux the noise builder.Connect(p_short_Dx.get_output_port(0), expgain_x.get_input_port(0)) # normalize Exp dist builder.Connect(expgain_x.get_output_port(0), x_meas_fb.get_input_port(2)) # Exp dist builder.Connect(p_short_Dx.get_output_port(1), expgain_y.get_input_port(0)) # normalize Exp dist builder.Connect(expgain_y.get_output_port(0), y_meas_fb.get_input_port(2)) # Exp dist #builder.Connect(p_max.get_output_port(0), x_meas_fb.get_input_port(3)) # Uniform dist #builder.Connect(p_max.get_output_port(0), y_meas_fb.get_input_port(3)) # Uniform dist builder.Connect(p_rand.get_output_port(0), p_rand_Dx.get_input_port(0)) # normalize Uniform dist builder.Connect(p_rand_Dx.get_output_port(0), randgain_x.get_input_port(0)) # normalize Uniform dist builder.Connect(randgain_x.get_output_port(0), x_meas_fb.get_input_port(3)) # Uniform dist builder.Connect(p_rand_Dx.get_output_port(1), randgain_y.get_input_port(0)) # normalize Uniform dist builder.Connect(randgain_y.get_output_port(0), y_meas_fb.get_input_port(3)) # Uniform dist builder.Connect(x_meas_fb.get_output_port(0), neg_x.get_input_port(0)) # -x builder.Connect(y_meas_fb.get_output_port(0), neg_y.get_input_port(0)) # -y builder.Connect(neg_x.get_output_port(0), summ_x.get_input_port(1)) # r-x builder.Connect(neg_y.get_output_port(0), summ_y.get_input_port(1)) # r-y # Make the desired_state input of the controller, an input to the diagram. builder.ExportInput(summ_x.get_input_port(0)) builder.ExportInput(summ_y.get_input_port(0)) # get telemetry logger_x = LogOutput(plant_x.get_output_port(0), builder) logger_noise_x = LogOutput(x_meas_fb.get_output_port(0), builder) logger_y = LogOutput(plant_y.get_output_port(0), builder) logger_noise_y = LogOutput(y_meas_fb.get_output_port(0), builder) logger_x.set_name("logger_x_state") logger_y.set_name("logger_y_state") logger_noise_x.set_name("x_state_meas") logger_noise_y.set_name("y_meas") # compile the system diagram = builder.Build() diagram.set_name("diagram") # Create the simulator, and simulate for 10 seconds. simulator = Simulator(diagram) context = simulator.get_mutable_context() # First we extract the subsystem context for the plant plant_context_x = diagram.GetMutableSubsystemContext(plant_x, context) plant_context_y = diagram.GetMutableSubsystemContext(plant_y, context) # Then we can set the pendulum state, which is (x, y). z0 = X_0 plant_context_x.get_mutable_continuous_state_vector().SetFromVector(z0) z0 = Y_0 plant_context_y.get_mutable_continuous_state_vector().SetFromVector(z0) # The diagram has a single input port (port index 0), which is the desired_state. context.FixInputPort( 0, [X_d]) # here we assume no perception, closing feedback on state X context.FixInputPort( 1, [Z_d]) #Z_y, keep 3m away, basically we want to be at Y=0 # run the simulation simulator.AdvanceTo(10) t = logger_x.sample_times() #import pdb; pdb.set_trace() # Plot the results. plt.figure() plot_system_graphviz(diagram, max_depth=2) plt.figure() plt.plot(t, logger_noise_x.data().transpose(), label='x_state_meas') plt.plot(t, logger_noise_y.data().transpose(), label='y_meas (z(y))') plt.plot(t, logger_x.data().transpose(), label='x_state') plt.plot(t, logger_y.data().transpose(), label='y_state') plt.xlabel('t') plt.ylabel('x(t),y(t)') plt.legend() plt.show(block=True)
controller_plant.WeldFrames(controller_plant.world_frame(), controller_plant.GetFrameByName("panda_link0", control_only_panda), X_AB=RigidTransform(np.array([0.0, 0.5, 0.0]))) controller_plant.Finalize() diff_inv_controller = builder.AddSystem( DifferentialInverseKinematicsIntegrator(controller_plant, controller_plant.GetFrameByName("panda_hand", control_only_panda), time_step, DifferentialInverseKinematicsParameters(num_positions=9, num_velocities=9)) ) diff_inv_controller.set_name("Inverse Kinematics") rb_conv = builder.AddSystem(TrajToRB(traj_p_G, traj_R_G)) rb_conv.set_name("RB Conv") builder.Connect(rb_conv.get_output_port(), diff_inv_controller.get_input_port()) diff_arm_demux = builder.AddSystem(Demultiplexer(np.array([7, 2]))) diff_arm_demux.set_name("Diff Arm Demux") builder.Connect(diff_inv_controller.get_output_port(), diff_arm_demux.get_input_port()) kp = np.full(9, 100) ki = np.full(9, 1) kd = 2 * np.sqrt(kp) inv_d_controller = builder.AddSystem(InverseDynamicsController(controller_plant, kp, ki, kd, False)) inv_d_controller.set_name("inv_d") builder.Connect(plant.get_state_output_port(panda), inv_d_controller.get_input_port_estimated_state()) hand_comms = builder.AddSystem(GripperTrajectoriesToPosition(controller_plant, traj_h)) arm_hand_mux = builder.AddSystem(Multiplexer(np.array([7, 2]))) arm_hand_mux.set_name("Arm-Hand Mux")
def add_arm_gripper(self, arm_name, arm_path, arm_base, X_arm, gripper_path, arm_ee, gripper_base, X_gripper): # Add arm parser = Parser(self._mbp, self._sg) arm_model_id = parser.AddModelFromFile(arm_path, arm_name) arm_base_frame = self._mbp.GetFrameByName(arm_base, arm_model_id) self._mbp.WeldFrames(self._mbp.world_frame(), arm_base_frame, X_arm) self._model_ids[arm_name] = arm_model_id # Add gripper gripper_name = arm_name+"_gripper" arm_end_frame = self._mbp.GetFrameByName(arm_ee, arm_model_id) self.add_floating_gripper(gripper_name, gripper_path, arm_end_frame, gripper_base, X_gripper) # Add arm controller stack ctrl_plant = MultibodyPlant(0) parser = Parser(ctrl_plant) ctrl_arm_id = parser.AddModelFromFile(arm_path, arm_name) arm_base_frame = ctrl_plant.GetFrameByName(arm_base, ctrl_arm_id) ctrl_plant.WeldFrames(ctrl_plant.world_frame(), arm_base_frame, X_arm) gripper_equivalent = ctrl_plant.AddRigidBody( gripper_name+"_equivalent", ctrl_arm_id, self.calculate_ee_composite_inertia(gripper_path)) arm_end_frame = ctrl_plant.GetFrameByName(arm_ee, ctrl_arm_id) ctrl_plant.WeldFrames(arm_end_frame, gripper_equivalent.body_frame(), X_gripper) ctrl_plant.Finalize() self._control_mbp[arm_name] = ctrl_plant arm_num_positions = ctrl_plant.num_positions(ctrl_arm_id) kp = 4000*np.ones(arm_num_positions) ki = 0 * np.ones(arm_num_positions) kd = 5*np.sqrt(kp) arm_controller = self._builder.AddSystem(InverseDynamicsController( ctrl_plant, kp, ki, kd, False)) adder = self._builder.AddSystem(Adder(2, arm_num_positions)) state_from_position = self._builder.AddSystem( StateInterpolatorWithDiscreteDerivative( arm_num_positions, self._mbp.time_step(), True)) # Add command pass through and state splitter arm_command = self._builder.AddSystem(PassThrough(arm_num_positions)) state_split = self._builder.AddSystem(Demultiplexer( 2*arm_num_positions, arm_num_positions)) def finalize_func(): builder = self._builder # Export positions commanded command_input_name = arm_name + "_position" command_output_name = arm_name + "_position_commanded" self._port_names.extend([command_input_name, command_output_name]) builder.ExportInput(arm_command.get_input_port(0), command_input_name) builder.ExportOutput(arm_command.get_output_port(0), command_output_name) # Export arm state ports builder.Connect(self._mbp.get_state_output_port(arm_model_id), state_split.get_input_port(0)) arm_q_name = arm_name + "_position_measured" arm_v_name = arm_name + "_velocity_estimated" arm_state_name = arm_name + "_state_measured" self._port_names.extend([arm_q_name, arm_v_name, arm_state_name]) builder.ExportOutput(state_split.get_output_port(0), arm_q_name) builder.ExportOutput(state_split.get_output_port(1), arm_v_name) builder.ExportOutput(self._mbp.get_state_output_port(arm_model_id), arm_state_name) # Export controller stack ports builder.Connect(self._mbp.get_state_output_port(arm_model_id), arm_controller.get_input_port_estimated_state()) builder.Connect(arm_controller.get_output_port_control(), adder.get_input_port(0)) builder.Connect(adder.get_output_port(0), self._mbp.get_actuation_input_port(arm_model_id)) builder.Connect(state_from_position.get_output_port(0), arm_controller.get_input_port_desired_state()) builder.Connect(arm_command.get_output_port(0), state_from_position.get_input_port(0)) torque_input_name = arm_name + "_feedforward_torque" torque_output_cmd_name = arm_name + "_torque_commanded" torque_output_est_name = arm_name + "_torque_measured" self._port_names.extend([torque_input_name, torque_output_cmd_name, torque_output_est_name]) builder.ExportInput(adder.get_input_port(1), torque_input_name) builder.ExportOutput(adder.get_output_port(0), torque_output_cmd_name) builder.ExportOutput(adder.get_output_port(0), torque_output_est_name) external_torque_name = arm_name + "_torque_external" self._port_names.append(external_torque_name) builder.ExportOutput( self._mbp.get_generalized_contact_forces_output_port(arm_model_id), external_torque_name) self._finalize_functions.append(finalize_func)
def RunSimulation(self, plans_list, gripper_setpoint_list, extra_time=0, real_time_rate=1.0, q0_kuka=np.zeros(7), is_visualizing=True): """ Constructs a Diagram that sends commands to ManipulationStation. @param plans_list: A list of Plans to be executed. @param gripper_setpoint_list: A list of gripper setpoints. Each setpoint corresponds to a Plan. @param extra_time: Time in seconds that the simulation is run, in addition to the sum of the durations of all plans. @param real_time_rate: 1.0 means realtime; 0 means as fast as possible. @param q0_kuka: initial configuration of the robot. @param is_visualizing: if true, adds MeshcatVisualizer to the Diagram. It should be set to False when running tests. @return: logs of robot configuration and MultibodyPlant, generated by simulation. Logs are SignalLogger systems, whose data can be accessed by SignalLogger.data(). """ builder = DiagramBuilder() builder.AddSystem(self.station) # Add plan runner. plan_runner = ManipStationPlanRunner( station=self.station, kuka_plans=plans_list, gripper_setpoint_list=gripper_setpoint_list) builder.AddSystem(plan_runner) builder.Connect(plan_runner.hand_setpoint_output_port, self.station.GetInputPort("wsg_position")) builder.Connect(plan_runner.gripper_force_limit_output_port, self.station.GetInputPort("wsg_force_limit")) demux = builder.AddSystem(Demultiplexer(14, 7)) builder.Connect( plan_runner.GetOutputPort("iiwa_position_and_torque_command"), demux.get_input_port(0)) builder.Connect(demux.get_output_port(0), self.station.GetInputPort("iiwa_position")) builder.Connect(demux.get_output_port(1), self.station.GetInputPort("iiwa_feedforward_torque")) builder.Connect(self.station.GetOutputPort("iiwa_position_measured"), plan_runner.iiwa_position_input_port) builder.Connect(self.station.GetOutputPort("iiwa_velocity_estimated"), plan_runner.iiwa_velocity_input_port) # Add meshcat visualizer if is_visualizing: scene_graph = self.station.get_mutable_scene_graph() viz = MeshcatVisualizer(scene_graph, is_drawing_contact_force=True, plant=self.plant) builder.AddSystem(viz) builder.Connect(self.station.GetOutputPort("pose_bundle"), viz.GetInputPort("lcm_visualization")) builder.Connect(self.station.GetOutputPort("contact_results"), viz.GetInputPort("contact_results")) # Add logger iiwa_position_command_log = LogOutput(demux.get_output_port(0), builder) iiwa_position_command_log._DeclarePeriodicPublish(0.005) iiwa_external_torque_log = LogOutput( self.station.GetOutputPort("iiwa_torque_external"), builder) iiwa_external_torque_log._DeclarePeriodicPublish(0.005) iiwa_position_measured_log = LogOutput( self.station.GetOutputPort("iiwa_position_measured"), builder) iiwa_position_measured_log._DeclarePeriodicPublish(0.005) plant_state_log = LogOutput( self.station.GetOutputPort("plant_continuous_state"), builder) plant_state_log._DeclarePeriodicPublish(0.005) # build diagram diagram = builder.Build() if is_visualizing: viz.load() time.sleep(2.0) # RenderSystemWithGraphviz(diagram) # construct simulator simulator = Simulator(diagram) context = diagram.GetMutableSubsystemContext( self.station, simulator.get_mutable_context()) # set initial state of the robot self.station.SetIiwaPosition(q0_kuka, context) self.station.SetIiwaVelocity(np.zeros(7), context) self.station.SetWsgPosition(0.05, context) self.station.SetWsgVelocity(0, context) # set initial hinge angles of the cupboard. # setting hinge angle to exactly 0 or 90 degrees will result in intermittent contact # with small contact forces between the door and the cupboard body. left_hinge_joint = self.plant.GetJointByName("left_door_hinge") left_hinge_joint.set_angle( context=self.station.GetMutableSubsystemContext( self.plant, context), angle=-0.001) right_hinge_joint = self.plant.GetJointByName("right_door_hinge") right_hinge_joint.set_angle( context=self.station.GetMutableSubsystemContext( self.plant, context), angle=0.001) # set initial pose of the object if self.object_base_link_name is not None: self.plant.tree().SetFreeBodyPoseOrThrow( self.plant.GetBodyByName(self.object_base_link_name, self.object), self.X_WObject, self.station.GetMutableSubsystemContext(self.plant, context)) simulator.set_publish_every_time_step(False) simulator.set_target_realtime_rate(real_time_rate) simulator.Initialize() sim_duration = 0. for plan in plans_list: sim_duration += plan.get_duration() * 1.1 sim_duration += extra_time print "simulation duration", sim_duration simulator.StepTo(sim_duration) return iiwa_position_command_log, \ iiwa_position_measured_log, \ iiwa_external_torque_log, \ plant_state_log
def RunRealRobot(self, plans_list, gripper_setpoint_list): """ Constructs a Diagram that sends commands to ManipulationStationHardwareInterface. @param plans_list: A list of Plans to be executed. @param gripper_setpoint_list: A list of gripper setpoints. Each setpoint corresponds to a Plan. @return: logs of robot configuration and torque, decoded from LCM messges sent by the robot's driver. Logs are SignalLogger systems, whose data can be accessed by SignalLogger.data(). """ builder = DiagramBuilder() camera_ids = ["805212060544"] station_hardware = ManipulationStationHardwareInterface(camera_ids) station_hardware.Connect(wait_for_cameras=False) builder.AddSystem(station_hardware) # Add plan runner # Add plan runner. plan_runner = ManipStationPlanRunner( station=self.station, kuka_plans=plans_list, gripper_setpoint_list=gripper_setpoint_list) builder.AddSystem(plan_runner) builder.Connect(plan_runner.hand_setpoint_output_port, station_hardware.GetInputPort("wsg_position")) builder.Connect(plan_runner.gripper_force_limit_output_port, station_hardware.GetInputPort("wsg_force_limit")) demux = builder.AddSystem(Demultiplexer(14, 7)) builder.Connect( plan_runner.GetOutputPort("iiwa_position_and_torque_command"), demux.get_input_port(0)) builder.Connect(demux.get_output_port(0), station_hardware.GetInputPort("iiwa_position")) builder.Connect( demux.get_output_port(1), station_hardware.GetInputPort("iiwa_feedforward_torque")) builder.Connect( station_hardware.GetOutputPort("iiwa_position_measured"), plan_runner.iiwa_position_input_port) builder.Connect( station_hardware.GetOutputPort("iiwa_velocity_estimated"), plan_runner.iiwa_velocity_input_port) # Add logger iiwa_position_command_log = LogOutput(demux.get_output_port(0), builder) iiwa_position_command_log._DeclarePeriodicPublish(0.005) iiwa_position_measured_log = LogOutput( station_hardware.GetOutputPort("iiwa_position_measured"), builder) iiwa_position_measured_log._DeclarePeriodicPublish(0.005) iiwa_external_torque_log = LogOutput( station_hardware.GetOutputPort("iiwa_torque_external"), builder) iiwa_external_torque_log._DeclarePeriodicPublish(0.005) # build diagram diagram = builder.Build() # RenderSystemWithGraphviz(diagram) # construct simulator simulator = Simulator(diagram) simulator.set_target_realtime_rate(1.0) simulator.set_publish_every_time_step(False) sim_duration = 0. for plan in plans_list: sim_duration += plan.get_duration() * 1.1 print "sending trajectories in 2 seconds..." time.sleep(1.0) print "sending trajectories in 1 second..." time.sleep(1.0) print "sending trajectories now!" simulator.StepTo(sim_duration) return iiwa_position_command_log, \ iiwa_position_measured_log, iiwa_external_torque_log
def build(self, real_time_rate=0, is_visualizing=False): # Create manipulation station simulator self.manip_station_sim = ManipulationStationSimulator( time_step=5e-3, object_file_path=object_file_path, object_base_link_name="base_link", ) # Create plan runner plan_runner, self.plan_scheduler = CreateManipStationPlanRunnerDiagram( station=self.manip_station_sim.station, kuka_plans=[], gripper_setpoint_list=[], rl_environment=True) self.manip_station_sim.plan_runner = plan_runner # Create builder and add systems builder = DiagramBuilder() builder.AddSystem(self.manip_station_sim.station) builder.AddSystem(plan_runner) # Connect systems builder.Connect( plan_runner.GetOutputPort("gripper_setpoint"), self.manip_station_sim.station.GetInputPort("wsg_position")) builder.Connect( plan_runner.GetOutputPort("force_limit"), self.manip_station_sim.station.GetInputPort("wsg_force_limit")) demux = builder.AddSystem(Demultiplexer(14, 7)) builder.Connect( plan_runner.GetOutputPort("iiwa_position_and_torque_command"), demux.get_input_port(0)) builder.Connect( demux.get_output_port(0), self.manip_station_sim.station.GetInputPort("iiwa_position")) builder.Connect( demux.get_output_port(1), self.manip_station_sim.station.GetInputPort( "iiwa_feedforward_torque")) builder.Connect( self.manip_station_sim.station.GetOutputPort( "iiwa_position_measured"), plan_runner.GetInputPort("iiwa_position")) builder.Connect( self.manip_station_sim.station.GetOutputPort( "iiwa_velocity_estimated"), plan_runner.GetInputPort("iiwa_velocity")) # Add meshcat visualizer if is_visualizing: scene_graph = self.manip_station_sim.station.get_mutable_scene_graph( ) viz = MeshcatVisualizer(scene_graph, is_drawing_contact_force=False, plant=self.manip_station_sim.plant) builder.AddSystem(viz) builder.Connect( self.manip_station_sim.station.GetOutputPort("pose_bundle"), viz.GetInputPort("lcm_visualization")) # Build diagram self.diagram = builder.Build() if is_visualizing: print("Setting up visualizer...") viz.load() time.sleep(2.0) # Construct Simulator self.simulator = Simulator(self.diagram) self.manip_station_sim.simulator = self.simulator self.simulator.set_publish_every_time_step(False) self.simulator.set_target_realtime_rate(real_time_rate) self.context = self.diagram.GetMutableSubsystemContext( self.manip_station_sim.station, self.simulator.get_mutable_context()) self.left_hinge_joint = self.manip_station_sim.plant.GetJointByName( "left_door_hinge") self.right_hinge_joint = self.manip_station_sim.plant.GetJointByName( "right_door_hinge") # Goal for training self.goal_position = np.array([0.85, 0, 0.31]) # Object body self.obj = self.manip_station_sim.plant.GetBodyByName( self.manip_station_sim.object_base_link_name, self.manip_station_sim.object) # Properties for RL max_action = np.ones(8) * 0.1 max_action[-1] = 0.03 low_action = -1 * max_action low_action[-1] = 0 self.action_space = ActionSpace(low=low_action, high=max_action) self.state_dim = self._getObservation().shape[0] self._episode_steps = 0 self._max_episode_steps = 75 # Set initial state of the robot self.reset_sim = False self.reset()
def RunRealRobot(self, plan_list, gripper_setpoint_list, sim_duration=None, extra_time=2.0, is_plan_runner_diagram=False,): """ Constructs a Diagram that sends commands to ManipulationStationHardwareInterface. @param plan_list: A list of Plans to be executed. @param gripper_setpoint_list: A list of gripper setpoints. Each setpoint corresponds to a Plan. @param sim_duration: the duration of simulation in seconds. If unset, it is set to the sum of the durations of all plans in plan_list plus extra_time. @param extra_time: the amount of time for which commands are sent, in addition to the duration of all plans. @param is_plan_runner_diagram: True: use the diagram version of PlanRunner; False: use the leaf version of PlanRunner. @return: logs of robot configuration and torque, decoded from LCM messges sent by the robot's driver. Logs are SignalLogger systems, whose data can be accessed by SignalLogger.data(). """ builder = DiagramBuilder() camera_ids = ["805212060544"] station_hardware = ManipulationStationHardwareInterface(camera_ids) station_hardware.Connect(wait_for_cameras=False) builder.AddSystem(station_hardware) # Add plan runner. if is_plan_runner_diagram: plan_runner = CreateManipStationPlanRunnerDiagram( station=self.station, kuka_plans=plan_list, gripper_setpoint_list=gripper_setpoint_list, print_period=0,) else: plan_runner = ManipStationPlanRunner( station=self.station, kuka_plans=plan_list, gripper_setpoint_list=gripper_setpoint_list, print_period=0,) builder.AddSystem(plan_runner) builder.Connect(plan_runner.GetOutputPort("gripper_setpoint"), station_hardware.GetInputPort("wsg_position")) builder.Connect(plan_runner.GetOutputPort("force_limit"), station_hardware.GetInputPort("wsg_force_limit")) demux = builder.AddSystem(Demultiplexer(14, 7)) builder.Connect( plan_runner.GetOutputPort("iiwa_position_and_torque_command"), demux.get_input_port(0)) builder.Connect(demux.get_output_port(0), station_hardware.GetInputPort("iiwa_position")) builder.Connect(demux.get_output_port(1), station_hardware.GetInputPort("iiwa_feedforward_torque")) builder.Connect(station_hardware.GetOutputPort("iiwa_position_measured"), plan_runner.GetInputPort("iiwa_position")) builder.Connect(station_hardware.GetOutputPort("iiwa_velocity_estimated"), plan_runner.GetInputPort("iiwa_velocity")) # Add logger iiwa_position_command_log = LogOutput(demux.get_output_port(0), builder) iiwa_position_command_log._DeclarePeriodicPublish(0.005) iiwa_position_measured_log = LogOutput( station_hardware.GetOutputPort("iiwa_position_measured"), builder) iiwa_position_measured_log._DeclarePeriodicPublish(0.005) iiwa_external_torque_log = LogOutput( station_hardware.GetOutputPort("iiwa_torque_external"), builder) iiwa_external_torque_log._DeclarePeriodicPublish(0.005) # build diagram diagram = builder.Build() # RenderSystemWithGraphviz(diagram) # construct simulator simulator = Simulator(diagram) self.simulator = simulator simulator.set_target_realtime_rate(1.0) simulator.set_publish_every_time_step(False) t_plan = GetPlanStartingTimes(plan_list) if sim_duration is None: sim_duration = t_plan[-1] + extra_time print "sending trajectories in 2 seconds..." time.sleep(1.0) print "sending trajectories in 1 second..." time.sleep(1.0) print "sending trajectories now!" simulator.StepTo(sim_duration) return iiwa_position_command_log, \ iiwa_position_measured_log, iiwa_external_torque_log
def RunSimulation(self, plans_list, gripper_setpoint_list, extra_time=0, real_time_rate=1.0, q0_kuka=np.zeros(7)): ''' Constructs a Diagram that sends commands to ManipulationStation. :param plans_list: :param gripper_setpoint_list: :param extra_time: :param real_time_rate: :param q0_kuka: :return: ''' builder = DiagramBuilder() builder.AddSystem(self.station) # Add plan runner. plan_runner = ManipStationPlanRunner( station=self.station, kuka_plans=plans_list, gripper_setpoint_list=gripper_setpoint_list) builder.AddSystem(plan_runner) builder.Connect(plan_runner.hand_setpoint_output_port, self.station.GetInputPort("wsg_position")) builder.Connect(plan_runner.gripper_force_limit_output_port, self.station.GetInputPort("wsg_force_limit")) demux = builder.AddSystem(Demultiplexer(14, 7)) builder.Connect( plan_runner.GetOutputPort("iiwa_position_and_torque_command"), demux.get_input_port(0)) builder.Connect(demux.get_output_port(0), self.station.GetInputPort("iiwa_position")) builder.Connect(demux.get_output_port(1), self.station.GetInputPort("iiwa_feedforward_torque")) builder.Connect(self.station.GetOutputPort("iiwa_position_measured"), plan_runner.iiwa_position_input_port) builder.Connect(self.station.GetOutputPort("iiwa_velocity_estimated"), plan_runner.iiwa_velocity_input_port) # Add meshcat visualizer scene_graph = self.station.get_mutable_scene_graph() viz = MeshcatVisualizer(scene_graph, is_drawing_contact_force=True, plant=self.plant) self.viz = viz builder.AddSystem(viz) builder.Connect(self.station.GetOutputPort("pose_bundle"), viz.GetInputPort("lcm_visualization")) builder.Connect(self.station.GetOutputPort("contact_results"), viz.GetInputPort("contact_results")) # Add logger iiwa_position_command_log = LogOutput(demux.get_output_port(0), builder) iiwa_position_command_log._DeclarePeriodicPublish(0.005) iiwa_external_torque_log = LogOutput( self.station.GetOutputPort("iiwa_torque_external"), builder) iiwa_external_torque_log._DeclarePeriodicPublish(0.005) iiwa_position_measured_log = LogOutput( self.station.GetOutputPort("iiwa_position_measured"), builder) iiwa_position_measured_log._DeclarePeriodicPublish(0.005) wsg_state_log = LogOutput( self.station.GetOutputPort("wsg_state_measured"), builder) wsg_state_log._DeclarePeriodicPublish(0.1) wsg_command_log = LogOutput(plan_runner.hand_setpoint_output_port, builder) wsg_command_log._DeclarePeriodicPublish(0.1) # build diagram diagram = builder.Build() viz.load() time.sleep(2.0) RenderSystemWithGraphviz(diagram) # construct simulator simulator = Simulator(diagram) context = diagram.GetMutableSubsystemContext( self.station, simulator.get_mutable_context()) # set initial state of the robot self.station.SetIiwaPosition(q0_kuka, context) self.station.SetIiwaVelocity(np.zeros(7), context) self.station.SetWsgPosition(0.05, context) self.station.SetWsgVelocity(0, context) # set initial hinge angles of the cupboard. # setting hinge angle to exactly 0 or 90 degrees will result in intermittent contact # with small contact forces between the door and the cupboard body. left_hinge_joint = self.plant.GetJointByName("left_door_hinge") left_hinge_joint.set_angle( context=self.station.GetMutableSubsystemContext( self.plant, context), angle=-0.001) right_hinge_joint = self.plant.GetJointByName("right_door_hinge") right_hinge_joint.set_angle( context=self.station.GetMutableSubsystemContext( self.plant, context), angle=0.001) # set initial pose of the object self.plant.tree().SetFreeBodyPoseOrThrow( self.plant.GetBodyByName(self.object_base_link_name, self.object), self.X_WObject, self.station.GetMutableSubsystemContext(self.plant, context)) simulator.set_publish_every_time_step(False) simulator.set_target_realtime_rate(real_time_rate) simulator.Initialize() sim_duration = 0. for plan in plans_list: sim_duration += plan.get_duration() * 1.1 sim_duration += extra_time print "simulation duration", sim_duration simulator.StepTo(sim_duration) return iiwa_position_command_log, \ iiwa_position_measured_log, \ iiwa_external_torque_log, \ wsg_state_log, \ wsg_command_log
def RunRealRobot(self, plans_list, gripper_setpoint_list): ''' Constructs a Diagram that sends commands to ManipulationStationHardwareInterface. :param plans_list: :param gripper_setpoint_list: :param extra_time: :param real_time_rate: :param q0_kuka: :return: ''' builder = DiagramBuilder() camera_ids = ["805212060544"] station_hardware = ManipulationStationHardwareInterface(camera_ids) station_hardware.Connect(wait_for_cameras=False) builder.AddSystem(station_hardware) # Add plan runner # Add plan runner. plan_runner = ManipStationPlanRunner( station=self.station, kuka_plans=plans_list, gripper_setpoint_list=gripper_setpoint_list) builder.AddSystem(plan_runner) builder.Connect(plan_runner.hand_setpoint_output_port, station_hardware.GetInputPort("wsg_position")) builder.Connect(plan_runner.gripper_force_limit_output_port, station_hardware.GetInputPort("wsg_force_limit")) demux = builder.AddSystem(Demultiplexer(14, 7)) builder.Connect( plan_runner.GetOutputPort("iiwa_position_and_torque_command"), demux.get_input_port(0)) builder.Connect(demux.get_output_port(0), station_hardware.GetInputPort("iiwa_position")) builder.Connect( demux.get_output_port(1), station_hardware.GetInputPort("iiwa_feedforward_torque")) builder.Connect( station_hardware.GetOutputPort("iiwa_position_measured"), plan_runner.iiwa_position_input_port) builder.Connect( station_hardware.GetOutputPort("iiwa_velocity_estimated"), plan_runner.iiwa_velocity_input_port) # Add logger iiwa_position_measured_log = LogOutput( station_hardware.GetOutputPort("iiwa_position_measured"), builder) iiwa_position_measured_log._DeclarePeriodicPublish(0.005) iiwa_external_torque_log = LogOutput( station_hardware.GetOutputPort("iiwa_torque_external"), builder) iiwa_external_torque_log._DeclarePeriodicPublish(0.005) wsg_state_log = LogOutput( station_hardware.GetOutputPort("wsg_state_measured"), builder) wsg_state_log._DecalrePeriodicPublish(0.1) wsg_command_log = LogOutput(plan_runner.hand_setpoint_output_port, builder) wsg_command_log._DeclarePeriodicPublish(0.1) # build diagram diagram = builder.Build() RenderSystemWithGraphviz(diagram) # construct simulator simulator = Simulator(diagram) simulator.set_target_realtime_rate(1.0) simulator.set_publish_every_time_step(False) sim_duration = 0. for plan in plans_list: sim_duration += plan.get_duration() * 1.1 print "sending trajectories in 2 seconds..." time.sleep(1.0) print "sending trajectories in 1 second..." time.sleep(1.0) print "sending trajectories now!" simulator.StepTo(sim_duration) return iiwa_position_measured_log, iiwa_external_torque_log, wsg_state_log, wsg_command_log