def simulate(*, initial_state, controller_params, t_final, tape_period): """Simulates an Acrobot + Spong controller from the given initial state and parameters until the given final time. Returns the state sampled at the given tape_period. """ builder = DiagramBuilder() plant = builder.AddSystem(AcrobotPlant()) controller = builder.AddSystem(AcrobotSpongController()) builder.Connect(plant.get_output_port(0), controller.get_input_port(0)) builder.Connect(controller.get_output_port(0), plant.get_input_port(0)) state_logger = LogOutput(plant.get_output_port(0), builder) state_logger.set_publish_period(tape_period) diagram = builder.Build() simulator = Simulator(diagram) context = simulator.get_mutable_context() plant_context = diagram.GetMutableSubsystemContext(plant, context) controller_context = diagram.GetMutableSubsystemContext( controller, context) plant_context.SetContinuousState(initial_state) controller_context.get_mutable_numeric_parameter(0).SetFromVector( controller_params) simulator.AdvanceTo(t_final) x_tape = state_logger.data() return x_tape
def ConnectPortsAndAddLoggers(builder, station, plan_runner): builder.AddSystem(plan_runner) 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")) builder.Connect(station.GetOutputPort("iiwa_position_measured"), plan_runner.GetInputPort("iiwa_position")) builder.Connect(station.GetOutputPort("iiwa_position_commanded"), plan_runner.GetInputPort("iiwa_position_cmd")) builder.Connect(station.GetOutputPort("iiwa_velocity_estimated"), plan_runner.GetInputPort("iiwa_velocity")) builder.Connect(station.GetOutputPort("iiwa_torque_external"), plan_runner.GetInputPort("iiwa_torque_external")) # Add loggers iiwa_position_command_log = LogOutput( plan_runner.GetOutputPort("iiwa_position_command"), builder) iiwa_position_command_log.set_publish_period(0.01) iiwa_external_torque_log = LogOutput( station.GetOutputPort("iiwa_torque_external"), builder) iiwa_external_torque_log.set_publish_period(0.01) iiwa_position_measured_log = LogOutput( station.GetOutputPort("iiwa_position_measured"), builder) iiwa_position_measured_log.set_publish_period(0.01) iiwa_velocity_estimated_log = LogOutput( station.GetOutputPort("iiwa_velocity_estimated"), builder) iiwa_velocity_estimated_log.set_publish_period(0.01) return (iiwa_position_command_log, iiwa_position_measured_log, iiwa_external_torque_log, iiwa_velocity_estimated_log)
def RunSimulation(self, q0_iiwa, door_angles, plan_list, gripper_setpoint_list, extra_time=0, real_time_rate=1.0, is_visualizing=True, gripper_setpoint_initial=0.025): """ Constructs a Diagram that sends commands to ManipulationStation. @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 extra_time: the amount of time for which commands are sent, 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_iiwa: initial configuration of the robot. @param is_visualizing: if true, adds MeshcatVisualizer to the Diagram. It should be set to False when running tests. @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. @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. if is_visualizing: print_period=1.0 else: print_period=np.inf plan_runner = IiwaPlanRunner( iiwa_plans=plan_list, gripper_setpoint_list=gripper_setpoint_list, print_period=print_period, gripper_setpoint_initial=gripper_setpoint_initial) self.plan_runner = plan_runner iiwa_position_command_log, iiwa_position_measured_log, \ iiwa_external_torque_log, iiwa_velocity_estimated_log = \ self.ConnectPortsAndAddLoggers(builder, self.station, plan_runner) plant_state_log = LogOutput( self.station.GetOutputPort("plant_continuous_state"), builder) plant_state_log.set_publish_period(0.01) if is_visualizing: # Add meshcat visualizer scene_graph = self.station.get_mutable_scene_graph() viz = MeshcatVisualizer(scene_graph, zmq_url="tcp://127.0.0.1:6000", frames_to_draw=self.frames_to_draw, frames_opacity=0.8) self.meshcat_vis = viz builder.AddSystem(viz) builder.Connect(self.station.GetOutputPort("pose_bundle"), viz.GetInputPort("lcm_visualization")) contact_viz = MeshcatContactVisualizer( meshcat_viz=viz, plant=self.plant) builder.AddSystem(contact_viz) builder.Connect( self.station.GetOutputPort("pose_bundle"), contact_viz.GetInputPort("pose_bundle")) builder.Connect( self.station.GetOutputPort("contact_results"), contact_viz.GetInputPort("contact_results")) # build diagram diagram = builder.Build() self.diagram = diagram # RenderSystemWithGraphviz(diagram) # construct simulator simulator = Simulator(diagram) self.simulator = simulator context = diagram.GetMutableSubsystemContext( self.station, simulator.get_mutable_context()) # set initial state of the robot self.station.SetIiwaPosition(context, q0_iiwa) self.station.SetIiwaVelocity(context, np.zeros(7)) self.station.SetWsgPosition(context, 0.03) self.station.SetWsgVelocity(context, 0) # 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=door_angles[0]) right_hinge_joint = self.plant.GetJointByName("right_door_hinge") right_hinge_joint.set_angle( context=self.station.GetMutableSubsystemContext(self.plant, context), angle=door_angles[1]) simulator.set_publish_every_time_step(False) simulator.set_target_realtime_rate(real_time_rate) # calculate starting time for all plans. t_plan = GetPlanStartingTimes(plan_list) sim_duration = t_plan[-1] + extra_time if is_visualizing: print("simulation duration", sim_duration) print("plan starting times\n", t_plan) self.SetInitialPlanRunnerState(plan_runner, simulator, diagram) simulator.Initialize() simulator.AdvanceTo(sim_duration) return (iiwa_position_command_log, iiwa_position_measured_log, iiwa_external_torque_log, iiwa_velocity_estimated_log, plant_state_log, t_plan)