def test_thigh_torque_return_type(self): """Verify the signature of ChooseThighTorque""" from hopper_2d import Hopper2dController builder = DiagramBuilder() plant = builder.AddSystem(MultibodyPlant(0.0005)) parser = Parser(plant) parser.AddModelFromFile("raibert_hopper_2d.sdf") plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("ground")) plant.Finalize() controller = Hopper2dController(plant, desired_lateral_velocity=0.0) x0 = np.zeros(10) x0[1] = 4. # in air x0[4] = 0.5 # feasible leg length x0[5] = 0.1 # initial speed torquedes = controller.ChooseThighTorque(x0) self.assertIsInstance(torquedes, float, "ChooseThighTorque returned a type other than "\ "float for X0 = %s, desired_lateral_velocity = %f" % (np.array_str(x0), controller.desired_lateral_velocity)) # Try from another desired velocity controller.desired_lateral_velocity = -1.0 torquedes = controller.ChooseThighTorque(x0) self.assertIsInstance(torquedes, float, "ChooseThighTorque returned a type other than "\ "float for X0 = %s, desired_lateral_velocity = %f" % (np.array_str(x0), controller.desired_lateral_velocity))
def main(): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 1e-3) body = add_body(plant, "welded_body") add_geometry(plant, body) plant.WeldFrames(plant.world_frame(), body.body_frame()) body = add_body(plant, "floating_body") add_geometry(plant, body) plant.SetDefaultFreeBodyPose(body, RigidTransform([1., 0, 1.])) plant.Finalize() ConnectDrakeVisualizer(builder, scene_graph) diagram = builder.Build() simulator = Simulator(diagram) context = simulator.get_context() # plant.SetFreeBodyPose(context, body, X_WB) # plant.SetFreeBodySpatialVelocity(body, V_WB, context) # Should look at, show 40sec to 50sec. # TODO(eric.cousineau): Without reset stats, it freezes? :( # simulator.AdvanceTo(40.) simulator.set_target_realtime_rate(100.) # simulator.ResetStatistics() dt = 0.1 while context.get_time() < 240.: simulator.AdvanceTo(context.get_time() + dt)
def runPendulumExample(args): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder) parser = Parser(plant) parser.AddModelFromFile(FindResource("pendulum/pendulum.urdf")) plant.Finalize() pose_bundle_output_port = scene_graph.get_pose_bundle_output_port() Tview = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]], dtype=np.float64) visualizer = builder.AddSystem( PlanarSceneGraphVisualizer(scene_graph, Tview=Tview, xlim=[-1.2, 1.2], ylim=[-1.2, 1.2])) builder.Connect(pose_bundle_output_port, visualizer.get_input_port(0)) diagram = builder.Build() simulator = Simulator(diagram) simulator.Initialize() simulator.set_target_realtime_rate(1.0) # Fix the input port to zero. plant_context = diagram.GetMutableSubsystemContext( plant, simulator.get_mutable_context()) plant_context.FixInputPort(plant.get_actuation_input_port().get_index(), np.zeros(plant.num_actuators())) plant_context.SetContinuousState([0.5, 0.1]) simulator.StepTo(args.duration)
def RunSimulation(quadrotor_plant, control_law, x0=np.random.random((8, 1)), duration=30, control_period=0.0333, print_period=1.0, simulation_period=0.0333): quadrotor_controller = QuadrotorController(control_law, control_period=control_period, print_period=print_period) # Create a simple block diagram containing the plant in feedback # with the controller. builder = DiagramBuilder() # The last pendulum plant we made is now owned by a deleted # system, so easiest path is for us to make a new one. plant = builder.AddSystem( QuadrotorPendulum(mb=quadrotor_plant.mb, lb=quadrotor_plant.lb, m1=quadrotor_plant.m1, l1=quadrotor_plant.l1, g=quadrotor_plant.g, input_max=quadrotor_plant.input_max)) controller = builder.AddSystem(quadrotor_controller) builder.Connect(plant.get_output_port(0), controller.get_input_port(0)) builder.Connect(controller.get_output_port(0), plant.get_input_port(0)) # Create a logger to capture the simulation of our plant input_log = builder.AddSystem(SignalLogger(2)) input_log._DeclarePeriodicPublish(control_period, 0.0) builder.Connect(controller.get_output_port(0), input_log.get_input_port(0)) state_log = builder.AddSystem(SignalLogger(8)) state_log._DeclarePeriodicPublish(control_period, 0.0) builder.Connect(plant.get_output_port(0), state_log.get_input_port(0)) diagram = builder.Build() # Set the initial conditions for the simulation. context = diagram.CreateDefaultContext() state = context.get_mutable_continuous_state_vector() state.SetFromVector(x0) # Create the simulator. simulator = Simulator(diagram, context) simulator.Initialize() simulator.set_publish_every_time_step(True) simulator.get_integrator().set_fixed_step_mode(True) simulator.get_integrator().set_maximum_step_size(control_period) # Simulate for the requested duration. simulator.StepTo(duration) return input_log, state_log
def get_basic_manip_station(): """Used for IK""" builder = DiagramBuilder() station = builder.AddSystem(ManipulationStation()) station.SetupClutterClearingStation() station.Finalize() diagram = builder.Build() context = diagram.CreateDefaultContext() plant = station.get_multibody_plant() return plant, context
def setup_manipulation_station(T_world_objectInitial, zmq_url, T_world_targetBin, manipuland="foam", include_bin=True, include_hoop=False): builder = DiagramBuilder() station = builder.AddSystem(ManipulationStation(time_step=1e-3)) station.SetupClutterClearingStation() if manipuland is "foam": station.AddManipulandFromFile( "drake/examples/manipulation_station/models/061_foam_brick.sdf", T_world_objectInitial) elif manipuland is "ball": station.AddManipulandFromFile( "drake/examples/manipulation_station/models/sphere.sdf", T_world_objectInitial) elif manipuland is "bball": station.AddManipulandFromFile( "drake/../../../../../../manipulation/sdfs/bball.sdf", # this is some path hackery T_world_objectInitial) elif manipuland is "rod": station.AddManipulandFromFile( "drake/examples/manipulation_station/models/rod.sdf", T_world_objectInitial) station_plant = station.get_multibody_plant() parser = Parser(station_plant) if include_bin: parser.AddModelFromFile("extra_bin.sdf") station_plant.WeldFrames(station_plant.world_frame(), station_plant.GetFrameByName("extra_bin_base"), T_world_targetBin) if include_hoop: parser.AddModelFromFile("sdfs/hoop.sdf") station_plant.WeldFrames(station_plant.world_frame(), station_plant.GetFrameByName("base_link_hoop"), T_world_targetBin) station.Finalize() frames_to_draw = {"gripper": {"body"}} meshcat = None if zmq_url is not None: meshcat = ConnectMeshcatVisualizer(builder, station.get_scene_graph(), output_port=station.GetOutputPort("pose_bundle"), delete_prefix_on_load=False, frames_to_draw=frames_to_draw, zmq_url=zmq_url) diagram = builder.Build() plant = station.get_multibody_plant() context = plant.CreateDefaultContext() gripper = plant.GetBodyByName("body") initial_pose = plant.EvalBodyPoseInWorld(context, gripper) simulator = Simulator(diagram) simulator.set_target_realtime_rate(1.0) simulator.AdvanceTo(0.01) return initial_pose, meshcat
def simulateRobot(time, B, v_command): tree = RigidBodyTree( FindResource( os.path.dirname(os.path.realpath(__file__)) + "/block_pusher2.urdf"), FloatingBaseType.kFixed) # Set up a block diagram with the robot (dynamics), the controller, and a # visualization block. builder = DiagramBuilder() robot = builder.AddSystem(RigidBodyPlant(tree)) controller = builder.AddSystem(DController(tree, B, v_command)) builder.Connect(robot.get_output_port(0), controller.get_input_port(0)) builder.Connect(controller.get_output_port(0), robot.get_input_port(0)) Tview = np.array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 0., 1.]], dtype=np.float64) visualizer = builder.AddSystem( PlanarRigidBodyVisualizer(tree, Tview, xlim=[-2.8, 4.8], ylim=[-2.8, 10])) builder.Connect(robot.get_output_port(0), visualizer.get_input_port(0)) logger = builder.AddSystem( SignalLogger(tree.get_num_positions() + tree.get_num_velocities())) builder.Connect(robot.get_output_port(0), logger.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(True) # Set the initial conditions context = simulator.get_mutable_context() state = context.get_mutable_continuous_state_vector() start1 = 3 * np.pi / 16 start2 = 15 * np.pi / 16 #np.pi/6 - eps, 2*np.pi/3 + eps, -np.pi/6 + eps, -2*np.pi/3 - eps, np.pi/6 - eps, 2*np.pi/3 + eps, -np.pi/6 + eps, -2*np.pi/3 - eps state.SetFromVector( (start1, start2, -start1, -start2, np.pi + start1, start2, np.pi - start1, -start2, 1, 1, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.)) # (theta1, theta2, theta1dot, theta2dot) # Simulate for 10 seconds simulator.StepTo(time) #import pdb; pdb.set_trace() return (logger.data()[8:11, :], logger.data()[:8, :], logger.data()[19:22, :], logger.data()[11:19, :], logger.sample_times())
def RunSimulation(pendulum_plant, control_law, x0=np.random.random((4, 1)), duration=30): pendulum_controller = PendulumController(control_law) # Create a simple block diagram containing the plant in feedback # with the controller. builder = DiagramBuilder() # The last pendulum plant we made is now owned by a deleted # system, so easiest path is for us to make a new one. plant = builder.AddSystem( InertialWheelPendulum(m1=pendulum_plant.m1, l1=pendulum_plant.l1, m2=pendulum_plant.m2, l2=pendulum_plant.l2, r=pendulum_plant.r, g=pendulum_plant.g, input_max=pendulum_plant.input_max)) controller = builder.AddSystem(pendulum_controller) builder.Connect(plant.get_output_port(0), controller.get_input_port(0)) builder.Connect(controller.get_output_port(0), plant.get_input_port(0)) # Create a logger to capture the simulation of our plant input_log = builder.AddSystem(SignalLogger(1)) input_log._DeclarePeriodicPublish(0.033333, 0.0) builder.Connect(controller.get_output_port(0), input_log.get_input_port(0)) state_log = builder.AddSystem(SignalLogger(4)) state_log._DeclarePeriodicPublish(0.033333, 0.0) builder.Connect(plant.get_output_port(0), state_log.get_input_port(0)) diagram = builder.Build() # Set the initial conditions for the simulation. context = diagram.CreateDefaultContext() state = context.get_mutable_continuous_state_vector() state.SetFromVector(x0) # Create the simulator. simulator = Simulator(diagram, context) simulator.Initialize() simulator.set_publish_every_time_step(False) simulator.get_integrator().set_fixed_step_mode(True) simulator.get_integrator().set_maximum_step_size(0.005) # Simulate for the requested duration. simulator.StepTo(duration) return input_log, state_log
def playbackMotion(data1, data2, data3, data4, times): data = np.concatenate((data2, data1, data4, data3), axis=0) tree = RigidBodyTree( FindResource( os.path.dirname(os.path.realpath(__file__)) + "/block_pusher2.urdf"), FloatingBaseType.kFixed) # Set up a block diagram with the robot (dynamics), the controller, and a # visualization block. builder = DiagramBuilder() robot = builder.AddSystem(Player(data, times)) Tview = np.array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 0., 1.]], dtype=np.float64) visualizer = builder.AddSystem( PlanarRigidBodyVisualizer(tree, Tview, xlim=[-2.8, 4.8], ylim=[-2.8, 10])) #print(robot.get_output_port(0).size()) builder.Connect(robot.get_output_port(0), visualizer.get_input_port(0)) logger = builder.AddSystem( SignalLogger(tree.get_num_positions() + tree.get_num_velocities())) builder.Connect(robot.get_output_port(0), logger.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(True) # Simulate for 10 seconds simulator.StepTo(times[-1] + 0.5)
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 Simulate2dRamone(x0, duration, desired_goal = 0.0, print_period = 0.0): builder = DiagramBuilder() tree = RigidBodyTree() AddModelInstanceFromUrdfFile("ramone_act.urdf", FloatingBaseType.kRollPitchYaw, None, tree) plant = builder.AddSystem(RigidBodyPlant(tree)) controller = builder.AddSystem( Ramone2dController(tree, desired_goal=desired_goal, print_period=print_period)) 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_log = builder.AddSystem(SignalLogger(plant.get_num_states())) builder.Connect(plant.get_output_port(0), state_log.get_input_port(0)) diagram = builder.Build() simulator = Simulator(diagram) simulator.set_target_realtime_rate(1.0) simulator.set_publish_every_time_step(True) simulator.get_mutable_context().set_accuracy(1e-4) state = simulator.get_mutable_context().get_mutable_continuous_state_vector() state.SetFromVector(x0) simulator.StepTo(duration) return tree, controller, state_log, plant
def compile_scene_tree_clearance_geometry_to_mbp_and_sg( scene_tree, timestep=0.001, alpha=0.25): builder = DiagramBuilder() mbp, scene_graph = AddMultibodyPlantSceneGraph( builder, MultibodyPlant(time_step=timestep)) parser = Parser(mbp) parser.package_map().PopulateFromEnvironment("ROS_PACKAGE_PATH") world_body = mbp.world_body() free_body_poses = [] # For generating colors. node_class_to_color_dict = {} cmap = plt.cm.get_cmap('jet') cmap_counter = 0. for node in scene_tree.nodes: if node.tf is not None and node.physics_geometry_info is not None: # Don't have to do anything if this does not introduce geometry. sanity_check_node_tf_and_physics_geom_info(node) phys_geom_info = node.physics_geometry_info has_clearance_geometry = len(phys_geom_info.clearance_geometry) > 0 if not has_clearance_geometry: continue # Add a body for this node and register the clearance geometry. # TODO(gizatt) This tree body index is built in to disambiguate names. # But I forsee a name-to-stuff resolution crisis when inference time comes... # this might get resolved by the solution to that. body = mbp.AddRigidBody(name=node.name, M_BBo_B=phys_geom_info.spatial_inertia) tf = torch_tf_to_drake_tf(node.tf) mbp.SetDefaultFreeBodyPose(body, tf) # Pick out a color for this class. node_type_string = node.__class__.__name__ if node_type_string in node_class_to_color_dict.keys(): color = node_class_to_color_dict[node_type_string] else: color = list(cmap(cmap_counter)) color[3] = alpha node_class_to_color_dict[node_type_string] = color cmap_counter = np.fmod(cmap_counter + np.pi * 2., 1.) # Handle adding primitive geometry by adding it all to one # mbp. if len(phys_geom_info.clearance_geometry) > 0: for k, (tf, geometry) in enumerate( phys_geom_info.clearance_geometry): mbp.RegisterCollisionGeometry( body=body, X_BG=torch_tf_to_drake_tf(tf), shape=geometry, name=node.name + "_col_%03d" % k, coulomb_friction=default_friction) mbp.RegisterVisualGeometry(body=body, X_BG=torch_tf_to_drake_tf(tf), shape=geometry, name=node.name + "_vis_%03d" % k, diffuse_color=color) return builder, mbp, scene_graph
def main(): state_rec = StateReceiver() assert type(state_rec) is StateReceiver assert state_rec.get_utime_output_port().size() == 1 assert state_rec.get_position_output_port().size() == kCassiePositions assert state_rec.get_velocity_output_port().size() == kCassieVelocities assert state_rec.get_contact_output_port().size() == 2 command_pub = CommandSender() assert type(command_pub) is CommandSender assert command_pub.get_utime_input_port().size() == 1 assert command_pub.get_torque_input_port().size() == kCassieActuators assert command_pub.get_motor_pos_input_port().size() == kCassieActuators assert command_pub.get_motor_vel_input_port().size() == kCassieActuators assert command_pub.get_kp_input_port().size() == kCassieActuators assert command_pub.get_kd_input_port().size() == kCassieActuators assert command_pub.get_ki_input_port().size() == kCassieActuators assert command_pub.get_leak_input_port().size() == kCassieActuators assert command_pub.get_clamp_input_port().size() == kCassieActuators lcm = DrakeLcm() builder = DiagramBuilder() state_rec = addLcmStateReceiver(builder, lcm) assert type(state_rec) is StateReceiver command_pub = addLcmCommandSender(builder, lcm, 0.0005) assert type(command_pub) is CommandSender state, command = addLcmControlComms(builder, lcm, 0.0005) assert type(state) is StateReceiver assert type(command) is CommandSender
def test_visualizer(self): builder = DiagramBuilder() scene_graph = builder.AddSystem(SceneGraph()) state = GliderState(np.zeros((7,))) state.x = -.3 state.z = 0.1 state.pitch = 0.2 state.elevator = .4 source = builder.AddSystem(ConstantVectorSource(state[:])) geom = GliderGeometry.AddToBuilder(builder, source.get_output_port(0), scene_graph) # plt_vis = ConnectPlanarSceneGraphVisualizer(builder, scene_graph) drake_viz = ConnectDrakeVisualizer(builder, scene_graph) diagram = builder.Build() context = diagram.CreateDefaultContext() diagram.Publish(context)
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 main(): parser = argparse.ArgumentParser(description=__doc__) parser.add_argument("-t1", default=0.05, help="Extend leg") parser.add_argument("-t2", default=0.5, help="Dwell at top") parser.add_argument("-t3", default=0.5, help="Contract leg") parser.add_argument("-t4", default=0.1, help="Wait at bottom") setup_argparse_for_setup_dot_diagram(parser) MeshcatVisualizer.add_argparse_argument(parser) args = parser.parse_args() t1 = float(args.t1) t2 = float(args.t2) t3 = float(args.t3) t4 = float(args.t4) q_crouch = np.array([ 1600, 2100, 2000, 1600, 2100, 2000, 1400, 2100, 2000, 1400, 2100, 2000 ]) q_extend = np.array([ 1600, 1600, 2400, 1600, 1600, 2400, 1400, 1600, 2400, 1400, 1600, 2400 ]) breaks = np.cumsum([0., t1, t2, t3, t4]) samples = np.stack([q_crouch, q_extend, q_extend, q_crouch, q_crouch]).T trajectory = PiecewisePolynomial.FirstOrderHold(breaks, samples) builder = DiagramBuilder() plant, scene_graph, servo_controller = setup_dot_diagram(builder, args) trajectory_source = builder.AddSystem(TrajectoryLooper(trajectory)) builder.Connect(trajectory_source.get_output_port(0), servo_controller.get_input_port(0)) if args.meshcat: meshcat = ConnectMeshcatVisualizer( builder, output_port=scene_graph.get_query_output_port(), zmq_url=args.meshcat, open_browser=args.open_browser) diagram = builder.Build() simulator = Simulator(diagram) simulator.set_target_realtime_rate(1.0) simulator.AdvanceTo(1E6)
def gripper_forward_kinematics_example(): builder = DiagramBuilder() station = builder.AddSystem(ManipulationStation()) station.SetupClutterClearingStation() station.Finalize() frames_to_draw = { "iiwa": { "iiwa_link_1", "iiwa_link_2", "iiwa_link_3", "iiwa_link_4", "iiwa_link_5", "iiwa_link_6", "iiwa_link_7" }, "gripper": {"body"} } meshcat = ConnectMeshcatVisualizer( builder, station.get_scene_graph(), output_port=station.GetOutputPort("pose_bundle"), frames_to_draw=frames_to_draw, axis_length=0.3, axis_radius=0.01) diagram = builder.Build() context = diagram.CreateDefaultContext() # xyz = Text(value="", description="gripper position (m): ", layout=Layout(width='500px'), style={'description_width':'initial'}) # rpy = Text(value="", description="gripper roll-pitch-yaw (rad): ", layout=Layout(width='500px'), style={'description_width':'initial'}) plant = station.get_multibody_plant() gripper = plant.GetBodyByName("body") def pose_callback(context): pose = plant.EvalBodyPoseInWorld(context, gripper) # Important # xyz.value = np.array2string(pose.translation(), formatter={'float': lambda x: "{:3.2f}".format(x)}) # rpy.value = np.array2string(RollPitchYaw(pose.rotation()).vector(), formatter={'float': lambda x: "{:3.2f}".format(x)}) meshcat.load() MakeJointSlidersThatPublishOnCallback(station.get_multibody_plant(), meshcat, context, my_callback=pose_callback)
def animate(plant, x_trajectory): builder = DiagramBuilder() source = builder.AddSystem(TrajectorySource(x_trajectory)) builder.AddSystem(scene_graph) pos_to_pose = builder.AddSystem( MultibodyPositionToGeometryPose(plant, input_multibody_state=True)) builder.Connect(source.get_output_port(0), pos_to_pose.get_input_port()) builder.Connect(pos_to_pose.get_output_port(), scene_graph.get_source_pose_port(plant.get_source_id())) visualizer = builder.AddSystem( PlanarSceneGraphVisualizer(scene_graph, xlim=[-2, 2], ylim=[-1.25, 2], ax=ax[0])) builder.Connect(scene_graph.get_pose_bundle_output_port(), visualizer.get_input_port(0)) simulator = Simulator(builder.Build()) simulator.AdvanceTo(x_trajectory.end_time())
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") brick = parser.AddModelFromFile( FindResourceOrThrow( "drake/examples/manipulation_station/models/061_foam_brick.sdf")) plant.Finalize() meshcat = ConnectMeshcatVisualizer(builder, scene_graph) diagram = builder.Build() context = diagram.CreateDefaultContext() plant_context = plant.GetMyContextFromRoot(context) B_O = plant.GetBodyByName("base_link", brick) X_WO = plant.EvalBodyPoseInWorld(plant_context, B_O) B_Ggrasp = plant.GetBodyByName("body", 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() X_WGgrasp = X_WO.multiply(X_OGgrasp) plant.SetFreBodyPose(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) meshcat.load() diagram.Publish(context)
def Simulate2dBallAndBeam(x0, duration): builder = DiagramBuilder() # Load in the ball and beam from a description file. tree = RigidBodyTree() AddModelInstancesFromSdfString( open("ball_and_beam.sdf", 'r').read(), FloatingBaseType.kFixed, None, tree) # A RigidBodyPlant wraps a RigidBodyTree to allow # forward dynamical simulation. plant = builder.AddSystem(RigidBodyPlant(tree)) # Spawn a controller and hook it up controller = builder.AddSystem(BallAndBeam2dController(tree)) builder.Connect(plant.get_output_port(0), controller.get_input_port(0)) builder.Connect(controller.get_output_port(0), plant.get_input_port(0)) # Create a logger to log at 30hz state_log = builder.AddSystem(SignalLogger(plant.get_num_states())) state_log._DeclarePeriodicPublish(0.0333, 0.0) # 30hz logging builder.Connect(plant.get_output_port(0), state_log.get_input_port(0)) # Create a simulator diagram = builder.Build() simulator = Simulator(diagram) # Don't limit realtime rate for this sim, since we # produce a video to render it after simulating the whole thing. #simulator.set_target_realtime_rate(100.0) simulator.set_publish_every_time_step(False) # Force the simulator to use a fixed-step integrator, # which is much faster for this stiff system. (Due to the # spring-model of collision, the default variable-timestep # integrator will take very short steps. I've chosen the step # size here to be fast while still being stable in most situations.) integrator = simulator.get_mutable_integrator() integrator.set_fixed_step_mode(True) integrator.set_maximum_step_size(0.001) # Set the initial state state = simulator.get_mutable_context( ).get_mutable_continuous_state_vector() state.SetFromVector(x0) # Simulate! simulator.StepTo(duration) return tree, controller, state_log
def RunSimulation(self, real_time_rate=1.0): ''' Here we test using the NNSystem in a Simulator to drive an acrobot. ''' sdf_file = "assets/acrobot.sdf" urdf_file = "assets/acrobot.urdf" builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder) parser = Parser(plant=plant, scene_graph=scene_graph) parser.AddModelFromFile(sdf_file) plant.Finalize(scene_graph) # Add nn_system = NNSystem(self.pytorch_nn_object) builder.AddSystem(nn_system) # NN -> plant builder.Connect(nn_system.NN_out_output_port, plant.get_actuation_input_port()) # plant -> NN builder.Connect(plant.get_continuous_state_output_port(), nn_system.NN_in_input_port) # Add meshcat visualizer meshcat = MeshcatVisualizer(scene_graph) builder.AddSystem(meshcat) # builder.Connect(scene_graph.GetOutputPort("lcm_visualization"), builder.Connect(scene_graph.get_pose_bundle_output_port(), meshcat.GetInputPort("lcm_visualization")) # build diagram diagram = builder.Build() meshcat.load() # time.sleep(2.0) RenderSystemWithGraphviz(diagram) # construct simulator simulator = Simulator(diagram) # context = diagram.GetMutableSubsystemContext( # self.station, simulator.get_mutable_context()) simulator.set_publish_every_time_step(False) simulator.set_target_realtime_rate(real_time_rate) simulator.Initialize() sim_duration = 5. simulator.StepTo(sim_duration) print("stepping complete")
def simulate_drake_system(plant): # Create a simple block diagram containing our system. builder = DiagramBuilder() system = builder.AddSystem(plant) logger = LogOutput(system.get_output_port(0), builder) diagram = builder.Build() # Set the initial conditions, x(0). context = diagram.CreateDefaultContext() context.SetContinuousState([0, 0, 20, 10, 0, 0]) # Create the simulator, and simulate for 10 seconds. simulator = Simulator(diagram, context) simulator.AdvanceTo(30) # Plotting x_sol = logger.data().T plot_trj_3_wind(x_sol[:, 0:3], np.array([0, 0, 0])) plt.show() return 0
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 RunPendulumSimulation(pendulum_plant, pendulum_controller, x0=[0.9, 0.0], duration=10): ''' Accepts a pendulum_plant (which should be a DampedOscillatingPendulumPlant) and simulates it for 'duration' seconds from initial state `x0`. Returns a logger object which can return simulated timesteps ` logger.sample_times()` (N-by-1) and simulated states `logger.data()` (2-by-N). ''' # Create a simple block diagram containing the plant in feedback # with the controller. builder = DiagramBuilder() plant = builder.AddSystem(pendulum_plant) controller = builder.AddSystem(pendulum_controller) builder.Connect(plant.get_output_port(0), controller.get_input_port(0)) builder.Connect(controller.get_output_port(0), plant.get_input_port(0)) # Create a logger to capture the simulation of our plant # (We tell the logger to expect a 3-variable input, # and hook it up to the pendulum plant's 3-variable output.) logger = builder.AddSystem(SignalLogger(3)) logger.DeclarePeriodicPublish(0.033333, 0.0) builder.Connect(plant.get_output_port(0), logger.get_input_port(0)) diagram = builder.Build() # Create the simulator. simulator = Simulator(diagram) simulator.Initialize() simulator.set_publish_every_time_step(False) # Set the initial conditions for the simulation. state = simulator.get_mutable_context().get_mutable_state()\ .get_mutable_continuous_state().get_mutable_vector() state.SetFromVector(x0) # Simulate for the requested duration. simulator.AdvanceTo(duration) # Return the logger, which stores the output of the # plant across the time steps of the simulation. return logger
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 = ConnectMeshcatVisualizer( builder, station.get_scene_graph(), output_port=station.GetOutputPort("pose_bundle")) 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.GetMyContextFromRoot(simulator.get_mutable_context( )).get_mutable_continuous_state_vector().SetFromVector( station.GetIiwaPosition(station_context)) simulator.set_target_realtime_rate(1.0) simulator.AdvanceTo(0.01) return simulator
def cartPoleTest(args): file_name = FindResourceOrThrow( "drake/examples/multibody/cart_pole/cart_pole.sdf") builder = DiagramBuilder() scene_graph = builder.AddSystem(SceneGraph()) cart_pole = builder.AddSystem(MultibodyPlant()) AddModelFromSdfFile( file_name=file_name, plant=cart_pole, scene_graph=scene_graph) cart_pole.AddForceElement(UniformGravityFieldElement([0, 0, -9.81])) cart_pole.Finalize(scene_graph) assert cart_pole.geometry_source_is_registered() builder.Connect( cart_pole.get_geometry_poses_output_port(), scene_graph.get_source_pose_port(cart_pole.get_source_id())) visualizer = builder.AddSystem(MeshcatVisualizer(scene_graph)) builder.Connect(scene_graph.get_pose_bundle_output_port(), visualizer.get_input_port(0)) diagram = builder.Build() visualizer.load() diagram_context = diagram.CreateDefaultContext() cart_pole_context = diagram.GetMutableSubsystemContext( cart_pole, diagram_context) cart_pole_context.FixInputPort( cart_pole.get_actuation_input_port().get_index(), [0]) cart_slider = cart_pole.GetJointByName("CartSlider") pole_pin = cart_pole.GetJointByName("PolePin") cart_slider.set_translation(context=cart_pole_context, translation=0.) pole_pin.set_angle(context=cart_pole_context, angle=2.) simulator = Simulator(diagram, diagram_context) simulator.set_publish_every_time_step(False) simulator.set_target_realtime_rate(args.target_realtime_rate) simulator.Initialize() simulator.StepTo(args.duration)
def __init__(self, file=None, terrain=FlatTerrain(), dlevel=1): """ Initialize TimeSteppingMultibodyPlant with a model from a file and an arbitrary terrain geometry. Initialization also welds the first frame in the MultibodyPlant to the world frame """ self.builder = DiagramBuilder() self.multibody, self.scene_graph = AddMultibodyPlantSceneGraph( self.builder, 0.001) # Store the terrain self.terrain = terrain self._dlevel = dlevel # Build the MultibodyPlant from the file, if one exists self.model_index = [] if file is not None: # Parse the file self.model_index = Parser(self.multibody).AddModelFromFile( FindResource(file)) # Initialize the collision data self.collision_frames = [] self.collision_poses = [] self.collision_radius = []
def kukaTest(args): file_name = FindResourceOrThrow( "drake/manipulation/models/iiwa_description/sdf/iiwa14_no_collision" ".sdf") builder = DiagramBuilder() scene_graph = builder.AddSystem(SceneGraph()) kuka = builder.AddSystem(MultibodyPlant()) AddModelFromSdfFile( file_name=file_name, plant=kuka, scene_graph=scene_graph) kuka.AddForceElement(UniformGravityFieldElement([0, 0, -9.81])) kuka.Finalize(scene_graph) assert kuka.geometry_source_is_registered() builder.Connect( kuka.get_geometry_poses_output_port(), scene_graph.get_source_pose_port(kuka.get_source_id())) visualizer = builder.AddSystem(MeshcatVisualizer(scene_graph)) builder.Connect(scene_graph.get_pose_bundle_output_port(), visualizer.get_input_port(0)) diagram = builder.Build() visualizer.load() diagram_context = diagram.CreateDefaultContext() kuka_context = diagram.GetMutableSubsystemContext( kuka, diagram_context) kuka_context.FixInputPort( kuka.get_actuation_input_port().get_index(), np.zeros( kuka.get_actuation_input_port().size())) simulator = Simulator(diagram, diagram_context) simulator.set_publish_every_time_step(False) simulator.set_target_realtime_rate(args.target_realtime_rate) simulator.Initialize() simulator.StepTo(args.duration)
def simulate(): # Animate the resulting policy. builder = DiagramBuilder() plant = builder.AddSystem(DoubleIntegrator()) vi_policy = builder.AddSystem(policy) builder.Connect(plant.get_output_port(0), vi_policy.get_input_port(0)) builder.Connect(vi_policy.get_output_port(0), plant.get_input_port(0)) diagram = builder.Build() simulator = Simulator(diagram) logger = LogOutput(plant.get_output_port(0), builder) logger.set_name("logger") simulator.get_mutable_context().SetContinuousState(env.reset()) simulator.AdvanceTo(10. if get_ipython() is not None else 5.) return logger
def main(): parser = argparse.ArgumentParser(description=__doc__) setup_argparse_for_setup_dot_diagram(parser) parser.add_argument("--interactive", action='store_true') MeshcatVisualizer.add_argparse_argument(parser) args = parser.parse_args() q_init = np.array([ 1700, 2000, 2000, 1700, 2000, 2000, 1300, 2000, 2000, 1300, 2000, 2000 ]) builder = DiagramBuilder() plant, scene_graph, servo_controller = setup_dot_diagram(builder, args) if args.interactive: # Add sliders to set positions of the joints. sliders = builder.AddSystem(ServoSliders(servo_controller)) sliders.set_position(q_init) builder.Connect(sliders.get_output_port(0), servo_controller.get_input_port(0)) else: source = builder.AddSystem( ConstantVectorSource(np.zeros(servo_controller.nu))) builder.Connect(source.get_output_port(0), servo_controller.get_input_port(0)) if args.meshcat: meshcat = ConnectMeshcatVisualizer( builder, output_port=scene_graph.get_query_output_port(), zmq_url=args.meshcat, open_browser=args.open_browser) diagram = builder.Build() simulator = Simulator(diagram) simulator.set_target_realtime_rate(1.0) simulator.AdvanceTo(1E6)