def test_geometry(self): builder = DiagramBuilder() quadrotor = builder.AddSystem(QuadrotorPlant()) scene_graph = builder.AddSystem(SceneGraph()) state_port = quadrotor.get_output_port(0) geom = QuadrotorGeometry.AddToBuilder(builder, state_port, scene_graph) self.assertTrue(geom.get_frame_id().is_valid())
def test_kuka(self): """Kuka IIWA with mesh geometry.""" file_name = FindResourceOrThrow( "drake/manipulation/models/iiwa_description/sdf/" "iiwa14_no_collision.sdf") builder = DiagramBuilder() scene_graph = builder.AddSystem(SceneGraph()) kuka = builder.AddSystem(MultibodyPlant()) parser = Parser(plant=kuka, scene_graph=scene_graph) parser.AddModelFromFile(file_name) 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, zmq_url=None, open_browser=False)) builder.Connect(scene_graph.get_pose_bundle_output_port(), visualizer.get_input_port(0)) diagram = builder.Build() 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.StepTo(.1)
def RunSimulation(self, real_time_rate=1.0): ''' The Princess Diaries was a good movie. ''' builder = DiagramBuilder() scene_graph = builder.AddSystem(SceneGraph()) # object_file_path = FindResourceOrThrow( # "drake/examples/manipulation_station/models/061_foam_brick.sdf") # sdf_file = FindResourceOrThrow("drake/multibody/benchmarks/acrobot/acrobot.sdf") # urdf_file = FindResourceOrThrow("drake/multibody/benchmarks/acrobot/acrobot.urdf") sdf_file = "assets/acrobot.sdf" urdf_file = "assets/acrobot.urdf" plant = builder.AddSystem(MultibodyPlant()) plant.RegisterAsSourceForSceneGraph(scene_graph) Parser(plant, scene_graph).AddModelFromFile(sdf_file) plant.Finalize(scene_graph) assert plant.geometry_source_is_registered() builder.Connect( plant.get_geometry_poses_output_port(), scene_graph.get_source_pose_port(plant.get_source_id())) builder.Connect(scene_graph.get_query_output_port(), plant.get_geometry_query_input_port()) # 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 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()) meshcat = builder.AddSystem( MeshcatVisualizer(scene_graph, zmq_url=ZMQ_URL, open_browser=False)) builder.Connect( scene_graph.get_pose_bundle_output_port(), meshcat.get_input_port(0)) 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 __init__(self, config): self._config = config builder = DiagramBuilder() dt = config['env']['mbp_dt'] mbp, sg = AddMultibodyPlantSceneGraph(builder, MultibodyPlant(dt), SceneGraph()) parser = Parser(mbp, sg) # renderer_params = RenderEngineVtkParams() # renderer_name = "vtk_renderer" # sg.AddRenderer(renderer_name, MakeRenderEngineVtk(renderer_params)) self._mbp = mbp self._sg = sg self._parser = parser self._builder = builder self._rgbd_sensors = dict() self._finalized = False self._built = False self._diagram = None # add rendered # Add renderer to scene renderer_params = RenderEngineVtkParams() self._renderer_name = "vtk_renderer" sg.AddRenderer(self._renderer_name, MakeRenderEngineVtk(renderer_params))
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("--simulation_time", type=float, default=10.0, help="Desired duration of the simulation in seconds.") parser.add_argument( "--time_step", type=float, default=0., help="If greater than zero, the plant is modeled as a system with " "discrete updates and period equal to this time_step. " "If 0, the plant is modeled as a continuous system.") 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(time_step=args.time_step)) 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())) lcm = DrakeLcm() ConnectVisualization(scene_graph=scene_graph, builder=builder, lcm=lcm) diagram = builder.Build() DispatchLoadMessage(scene_graph=scene_graph, lcm=lcm) 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.simulation_time)
def test_geometry_with_params(self): builder = DiagramBuilder() plant = builder.AddSystem(AcrobotPlant()) scene_graph = builder.AddSystem(SceneGraph()) geom = AcrobotGeometry.AddToBuilder( builder=builder, acrobot_state_port=plant.get_output_port(0), acrobot_params=AcrobotParams(), scene_graph=scene_graph) builder.Build() self.assertIsInstance(geom, AcrobotGeometry)
def test_geometry(self): builder = DiagramBuilder() plant = builder.AddSystem(PendulumPlant()) scene_graph = builder.AddSystem(SceneGraph()) geom = PendulumGeometry.AddToBuilder( builder=builder, pendulum_state_port=plant.get_output_port(0), scene_graph=scene_graph) builder.Build() self.assertIsInstance(geom, PendulumGeometry)
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("--simulation_time", type=float, default=10.0, help="Desired duration of the simulation in seconds.") parser.add_argument( "--time_step", type=float, default=0., help="If greater than zero, the plant is modeled as a system with " "discrete updates and period equal to this time_step. " "If 0, the plant is modeled as a continuous system.") 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(time_step=args.time_step)) cart_pole.RegisterAsSourceForSceneGraph(scene_graph) Parser(plant=cart_pole).AddModelFromFile(file_name) 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())) DrakeVisualizer.AddToBuilder(builder=builder, scene_graph=scene_graph) diagram = builder.Build() diagram_context = diagram.CreateDefaultContext() cart_pole_context = diagram.GetMutableSubsystemContext( cart_pole, diagram_context) cart_pole.get_actuation_input_port().FixValue(cart_pole_context, 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.AdvanceTo(args.simulation_time)
def add_plant_and_scene_graph(builder): # TODO(eric.cousineau): Hoist to C++. plant = builder.AddSystem(MultibodyPlant()) scene_graph = builder.AddSystem(SceneGraph()) plant.RegisterAsSourceForSceneGraph(scene_graph) builder.Connect(scene_graph.get_query_output_port(), plant.get_geometry_query_input_port()) builder.Connect(plant.get_geometry_poses_output_port(), scene_graph.get_source_pose_port(plant.get_source_id())) return plant, scene_graph
def build_block_diagram(actuators_off=False, desired_lateral_velocity=0.0, desired_height=3.0, print_period=0.0): builder = DiagramBuilder() # Build the plant plant = builder.AddSystem(MultibodyPlant(0.0005)) scene_graph = builder.AddSystem(SceneGraph()) plant.RegisterAsSourceForSceneGraph(scene_graph) builder.Connect(plant.get_geometry_poses_output_port(), scene_graph.get_source_pose_port(plant.get_source_id())) builder.Connect(scene_graph.get_query_output_port(), plant.get_geometry_query_input_port()) # Build the robot parser = Parser(plant) parser.AddModelFromFile("raibert_hopper_2d.sdf") plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("ground")) plant.Finalize() plant.set_name('plant') # Create a logger to log at 30hz state_dim = plant.num_positions() + plant.num_velocities() state_log = builder.AddSystem(SignalLogger(state_dim)) state_log.DeclarePeriodicPublish(0.0333, 0.0) # 30hz logging builder.Connect(plant.get_state_output_port(), state_log.get_input_port(0)) state_log.set_name('state_log') # The controller controller = builder.AddSystem( Hopper2dController(plant, desired_lateral_velocity=desired_lateral_velocity, desired_height=desired_height, actuators_off=actuators_off, print_period=print_period)) builder.Connect(plant.get_state_output_port(), controller.get_input_port(0)) builder.Connect(controller.get_output_port(0), plant.get_actuation_input_port()) controller.set_name('controller') # Create visualizer visualizer = builder.AddSystem( PlanarSceneGraphVisualizer(scene_graph, xlim=[-1, 10], ylim=[-.2, 4.5], show=False)) builder.Connect(scene_graph.get_pose_bundle_output_port(), visualizer.get_input_port(0)) visualizer.set_name('visualizer') diagram = builder.Build() return diagram
def test_geometry_with_params(self): builder = DiagramBuilder() plant = builder.AddSystem(RimlessWheel()) scene_graph = builder.AddSystem(SceneGraph()) geom = RimlessWheelGeometry.AddToBuilder( builder=builder, floating_base_state_port=plant.get_floating_base_state_output_port(), # noqa rimless_wheel_params=RimlessWheelParams(), scene_graph=scene_graph) # Confirming that the resulting diagram builds. builder.Build() self.assertIsInstance(geom, RimlessWheelGeometry)
def test_geometry(self): builder = DiagramBuilder() plant = builder.AddSystem(CompassGait()) scene_graph = builder.AddSystem(SceneGraph()) geom = CompassGaitGeometry.AddToBuilder( builder=builder, floating_base_state_port=plant.get_floating_base_state_output_port( ), # noqa scene_graph=scene_graph) # Confirming that the resulting diagram builds. builder.Build() self.assertIsInstance(geom, CompassGaitGeometry)
def load_tables(time_step=0.0): mbp = MultibodyPlant(time_step=time_step) scene_graph = SceneGraph() # TODO: meshes aren't supported during collision checking robot = AddModelFromSdfFile(file_name=IIWA14_SDF_PATH, model_name='iiwa', scene_graph=scene_graph, plant=mbp) gripper = AddModelFromSdfFile(file_name=WSG50_SDF_PATH, model_name='gripper', scene_graph=scene_graph, plant=mbp) # TODO: sdf frame/link error table = AddModelFromSdfFile(file_name=TABLE_SDF_PATH, model_name='table', scene_graph=scene_graph, plant=mbp) table2 = AddModelFromSdfFile(file_name=TABLE_SDF_PATH, model_name='table2', scene_graph=scene_graph, plant=mbp) sink = AddModelFromSdfFile(file_name=SINK_PATH, model_name='sink', scene_graph=scene_graph, plant=mbp) stove = AddModelFromSdfFile(file_name=STOVE_PATH, model_name='stove', scene_graph=scene_graph, plant=mbp) broccoli = AddModelFromSdfFile(file_name=BROCCOLI_PATH, model_name='broccoli', scene_graph=scene_graph, plant=mbp) #wall = AddModelFromSdfFile(file_name=WALL_PATH, model_name='wall', # scene_graph=scene_graph, plant=mbp) wall = None table2_x = 0.75 table_top_z = get_aabb_z_placement(AABBs['sink'], AABBs['table']) # TODO: use geometry weld_gripper(mbp, robot, gripper) weld_to_world(mbp, robot, create_transform(translation=[0, 0, table_top_z])) weld_to_world(mbp, table, create_transform()) weld_to_world(mbp, table2, create_transform(translation=[table2_x, 0, 0])) weld_to_world(mbp, sink, create_transform(translation=[table2_x, 0.25, table_top_z])) weld_to_world(mbp, stove, create_transform(translation=[table2_x, -0.25, table_top_z])) if wall is not None: weld_to_world(mbp, wall, create_transform(translation=[table2_x / 2, 0, table_top_z])) mbp.Finalize(scene_graph) movable = [broccoli] surfaces = [ VisualElement(sink, 'base_link', 0), # Could also just pass the link index VisualElement(stove, 'base_link', 0), ] initial_poses = { broccoli: create_transform(translation=[table2_x, 0, table_top_z]), } task = Task(mbp, scene_graph, robot, gripper, movable=movable, surfaces=surfaces, initial_poses=initial_poses, goal_cooked=[broccoli]) return mbp, scene_graph, task
def Simulate2dHopper(x0, duration, desired_lateral_velocity=0.0, print_period=0.0): builder = DiagramBuilder() plant = builder.AddSystem(MultibodyPlant(0.0005)) scene_graph = builder.AddSystem(SceneGraph()) plant.RegisterAsSourceForSceneGraph(scene_graph) builder.Connect(plant.get_geometry_poses_output_port(), scene_graph.get_source_pose_port(plant.get_source_id())) builder.Connect(scene_graph.get_query_output_port(), plant.get_geometry_query_input_port()) # Build the plant parser = Parser(plant) parser.AddModelFromFile("raibert_hopper_2d.sdf") plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("ground")) plant.AddForceElement(UniformGravityFieldElement()) plant.Finalize() # Create a logger to log at 30hz state_dim = plant.num_positions() + plant.num_velocities() state_log = builder.AddSystem(SignalLogger(state_dim)) state_log.DeclarePeriodicPublish(0.0333, 0.0) # 30hz logging builder.Connect(plant.get_continuous_state_output_port(), state_log.get_input_port(0)) # The controller controller = builder.AddSystem( Hopper2dController(plant, desired_lateral_velocity=desired_lateral_velocity, print_period=print_period)) builder.Connect(plant.get_continuous_state_output_port(), controller.get_input_port(0)) builder.Connect(controller.get_output_port(0), plant.get_actuation_input_port()) # The diagram diagram = builder.Build() simulator = Simulator(diagram) simulator.Initialize() plant_context = diagram.GetMutableSubsystemContext( plant, simulator.get_mutable_context()) plant_context.get_mutable_discrete_state_vector().SetFromVector(x0) simulator.StepTo(duration) return plant, controller, state_log
def main(): parser = argparse.ArgumentParser( description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) parser.add_argument( "filename", type=str, help="Path to an SDF or URDF file.") MeshcatVisualizer.add_argparse_argument(parser) args = parser.parse_args() filename = args.filename if not os.path.isfile(filename): parser.error("File does not exist: {}".format(filename)) # Construct Plant + SceneGraph. builder = DiagramBuilder() plant = builder.AddSystem(MultibodyPlant()) scene_graph = builder.AddSystem(SceneGraph()) plant.RegisterAsSourceForSceneGraph(scene_graph) builder.Connect( scene_graph.get_query_output_port(), plant.get_geometry_query_input_port()) builder.Connect( plant.get_geometry_poses_output_port(), scene_graph.get_source_pose_port(plant.get_source_id())) # Load the model file. Parser(plant).AddModelFromFile(filename) plant.Finalize() # Publish to Drake Visualizer. drake_viz_pub = ConnectDrakeVisualizer(builder, scene_graph) # Publish 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)) diagram = builder.Build() context = diagram.CreateDefaultContext() # Use Simulator to dispatch initialization events. # TODO(eric.cousineau): Simplify as part of #10015. Simulator(diagram).Initialize() # Publish draw messages with current state. diagram.Publish(context)
def testMultibodyPositionToGeometryPose(self): file_name = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") plant = MultibodyPlant(time_step=0.01) model_instance = AddModelFromSdfFile(file_name=file_name, plant=plant) scene_graph = SceneGraph() plant.RegisterAsSourceForSceneGraph(scene_graph) plant.Finalize() to_pose = MultibodyPositionToGeometryPose(plant) # Check the size of the input. self.assertEqual(to_pose.get_input_port().size(), 2) # Just check the spelling of the output port (size is not meaningful # for Abstract-valued ports). to_pose.get_output_port()
def test_basics(self): # call default constructor QuadrotorPlant() quadrotor = QuadrotorPlant(m_arg=1, L_arg=2, I_arg=np.eye(3), kF_arg=1., kM_arg=1.) self.assertEqual(quadrotor.m(), 1) self.assertEqual(quadrotor.g(), 9.81) scene_graph = SceneGraph() quadrotor.RegisterGeometry(scene_graph) self.assertTrue(quadrotor.source_id().is_valid()) quadrotor.get_geometry_pose_output_port() StabilizingLQRController(quadrotor, np.zeros(3))
def cartPoleTest(self): 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, zmq_url=None)) 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 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 load_manipulation(time_step=0.0, use_meshcat=True, use_controllers=True, use_external=False): if not use_external: AMAZON_TABLE_PATH = FindResourceOrThrow( "drake/examples/manipulation_station/models/amazon_table_simplified.sdf" ) CUPBOARD_PATH = FindResourceOrThrow( "drake/examples/manipulation_station/models/cupboard.sdf") IIWA7_PATH = FindResourceOrThrow( "drake/manipulation/models/iiwa_description/iiwa7/iiwa7_with_box_collision.sdf" ) FOAM_BRICK_PATH = FindResourceOrThrow( "drake/examples/manipulation_station/models/061_foam_brick.sdf") goal_shelf = 'shelf_lower' #goal_shelf = 'shelf_upper' else: AMAZON_TABLE_PATH = FindResourceOrThrow( "drake/external/models_robotlocomotion/manipulation_station/amazon_table_simplified.sdf" ) CUPBOARD_PATH = FindResourceOrThrow( "drake/external/models_robotlocomotion/manipulation_station/cupboard.sdf" ) IIWA7_PATH = FindResourceOrThrow( "drake/external/models_robotlocomotion/iiwa7/iiwa7_no_collision.sdf" ) FOAM_BRICK_PATH = FindResourceOrThrow( "drake/external/models_robotlocomotion/ycb_objects/061_foam_brick.sdf" ) goal_shelf = 'bottom' #IIWA7_PATH = os.path.join(MODELS_DIR, "iiwa_description/iiwa7/iiwa7_with_box_collision.sdf") plant = MultibodyPlant(time_step=time_step) scene_graph = SceneGraph() dx_table_center_to_robot_base = 0.3257 dz_table_top_robot_base = 0.0127 dx_cupboard_to_table_center = 0.43 + 0.15 dz_cupboard_to_table_center = 0.02 cupboard_height = 0.815 cupboard_x = dx_table_center_to_robot_base + dx_cupboard_to_table_center cupboard_z = dz_cupboard_to_table_center + cupboard_height / 2.0 - dz_table_top_robot_base robot = AddModelFromSdfFile(file_name=IIWA7_PATH, model_name='iiwa', scene_graph=scene_graph, plant=plant) gripper = AddModelFromSdfFile(file_name=WSG50_SDF_PATH, model_name='gripper', scene_graph=scene_graph, plant=plant) # TODO: sdf frame/link error table = AddModelFromSdfFile(file_name=AMAZON_TABLE_PATH, model_name='table', scene_graph=scene_graph, plant=plant) cupboard = AddModelFromSdfFile(file_name=CUPBOARD_PATH, model_name='cupboard', scene_graph=scene_graph, plant=plant) brick = AddModelFromSdfFile(file_name=FOAM_BRICK_PATH, model_name='brick', scene_graph=scene_graph, plant=plant) weld_gripper(plant, robot, gripper) weld_to_world(plant, robot, create_transform()) weld_to_world( plant, table, create_transform(translation=[ dx_table_center_to_robot_base, 0, -dz_table_top_robot_base ])) weld_to_world( plant, cupboard, create_transform(translation=[cupboard_x, 0, cupboard_z], rotation=[0, 0, np.pi])) plant.Finalize(scene_graph) shelves = [ 'bottom', 'shelf_lower', 'shelf_upper', 'top', ] goal_surface = Surface(plant, cupboard, 'top_and_bottom', shelves.index(goal_shelf)) surfaces = [ #Surface(plant, table, 'amazon_table', 0), #goal_surface, ] door_names = ['left_door', 'right_door'] #door_names = [] doors = [plant.GetBodyByName(name).index() for name in door_names] door_position = DOOR_CLOSED # ~np.pi/2 #door_position = DOOR_OPEN #door_position = np.pi #door_position = np.pi/2 #door_position = np.pi/8 initial_positions = { plant.GetJointByName('left_door_hinge'): -door_position, #plant.GetJointByName('right_door_hinge'): door_position, plant.GetJointByName('right_door_hinge'): np.pi / 2, } initial_conf = [0, 0.6 - np.pi / 6, 0, -1.75, 0, 1.0, 0] #initial_conf[1] += np.pi / 6 initial_positions.update( zip(get_movable_joints(plant, robot), initial_conf)) initial_poses = { #brick: create_transform(translation=[0.6, 0, 0]), brick: create_transform(translation=[0.3, 0, 0], rotation=[0, 0, np.pi / 2]), #brick: create_transform(translation=[0.2, -0.3, 0], rotation=[0, 0, np.pi/8]), } goal_poses = { brick: create_transform(translation=[0.8, 0.2, 0.2927], rotation=[0, 0, np.pi / 8]), } goal_holding = [ #brick, ] goal_on = [ #(brick, goal_surface), ] diagram, state_machine = build_diagram(plant, scene_graph, robot, gripper, meshcat=use_meshcat, controllers=use_controllers) task = Task(diagram, plant, scene_graph, robot, gripper, movable=[brick], surfaces=surfaces, doors=doors, initial_positions=initial_positions, initial_poses=initial_poses, goal_poses=goal_poses, goal_holding=goal_holding, goal_on=goal_on, reset_robot=True, reset_doors=False) return task, diagram, state_machine
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)
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 test_make_from_scene_graph_and_iris(self): """ Tests the make from scene graph and iris functionality together as the Iris code makes obstacles from geometries registered in SceneGraph. """ scene_graph = SceneGraph() source_id = scene_graph.RegisterSource("source") frame_id = scene_graph.RegisterFrame( source_id=source_id, frame=GeometryFrame("frame")) box_geometry_id = scene_graph.RegisterGeometry( source_id=source_id, frame_id=frame_id, geometry=GeometryInstance(X_PG=RigidTransform(), shape=Box(1., 1., 1.), name="box")) cylinder_geometry_id = scene_graph.RegisterGeometry( source_id=source_id, frame_id=frame_id, geometry=GeometryInstance(X_PG=RigidTransform(), shape=Cylinder(1., 1.), name="cylinder")) sphere_geometry_id = scene_graph.RegisterGeometry( source_id=source_id, frame_id=frame_id, geometry=GeometryInstance(X_PG=RigidTransform(), shape=Sphere(1.), name="sphere")) capsule_geometry_id = scene_graph.RegisterGeometry( source_id=source_id, frame_id=frame_id, geometry=GeometryInstance(X_PG=RigidTransform(), shape=Capsule(1., 1.0), name="capsule")) context = scene_graph.CreateDefaultContext() pose_vector = FramePoseVector() pose_vector.set_value(frame_id, RigidTransform()) scene_graph.get_source_pose_port(source_id).FixValue( context, pose_vector) query_object = scene_graph.get_query_output_port().Eval(context) H = mut.HPolyhedron( query_object=query_object, geometry_id=box_geometry_id, reference_frame=scene_graph.world_frame_id()) self.assertEqual(H.ambient_dimension(), 3) C = mut.CartesianProduct( query_object=query_object, geometry_id=cylinder_geometry_id, reference_frame=scene_graph.world_frame_id()) self.assertEqual(C.ambient_dimension(), 3) E = mut.Hyperellipsoid( query_object=query_object, geometry_id=sphere_geometry_id, reference_frame=scene_graph.world_frame_id()) self.assertEqual(E.ambient_dimension(), 3) S = mut.MinkowskiSum( query_object=query_object, geometry_id=capsule_geometry_id, reference_frame=scene_graph.world_frame_id()) self.assertEqual(S.ambient_dimension(), 3) P = mut.Point( query_object=query_object, geometry_id=sphere_geometry_id, reference_frame=scene_graph.world_frame_id(), maximum_allowable_radius=1.5) self.assertEqual(P.ambient_dimension(), 3) V = mut.VPolytope( query_object=query_object, geometry_id=box_geometry_id, reference_frame=scene_graph.world_frame_id()) self.assertEqual(V.ambient_dimension(), 3) obstacles = mut.MakeIrisObstacles( query_object=query_object, reference_frame=scene_graph.world_frame_id()) options = mut.IrisOptions() options.require_sample_point_is_contained = True options.iteration_limit = 1 options.termination_threshold = 0.1 options.relative_termination_threshold = 0.01 self.assertNotIn("object at 0x", repr(options)) region = mut.Iris( obstacles=obstacles, sample=[2, 3.4, 5], domain=mut.HPolyhedron.MakeBox( lb=[-5, -5, -5], ub=[5, 5, 5]), options=options) self.assertIsInstance(region, mut.HPolyhedron) obstacles = [ mut.HPolyhedron.MakeUnitBox(3), mut.Hyperellipsoid.MakeUnitBall(3), mut.Point([0, 0, 0]), mut.VPolytope.MakeUnitBox(3)] region = mut.Iris( obstacles=obstacles, sample=[2, 3.4, 5], domain=mut.HPolyhedron.MakeBox( lb=[-5, -5, -5], ub=[5, 5, 5]), options=options) self.assertIsInstance(region, mut.HPolyhedron)
def LittleDogSimulationDiagram2(lcm, rb_tree, dt, drake_visualizer): builder = DiagramBuilder() num_pos = rb_tree.get_num_positions() num_actuators = rb_tree.get_num_actuators() zero_config = np.zeros( (rb_tree.get_num_actuators() * 2, 1)) # just for test # zero_config = np.concatenate((np.array([0, 1, 1, 1, 1.7, 0.5, 0, 0, 0]),np.zeros(9)), axis=0) # zero_config = np.concatenate((rb_tree.getZeroConfiguration(),np.zeros(9)), axis=0) zero_config = np.concatenate( (np.array([1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]), np.zeros(12)), axis=0) # zero_config[0] = 1.7 # zero_config[1] = 1.7 # zero_config[2] = 1.7 # zero_config[3] = 1.7 # 1.SceneGraph system scene_graph = builder.AddSystem(SceneGraph()) scene_graph.RegisterSource('scene_graph_n') plant = builder.AddSystem(MultibodyPlant()) plant_parser = Parser(plant, scene_graph) plant_parser.AddModelFromFile(file_name=model_path, model_name="littledog") plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("body")) # Add gravity to the model. plant.AddForceElement(UniformGravityFieldElement([0, 0, -9.81])) # Model is completed plant.Finalize() builder.Connect(scene_graph.get_query_output_port(), plant.get_geometry_query_input_port()) builder.Connect(plant.get_geometry_poses_output_port(), scene_graph.get_source_pose_port(plant.get_source_id())) ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph) # Robot State Encoder robot_state_encoder = builder.AddSystem( RobotStateEncoder(rb_tree)) # force_sensor_info robot_state_encoder.set_name('robot_state_encoder') builder.Connect(plant.get_continuous_state_output_port(), robot_state_encoder.joint_state_results_input_port()) # Robot command to Plant Converter robot_command_to_rigidbodyplant_converter = builder.AddSystem( RobotCommandToRigidBodyPlantConverter( rb_tree.actuators, motor_gain=None)) # input argu: rigidbody actuators robot_command_to_rigidbodyplant_converter.set_name( 'robot_command_to_rigidbodyplant_converter') builder.Connect( robot_command_to_rigidbodyplant_converter.desired_effort_output_port(), plant.get_actuation_input_port()) # PID controller kp, ki, kd = joints_PID_params(rb_tree) # PIDcontroller = builder.AddSystem(RobotPDAndFeedForwardController(rb_tree, kp, ki, kd)) # PIDcontroller.set_name('PID_controller') rb = RigidBodyTree() robot_frame = RigidBodyFrame("robot_frame", rb_tree.world(), [0, 0, 0], [0, 0, 0]) # insert a robot from urdf files pmap = PackageMap() pmap.PopulateFromFolder(os.path.dirname(model_path)) AddModelInstanceFromUrdfStringSearchingInRosPackages( open(model_path, 'r').read(), pmap, os.path.dirname(model_path), FloatingBaseType.kFixed, # FloatingBaseType.kRollPitchYaw, robot_frame, rb) PIDcontroller = builder.AddSystem( RbtInverseDynamicsController(rb, kp, ki, kd, False)) PIDcontroller.set_name('PID_controller') builder.Connect(robot_state_encoder.joint_state_outport_port(), PIDcontroller.get_input_port(0)) builder.Connect( PIDcontroller.get_output_port(0), robot_command_to_rigidbodyplant_converter.robot_command_input_port()) # Ref trajectory traj_src = builder.AddSystem(ConstantVectorSource(zero_config)) builder.Connect(traj_src.get_output_port(0), PIDcontroller.get_input_port(1)) # Signal logger logger = builder.AddSystem(SignalLogger(num_pos * 2)) builder.Connect(plant.get_continuous_state_output_port(), logger.get_input_port(0)) return builder.Build(), logger, plant
def load_manipulation(time_step=0.0): source = True if source: AMAZON_TABLE_PATH = FindResourceOrThrow( "drake/examples/manipulation_station/models/amazon_table_simplified.sdf") CUPBOARD_PATH = FindResourceOrThrow( "drake/examples/manipulation_station/models/cupboard.sdf") #IIWA7_PATH = FindResourceOrThrow( # "drake/manipulation/models/iiwa_description/iiwa7/iiwa7_with_box_collision.sdf") IIWA7_PATH = os.path.join(MODELS_DIR, "iiwa_description/iiwa7/iiwa7_with_box_collision.sdf") FOAM_BRICK_PATH = FindResourceOrThrow( "drake/examples/manipulation_station/models/061_foam_brick.sdf") goal_shelf = 'shelf_lower' else: AMAZON_TABLE_PATH = FindResourceOrThrow( "drake/external/models_robotlocomotion/manipulation_station/amazon_table_simplified.sdf") CUPBOARD_PATH = FindResourceOrThrow( "drake/external/models_robotlocomotion/manipulation_station/cupboard.sdf") IIWA7_PATH = FindResourceOrThrow( "drake/external/models_robotlocomotion/iiwa7/iiwa7_no_collision.sdf") #IIWA7_PATH = os.path.join(MODELS_DIR, "iiwa_description/iiwa7/iiwa7_with_box_collision.sdf") FOAM_BRICK_PATH = FindResourceOrThrow( "drake/external/models_robotlocomotion/ycb_objects/061_foam_brick.sdf") goal_shelf = 'bottom' mbp = MultibodyPlant(time_step=time_step) scene_graph = SceneGraph() dx_table_center_to_robot_base = 0.3257 dz_table_top_robot_base = 0.0127 dx_cupboard_to_table_center = 0.43 + 0.15 dz_cupboard_to_table_center = 0.02 cupboard_height = 0.815 cupboard_x = dx_table_center_to_robot_base + dx_cupboard_to_table_center cupboard_z = dz_cupboard_to_table_center + cupboard_height / 2.0 - dz_table_top_robot_base robot = AddModelFromSdfFile(file_name=IIWA7_PATH, model_name='iiwa', scene_graph=scene_graph, plant=mbp) gripper = AddModelFromSdfFile(file_name=WSG50_SDF_PATH, model_name='gripper', scene_graph=scene_graph, plant=mbp) # TODO: sdf frame/link error amazon_table = AddModelFromSdfFile(file_name=AMAZON_TABLE_PATH, model_name='amazon_table', scene_graph=scene_graph, plant=mbp) cupboard = AddModelFromSdfFile(file_name=CUPBOARD_PATH, model_name='cupboard', scene_graph=scene_graph, plant=mbp) brick = AddModelFromSdfFile(file_name=FOAM_BRICK_PATH, model_name='brick', scene_graph=scene_graph, plant=mbp) # left_door, left_door_hinge, cylinder weld_gripper(mbp, robot, gripper) weld_to_world(mbp, robot, create_transform()) weld_to_world(mbp, amazon_table, create_transform( translation=[dx_table_center_to_robot_base, 0, -dz_table_top_robot_base])) weld_to_world(mbp, cupboard, create_transform( translation=[cupboard_x, 0, cupboard_z], rotation=[0, 0, np.pi])) mbp.Finalize(scene_graph) shelves = [ 'bottom', 'shelf_lower', 'shelf_upper' 'top', ] goal_surface = VisualElement(cupboard, 'top_and_bottom', shelves.index(goal_shelf)) surfaces = [ #VisualElement(amazon_table, 'amazon_table', 0), goal_surface, ] door_names = [ 'left_door', 'right_door', ] doors = [mbp.GetBodyByName(name).index() for name in door_names] door_position = DOOR_CLOSED # np.pi/2 #door_position = DOOR_OPEN #door_position = np.pi #door_position = np.pi/8 initial_positions = { mbp.GetJointByName('left_door_hinge'): -door_position, #mbp.GetJointByName('right_door_hinge'): door_position, mbp.GetJointByName('right_door_hinge'): np.pi, } initial_conf = [0, 0.6 - np.pi / 6, 0, -1.75, 0, 1.0, 0] #initial_conf[1] += np.pi / 6 initial_positions.update(zip(get_movable_joints(mbp, robot), initial_conf)) initial_poses = { #brick: create_transform(translation=[0.6, 0, 0]), brick: create_transform(translation=[0.4, 0, 0]), } goal_on = [ (brick, goal_surface), ] task = Task(mbp, scene_graph, robot, gripper, movable=[brick], surfaces=surfaces, doors=doors, initial_positions=initial_positions, initial_poses=initial_poses, goal_on=goal_on) return mbp, scene_graph, task
def test_warnings_and_errors(self): builder = DiagramBuilder() sg = builder.AddSystem(SceneGraph()) v2 = builder.AddSystem(MeshcatVisualizer(scene_graph=sg)) builder.Connect(sg.get_query_output_port(), v2.get_geometry_query_input_port()) v2.set_name("v2") v4 = builder.AddSystem(MeshcatVisualizer(scene_graph=None)) builder.Connect(sg.get_query_output_port(), v4.get_geometry_query_input_port()) v4.set_name("v4") v5 = ConnectMeshcatVisualizer(builder, scene_graph=sg) v5.set_name("v5") v7 = ConnectMeshcatVisualizer(builder, scene_graph=sg, output_port=sg.get_query_output_port()) v7.set_name("v7") with self.assertRaises(AssertionError): v8 = ConnectMeshcatVisualizer(builder, scene_graph=None, output_port=None) v8.set_name("v8") v10 = ConnectMeshcatVisualizer(builder, scene_graph=None, output_port=sg.get_query_output_port()) v10.set_name("v10") diagram = builder.Build() context = diagram.CreateDefaultContext() def PublishWithNoWarnings(v): with warnings.catch_warnings(record=True) as w: v.Publish(v.GetMyContextFromRoot(context)) self.assertEqual(len(w), 0, [x.message for x in w]) # Use geometry_query API v2.load(v2.GetMyContextFromRoot(context)) PublishWithNoWarnings(v2) v2.load() PublishWithNoWarnings(v2) # Use geometry_query API v4.load(v4.GetMyContextFromRoot(context)) PublishWithNoWarnings(v4) with self.assertRaises(RuntimeError): v4.load() # Can't work without scene_graph nor context. # Use geometry_query API v5.load(v5.GetMyContextFromRoot(context)) PublishWithNoWarnings(v5) v5.load() PublishWithNoWarnings(v5) # Use geometry_query API v7.load(v7.GetMyContextFromRoot(context)) PublishWithNoWarnings(v7) v7.load() PublishWithNoWarnings(v7) # Use geometry_query API v10.load(v10.GetMyContextFromRoot(context)) PublishWithNoWarnings(v10) with self.assertRaises(RuntimeError): v10.load() # Can't work without scene_graph nor context.
def test_geometry(self): builder = DiagramBuilder() quadrotor = builder.AddSystem(QuadrotorPlant()) scene_graph = builder.AddSystem(SceneGraph()) state_port = quadrotor.get_output_port(0) QuadrotorGeometry.AddToBuilder(builder, state_port, scene_graph)
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 load_tables(time_step=0.0, use_meshcat=True, use_controllers=True): plant = MultibodyPlant(time_step=time_step) scene_graph = SceneGraph() # TODO: meshes aren't supported during collision checking robot = AddModelFromSdfFile(file_name=IIWA14_SDF_PATH, model_name='iiwa', scene_graph=scene_graph, plant=plant) gripper = AddModelFromSdfFile(file_name=WSG50_SDF_PATH, model_name='gripper', scene_graph=scene_graph, plant=plant) # TODO: sdf frame/link error table = AddModelFromSdfFile(file_name=TABLE_SDF_PATH, model_name='table', scene_graph=scene_graph, plant=plant) table2 = AddModelFromSdfFile(file_name=TABLE_SDF_PATH, model_name='table2', scene_graph=scene_graph, plant=plant) sink = AddModelFromSdfFile(file_name=SINK_PATH, model_name='sink', scene_graph=scene_graph, plant=plant) stove = AddModelFromSdfFile(file_name=STOVE_PATH, model_name='stove', scene_graph=scene_graph, plant=plant) broccoli = AddModelFromSdfFile(file_name=BROCCOLI_PATH, model_name='broccoli', scene_graph=scene_graph, plant=plant) #wall = AddModelFromSdfFile(file_name=WALL_PATH, model_name='wall', # scene_graph=scene_graph, plant=mbp) wall = None table2_x = 0.75 table_top_z = 0.7655 # TODO: use geometry weld_gripper(plant, robot, gripper) weld_to_world(plant, robot, create_transform(translation=[0, 0, table_top_z])) weld_to_world(plant, table, create_transform()) weld_to_world(plant, table2, create_transform(translation=[table2_x, 0, 0])) weld_to_world(plant, sink, create_transform(translation=[table2_x, 0.25, table_top_z])) weld_to_world(plant, stove, create_transform(translation=[table2_x, -0.25, table_top_z])) if wall is not None: weld_to_world( plant, wall, create_transform(translation=[table2_x / 2, 0, table_top_z])) plant.Finalize(scene_graph) movable = [broccoli] surfaces = [ Surface(plant, sink, 'base_link', 0), # Could also just pass the link index Surface(plant, stove, 'base_link', 0), ] initial_poses = { broccoli: create_transform(translation=[table2_x, 0, table_top_z]), } diagram, state_machine = build_diagram(plant, scene_graph, robot, gripper, meshcat=use_meshcat, controllers=use_controllers) task = Task(diagram, plant, scene_graph, robot, gripper, movable=movable, surfaces=surfaces, initial_poses=initial_poses, goal_cooked=[broccoli]) return task, diagram, state_machine