def test_adder_simulation(self): builder = DiagramBuilder() adder = builder.AddSystem(self._create_adder_system()) adder.set_name("custom_adder") # Add ZOH so we can easily extract state. zoh = builder.AddSystem(ZeroOrderHold(0.1, 3)) zoh.set_name("zoh") builder.ExportInput(adder.get_input_port(0)) builder.ExportInput(adder.get_input_port(1)) builder.Connect(adder.get_output_port(0), zoh.get_input_port(0)) diagram = builder.Build() context = diagram.CreateDefaultContext() self._fix_adder_inputs(context) simulator = Simulator(diagram, context) simulator.Initialize() simulator.AdvanceTo(1) # Ensure that we have the outputs we want. value = (diagram.GetMutableSubsystemContext( zoh, context).get_discrete_state_vector().get_value()) self.assertTrue(np.allclose([5, 7, 9], value))
def run_manipulation_example(args): builder = DiagramBuilder() station = builder.AddSystem(ManipulationStation(time_step=0.005)) station.SetupManipulationClassStation() station.Finalize() plant = station.get_multibody_plant() scene_graph = station.get_scene_graph() pose_bundle_output_port = station.GetOutputPort("pose_bundle") # Side-on view of the station. T_VW = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]]) visualizer = builder.AddSystem( PlanarSceneGraphVisualizer(scene_graph, T_VW=T_VW, xlim=[-0.5, 1.0], ylim=[-1.2, 1.2], draw_period=0.1)) builder.Connect(pose_bundle_output_port, visualizer.get_input_port(0)) if args.playback: visualizer.start_recording() diagram = builder.Build() simulator = Simulator(diagram) simulator.Initialize() simulator.set_target_realtime_rate(1.0) # Fix the control inputs to zero. station_context = diagram.GetMutableSubsystemContext( station, simulator.get_mutable_context()) station.GetInputPort("iiwa_position").FixValue( station_context, station.GetIiwaPosition(station_context)) station.GetInputPort("iiwa_feedforward_torque").FixValue( station_context, np.zeros(7)) station.GetInputPort("wsg_position").FixValue(station_context, np.zeros(1)) station.GetInputPort("wsg_force_limit").FixValue(station_context, [40.0]) simulator.AdvanceTo(args.duration) if args.playback: visualizer.stop_recording() ani = visualizer.get_recording_as_animation() # Playback the recording and save the output. ani.save("{}/manip_playback.mp4".format(temp_directory()), fps=30)
def __init__(self, is_visualizing=False): self.station = ManipulationStation() self.station.SetupManipulationClassStation( IiwaCollisionModel.kBoxCollision) self.station.Finalize() self.plant = self.station.get_mutable_multibody_plant() self.scene_graph = self.station.get_mutable_scene_graph() self.is_visualizing = is_visualizing # scene graph query output port. self.query_output_port = self.scene_graph.GetOutputPort("query") builder = DiagramBuilder() builder.AddSystem(self.station) # meshcat visualizer if is_visualizing: self.viz = MeshcatVisualizer(self.scene_graph, zmq_url="tcp://127.0.0.1:6000") # connect meshcat to manipulation station builder.AddSystem(self.viz) builder.Connect(self.station.GetOutputPort("pose_bundle"), self.viz.GetInputPort("lcm_visualization")) self.diagram = builder.Build() # contexts self.context_diagram = self.diagram.CreateDefaultContext() self.context_station = self.diagram.GetSubsystemContext( self.station, self.context_diagram) self.context_scene_graph = self.station.GetSubsystemContext( self.scene_graph, self.context_station) self.context_plant = self.station.GetMutableSubsystemContext( self.plant, self.context_station) if is_visualizing: self.viz.load() self.context_meshcat = self.diagram.GetSubsystemContext( self.viz, self.context_diagram)
def test_custom_geometry_name_parsing(self): """ Ensure that name parsing does not fail on programmatically added anchored geometries. """ # Make a minimal example to ensure MeshcatVisualizer loads anchored # geometry. builder = DiagramBuilder() scene_graph = builder.AddSystem(SceneGraph()) visualizer = builder.AddSystem( MeshcatVisualizer(zmq_url=ZMQ_URL, open_browser=False)) builder.Connect(scene_graph.get_query_output_port(), visualizer.get_geometry_query_input_port()) source_id = scene_graph.RegisterSource() geom_inst = GeometryInstance(RigidTransform(), Box(1., 1., 1.), "box") geom_id = scene_graph.RegisterAnchoredGeometry(source_id, geom_inst) # Illustration properties required to ensure geometry is parsed scene_graph.AssignRole(source_id, geom_id, IllustrationProperties()) diagram = builder.Build() simulator = Simulator(diagram) simulator.Initialize()
def run_manipulation_example(args): builder = DiagramBuilder() station = builder.AddSystem(ManipulationStation(time_step=0.005)) station.SetupManipulationClassStation() station.Finalize() plant = station.get_multibody_plant() scene_graph = station.get_scene_graph() pose_bundle_output_port = station.GetOutputPort("pose_bundle") # Side-on view of the station. Tview = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]], dtype=np.float64) visualizer = builder.AddSystem(PlanarSceneGraphVisualizer( scene_graph, T_VW=Tview, xlim=[-0.5, 1.0], ylim=[-1.2, 1.2], draw_period=0.1)) 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 control inputs to zero. station_context = diagram.GetMutableSubsystemContext( station, simulator.get_mutable_context()) station.GetInputPort("iiwa_position").FixValue( station_context, station.GetIiwaPosition(station_context)) station.GetInputPort("iiwa_feedforward_torque").FixValue( station_context, np.zeros(7)) station.GetInputPort("wsg_position").FixValue( station_context, np.zeros(1)) station.GetInputPort("wsg_force_limit").FixValue( station_context, [40.0]) simulator.AdvanceTo(args.duration)
def check_diagram_print_capture(self, add_systems, expected_text, do_advance=True): # noqa # TODO(eric.cousineau): Checking printing is a bit awkward, especially # this way. Find a simpler way to test, but one that's still evident in # how it functions. builder = DiagramBuilder() # Define some constants. # - Abstract (string). s_port = builder.AddSystem( ConstantSource("hello world")).get_output_port(0) # - Vector. x_port = builder.AddSystem(ConstantSource([1, 2, 3])).get_output_port(0) # - Vector of size 1, which could be interpreted as a scalar. z_port = builder.AddSystem(ConstantSource([10.])).get_output_port(0) dt = 0.1 # Add test-specific systems. add_systems(builder, dt, x_port, s_port, z_port) # Build and simulate, capturing output. diagram = builder.Build() simulator = Simulator(diagram) t_end = dt with capture_custom_print_lines() as lines: custom_print("--- Initialize ---") simulator.Initialize() if do_advance: custom_print("--- AdvanceTo ---") simulator.AdvanceTo(t_end) # Check. actual_text = "\n".join(lines) if DEBUG: print() print(actual_text) self.assertEqual(actual_text, expected_text)
from pydrake.systems.drawing import plot_system_graphviz from pydrake.systems.framework import DiagramBuilder import argparse parser = argparse.ArgumentParser() parser.add_argument("--test", action='store_true', help="Causes the script to run without blocking for " "user input.", default=False) args = parser.parse_args() file_name = FindResourceOrThrow( "drake/examples/multibody/cart_pole/cart_pole.sdf") builder = DiagramBuilder() scene_graph = builder.AddSystem(SceneGraph()) cart_pole = builder.AddSystem(MultibodyPlant()) cart_pole.RegisterAsSourceForSceneGraph(scene_graph) AddModelFromSdfFile(file_name=file_name, plant=cart_pole) cart_pole.AddForceElement(UniformGravityFieldElement([0, 0, -9.81])) cart_pole.Finalize() assert cart_pole.geometry_source_is_registered() builder.Connect(scene_graph.get_query_output_port(), cart_pole.get_geometry_query_input_port()) builder.Connect(cart_pole.get_geometry_poses_output_port(), scene_graph.get_source_pose_port(cart_pole.get_source_id())) builder.ExportInput(cart_pole.get_actuation_input_port()) ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph)
def test_diagram_simulation(self): # Similar to: //systems/framework:diagram_test, ExampleDiagram size = 3 builder = DiagramBuilder() adder0 = builder.AddSystem(Adder(2, size)) adder0.set_name("adder0") adder1 = builder.AddSystem(Adder(2, size)) adder1.set_name("adder1") integrator = builder.AddSystem(Integrator(size)) integrator.set_name("integrator") builder.Connect(adder0.get_output_port(0), adder1.get_input_port(0)) builder.Connect(adder1.get_output_port(0), integrator.get_input_port(0)) builder.ExportInput(adder0.get_input_port(0)) builder.ExportInput(adder0.get_input_port(1)) builder.ExportInput(adder1.get_input_port(1)) builder.ExportOutput(integrator.get_output_port(0)) diagram = builder.Build() # TODO(eric.cousineau): Figure out unicode handling if needed. # See //systems/framework/test/diagram_test.cc:349 (sha: bc84e73) # for an example name. diagram.set_name("test_diagram") simulator = Simulator(diagram) context = simulator.get_mutable_context() # Create and attach inputs. # TODO(eric.cousineau): Not seeing any assertions being printed if no # inputs are connected. Need to check this behavior. input0 = np.array([0.1, 0.2, 0.3]) context.FixInputPort(0, input0) input1 = np.array([0.02, 0.03, 0.04]) context.FixInputPort(1, input1) input2 = BasicVector([0.003, 0.004, 0.005]) context.FixInputPort(2, input2) # Test the BasicVector overload. # Initialize integrator states. integrator_xc = ( diagram.GetMutableSubsystemState(integrator, context) .get_mutable_continuous_state().get_vector()) integrator_xc.SetFromVector([0, 1, 2]) simulator.Initialize() # Simulate briefly, and take full-context snapshots at intermediate # points. n = 6 times = np.linspace(0, 1, n) context_log = [] for t in times: simulator.StepTo(t) # Record snapshot of *entire* context. context_log.append(context.Clone()) xc_initial = np.array([0, 1, 2]) xc_final = np.array([0.123, 1.234, 2.345]) for i, context_i in enumerate(context_log): t = times[i] self.assertEqual(context_i.get_time(), t) xc = context_i.get_continuous_state_vector().CopyToVector() xc_expected = (float(i) / (n - 1) * (xc_final - xc_initial) + xc_initial) print("xc[t = {}] = {}".format(t, xc)) self.assertTrue(np.allclose(xc, xc_expected))
"Simulator::set_target_realtime_rate() for details.") parser.add_argument("--duration", type=float, default=np.inf, help="Desired duration of the simulation in seconds.") parser.add_argument( "--hardware", action='store_true', help="Use the ManipulationStationHardwareInterface instead of an " "in-process simulation.") parser.add_argument("--test", action='store_true', help="Disable opening the gui window for testing.") args = parser.parse_args() builder = DiagramBuilder() if args.hardware: # TODO(russt): Replace this hard-coded camera serial number with a config # file. camera_ids = ["805212060544"] station = builder.AddSystem( ManipulationStationHardwareInterface(camera_ids)) station.Connect(wait_for_cameras=False) else: station = builder.AddSystem(ManipulationStation()) station.AddCupboard() object = AddModelFromSdfFile( FindResourceOrThrow( "drake/examples/manipulation_station/models/061_foam_brick.sdf"), "object", station.get_mutable_multibody_plant(),
def main(): parser = argparse.ArgumentParser(description=__doc__) parser.add_argument( "--target_realtime_rate", type=float, default=1.0, help="Desired rate relative to real time. See documentation for " "Simulator::set_target_realtime_rate() for details.") parser.add_argument("--duration", type=float, default=np.inf, help="Desired duration of the simulation in seconds.") parser.add_argument( "--hardware", action='store_true', help="Use the ManipulationStationHardwareInterface instead of an " "in-process simulation.") parser.add_argument("--test", action='store_true', help="Disable opening the gui window for testing.") parser.add_argument( '--setup', type=str, default='manipulation_class', help="The manipulation station setup to simulate. ", choices=['manipulation_class', 'clutter_clearing', 'planar']) parser.add_argument( "-w", "--open-window", dest="browser_new", action="store_const", const=1, default=None, help="Open the MeshCat display in a new browser window.") args = parser.parse_args() builder = DiagramBuilder() # NOTE: the meshcat instance is always created in order to create the # teleop controls (joint sliders and open/close gripper button). When # args.hardware is True, the meshcat server will *not* display robot # geometry, but it will contain the joint sliders and open/close gripper # button in the "Open Controls" tab in the top-right of the viewing server. meshcat = Meshcat() if args.hardware: # TODO(russt): Replace this hard-coded camera serial number with a # config file. camera_ids = ["805212060544"] station = builder.AddSystem( ManipulationStationHardwareInterface(camera_ids)) station.Connect(wait_for_cameras=False) else: station = builder.AddSystem(ManipulationStation()) # Initializes the chosen station type. if args.setup == 'manipulation_class': station.SetupManipulationClassStation() station.AddManipulandFromFile( "drake/examples/manipulation_station/models/" + "061_foam_brick.sdf", RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0])) elif args.setup == 'clutter_clearing': station.SetupClutterClearingStation() ycb_objects = CreateClutterClearingYcbObjectList() for model_file, X_WObject in ycb_objects: station.AddManipulandFromFile(model_file, X_WObject) elif args.setup == 'planar': station.SetupPlanarIiwaStation() station.AddManipulandFromFile( "drake/examples/manipulation_station/models/" + "061_foam_brick.sdf", RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0])) station.Finalize() geometry_query_port = station.GetOutputPort("geometry_query") DrakeVisualizer.AddToBuilder(builder, geometry_query_port) meshcat_visualizer = MeshcatVisualizerCpp.AddToBuilder( builder=builder, query_object_port=geometry_query_port, meshcat=meshcat) if args.setup == 'planar': meshcat.Set2dRenderMode() pyplot_visualizer = ConnectPlanarSceneGraphVisualizer( builder, station.get_scene_graph(), geometry_query_port) if args.browser_new is not None: url = meshcat.web_url() webbrowser.open(url=url, new=args.browser_new) teleop = builder.AddSystem( JointSliders(meshcat=meshcat, plant=station.get_controller_plant())) num_iiwa_joints = station.num_iiwa_joints() filter = builder.AddSystem( FirstOrderLowPassFilter(time_constant=2.0, size=num_iiwa_joints)) builder.Connect(teleop.get_output_port(0), filter.get_input_port(0)) builder.Connect(filter.get_output_port(0), station.GetInputPort("iiwa_position")) wsg_buttons = builder.AddSystem(SchunkWsgButtons(meshcat=meshcat)) builder.Connect(wsg_buttons.GetOutputPort("position"), station.GetInputPort("wsg_position")) builder.Connect(wsg_buttons.GetOutputPort("force_limit"), station.GetInputPort("wsg_force_limit")) # When in regression test mode, log our joint velocities to later check # that they were sufficiently quiet. if args.test: iiwa_velocities = builder.AddSystem(VectorLogSink(num_iiwa_joints)) builder.Connect(station.GetOutputPort("iiwa_velocity_estimated"), iiwa_velocities.get_input_port(0)) else: iiwa_velocities = None diagram = builder.Build() simulator = Simulator(diagram) # This is important to avoid duplicate publishes to the hardware interface: simulator.set_publish_every_time_step(False) station_context = diagram.GetMutableSubsystemContext( station, simulator.get_mutable_context()) station.GetInputPort("iiwa_feedforward_torque").FixValue( station_context, np.zeros(num_iiwa_joints)) # If the diagram is only the hardware interface, then we must advance it a # little bit so that first LCM messages get processed. A simulated plant is # already publishing correct positions even without advancing, and indeed # we must not advance a simulated plant until the sliders and filters have # been initialized to match the plant. if args.hardware: simulator.AdvanceTo(1e-6) # Eval the output port once to read the initial positions of the IIWA. q0 = station.GetOutputPort("iiwa_position_measured").Eval(station_context) teleop.SetPositions(q0) filter.set_initial_output_value( diagram.GetMutableSubsystemContext(filter, simulator.get_mutable_context()), q0) simulator.set_target_realtime_rate(args.target_realtime_rate) simulator.AdvanceTo(args.duration) # Ensure that our initialization logic was correct, by inspecting our # logged joint velocities. if args.test: iiwa_velocities_log = iiwa_velocities.FindLog(simulator.get_context()) for time, qdot in zip(iiwa_velocities_log.sample_times(), iiwa_velocities_log.data().transpose()): # TODO(jwnimmer-tri) We should be able to do better than a 40 # rad/sec limit, but that's the best we can enforce for now. if qdot.max() > 0.1: print(f"ERROR: large qdot {qdot} at time {time}") sys.exit(1)
def main(): parser = argparse.ArgumentParser(description=__doc__) parser.add_argument( "--num_samples", type=int, default=50, help="Number of Monte Carlo samples to use to estimate performance.") parser.add_argument("--torque_limit", type=float, default=2.0, help="Torque limit of the pendulum.") args = parser.parse_args() if args.torque_limit < 0: raise InvalidArgumentError("Please supply a nonnegative torque limit.") # Assemble the Pendulum plant. builder = DiagramBuilder() pendulum = builder.AddSystem(MultibodyPlant(0.0)) file_name = FindResourceOrThrow("drake/examples/pendulum/Pendulum.urdf") Parser(pendulum).AddModelFromFile(file_name) pendulum.WeldFrames(pendulum.world_frame(), pendulum.GetFrameByName("base")) pendulum.Finalize() # Set the pendulum to start at uniformly random # positions (but always zero velocity). elbow = pendulum.GetMutableJointByName("theta") upright_theta = np.pi theta_expression = Variable(name="theta", type=Variable.Type.RANDOM_UNIFORM) * 2. * np.pi elbow.set_random_angle_distribution(theta_expression) # Set up LQR, with high position gains to try to ensure the # ROA is close to the theoretical torque-limited limit. Q = np.diag([100., 1.]) R = np.identity(1) * 0.01 linearize_context = pendulum.CreateDefaultContext() linearize_context.SetContinuousState(np.array([upright_theta, 0.])) actuation_port = pendulum.get_actuation_input_port() actuation_port.FixValue(linearize_context, 0) controller = builder.AddSystem( LinearQuadraticRegulator(pendulum, linearize_context, Q, R, np.zeros(0), actuation_port.get_index())) # Apply the torque limit. torque_limit = args.torque_limit torque_limiter = builder.AddSystem( Saturation(min_value=np.array([-torque_limit]), max_value=np.array([torque_limit]))) builder.Connect(controller.get_output_port(0), torque_limiter.get_input_port(0)) builder.Connect(torque_limiter.get_output_port(0), pendulum.get_actuation_input_port()) builder.Connect(pendulum.get_state_output_port(), controller.get_input_port(0)) diagram = builder.Build() # Perform the Monte Carlo simulation. def make_simulator(generator): ''' Create a simulator for the system using the given generator. ''' simulator = Simulator(diagram) simulator.set_target_realtime_rate(0) simulator.Initialize() return simulator def calc_wrapped_error(system, context): ''' Given a context from the end of the simulation, calculate an error -- which for this stabilizing task is the distance from the fixed point. ''' state = diagram.GetSubsystemContext( pendulum, context).get_continuous_state_vector() error = state.GetAtIndex(0) - upright_theta # Wrap error to [-pi, pi]. return (error + np.pi) % (2 * np.pi) - np.pi num_samples = args.num_samples results = MonteCarloSimulation(make_simulator=make_simulator, output=calc_wrapped_error, final_time=1.0, num_samples=num_samples, generator=RandomGenerator()) # Compute results. # The "success" region is fairly large since some "stabilized" trials # may still be oscillating around the fixed point. Failed examples are # consistently much farther from the fixed point than this. binary_results = np.array([abs(res.output) < 0.1 for res in results]) passing_ratio = float(sum(binary_results)) / len(results) # 95% confidence interval for the passing ratio. passing_ratio_var = 1.96 * np.sqrt(passing_ratio * (1. - passing_ratio) / len(results)) print("Monte-Carlo estimated performance across %d samples: " "%.2f%% +/- %0.2f%%" % (num_samples, passing_ratio * 100, passing_ratio_var * 100)) # Analytically compute the best possible ROA, for comparison, but # calculating where the torque needed to lift the pendulum exceeds # the torque limit. arm_radius = 0.5 arm_mass = 1.0 # torque = r x f = r * (m * 9.81 * sin(theta)) # theta = asin(torque / (r * m)) if torque_limit <= (arm_radius * arm_mass * 9.81): roa_half_width = np.arcsin(torque_limit / (arm_radius * arm_mass * 9.81)) else: roa_half_width = np.pi roa_as_fraction_of_state_space = roa_half_width / np.pi print("Max possible ROA = %0.2f%% of state space, which should" " match with the above estimate." % (100 * roa_as_fraction_of_state_space))
parser.add_argument("--duration", type=float, default=np.inf, help="Desired duration of the simulation in seconds.") parser.add_argument( "--hardware", action='store_true', help="Use the ManipulationStationHardwareInterface instead of an " "in-process simulation.") parser.add_argument("--test", action='store_true', help="Disable opening the gui window for testing.") MeshcatVisualizer.add_argparse_argument(parser) args = parser.parse_args() builder = DiagramBuilder() if args.hardware: # TODO(russt): Replace this hard-coded camera serial number with a config # file. camera_ids = ["805212060544"] station = builder.AddSystem( ManipulationStationHardwareInterface(camera_ids)) station.Connect(wait_for_cameras=False) else: station = builder.AddSystem(ManipulationStation()) station.SetupDefaultStation() station.Finalize() ConnectDrakeVisualizer(builder, station.get_scene_graph(), station.GetOutputPort("pose_bundle"))
def test_decomposition_controller_like_workflow(self): """Tests subgraphs (post-finalize) for decomposition, with a scene-graph. Also shows a workflow of replacing joints / welding joints.""" builder = DiagramBuilder() # N.B. I (Eric) am using ManipulationStation because it's currently # the simplest way to get a complex scene in pydrake. station = ManipulationStation(time_step=0.001) station.SetupManipulationClassStation() station.Finalize() builder.AddSystem(station) plant = station.get_multibody_plant() scene_graph = station.get_scene_graph() iiwa = plant.GetModelInstanceByName("iiwa") wsg = plant.GetModelInstanceByName("gripper") if VISUALIZE: print("test_decomposition_controller_like_workflow") DrakeVisualizer.AddToBuilder(builder, station.GetOutputPort("query_object")) diagram = builder.Build() # Set the context with which things should be computed. d_context = diagram.CreateDefaultContext() context = plant.GetMyContextFromRoot(d_context) q_iiwa = [0.3, 0.7, 0.3, 0.6, 0.5, 0.6, 0.7] ndof = 7 q_wsg = [-0.03, 0.03] plant.SetPositions(context, iiwa, q_iiwa) plant.SetPositions(context, wsg, q_wsg) # Add copy of only the IIWA to a control diagram. control_builder = DiagramBuilder() control_plant = control_builder.AddSystem(MultibodyPlant(time_step=0)) # N.B. This has the same scene, but with all joints outside of the # IIWA frozen # TODO(eric.cousineau): Re-investigate weird "build_with_no_control" # behavior (with scene graph) with default conditions and time_step=0 # - see Anzu commit 2cf08cfc3. to_control = mut.add_plant_with_articulated_subset_to( plant_src=plant, scene_graph_src=scene_graph, articulated_models_src=[iiwa], context_src=context, plant_dest=control_plant) self.assertIsInstance(to_control, mut.MultibodyPlantElementsMap) control_iiwa = to_control.model_instances[iiwa] control_plant.Finalize() self.assertEqual(control_plant.num_positions(), plant.num_positions(iiwa)) kp = np.array([2000., 1500, 1500, 1500, 1500, 500, 500]) kd = np.sqrt(2 * kp) ki = np.zeros(7) controller = control_builder.AddSystem( InverseDynamicsController(robot=control_plant, kp=kp, ki=ki, kd=kd, has_reference_acceleration=False)) # N.B. Rather than use model instances for direct correspence, we could # use the mappings themselves within a custom system. control_builder.Connect( control_plant.get_state_output_port(control_iiwa), controller.get_input_port_estimated_state()) control_builder.Connect( controller.get_output_port_control(), control_plant.get_actuation_input_port(control_iiwa)) # Control to having the elbow slightly bent. q_iiwa_final = np.zeros(7) q_iiwa_final[3] = -np.pi / 2 t_start = 0. t_end = 1. nx = 2 * ndof def q_desired_interpolator(t: ContextTimeArg) -> VectorArg(nx): s = (t - t_start) / (t_end - t_start) ds = 1 / (t_end - t_start) q = q_iiwa + s * (q_iiwa_final - q_iiwa) v = ds * (q_iiwa_final - q_iiwa) x = np.hstack((q, v)) return x interpolator = control_builder.AddSystem( FunctionSystem(q_desired_interpolator)) control_builder.Connect(interpolator.get_output_port(0), controller.get_input_port_desired_state()) control_diagram = control_builder.Build() control_d_context = control_diagram.CreateDefaultContext() control_context = control_plant.GetMyContextFromRoot(control_d_context) to_control.copy_state(context, control_context) util.compare_frame_poses(plant, context, control_plant, control_context, "iiwa_link_0", "iiwa_link_7") util.compare_frame_poses(plant, context, control_plant, control_context, "body", "left_finger") from_control = to_control.inverse() def viz_monitor(control_d_context): # Simulate control, visualizing in original diagram. assert (control_context is control_plant.GetMyContextFromRoot(control_d_context)) from_control.copy_state(control_context, context) d_context.SetTime(control_d_context.get_time()) diagram.Publish(d_context) return EventStatus.DidNothing() simulator = Simulator(control_diagram, control_d_context) simulator.Initialize() if VISUALIZE: simulator.set_monitor(viz_monitor) simulator.set_target_realtime_rate(1.) simulator.AdvanceTo(t_end)
def test_exploding_iiwa_sim(self): """ Shows a simulation of a falling + exploding IIWA which "changes topology" by being rebuilt without joints. """ builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=1e-3) iiwa_file = FindResourceOrThrow( "drake/manipulation/models/iiwa_description/urdf/" "iiwa14_spheres_dense_elbow_collision.urdf") Parser(plant).AddModelFromFile(iiwa_file, "iiwa") # Add ground plane. X_FH = HalfSpace.MakePose([0, 0, 1], [0, 0, 0]) plant.RegisterCollisionGeometry(plant.world_body(), X_FH, HalfSpace(), "ground_plane_collision", CoulombFriction(0.8, 0.3)) plant.Finalize() # Loosey-goosey - no control. for model in me.get_model_instances(plant): util.build_with_no_control(builder, plant, model) if VISUALIZE: print("test_exploding_iiwa_sim") role = Role.kIllustration DrakeVisualizer.AddToBuilder( builder, scene_graph, params=DrakeVisualizerParams(role=role)) ConnectContactResultsToDrakeVisualizer(builder, plant, scene_graph) diagram = builder.Build() # Set up context. d_context = diagram.CreateDefaultContext() context = plant.GetMyContextFromRoot(d_context) # - Hoist IIWA up in the air. plant.SetFreeBodyPose(context, plant.GetBodyByName("base"), RigidTransform([0, 0, 1.])) # - Set joint velocities to "spin" it in the air. for joint in me.get_joints(plant): if isinstance(joint, RevoluteJoint): me.set_joint_positions(plant, context, joint, 0.7) me.set_joint_velocities(plant, context, joint, -5.) def monitor(d_context): # Stop the simulation once there's any contact. context = plant.GetMyContextFromRoot(d_context) query_object = plant.get_geometry_query_input_port().Eval(context) if query_object.HasCollisions(): return EventStatus.ReachedTermination(plant, "Contact") else: return EventStatus.DidNothing() # Forward simulate. simulator = Simulator(diagram, d_context) simulator.Initialize() simulator.set_monitor(monitor) if VISUALIZE: simulator.set_target_realtime_rate(1.) simulator.AdvanceTo(2.) # Try to push a bit further. simulator.clear_monitor() simulator.AdvanceTo(d_context.get_time() + 0.05) diagram.Publish(d_context) # Recreate simulator. builder_new = DiagramBuilder() plant_new, scene_graph_new = AddMultibodyPlantSceneGraph( builder_new, time_step=plant.time_step()) subgraph = mut.MultibodyPlantSubgraph( mut.get_elements_from_plant(plant, scene_graph)) # Remove all joints; make them floating bodies. for joint in me.get_joints(plant): subgraph.remove_joint(joint) # Remove massless bodies. # For more info, see: https://stackoverflow.com/a/62035705/7829525 for body in me.get_bodies(plant): if body is plant.world_body(): continue if body.default_mass() == 0.: subgraph.remove_body(body) # Finalize. to_new = subgraph.add_to(plant_new, scene_graph_new) plant_new.Finalize() if VISUALIZE: DrakeVisualizer.AddToBuilder( builder_new, scene_graph_new, params=DrakeVisualizerParams(role=role)) ConnectContactResultsToDrakeVisualizer(builder_new, plant_new, scene_graph_new) diagram_new = builder_new.Build() # Remap state. d_context_new = diagram_new.CreateDefaultContext() d_context_new.SetTime(d_context.get_time()) context_new = plant_new.GetMyContextFromRoot(d_context_new) to_new.copy_state(context, context_new) # Simulate. simulator_new = Simulator(diagram_new, d_context_new) simulator_new.Initialize() diagram_new.Publish(d_context_new) if VISUALIZE: simulator_new.set_target_realtime_rate(1.) simulator_new.AdvanceTo(context_new.get_time() + 2) if VISUALIZE: input(" Press enter...")
def test_composition_gripper_workflow(self): """Tests subgraphs (pre-finalize) for composition, with a scene graph, welding bodies together across different subgraphs.""" # N.B. The frame-welding is done so that we can easily set the # positions of the IIWA / WSG without having to worry about / work # around the floating body coordinates. # Create IIWA. iiwa_builder = DiagramBuilder() iiwa_plant, iiwa_scene_graph = AddMultibodyPlantSceneGraph( iiwa_builder, time_step=0.) iiwa_file = FindResourceOrThrow( "drake/manipulation/models/iiwa_description/urdf/" "iiwa14_spheres_dense_elbow_collision.urdf") iiwa_model = Parser(iiwa_plant).AddModelFromFile(iiwa_file, "iiwa") iiwa_plant.WeldFrames(iiwa_plant.world_frame(), iiwa_plant.GetFrameByName("base")) iiwa_plant.Finalize() if VISUALIZE: print("test_composition") DrakeVisualizer.AddToBuilder(iiwa_builder, iiwa_scene_graph) iiwa_diagram = iiwa_builder.Build() iiwa_subgraph = mut.MultibodyPlantSubgraph( mut.get_elements_from_plant(iiwa_plant, iiwa_scene_graph)) self.assertIsInstance(iiwa_subgraph, mut.MultibodyPlantSubgraph) iiwa_subgraph.remove_body(iiwa_plant.world_body()) # Create WSG. wsg_builder = DiagramBuilder() wsg_plant, wsg_scene_graph = AddMultibodyPlantSceneGraph(wsg_builder, time_step=0.) wsg_file = FindResourceOrThrow( "drake/manipulation/models/wsg_50_description/sdf/" "schunk_wsg_50.sdf") wsg_model = Parser(wsg_plant).AddModelFromFile(wsg_file, "gripper_model") wsg_plant.WeldFrames(wsg_plant.world_frame(), wsg_plant.GetFrameByName("__model__")) wsg_plant.Finalize() if VISUALIZE: DrakeVisualizer.AddToBuilder(wsg_builder, wsg_scene_graph) wsg_diagram = wsg_builder.Build() wsg_subgraph = mut.MultibodyPlantSubgraph( mut.get_elements_from_plant(wsg_plant, wsg_scene_graph)) self.assertIsInstance(wsg_subgraph, mut.MultibodyPlantSubgraph) wsg_subgraph.remove_body(wsg_plant.world_body()) builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=1e-3) iiwa_to_plant = iiwa_subgraph.add_to(plant, scene_graph) self.assertIsInstance(iiwa_to_plant, mut.MultibodyPlantElementsMap) wsg_to_plant = wsg_subgraph.add_to(plant, scene_graph) self.assertIsInstance(wsg_to_plant, mut.MultibodyPlantElementsMap) if VISUALIZE: DrakeVisualizer.AddToBuilder(builder, scene_graph) rpy_deg = np.array([90., 0., 90]) X_7G = RigidTransform(p=[0, 0, 0.114], rpy=RollPitchYaw(rpy_deg * np.pi / 180)) frame_7 = plant.GetFrameByName("iiwa_link_7") frame_G = plant.GetFrameByName("body") plant.WeldFrames(frame_7, frame_G, X_7G) plant.Finalize() q_iiwa = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7] q_wsg = [-0.03, 0.03] iiwa_d_context = iiwa_diagram.CreateDefaultContext() iiwa_context = iiwa_plant.GetMyContextFromRoot(iiwa_d_context) iiwa_plant.SetPositions(iiwa_context, iiwa_model, q_iiwa) wsg_d_context = wsg_diagram.CreateDefaultContext() wsg_context = wsg_plant.GetMyContextFromRoot(wsg_d_context) wsg_plant.SetPositions(wsg_context, wsg_model, q_wsg) # Transfer state and briefly compare. diagram = builder.Build() d_context = diagram.CreateDefaultContext() context = plant.GetMyContextFromRoot(d_context) iiwa_to_plant.copy_state(iiwa_context, context) wsg_to_plant.copy_state(wsg_context, context) # Compare frames from sub-plants. util.compare_frame_poses(plant, context, iiwa_plant, iiwa_context, "base", "iiwa_link_7") util.compare_frame_poses(plant, context, wsg_plant, wsg_context, "body", "left_finger") # Visualize. if VISUALIZE: print(" Visualize IIWA") Simulator(iiwa_diagram, iiwa_d_context.Clone()).Initialize() input(" Press enter...") print(" Visualize WSG") Simulator(wsg_diagram, wsg_d_context.Clone()).Initialize() input(" Press enter...") print(" Visualize Composite") Simulator(diagram, d_context.Clone()).Initialize() input(" Press enter...")
def test_drake_visualizer(self, T): # Test visualization API. SceneGraph = mut.SceneGraph_[T] DiagramBuilder = DiagramBuilder_[T] Simulator = Simulator_[T] lcm = DrakeLcm() role = mut.Role.kIllustration params = mut.DrakeVisualizerParams( publish_period=0.1, role=mut.Role.kIllustration, default_color=mut.Rgba(0.1, 0.2, 0.3, 0.4)) self.assertEqual(repr(params), "".join([ "DrakeVisualizerParams(" "publish_period=0.1, " "role=Role.kIllustration, " "default_color=Rgba(r=0.1, g=0.2, b=0.3, a=0.4))"])) # Add some subscribers to detect message broadcast. load_channel = "DRAKE_VIEWER_LOAD_ROBOT" draw_channel = "DRAKE_VIEWER_DRAW" load_subscriber = Subscriber( lcm, load_channel, lcmt_viewer_load_robot) draw_subscriber = Subscriber( lcm, draw_channel, lcmt_viewer_draw) # There are three ways to configure DrakeVisualizer. def by_hand(builder, scene_graph, params): visualizer = builder.AddSystem( mut.DrakeVisualizer_[T](lcm=lcm, params=params)) builder.Connect(scene_graph.get_query_output_port(), visualizer.query_object_input_port()) def auto_connect_to_system(builder, scene_graph, params): mut.DrakeVisualizer_[T].AddToBuilder(builder=builder, scene_graph=scene_graph, lcm=lcm, params=params) def auto_connect_to_port(builder, scene_graph, params): mut.DrakeVisualizer_[T].AddToBuilder( builder=builder, query_object_port=scene_graph.get_query_output_port(), lcm=lcm, params=params) for func in [by_hand, auto_connect_to_system, auto_connect_to_port]: # Build the diagram. builder = DiagramBuilder() scene_graph = builder.AddSystem(SceneGraph()) func(builder, scene_graph, params) # Simulate to t = 0 to send initial load and draw messages. diagram = builder.Build() Simulator(diagram).AdvanceTo(0) lcm.HandleSubscriptions(0) self.assertEqual(load_subscriber.count, 1) self.assertEqual(draw_subscriber.count, 1) load_subscriber.clear() draw_subscriber.clear() # Ad hoc broadcasting. scene_graph = SceneGraph() mut.DrakeVisualizer_[T].DispatchLoadMessage( scene_graph, lcm, params) lcm.HandleSubscriptions(0) self.assertEqual(load_subscriber.count, 1) self.assertEqual(draw_subscriber.count, 0) load_subscriber.clear() draw_subscriber.clear()
def test_render_engine_api(self): class DummyRenderEngine(mut.render.RenderEngine): """Mirror of C++ DummyRenderEngine.""" # See comment below about `rgbd_sensor_test.cc`. latest_instance = None def __init__(self, render_label=None): mut.render.RenderEngine.__init__(self) # N.B. We do not hide these because this is a test class. # Normally, you would want to hide this. self.force_accept = False self.registered_geometries = set() self.updated_ids = {} self.include_group_name = "in_test" self.X_WC = RigidTransform_[float]() self.color_count = 0 self.depth_count = 0 self.label_count = 0 self.color_camera = None self.depth_camera = None self.label_camera = None def UpdateViewpoint(self, X_WC): DummyRenderEngine.latest_instance = self self.X_WC = X_WC def ImplementGeometry(self, shape, user_data): DummyRenderEngine.latest_instance = self def DoRegisterVisual(self, id, shape, properties, X_WG): DummyRenderEngine.latest_instance = self mut.GetRenderLabelOrThrow(properties) if self.force_accept or properties.HasGroup( self.include_group_name ): self.registered_geometries.add(id) return True return False def DoUpdateVisualPose(self, id, X_WG): DummyRenderEngine.latest_instance = self self.updated_ids[id] = X_WG def DoRemoveGeometry(self, id): DummyRenderEngine.latest_instance = self self.registered_geometries.remove(id) def DoClone(self): DummyRenderEngine.latest_instance = self new = DummyRenderEngine() new.force_accept = copy.copy(self.force_accept) new.registered_geometries = copy.copy( self.registered_geometries) new.updated_ids = copy.copy(self.updated_ids) new.include_group_name = copy.copy(self.include_group_name) new.X_WC = copy.copy(self.X_WC) new.color_count = copy.copy(self.color_count) new.depth_count = copy.copy(self.depth_count) new.label_count = copy.copy(self.label_count) new.color_camera = copy.copy(self.color_camera) new.depth_camera = copy.copy(self.depth_camera) new.label_camera = copy.copy(self.label_camera) return new def DoRenderColorImage(self, camera, color_image_out): DummyRenderEngine.latest_instance = self self.color_count += 1 self.color_camera = camera def DoRenderDepthImage(self, camera, depth_image_out): DummyRenderEngine.latest_instance = self self.depth_count += 1 self.depth_camera = camera def DoRenderLabelImage(self, camera, label_image_out): DummyRenderEngine.latest_instance = self self.label_count += 1 self.label_camera = camera engine = DummyRenderEngine() self.assertIsInstance(engine, mut.render.RenderEngine) self.assertIsInstance(engine.Clone(), DummyRenderEngine) # Test implementation of C++ interface by using RgbdSensor. renderer_name = "renderer" builder = DiagramBuilder() scene_graph = builder.AddSystem(mut.SceneGraph()) # N.B. This passes ownership. scene_graph.AddRenderer(renderer_name, engine) sensor = builder.AddSystem(RgbdSensor( parent_id=scene_graph.world_frame_id(), X_PB=RigidTransform(), depth_camera=mut.render.DepthRenderCamera( mut.render.RenderCameraCore( renderer_name, CameraInfo(640, 480, np.pi/4), mut.render.ClippingRange(0.1, 5.0), RigidTransform()), mut.render.DepthRange(0.1, 5.0)))) builder.Connect( scene_graph.get_query_output_port(), sensor.query_object_input_port(), ) diagram = builder.Build() diagram_context = diagram.CreateDefaultContext() sensor_context = sensor.GetMyContextFromRoot(diagram_context) image = sensor.color_image_output_port().Eval(sensor_context) # N.B. Because there's context cloning going on under the hood, we # won't be interacting with our originally registered instance. # See `rgbd_sensor_test.cc` as well. current_engine = DummyRenderEngine.latest_instance self.assertIsNot(current_engine, engine) self.assertIsInstance(image, ImageRgba8U) self.assertEqual(current_engine.color_count, 1) image = sensor.depth_image_32F_output_port().Eval(sensor_context) self.assertIsInstance(image, ImageDepth32F) self.assertEqual(current_engine.depth_count, 1) image = sensor.depth_image_16U_output_port().Eval(sensor_context) self.assertIsInstance(image, ImageDepth16U) self.assertEqual(current_engine.depth_count, 2) image = sensor.label_image_output_port().Eval(sensor_context) self.assertIsInstance(image, ImageLabel16I) self.assertEqual(current_engine.label_count, 1)
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
parser = argparse.ArgumentParser() parser.add_argument("-N", "--trials", type=int, help="Number of trials to run.", default=5) parser.add_argument("-T", "--duration", type=float, help="Duration to run each sim.", default=4.0) MeshcatVisualizer.add_argparse_argument(parser) args = parser.parse_args() builder = DiagramBuilder() plant = builder.AddSystem(QuadrotorPlant()) controller = builder.AddSystem(StabilizingLQRController(plant, [0, 0, 1])) # returns y = Cx+Du+y0=-Kx+u0+Kx0=u0-K(x-x0) x = np.zeros(12) x[2] = 1 out = controller.D().dot(x) + controller.y0() # 1.22625 print(out) import pdb pdb.set_trace() builder.Connect(controller.get_output_port(0), plant.get_input_port(0)) builder.Connect(plant.get_output_port(0), controller.get_input_port(0))
def test_lcm_driven_loop(self): """Duplicates the test logic in `lcm_driven_loop_test.cc`.""" lcm_url = "udpm://239.255.76.67:7669" t_start = 3. t_end = 10. def publish_loop(): # Publishes a set of messages for the driven loop. This should be # run from a separate process. # N.B. Because of this, care should be taken not to share C++ # objects between process boundaries. t = t_start while t <= t_end: message = header_t() message.utime = int(1e6 * t) lcm.Publish("TEST_LOOP", message.encode()) time.sleep(0.1) t += 1 class DummySys(LeafSystem): # Converts message to time in seconds. def __init__(self): LeafSystem.__init__(self) self.DeclareAbstractInputPort("header_t", AbstractValue.Make(header_t)) self.DeclareVectorOutputPort(BasicVector(1), self._calc_output) def _calc_output(self, context, output): message = self.EvalAbstractInput(context, 0).get_value() y = output.get_mutable_value() y[:] = message.utime / 1e6 # Construct diagram for LcmDrivenLoop. lcm = DrakeLcm(lcm_url) utime = mut.PyUtimeMessageToSeconds(header_t) sub = mut.LcmSubscriberSystem.Make("TEST_LOOP", header_t, lcm) builder = DiagramBuilder() builder.AddSystem(sub) dummy = builder.AddSystem(DummySys()) builder.Connect(sub.get_output_port(0), dummy.get_input_port(0)) logger = LogOutput(dummy.get_output_port(0), builder) logger.set_forced_publish_only() diagram = builder.Build() dut = mut.LcmDrivenLoop(diagram, sub, None, lcm, utime) dut.set_publish_on_every_received_message(True) # N.B. Use `multiprocessing` instead of `threading` so that we may # avoid issues with GIL deadlocks. publish_proc = Process(target=publish_loop) publish_proc.start() # Initialize to first message. first_msg = dut.WaitForMessage() dut.get_mutable_context().SetTime(utime.GetTimeInSeconds(first_msg)) # Run to desired amount. (Anything more will cause interpreter to # "freeze".) dut.RunToSecondsAssumingInitialized(t_end) publish_proc.join() # Check expected values. log_t_expected = np.array([4, 5, 6, 7, 8, 9]) log_t = logger.sample_times() self.assertTrue(np.allclose(log_t_expected, log_t)) log_y = logger.data() self.assertTrue(np.allclose(log_t_expected, log_y))
y_traj.add_constraint(t=tf, derivative_order=2, lb=[0, 0, 0]) y_traj.generate() # y_traj = PPTrajectory(sample_times, num_vars, degree, continuity_degree) # y_traj.add_constraint(t=0, derivative_order=0, lb=START_STATE[:3]) # initial position # y_traj.add_constraint(t=0, derivative_order=1, lb=[0, 0, 0]) # initial velocity # y_traj.add_constraint(t=0, derivative_order=2, lb=[0, 0, 0]) # initial acceleration # y_traj.add_constraint(t=1, derivative_order=0, lb=[0, 1.0, 0]) # y_traj.add_constraint(t=2, derivative_order=0, lb=[0, 2.0, 0]) # y_traj.add_constraint(t=3, derivative_order=0, lb=[0, 3.0, 0]) # y_traj.add_constraint(t=tf, derivative_order=0, lb=[0, 4.0, 0]) # end at zero # y_traj.add_constraint(t=tf, derivative_order=1, lb=[0, 0, 0]) # y_traj.add_constraint(t=tf, derivative_order=2, lb=[0, 0, 0]) # y_traj.generate() # Build builder = DiagramBuilder() plant = builder.AddSystem(QuadrotorPlant()) controller = builder.AddSystem(QuadrotorController(plant, y_traj, DURATION)) builder.Connect(controller.get_output_port(0), plant.get_input_port(0)) builder.Connect(plant.get_output_port(0), controller.get_input_port(0)) # Set up visualization and env in MeshCat from pydrake.geometry import Box from pydrake.common.eigen_geometry import Isometry3 from pydrake.all import AddMultibodyPlantSceneGraph from pydrake.multibody.tree import SpatialInertia, UnitInertia env_plant, scene_graph = AddMultibodyPlantSceneGraph(builder) for idx, object_ in enumerate(OBJECTS): sz, trans, rot = object_
def main(): args_parser = argparse.ArgumentParser( description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) args_parser.add_argument( "filename", type=str, help="Path to an SDF or URDF file.") args_parser.add_argument( "--package_path", type=str, default=None, help="Full path to the root package for reading in SDF resources.") position_group = args_parser.add_mutually_exclusive_group() position_group.add_argument( "--position", type=float, nargs="+", default=[], help="A list of positions which must be the same length as the number " "of positions in the sdf model. Note that most models have a " "floating-base joint by default (unless the sdf explicitly welds " "the base to the world, and so have 7 positions corresponding to " "the quaternion representation of that floating-base position.") position_group.add_argument( "--joint_position", type=float, nargs="+", default=[], help="A list of positions which must be the same length as the number " "of positions ASSOCIATED WITH JOINTS in the sdf model. This " "does not include, e.g., floating-base coordinates, which will " "be assigned a default value.") args_parser.add_argument( "--test", action='store_true', help="Disable opening the gui window for testing.") # TODO(russt): Add option to weld the base to the world pending the # availability of GetUniqueBaseBody requested in #9747. MeshcatVisualizer.add_argparse_argument(args_parser) args = args_parser.parse_args() filename = args.filename if not os.path.isfile(filename): args_parser.error("File does not exist: {}".format(filename)) builder = DiagramBuilder() scene_graph = builder.AddSystem(SceneGraph()) # Construct a MultibodyPlant. plant = MultibodyPlant(0.0) plant.RegisterAsSourceForSceneGraph(scene_graph) # Create the parser. parser = Parser(plant) # Get the package pathname. if args.package_path: # Verify that package.xml is found in the designated path. package_path = os.path.abspath(args.package_path) if not os.path.isfile(os.path.join(package_path, "package.xml")): parser.error("package.xml not found at: {}".format(package_path)) # Get the package map and populate it using the package path. package_map = parser.package_map() package_map.PopulateFromFolder(package_path) # Add the model from the file and finalize the plant. parser.AddModelFromFile(filename) plant.Finalize() # Add sliders to set positions of the joints. sliders = builder.AddSystem(JointSliders(robot=plant)) to_pose = builder.AddSystem(MultibodyPositionToGeometryPose(plant)) builder.Connect(sliders.get_output_port(0), to_pose.get_input_port()) builder.Connect( to_pose.get_output_port(), scene_graph.get_source_pose_port(plant.get_source_id())) # Connect this to drake_visualizer. ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph) # Connect to Meshcat. if args.meshcat is not None: meshcat_viz = builder.AddSystem( MeshcatVisualizer(scene_graph, zmq_url=args.meshcat)) builder.Connect( scene_graph.get_pose_bundle_output_port(), meshcat_viz.get_input_port(0)) if len(args.position): sliders.set_position(args.position) elif len(args.joint_position): sliders.set_joint_position(args.joint_position) # Make the diagram and run it. diagram = builder.Build() simulator = Simulator(diagram) simulator.set_publish_every_time_step(False) if args.test: sliders.window.withdraw() simulator.AdvanceTo(0.1) else: simulator.set_target_realtime_rate(1.0) simulator.AdvanceTo(np.inf)
def main(): args_parser = argparse.ArgumentParser( description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) add_filename_and_parser_argparse_arguments(args_parser) add_visualizers_argparse_arguments(args_parser) args_parser.add_argument( "--position", type=float, nargs="+", default=[], help="A list of positions which must be the same length as the number " "of positions in the sdf model. Note that most models have a " "floating-base joint by default (unless the sdf explicitly welds " "the base to the world, and so have 7 positions corresponding to " "the quaternion representation of that floating-base position.") # TODO(eric.cousineau): Support sliders (or widgets) for floating body # poses. # TODO(russt): Once floating body sliders are supported, add an option to # disable them too, either by welding via GetUniqueBaseBody #9747 or by # hiding the widgets. args_parser.add_argument( "--test", action='store_true', help="Disable opening the slider gui window for testing.") args = args_parser.parse_args() # NOTE: meshcat is required to create the JointSliders. args.meshcat = True filename, make_parser = parse_filename_and_parser(args_parser, args) update_visualization, connect_visualizers = parse_visualizers( args_parser, args) builder = DiagramBuilder() scene_graph = builder.AddSystem(SceneGraph()) # Construct a MultibodyPlant. # N.B. Do not use AddMultibodyPlantSceneGraph because we want to inject our # custom pose-bundle adjustments for the sliders. plant = MultibodyPlant(time_step=0.0) plant.RegisterAsSourceForSceneGraph(scene_graph) # Add the model from the file and finalize the plant. make_parser(plant).AddModelFromFile(filename) update_visualization(plant, scene_graph) plant.Finalize() meshcat = connect_visualizers(builder, plant, scene_graph) assert meshcat is not None, "Meshcat visualizer not created but required." # Add sliders to set positions of the joints. sliders = builder.AddSystem(JointSliders(meshcat=meshcat, plant=plant)) to_pose = builder.AddSystem(MultibodyPositionToGeometryPose(plant)) builder.Connect(sliders.get_output_port(0), to_pose.get_input_port()) builder.Connect( to_pose.get_output_port(), scene_graph.get_source_pose_port(plant.get_source_id())) if len(args.position): sliders.SetPositions(args.position) # Make the diagram and run it. diagram = builder.Build() simulator = Simulator(diagram) if args.test: simulator.AdvanceTo(0.1) else: simulator.set_target_realtime_rate(1.0) simulator.AdvanceTo(np.inf)
parser.add_argument("--duration", type=float, default=np.inf, help="Desired duration of the simulation in seconds.") parser.add_argument( "--hardware", action='store_true', help="Use the ManipulationStationHardwareInterface instead of an " "in-process simulation.") parser.add_argument("--test", action='store_true', help="Disable opening the gui window for testing.") MeshcatVisualizer.add_argparse_argument(parser) args = parser.parse_args() builder = DiagramBuilder() if args.hardware: station = builder.AddSystem(ManipulationStationHardwareInterface()) station.Connect(wait_for_cameras=False) else: station = builder.AddSystem(ManipulationStation()) station.SetupDefaultStation() parser = Parser(station.get_mutable_multibody_plant(), station.get_mutable_scene_graph()) object = parser.AddModelFromFile( FindResourceOrThrow( "drake/examples/manipulation_station/models/061_foam_brick.sdf"), "object") station.Finalize()
def test_contact_force(self): """A block sitting on a table.""" object_file_path = FindResourceOrThrow( "drake/examples/manipulation_station/models/061_foam_brick.sdf") table_file_path = FindResourceOrThrow( "drake/examples/kuka_iiwa_arm/models/table/" "extra_heavy_duty_table_surface_only_collision.sdf") # T: tabletop frame. X_TObject = RigidTransform([0, 0, 0.2]) builder = DiagramBuilder() plant = MultibodyPlant(0.002) _, scene_graph = AddMultibodyPlantSceneGraph(builder, plant) object_model = Parser(plant=plant).AddModelFromFile(object_file_path) table_model = Parser(plant=plant).AddModelFromFile(table_file_path) # Weld table to world. plant.WeldFrames(A=plant.world_frame(), B=plant.GetFrameByName("link", table_model)) plant.Finalize() # Add meshcat visualizer. viz = builder.AddSystem( MeshcatVisualizer(scene_graph, zmq_url=ZMQ_URL, open_browser=False)) builder.Connect(scene_graph.get_pose_bundle_output_port(), viz.get_input_port(0)) # Add contact visualizer. contact_viz = builder.AddSystem( MeshcatContactVisualizer(meshcat_viz=viz, force_threshold=0, contact_force_scale=10, plant=plant)) contact_input_port = contact_viz.GetInputPort("contact_results") builder.Connect(plant.GetOutputPort("contact_results"), contact_input_port) builder.Connect(scene_graph.get_pose_bundle_output_port(), contact_viz.GetInputPort("pose_bundle")) diagram = builder.Build() diagram_context = diagram.CreateDefaultContext() mbp_context = diagram.GetMutableSubsystemContext( plant, diagram_context) X_WT = plant.CalcRelativeTransform(mbp_context, plant.world_frame(), plant.GetFrameByName("top_center")) plant.SetFreeBodyPose(mbp_context, plant.GetBodyByName("base_link", object_model), X_WT.multiply(X_TObject)) simulator = Simulator(diagram, diagram_context) simulator.set_publish_every_time_step(False) simulator.AdvanceTo(1.0) contact_viz_context = (diagram.GetMutableSubsystemContext( contact_viz, diagram_context)) contact_results = contact_viz.EvalAbstractInput( contact_viz_context, contact_input_port.get_index()).get_value() self.assertGreater(contact_results.num_point_pair_contacts(), 0) self.assertEqual(contact_viz._contact_key_counter, 4)
def LittleDogSimulationDiagram(lcm, rb_tree, dt, drake_visualizer): builder = DiagramBuilder() num_pos = rb_tree.get_num_positions() num_actuators = rb_tree.get_num_actuators() # RigidBodyPlant plant = builder.AddSystem(RigidBodyPlant(rb_tree, dt)) plant.set_name('plant') # Robot command subscriber robot_command_subscriber = builder.AddSystem( LcmSubscriberSystem.Make('ROBOT_COMMAND', littledog_command_t, lcm)) robot_command_subscriber.set_name('robot_command_subscriber') # Robot command to Plant Converter robot_command_to_rigidbodyplant_converter = builder.AddSystem( RobotCommandToRigidBodyPlantLCMConverter( rb_tree.actuators)) # input argu: rigidbody actuators robot_command_to_rigidbodyplant_converter.set_name( 'robot_command_to_rigidbodyplant_converter') # Visualizer visualizer_publisher = builder.AddSystem(drake_visualizer) visualizer_publisher.set_name('visualizer_publisher') visualizer_publisher.set_publish_period(0.02) # Visualize #visualizer_publisher = builder.AddSystem(MeshcatRigidBodyVisualizer(rb_tree)) # Robot State Encoder robot_state_encoder = builder.AddSystem( RobotStateLCMEncoder(rb_tree)) # force_sensor_info robot_state_encoder.set_name('robot_state_encoder') # Robot State Publisher robot_state_publisher = builder.AddSystem( LcmPublisherSystem.Make('EST_ROBOT_STATE', robot_state_t, lcm)) robot_state_publisher.set_name('robot_state_publisher') robot_state_publisher.set_publish_period(0.005) # Connect everything builder.Connect( robot_command_subscriber.get_output_port(0), robot_command_to_rigidbodyplant_converter.robot_command_input_port()) builder.Connect( robot_command_to_rigidbodyplant_converter.desired_effort_output_port(), plant.get_input_port(0)) builder.Connect(plant.state_output_port(), visualizer_publisher.get_input_port(0)) # builder.Connect(plant.get_output_port(0), # visualizer_publisher.get_input_port(0)) builder.Connect(plant.state_output_port(), robot_state_encoder.joint_state_results_input_port()) builder.Connect(robot_state_encoder.lcm_message_output_port(), robot_state_publisher.get_input_port(0)) # Signal logger logger = builder.AddSystem(SignalLogger(num_pos * 2)) builder.Connect(plant.state_output_port(), logger.get_input_port(0)) return builder.Build(), logger, plant
surf.remove() surf2.remove() options.visualization_callback = draw policy, cost_to_go = FittedValueIteration(simulator, cost_function, state_grid, input_grid, timestep, options) J = np.reshape(cost_to_go, Q.shape) surf = ax.plot_surface(Q, Qdot, J, rstride=1, cstride=1, cmap=cm.jet) Pi = np.reshape(policy.get_output_values(), Q.shape) surf = ax2.plot_surface(Q, Qdot, Pi, rstride=1, cstride=1, cmap=cm.jet) # animate the resulting policy. builder = DiagramBuilder() plant = builder.AddSystem(DoubleIntegrator()) logger = LogOutput(plant.get_output_port(0), builder) vi_policy = builder.AddSystem(policy) builder.Connect(vi_policy.get_output_port(0), plant.get_input_port(0)) builder.Connect(plant.get_output_port(0), vi_policy.get_input_port(0)) diagram = builder.Build() simulator = Simulator(diagram) state = simulator.get_mutable_context().SetContinuousState([-10.0, 0.0]) simulator.AdvanceTo(10.) # Visualize the result as a video. vis = DoubleIntegratorVisualizer()
def DepthCameraDemoSystem(): builder = DiagramBuilder() # Create the physics engine + scene graph. plant, scene_graph = AddMultibodyPlantSceneGraph(builder) # Add a single object into it. Parser(plant, scene_graph).AddModelFromFile( FindResourceOrThrow( "drake/manipulation/models/ycb/sdf/006_mustard_bottle.sdf")) # Add a rendering engine renderer = "my_renderer" scene_graph.AddRenderer(renderer, MakeRenderEngineVtk(RenderEngineVtkParams())) plant.Finalize() # Add a visualizer just to help us see the object. use_meshcat = False if use_meshcat: meshcat = builder.AddSystem(MeshcatVisualizer(scene_graph)) builder.Connect(scene_graph.get_pose_bundle_output_port(), meshcat.get_input_port(0)) # Add a camera to the environment. pose = RigidTransform(RollPitchYaw(-0.2, 0.2, 0), [-.1, -0.1, -.5]) properties = DepthCameraProperties(width=640, height=480, fov_y=np.pi / 4.0, renderer_name=renderer, z_near=0.1, z_far=10.0) camera = builder.AddSystem( RgbdSensor(parent_id=scene_graph.world_frame_id(), X_PB=pose, properties=properties, show_window=False)) camera.set_name("rgbd_sensor") builder.Connect(scene_graph.get_query_output_port(), camera.query_object_input_port()) # Export the camera outputs builder.ExportOutput(camera.color_image_output_port(), "color_image") builder.ExportOutput(camera.depth_image_32F_output_port(), "depth_image") # Add a system to convert the camera output into a point cloud to_point_cloud = builder.AddSystem( DepthImageToPointCloud(camera_info=camera.depth_camera_info(), fields=BaseField.kXYZs | BaseField.kRGBs)) builder.Connect(camera.depth_image_32F_output_port(), to_point_cloud.depth_image_input_port()) builder.Connect(camera.color_image_output_port(), to_point_cloud.color_image_input_port()) # Export the point cloud output. builder.ExportOutput(to_point_cloud.point_cloud_output_port(), "point_cloud") diagram = builder.Build() diagram.set_name("depth_camera_demo_system") return diagram
def test_context_api(self): # Capture miscellaneous functions not yet tested. model_value = AbstractValue.Make("Hello") model_vector = BasicVector([1., 2.]) class TrivialSystem(LeafSystem): def __init__(self): LeafSystem.__init__(self) self.DeclareContinuousState(1) self.DeclareDiscreteState(2) self.DeclareAbstractState(model_value.Clone()) self.DeclareAbstractParameter(model_value.Clone()) self.DeclareNumericParameter(model_vector.Clone()) system = TrivialSystem() context = system.CreateDefaultContext() self.assertTrue(context.get_state() is context.get_mutable_state()) self.assertEqual(context.num_continuous_states(), 1) self.assertTrue(context.get_continuous_state_vector() is context.get_mutable_continuous_state_vector()) self.assertEqual(context.num_discrete_state_groups(), 1) self.assertTrue(context.get_discrete_state_vector() is context.get_mutable_discrete_state_vector()) self.assertTrue( context.get_discrete_state(0) is context.get_discrete_state_vector()) self.assertTrue( context.get_discrete_state(0) is context.get_discrete_state().get_vector(0)) self.assertTrue( context.get_mutable_discrete_state(0) is context.get_mutable_discrete_state_vector()) self.assertTrue( context.get_mutable_discrete_state(0) is context.get_mutable_discrete_state().get_vector(0)) self.assertEqual(context.num_abstract_states(), 1) self.assertTrue(context.get_abstract_state() is context.get_mutable_abstract_state()) self.assertTrue( context.get_abstract_state(0) is context.get_mutable_abstract_state(0)) self.assertEqual( context.get_abstract_state(0).get_value(), model_value.get_value()) # Check abstract state API (also test AbstractValues). values = context.get_abstract_state() self.assertEqual(values.size(), 1) self.assertEqual( values.get_value(0).get_value(), model_value.get_value()) self.assertEqual( values.get_mutable_value(0).get_value(), model_value.get_value()) values.SetFrom(values.Clone()) # Check parameter accessors. self.assertEqual(system.num_abstract_parameters(), 1) self.assertEqual( context.get_abstract_parameter(index=0).get_value(), model_value.get_value()) self.assertEqual(system.num_numeric_parameter_groups(), 1) np.testing.assert_equal( context.get_numeric_parameter(index=0).get_value(), model_vector.get_value()) # Check diagram context accessors. builder = DiagramBuilder() builder.AddSystem(system) diagram = builder.Build() context = diagram.CreateDefaultContext() # Existence check. self.assertIsNot(diagram.GetMutableSubsystemState(system, context), None) subcontext = diagram.GetMutableSubsystemContext(system, context) self.assertIsNot(subcontext, None) self.assertIs(diagram.GetSubsystemContext(system, context), subcontext)
def __init__(self, config=None): if config is None: config_path = os.path.join(os.path.dirname(__file__), "config.yaml") config = yaml.safe_load(open(config_path, 'r')) self._config = config self._step_dt = config["step_dt"] self._model_name = config["model_name"] self._sim_diagram = DrakeSimDiagram(config) mbp = self._sim_diagram.mbp builder = self._sim_diagram.builder # === Add table ===== dims = config["table"]["size"] color = np.array(config["table"]["color"]) friction_params = config["table"]["coulomb_friction"] box_shape = Box(*dims) X_W_T = RigidTransform(p=np.array([0., 0., -dims[2] / 2.])) # This rigid body will be added to the world model instance since # the model instance is not specified. box_body = mbp.AddRigidBody( "table", SpatialInertia(mass=1.0, p_PScm_E=np.array([0., 0., 0.]), G_SP_E=UnitInertia(1.0, 1.0, 1.0))) mbp.WeldFrames(mbp.world_frame(), box_body.body_frame(), X_W_T) mbp.RegisterVisualGeometry(box_body, RigidTransform(), box_shape, "table_vis", color) mbp.RegisterCollisionGeometry(box_body, RigidTransform(), box_shape, "table_collision", CoulombFriction(*friction_params)) # === Add pusher ==== parser = Parser(mbp, self._sim_diagram.sg) self._pusher_model_id = parser.AddModelFromFile( path_util.pusher_peg, "pusher") base_link = mbp.GetBodyByName("base", self._pusher_model_id) mbp.WeldFrames(mbp.world_frame(), base_link.body_frame()) def pusher_port_func(): actuation_input_port = mbp.get_actuation_input_port( self._pusher_model_id) state_output_port = mbp.get_state_output_port( self._pusher_model_id) builder.ExportInput(actuation_input_port, "pusher_actuation_input") builder.ExportOutput(state_output_port, "pusher_state_output") self._sim_diagram.finalize_functions.append(pusher_port_func) # === Add slider ==== parser = Parser(mbp, self._sim_diagram.sg) self._slider_model_id = parser.AddModelFromFile( path_util.model_paths[self._model_name], self._model_name) def slider_port_func(): state_output_port = mbp.get_state_output_port( self._slider_model_id) builder.ExportOutput(state_output_port, "slider_state_output") self._sim_diagram.finalize_functions.append(slider_port_func) if "rgbd_sensors" in config and config["rgbd_sensors"]["enabled"]: self._sim_diagram.add_rgbd_sensors_from_config(config) if "visualization" in config: if not config["visualization"]: pass elif config["visualization"] == "meshcat": self._sim_diagram.connect_to_meshcat() elif config["visualization"] == "drake_viz": self._sim_diagram.connect_to_drake_visualizer() else: raise ValueError("Unknown visualization:", config["visualization"]) self._sim_diagram.finalize() builder = DiagramBuilder() builder.AddSystem(self._sim_diagram) pid = builder.AddSystem( PidController(kp=[0, 0], kd=[1000, 1000], ki=[0, 0])) builder.Connect(self._sim_diagram.GetOutputPort("pusher_state_output"), pid.get_input_port_estimated_state()) builder.Connect( pid.get_output_port_control(), self._sim_diagram.GetInputPort("pusher_actuation_input")) builder.ExportInput(pid.get_input_port_desired_state(), "pid_input_port_desired_state") self._diagram = builder.Build() self._pid_desired_state_port = self._diagram.get_input_port(0) self._simulator = Simulator(self._diagram) self.reset() self.action_space = spaces.Box(low=-1., high=1., shape=(2, ), dtype=np.float32) # TODO: Observation space for images is currently too loose self.observation_space = convert_observation_to_space( self.get_observation())