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.
