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 testMultibodyPositionToGeometryPose(self): file_name = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") plant = MultibodyPlant(time_step=0.01) model_instance = Parser(plant).AddModelFromFile(file_name) 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 main(): parser = argparse.ArgumentParser(description=__doc__) parser.add_argument("sdf_path", help="path to sdf") parser.add_argument("--interactive", action='store_true') MeshcatVisualizer.add_argparse_argument(parser) args = parser.parse_args() builder = DiagramBuilder() scene_graph = builder.AddSystem(SceneGraph()) plant = MultibodyPlant(time_step=0.01) plant.RegisterAsSourceForSceneGraph(scene_graph) parser = Parser(plant) model = parser.AddModelFromFile(args.sdf_path) plant.Finalize() if args.meshcat: meshcat = ConnectMeshcatVisualizer( builder, output_port=scene_graph.get_query_output_port(), zmq_url=args.meshcat, open_browser=args.open_browser) if args.interactive: # Add sliders to set positions of the joints. sliders = builder.AddSystem(JointSliders(robot=plant)) else: q0 = plant.GetPositions(plant.CreateDefaultContext()) sliders = builder.AddSystem(ConstantVectorSource(q0)) 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())) diagram = builder.Build() simulator = Simulator(diagram) simulator.set_target_realtime_rate(1.0) simulator.AdvanceTo(1E6)
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)
# TODO(russt): Add option to weld the base to the world pending the # availability of GetUniqueBaseBody requested in #9747. args = parser.parse_args() builder = DiagramBuilder() scene_graph = builder.AddSystem(SceneGraph()) # Construct a MultibodyPlant and load the SDF into it. plant = MultibodyPlant() plant.RegisterAsSourceForSceneGraph(scene_graph) AddModelFromSdfFile(args.filename, plant) 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)