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)