def test_model_directives(self): model_dir = os.path.dirname( FindResourceOrThrow("drake/multibody/parsing/test/" "process_model_directives_test/package.xml")) plant = MultibodyPlant(time_step=0.01) parser = Parser(plant=plant) parser.package_map().PopulateFromFolder(model_dir) directives_file = model_dir + "/add_scoped_top.yaml" directives = LoadModelDirectives(directives_file) added_models = ProcessModelDirectives(directives=directives, plant=plant, parser=parser) # Check for an instance. model_names = [model.model_name for model in added_models] self.assertIn("extra_model", model_names) plant.GetModelInstanceByName("extra_model") # Test that other bound symbols exist. ModelInstanceInfo.model_name ModelInstanceInfo.model_path ModelInstanceInfo.parent_frame_name ModelInstanceInfo.child_frame_name ModelInstanceInfo.X_PC ModelInstanceInfo.model_instance AddFrame.name AddFrame.X_PF frame = GetScopedFrameByName(plant, "world") self.assertIsNotNone(GetScopedFrameName(plant, frame))
def MustardExampleSystem(): builder = DiagramBuilder() # Create the physics engine + scene graph. plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0) parser = Parser(plant) parser.package_map().Add("manipulation", FindResource("models")) ProcessModelDirectives( LoadModelDirectives(FindResource("models/mustard_w_cameras.yaml")), plant, parser) 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)) AddRgbdSensors(builder, plant, scene_graph) diagram = builder.Build() diagram.set_name("depth_camera_demo_system") return diagram
def _make_plant_parser_directives(self): """Returns a tuple (plant, parser, directives) for later testing.""" model_dir = os.path.dirname( FindResourceOrThrow("drake/multibody/parsing/test/" "process_model_directives_test/package.xml")) plant = MultibodyPlant(time_step=0.01) parser = Parser(plant=plant) parser.package_map().PopulateFromFolder(model_dir) directives_file = model_dir + "/add_scoped_top.dmd.yaml" directives = LoadModelDirectives(directives_file) return (plant, parser, directives)
def make_parser(plant): 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")): args_parser.error(f"package.xml not found at: {package_path}") # Get the package map and populate it using the package path. parser.package_map().PopulateFromFolder(package_path) return parser
def test_model_directives(self): model_dir = os.path.dirname(FindResourceOrThrow( "drake/multibody/parsing/test/" "process_model_directives_test/package.xml")) plant = MultibodyPlant(time_step=0.01) parser = Parser(plant=plant) parser.package_map().PopulateFromFolder(model_dir) directives_file = model_dir + "/add_scoped_top.yaml" directives = LoadModelDirectives(directives_file) added_models = ProcessModelDirectives( directives=directives, plant=plant, parser=parser) # Check for an instance. model_names = [model.model_name for model in added_models] self.assertIn("extra_model", model_names) plant.GetModelInstanceByName("extra_model")
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) 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. 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() 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(scene_graph) # 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) 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.StepTo(0.1) else: simulator.set_target_realtime_rate(1.0) simulator.StepTo(np.inf)
# RuntimeError: Actuation input port for model instance iiwa must be connected. def no_control(plant, builder, model): nu = plant.num_actuated_dofs(model) u0 = numpy.zeros(nu) constant = builder.AddSystem(ConstantVectorSource(u0)) builder.Connect(constant.get_output_port(0), plant.get_actuation_input_port(model)) if __name__ == '__main__': builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.001) parser = Parser(plant) parser.package_map().PopulateFromEnvironment('AMENT_PREFIX_PATH') sdf_file_path = os.path.join( parser.package_map().GetPath('ur10_description'), 'ur10.sdf') model_name = "ur10" model = parser.AddModelFromFile(sdf_file_path, model_name) # Weld to world so it doesn't fall through floor :D base_frame = plant.GetFrameByName("base", model) X_WB = RigidTransform([0, 0, 0]) plant.WeldFrames(plant.world_frame(), base_frame, X_WB) plant.Finalize() # Must happen after Finalize # RuntimeError: Pre-finalize calls to 'num_actuated_dofs()' are not allowed; you must call Finalize() first.