def plant_system(self): ''' Implements the plant_system method in DrakeEnv by constructing a RigidBodyPlant ''' # Add Systems builder = DiagramBuilder() self.scene_graph = builder.AddSystem(SceneGraph()) self.mbp = builder.AddSystem(MultibodyPlant()) # Load the model from the file AddModelFromSdfFile(file_name=self.model_path, plant=self.mbp, scene_graph=self.scene_graph) self.mbp.AddForceElement(UniformGravityFieldElement([0, 0, -9.81])) self.mbp.Finalize(self.scene_graph) assert self.mbp.geometry_source_is_registered() # Visualizer must be initialized after Finalize() and before CreateDefaultContext() self.init_visualizer() builder.Connect( self.mbp.get_geometry_poses_output_port(), self.scene_graph.get_source_pose_port(self.mbp.get_source_id())) # Expose the inputs and outputs and build the diagram self._input_port_index_action = builder.ExportInput( self.mbp.get_actuation_input_port()) self._output_port_index_state = builder.ExportOutput( self.mbp.get_continuous_state_output_port()) self.diagram = builder.Build() self._output = self.mbp.AllocateOutput() return self.diagram
def PendulumExample(): builder = DiagramBuilder() plant = builder.AddSystem(PendulumPlant()) scene_graph = builder.AddSystem(SceneGraph()) PendulumGeometry.AddToBuilder(builder, plant.get_state_output_port(), scene_graph) MeshcatVisualizerCpp.AddToBuilder( builder, scene_graph, meshcat, MeshcatVisualizerParams(publish_period=np.inf)) builder.ExportInput(plant.get_input_port(), "torque") # Note: The Pendulum-v0 in gym outputs [cos(theta), sin(theta), thetadot] builder.ExportOutput(plant.get_state_output_port(), "state") # Add a camera (I have sugar for this in the manip repo) X_WC = RigidTransform(RotationMatrix.MakeXRotation(-np.pi / 2), [0, -1.5, 0]) rgbd = AddRgbdSensor(builder, scene_graph, X_WC) builder.ExportOutput(rgbd.color_image_output_port(), "camera") diagram = builder.Build() simulator = Simulator(diagram) def reward(system, context): plant_context = plant.GetMyContextFromRoot(context) state = plant_context.get_continuous_state_vector() u = plant.get_input_port().Eval(plant_context)[0] theta = state[0] % 2 * np.pi # Wrap to 2*pi theta_dot = state[1] return (theta - np.pi)**2 + 0.1 * theta_dot**2 + 0.001 * u max_torque = 3 env = DrakeGymEnv(simulator, time_step=0.05, action_space=gym.spaces.Box(low=np.array([-max_torque]), high=np.array([max_torque])), observation_space=gym.spaces.Box( low=np.array([-np.inf, -np.inf]), high=np.array([np.inf, np.inf])), reward=reward, render_rgb_port_id="camera") check_env(env) if show: env.reset() image = env.render(mode='rgb_array') fig, ax = plt.subplots() ax.imshow(image) plt.show()
def build_block_diagram(boat, controller): builder = DiagramBuilder() boat = builder.AddSystem(boat) boat.set_name('boat') # logger to track trajectories logger = LogOutput(boat.get_output_port(0), builder) logger.set_name('logger') # expose boats input as an input port builder.ExportInput(boat.get_input_port(0)) # build diagram diagram = builder.Build() diagram.set_name('diagram') return diagram
def MakeManipulationStation(time_step=0.002, plant_setup_callback=None, camera_prefix="camera"): """ Sets up a manipulation station with the iiwa + wsg + controllers [+ cameras]. Cameras will be added to each body with a name starting with "camera". Args: time_step: the standard MultibodyPlant time step. plant_setup_callback: should be a python callable that takes one argument: `plant_setup_callback(plant)`. It will be called after the iiwa and WSG are added to the plant, but before the plant is finalized. Use this to add additional geometry. camera_prefix: Any bodies in the plant (created during the plant_setup_callback) starting with this prefix will get a camera attached. """ builder = DiagramBuilder() # Add (only) the iiwa, WSG, and cameras to the scene. plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=time_step) iiwa = AddIiwa(plant) wsg = AddWsg(plant, iiwa) if plant_setup_callback: plant_setup_callback(plant) plant.Finalize() num_iiwa_positions = plant.num_positions(iiwa) # I need a PassThrough system so that I can export the input port. iiwa_position = builder.AddSystem(PassThrough(num_iiwa_positions)) builder.ExportInput(iiwa_position.get_input_port(), "iiwa_position") builder.ExportOutput(iiwa_position.get_output_port(), "iiwa_position_command") # Export the iiwa "state" outputs. demux = builder.AddSystem( Demultiplexer(2 * num_iiwa_positions, num_iiwa_positions)) builder.Connect(plant.get_state_output_port(iiwa), demux.get_input_port()) builder.ExportOutput(demux.get_output_port(0), "iiwa_position_measured") builder.ExportOutput(demux.get_output_port(1), "iiwa_velocity_estimated") builder.ExportOutput(plant.get_state_output_port(iiwa), "iiwa_state_estimated") # Make the plant for the iiwa controller to use. controller_plant = MultibodyPlant(time_step=time_step) controller_iiwa = AddIiwa(controller_plant) AddWsg(controller_plant, controller_iiwa, welded=True) controller_plant.Finalize() # Add the iiwa controller iiwa_controller = builder.AddSystem( InverseDynamicsController(controller_plant, kp=[100] * num_iiwa_positions, ki=[1] * num_iiwa_positions, kd=[20] * num_iiwa_positions, has_reference_acceleration=False)) iiwa_controller.set_name("iiwa_controller") builder.Connect(plant.get_state_output_port(iiwa), iiwa_controller.get_input_port_estimated_state()) # Add in the feed-forward torque adder = builder.AddSystem(Adder(2, num_iiwa_positions)) builder.Connect(iiwa_controller.get_output_port_control(), adder.get_input_port(0)) # Use a PassThrough to make the port optional (it will provide zero values # if not connected). torque_passthrough = builder.AddSystem(PassThrough([0] * num_iiwa_positions)) builder.Connect(torque_passthrough.get_output_port(), adder.get_input_port(1)) builder.ExportInput(torque_passthrough.get_input_port(), "iiwa_feedforward_torque") builder.Connect(adder.get_output_port(), plant.get_actuation_input_port(iiwa)) # Add discrete derivative to command velocities. desired_state_from_position = builder.AddSystem( StateInterpolatorWithDiscreteDerivative( num_iiwa_positions, time_step, suppress_initial_transient=True)) desired_state_from_position.set_name("desired_state_from_position") builder.Connect(desired_state_from_position.get_output_port(), iiwa_controller.get_input_port_desired_state()) builder.Connect(iiwa_position.get_output_port(), desired_state_from_position.get_input_port()) # Export commanded torques. builder.ExportOutput(adder.get_output_port(), "iiwa_torque_commanded") builder.ExportOutput(adder.get_output_port(), "iiwa_torque_measured") builder.ExportOutput(plant.get_generalized_contact_forces_output_port(iiwa), "iiwa_torque_external") # Wsg controller. wsg_controller = builder.AddSystem(SchunkWsgPositionController()) wsg_controller.set_name("wsg_controller") builder.Connect(wsg_controller.get_generalized_force_output_port(), plant.get_actuation_input_port(wsg)) builder.Connect(plant.get_state_output_port(wsg), wsg_controller.get_state_input_port()) builder.ExportInput(wsg_controller.get_desired_position_input_port(), "wsg_position") builder.ExportInput(wsg_controller.get_force_limit_input_port(), "wsg_force_limit") wsg_mbp_state_to_wsg_state = builder.AddSystem( MakeMultibodyStateToWsgStateSystem()) builder.Connect(plant.get_state_output_port(wsg), wsg_mbp_state_to_wsg_state.get_input_port()) builder.ExportOutput(wsg_mbp_state_to_wsg_state.get_output_port(), "wsg_state_measured") builder.ExportOutput(wsg_controller.get_grip_force_output_port(), "wsg_force_measured") # Cameras. AddRgbdSensors(builder, plant, scene_graph, model_instance_prefix=camera_prefix) # Export "cheat" ports. builder.ExportOutput(scene_graph.get_query_output_port(), "geometry_query") builder.ExportOutput(plant.get_contact_results_output_port(), "contact_results") builder.ExportOutput(plant.get_state_output_port(), "plant_continuous_state") builder.ExportOutput(plant.get_body_poses_output_port(), "body_poses") diagram = builder.Build() diagram.set_name("ManipulationStation") return diagram
from pydrake.all import (AddMultibodyPlantSceneGraph, DiagramBuilder, Parser, Simulator) from underactuated import FindResource, PlanarSceneGraphVisualizer # Set up a block diagram with the robot (dynamics) and a visualization block. builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder) # Load the double pendulum from Universal Robot Description Format parser = Parser(plant, scene_graph) parser.AddModelFromFile(FindResource("double_pendulum/double_pendulum.urdf")) plant.Finalize() builder.ExportInput(plant.get_actuation_input_port()) visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(scene_graph, xlim=[-2.8, 2.8], ylim=[-2.8, 2.8])) builder.Connect(scene_graph.get_pose_bundle_output_port(), visualizer.get_input_port(0)) diagram = builder.Build() # Set up a simulator to run this diagram simulator = Simulator(diagram) simulator.set_target_realtime_rate(1.0) # Set the initial conditions context = simulator.get_mutable_context() # state is (theta1, theta2, theta1dot, theta2dot) context.SetContinuousState([1., 1., 0., 0.])
def make_box_flipup(generator, observations="state", meshcat=None, time_limit=10): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.001) # TODO(russt): randomize parameters. box = AddPlanarBinAndSimpleBox(plant) finger = AddPointFinger(plant) plant.Finalize() plant.set_name("plant") SetTransparency(scene_graph, alpha=0.5, source_id=plant.get_source_id()) controller_plant = MultibodyPlant(time_step=0.005) AddPointFinger(controller_plant) if meshcat: MeshcatVisualizerCpp.AddToBuilder(builder, scene_graph, meshcat) meshcat.Set2dRenderMode(xmin=-.35, xmax=.35, ymin=-0.1, ymax=0.3) ContactVisualizer.AddToBuilder( builder, plant, meshcat, ContactVisualizerParams(radius=0.005, newtons_per_meter=40.0)) # Use the controller plant to visualize the set point geometry. controller_scene_graph = builder.AddSystem(SceneGraph()) controller_plant.RegisterAsSourceForSceneGraph(controller_scene_graph) SetColor(controller_scene_graph, color=[1.0, 165.0 / 255, 0.0, 1.0], source_id=controller_plant.get_source_id()) controller_vis = MeshcatVisualizerCpp.AddToBuilder( builder, controller_scene_graph, meshcat, MeshcatVisualizerParams(prefix="controller")) controller_vis.set_name("controller meshcat") controller_plant.Finalize() # Stiffness control. (For a point finger with unit mass, the # InverseDynamicsController is identical) N = controller_plant.num_positions() kp = [100] * N ki = [1] * N kd = [2 * np.sqrt(kp[0])] * N controller = builder.AddSystem( InverseDynamicsController(controller_plant, kp, ki, kd, False)) builder.Connect(plant.get_state_output_port(finger), controller.get_input_port_estimated_state()) actions = builder.AddSystem(PassThrough(N)) positions_to_state = builder.AddSystem(Multiplexer([N, N])) builder.Connect(actions.get_output_port(), positions_to_state.get_input_port(0)) zeros = builder.AddSystem(ConstantVectorSource([0] * N)) builder.Connect(zeros.get_output_port(), positions_to_state.get_input_port(1)) builder.Connect(positions_to_state.get_output_port(), controller.get_input_port_desired_state()) builder.Connect(controller.get_output_port(), plant.get_actuation_input_port()) if meshcat: positions_to_poses = builder.AddSystem( MultibodyPositionToGeometryPose(controller_plant)) builder.Connect( positions_to_poses.get_output_port(), controller_scene_graph.get_source_pose_port( controller_plant.get_source_id())) builder.ExportInput(actions.get_input_port(), "actions") if observations == "state": builder.ExportOutput(plant.get_state_output_port(), "observations") # TODO(russt): Add 'time', and 'keypoints' else: raise ValueError("observations must be one of ['state']") class RewardSystem(LeafSystem): def __init__(self): LeafSystem.__init__(self) self.DeclareVectorInputPort("box_state", 6) self.DeclareVectorInputPort("finger_state", 4) self.DeclareVectorInputPort("actions", 2) self.DeclareVectorOutputPort("reward", 1, self.CalcReward) def CalcReward(self, context, output): box_state = self.get_input_port(0).Eval(context) finger_state = self.get_input_port(1).Eval(context) actions = self.get_input_port(2).Eval(context) angle_from_vertical = (box_state[2] % np.pi) - np.pi / 2 cost = 2 * angle_from_vertical**2 # box angle cost += 0.1 * box_state[5]**2 # box velocity effort = actions - finger_state[:2] cost += 0.1 * effort.dot(effort) # effort # finger velocity cost += 0.1 * finger_state[2:].dot(finger_state[2:]) # Add 10 to make rewards positive (to avoid rewarding simulator # crashes). output[0] = 10 - cost reward = builder.AddSystem(RewardSystem()) builder.Connect(plant.get_state_output_port(box), reward.get_input_port(0)) builder.Connect(plant.get_state_output_port(finger), reward.get_input_port(1)) builder.Connect(actions.get_output_port(), reward.get_input_port(2)) builder.ExportOutput(reward.get_output_port(), "reward") # Set random state distributions. uniform_random = Variable(name="uniform_random", type=Variable.Type.RANDOM_UNIFORM) box_joint = plant.GetJointByName("box") x, y = box_joint.get_default_translation() box_joint.set_random_pose_distribution([.2 * uniform_random - .1 + x, y], 0) diagram = builder.Build() simulator = Simulator(diagram) # Termination conditions: def monitor(context): if context.get_time() > time_limit: return EventStatus.ReachedTermination(diagram, "time limit") return EventStatus.Succeeded() simulator.set_monitor(monitor) return simulator
from pydrake.all import (BasicVector, DiagramBuilder, FloatingBaseType, RigidBodyPlant, RigidBodyTree, Simulator) from underactuated import (FindResource, PlanarRigidBodyVisualizer) # Load the double pendulum from Universal Robot Description Format tree = RigidBodyTree(FindResource("double_pendulum/double_pendulum.urdf"), FloatingBaseType.kFixed) # Set up a block diagram with the robot (dynamics) and a visualization block. builder = DiagramBuilder() robot = builder.AddSystem(RigidBodyPlant(tree)) builder.ExportInput(robot.get_input_port(0)) visualizer = builder.AddSystem( PlanarRigidBodyVisualizer(tree, xlim=[-2.8, 2.8], ylim=[-2.8, 2.8])) builder.Connect(robot.get_output_port(0), visualizer.get_input_port(0)) diagram = builder.Build() # Set up a simulator to run this diagram simulator = Simulator(diagram) simulator.set_target_realtime_rate(1.0) simulator.set_publish_every_time_step(False) # Set the initial conditions context = simulator.get_mutable_context() context.FixInputPort(0, BasicVector([0., 0.])) # Zero input torques state = context.get_mutable_continuous_state_vector() state.SetFromVector((1., 1., 0., 0.)) # (theta1, theta2, theta1dot, theta2dot) # Simulate for 10 seconds simulator.StepTo(10)
def makeDiagram(n_quadrotors, n_balls, use_visualizer=False, trajectory_u=None, trajectory_x=None, trajectory_K=None): builder = DiagramBuilder() # Setup quadrotor plants and controllers quadrotor_plants = [] quadrotor_controllers = [] for i in range(n_quadrotors): new_quad = Quadrotor2D(n_quadrotors=n_quadrotors - 1, n_balls=n_balls) new_quad.set_name('quad_' + str(i)) plant = builder.AddSystem(new_quad) quadrotor_plants.append(plant) # Setup ball plants ball_plants = [] for i in range(n_balls): new_ball = Ball2D(n_quadrotors=n_quadrotors, n_balls=n_balls - 1) new_ball.set_name('ball_' + str(i)) plant = builder.AddSystem(new_ball) ball_plants.append(plant) # Connect all plants so that each object (quadrotor or ball) has access to all other object states as inputs for i in range(n_quadrotors): for j in range(n_quadrotors): if i == j: continue k = j if j < i else j - 1 builder.Connect(quadrotor_plants[j].get_output_port(0), quadrotor_plants[i].GetInputPort('quad_' + str(k))) for j in range(n_balls): builder.Connect(ball_plants[j].get_output_port(0), quadrotor_plants[i].GetInputPort('ball_' + str(j))) for i in range(n_balls): for j in range(n_quadrotors): builder.Connect(quadrotor_plants[j].get_output_port(0), ball_plants[i].GetInputPort('quad_' + str(j))) for j in range(n_balls): if i == j: continue k = j if j < i else j - 1 builder.Connect(ball_plants[j].get_output_port(0), ball_plants[i].GetInputPort('ball_' + str(k))) # Setup visualization if use_visualizer: visualizer = builder.AddSystem( Visualizer(n_quadrotors=n_quadrotors, n_balls=n_balls)) visualizer.set_name('visualizer') for i in range(n_quadrotors): builder.Connect(quadrotor_plants[i].get_output_port(0), visualizer.get_input_port(i)) for i in range(n_balls): builder.Connect(ball_plants[i].get_output_port(0), visualizer.get_input_port(n_quadrotors + i)) # Setup trajectory source if trajectory_x is not None and trajectory_u is not None and trajectory_K is not None: demulti_u = builder.AddSystem(Demultiplexer(2 * n_quadrotors, 2)) demulti_u.set_name('feedforward input') demulti_x = builder.AddSystem(Demultiplexer(6 * n_quadrotors, 6)) demulti_x.set_name('reference trajectory') demulti_K = builder.AddSystem(Demultiplexer(12 * n_quadrotors, 12)) demulti_K.set_name('time-varying K') for i in range(n_quadrotors): ltv_lqr = builder.AddSystem(LTVController(6, 2)) ltv_lqr.set_name('LTV LQR ' + str(i)) builder.Connect(demulti_x.get_output_port(i), ltv_lqr.get_input_port(0)) builder.Connect(quadrotor_plants[i].get_output_port(0), ltv_lqr.get_input_port(1)) builder.Connect(demulti_u.get_output_port(i), ltv_lqr.get_input_port(2)) builder.Connect(demulti_K.get_output_port(i), ltv_lqr.get_input_port(3)) builder.Connect(ltv_lqr.get_output_port(0), quadrotor_plants[i].get_input_port(0)) source_u = builder.AddSystem(TrajectorySource(trajectory_u)) source_u.set_name('source feedforward input trajectory') source_x = builder.AddSystem(TrajectorySource(trajectory_x)) source_x.set_name('source reference trajectory') demulti_source_x = builder.AddSystem( Demultiplexer([6 * n_quadrotors, 4 * n_balls])) demulti_source_x.set_name('quad and ball trajectories') source_K = builder.AddSystem(TrajectorySource(trajectory_K)) source_K.set_name('source time-varying K') builder.Connect(source_u.get_output_port(0), demulti_u.get_input_port(0)) builder.Connect(source_x.get_output_port(0), demulti_source_x.get_input_port(0)) builder.Connect(demulti_source_x.get_output_port(0), demulti_x.get_input_port(0)) builder.Connect(source_K.get_output_port(0), demulti_K.get_input_port(0)) else: demulti_u = builder.AddSystem(Demultiplexer(2 * n_quadrotors, 2)) demulti_u.set_name('quadrotor input') for i in range(n_quadrotors): builder.Connect(demulti_u.get_output_port(i), quadrotor_plants[i].get_input_port(0)) builder.ExportInput(demulti_u.get_input_port(0)) diagram = builder.Build() return diagram
class Juggler: def __init__(self, kp=100, ki=1, kd=20, time_step=0.001, show_axis=False): """ Robotic Kuka IIWA juggler with paddle end effector Args: kp (int, optional): proportional gain. Defaults to 100. ki (int, optional): integral gain. Defaults to 1. kd (int, optional): derivative gain. Defaults to 20. time_step (float, optional): time step for internal manipulation station controller. Defaults to 0.001. show_axis (boolean, optional): whether or not to show the axis of reflection. """ self.time = 0 juggler_station = JugglerStation(kp=kp, ki=ki, kd=kd, time_step=time_step, show_axis=show_axis) self.builder = DiagramBuilder() self.station = self.builder.AddSystem(juggler_station.get_diagram()) self.position_log = [] self.velocity_log = [] self.visualizer = ConnectMeshcatVisualizer( self.builder, output_port=self.station.GetOutputPort("geometry_query"), zmq_url=zmq_url) self.plant = juggler_station.get_multibody_plant() # --------------------- ik_sys = self.builder.AddSystem(InverseKinematics(self.plant)) ik_sys.set_name("ik_sys") v_mirror = self.builder.AddSystem(VelocityMirror(self.plant)) v_mirror.set_name("v_mirror") w_tilt = self.builder.AddSystem(AngularVelocityTilt(self.plant)) w_tilt.set_name("w_tilt") integrator = self.builder.AddSystem(Integrator(7)) self.builder.Connect( self.station.GetOutputPort("iiwa_position_measured"), ik_sys.GetInputPort("iiwa_pos_measured")) self.builder.Connect(ik_sys.get_output_port(), integrator.get_input_port()) self.builder.Connect(integrator.get_output_port(), self.station.GetInputPort("iiwa_position")) self.builder.Connect( self.station.GetOutputPort("iiwa_position_measured"), w_tilt.GetInputPort("iiwa_pos_measured")) self.builder.Connect( self.station.GetOutputPort("iiwa_position_measured"), v_mirror.GetInputPort("iiwa_pos_measured")) self.builder.Connect( self.station.GetOutputPort("iiwa_velocity_estimated"), v_mirror.GetInputPort("iiwa_velocity_estimated")) self.builder.Connect( w_tilt.get_output_port(), ik_sys.GetInputPort("paddle_desired_angular_velocity")) self.builder.Connect(v_mirror.get_output_port(), ik_sys.GetInputPort("paddle_desired_velocity")) self.builder.ExportInput(v_mirror.GetInputPort("ball_pose"), "v_ball_pose") self.builder.ExportInput(v_mirror.GetInputPort("ball_velocity"), "v_ball_velocity") self.builder.ExportInput(w_tilt.GetInputPort("ball_pose"), "w_ball_pose") self.builder.ExportInput(w_tilt.GetInputPort("ball_velocity"), "w_ball_velocity") # Useful for debugging # desired_vel = self.builder.AddSystem(ConstantVectorSource([0, 0, 0])) # self.builder.Connect(desired_vel.get_output_port(), ik_sys.GetInputPort("paddle_desired_angular_velocity")) # --------------------- self.diagram = self.builder.Build() self.simulator = Simulator(self.diagram) self.simulator.set_target_realtime_rate(1.0) self.context = self.simulator.get_context() self.station_context = self.station.GetMyContextFromRoot(self.context) self.plant_context = self.plant.GetMyContextFromRoot(self.context) self.plant.SetPositions( self.plant_context, self.plant.GetModelInstanceByName("iiwa7"), [0, np.pi / 3, 0, -np.pi / 2, 0, -np.pi / 3, 0]) self.station.GetInputPort("iiwa_feedforward_torque").FixValue( self.station_context, np.zeros((7, 1))) iiwa_model_instance = self.plant.GetModelInstanceByName("iiwa7") iiwa_q = self.plant.GetPositions(self.plant_context, iiwa_model_instance) integrator.GetMyContextFromRoot( self.context).get_mutable_continuous_state_vector().SetFromVector( iiwa_q) def step(self, simulate=True, duration=0.1, final=True, verbose=False, display_pose=False): """ step the closed loop system Args: simulate (bool, optional): whether or not to visualize the command. Defaults to True. duration (float, optional): duration to complete command in simulation. Defaults to 0.1. final (bool, optional): whether or not this is the final command in the sequence; relevant for recording. Defaults to True. verbose (bool, optional): whether or not to print measured position change. Defaults to False. display_pose (bool, optional): whether or not to show the pose of the paddle in simulation. Defaults to False. """ ball_pose = self.plant.EvalBodyPoseInWorld( self.plant_context, self.plant.GetBodyByName("ball")).translation() ball_velocity = self.plant.EvalBodySpatialVelocityInWorld( self.plant_context, self.plant.GetBodyByName("ball")).translational() self.diagram.GetInputPort("v_ball_pose").FixValue( self.context, ball_pose) self.diagram.GetInputPort("v_ball_velocity").FixValue( self.context, ball_velocity) self.diagram.GetInputPort("w_ball_pose").FixValue( self.context, ball_pose) self.diagram.GetInputPort("w_ball_velocity").FixValue( self.context, ball_velocity) if display_pose: transform = self.plant.EvalBodyPoseInWorld( self.plant_context, self.plant.GetBodyByName("base_link")).GetAsMatrix4() AddTriad(self.visualizer.vis, name=f"paddle_{round(self.time, 1)}", prefix='', length=0.15, radius=.006) self.visualizer.vis[''][ f"paddle_{round(self.time, 1)}"].set_transform(transform) if simulate: self.visualizer.start_recording() self.simulator.AdvanceTo(self.time + duration) self.visualizer.stop_recording() self.position_log.append( self.station.GetOutputPort("iiwa_position_measured").Eval( self.station_context)) self.velocity_log.append( self.station.GetOutputPort("iiwa_velocity_estimated").Eval( self.station_context)) if verbose: print("Time: {}\nMeasured Position: {}\n\n".format( round(self.time, 3), np.around( self.station.GetOutputPort( "iiwa_position_measured").Eval( self.station_context), 3))) if len(self.position_log) >= 2 and np.equal( np.around(self.position_log[-1], 3), np.around(self.position_log[-2], 3)).all(): print("something went wrong, simulation terminated") final = True if final: self.visualizer.publish_recording() return self.time + duration self.time += duration