def run_pendulum_example(args):
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
parser = Parser(plant)
parser.AddModelFromFile(
FindResourceOrThrow("drake/examples/pendulum/Pendulum.urdf"))
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("base"))
plant.Finalize()
pose_bundle_output_port = scene_graph.get_pose_bundle_output_port()
Tview = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]],
dtype=np.float64)
visualizer = builder.AddSystem(
PlanarSceneGraphVisualizer(scene_graph,
T_VW=Tview,
xlim=[-1.2, 1.2],
ylim=[-1.2, 1.2]))
builder.Connect(pose_bundle_output_port, visualizer.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.Initialize()
simulator.set_target_realtime_rate(1.0)
# Fix the input port to zero.
plant_context = diagram.GetMutableSubsystemContext(
plant, simulator.get_mutable_context())
plant_context.FixInputPort(plant.get_actuation_input_port().get_index(),
np.zeros(plant.num_actuators()))
plant_context.SetContinuousState([0.5, 0.1])
simulator.AdvanceTo(args.duration)
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 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 test_scene_graph_queries(self):
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder)
parser = Parser(plant=plant, scene_graph=scene_graph)
parser.AddModelFromFile(
FindResourceOrThrow(
"drake/bindings/pydrake/multibody/test/two_bodies.sdf"))
plant.Finalize(scene_graph)
diagram = builder.Build()
# The model `two_bodies` has two (implicitly) floating bodies that are
# placed in the same position. The default state would be for these two
# bodies to be coincident, and thus collide.
context = diagram.CreateDefaultContext()
sg_context = diagram.GetMutableSubsystemContext(scene_graph, context)
query_object = scene_graph.get_query_output_port().Eval(sg_context)
# Implicitly require that this should be size 1.
point_pair, = query_object.ComputePointPairPenetration()
self.assertIsInstance(point_pair, PenetrationAsPointPair)
signed_distance_pair, = query_object.\
ComputeSignedDistancePairwiseClosestPoints()
self.assertIsInstance(signed_distance_pair, SignedDistancePair)
inspector = query_object.inspector()
bodies = {
plant.GetBodyFromFrameId(inspector.GetFrameId(id_))
for id_ in [point_pair.id_A, point_pair.id_B]
}
self.assertSetEqual(
bodies,
{plant.GetBodyByName("body1"),
plant.GetBodyByName("body2")})
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 test_viewer_applet(self):
"""Check that _ViewerApplet doesn't crash when receiving messages.
Note that many geometry types are not yet covered by this test.
"""
# Create the device under test.
dut = mut.Meldis()
meshcat = dut.meshcat
lcm = dut._lcm
# The path is created by the constructor.
self.assertEqual(meshcat.HasPath("/DRAKE_VIEWER"), True)
# Enqueue the load + draw messages.
sdf_file = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
parser = Parser(plant=plant)
parser.AddModelFromFile(sdf_file)
plant.Finalize()
DrakeVisualizer.AddToBuilder(builder=builder, scene_graph=scene_graph,
lcm=lcm)
diagram = builder.Build()
context = diagram.CreateDefaultContext()
diagram.Publish(context)
# The geometry isn't registered until the load is processed.
link_path = "/DRAKE_VIEWER/2/plant/acrobot/Link2/0"
self.assertEqual(meshcat.HasPath(link_path), False)
# Process the load + draw; make sure the geometry exists now.
lcm.HandleSubscriptions(timeout_millis=0)
dut._invoke_subscriptions()
self.assertEqual(meshcat.HasPath(link_path), True)
def run_pendulum_example(args):
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
parser = Parser(plant)
parser.AddModelFromFile(FindResourceOrThrow(
"drake/examples/pendulum/Pendulum.urdf"))
plant.Finalize()
T_VW = np.array([[1., 0., 0., 0.],
[0., 0., 1., 0.],
[0., 0., 0., 1.]])
visualizer = ConnectPlanarSceneGraphVisualizer(
builder, scene_graph, T_VW=T_VW, xlim=[-1.2, 1.2], ylim=[-1.2, 1.2])
if args.playback:
visualizer.start_recording()
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.Initialize()
simulator.set_target_realtime_rate(1.0)
# Fix the input port to zero.
plant_context = diagram.GetMutableSubsystemContext(
plant, simulator.get_mutable_context())
plant.get_actuation_input_port().FixValue(
plant_context, np.zeros(plant.num_actuators()))
plant_context.SetContinuousState([0.5, 0.1])
simulator.AdvanceTo(args.duration)
if args.playback:
visualizer.stop_recording()
ani = visualizer.get_recording_as_animation()
# Playback the recording and save the output.
ani.save("{}/pend_playback.mp4".format(temp_directory()), fps=30)
def test_model_instance_port_access(self):
# Create a MultibodyPlant with a kuka arm and a schunk gripper.
# the arm is welded to the world, the gripper is welded to the
# arm's end effector.
wsg50_sdf_path = FindResourceOrThrow(
"drake/manipulation/models/" +
"wsg_50_description/sdf/schunk_wsg_50.sdf")
iiwa_sdf_path = FindResourceOrThrow(
"drake/manipulation/models/" +
"iiwa_description/sdf/iiwa14_no_collision.sdf")
plant = MultibodyPlant(time_step=2e-3)
parser = Parser(plant)
iiwa_model = parser.AddModelFromFile(file_name=iiwa_sdf_path,
model_name='robot')
gripper_model = parser.AddModelFromFile(file_name=wsg50_sdf_path,
model_name='gripper')
plant.Finalize()
# Test that we can get an actuation input port and a continuous state
# output port.
self.assertIsInstance(plant.get_actuation_input_port(iiwa_model),
InputPort)
self.assertIsInstance(plant.get_state_output_port(gripper_model),
OutputPort)
self.assertIsInstance(
plant.get_generalized_contact_forces_output_port(
model_instance=gripper_model), OutputPort)
def pendulum_example(duration=1., playback=True, show=True):
"""
Simulate the pendulum
Arguments:
duration: Simulation duration (sec)
playback: enable pyplot animations
"""
# To make a visualization, we have to attach a multibody plant, a scene graph, and a visualizer together. In Drake, we can connect all these systems together in a Diagram.
builder = DiagramBuilder()
# AddMultibodyPlantSceneGraph: Adds a multibody plant and scene graph to the Diagram, and connects their geometry ports. The second input is the timestep for MultibodyPlant
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.)
# Now we create the plant model from a file
parser = Parser(plant)
parser.AddModelFromFile(
FindResourceOrThrow("drake/examples/pendulum/Pendulum.urdf"))
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("base"))
plant.Finalize()
# The SceneGraph port that communicates with the visualizer is the pose bundle output port. A PoseBundle is just a set of poses in SE(3) and a set of frame velocities, expressed in the world frame, used for rendering.
pose_bundle_output_port = scene_graph.get_pose_bundle_output_port()
# T_VW is the projection matrix from view coordinates to world coordinates
T_VW = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]])
# Now we add a planar visualizer to the the diagram, so we can see the results
visualizer = builder.AddSystem(
PlanarSceneGraphVisualizer(scene_graph,
T_VW=T_VW,
xlim=[-1.2, 1.2],
ylim=[-1.2, 1.2],
show=show))
# finally, we must connect the scene_graph to the visualizer so they can communicate
builder.Connect(pose_bundle_output_port, visualizer.get_input_port(0))
if playback:
visualizer.start_recording()
# To finalize the diagram, we build it
diagram = builder.Build()
# We create a simulator of our diagram to step through the diagram in time
simulator = Simulator(diagram)
# Initialize prepares the simulator for simulation
simulator.Initialize()
# Slow down the simulator to realtime. Otherwise it could run too fast
simulator.set_target_realtime_rate(1.)
# To set initial conditions, we modify the mutable simulator context (we could do this before Initialize)
plant_context = diagram.GetMutableSubsystemContext(
plant, simulator.get_mutable_context())
plant_context.SetContinuousState([0.5, 0.1])
# Now we fix the value of the actuation to get an unactuated simulation
plant.get_actuation_input_port().FixValue(
plant_context, np.zeros([plant.num_actuators()]))
# Run the simulation to the specified duration
simulator.AdvanceTo(duration)
# Return an animation, if one was made
if playback:
visualizer.stop_recording()
ani = visualizer.get_recording_as_animation()
return ani
else:
return None
def test_joint_sliders(self):
# Load up an acrobot.
acrobot_file = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.urdf")
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
parser = Parser(plant)
parser.AddModelFromFile(acrobot_file)
plant.Finalize()
# Construct a sliders system, using every available option.
meshcat = Meshcat()
dut = JointSliders(
meshcat=meshcat,
plant=plant,
initial_value=[0.1, 0.2],
lower_limit=[-3.0, -6.0],
upper_limit=[3.0, 6.0],
step=[0.25, 0.50],
)
# Various methods should not crash.
dut.get_output_port()
dut.Delete()
# The constructor also accepts single values for broadcast (except for
# the initial value).
dut = JointSliders(
meshcat=meshcat,
plant=plant,
initial_value=[0.1, 0.2],
lower_limit=-3.0,
upper_limit=3.0,
step=0.1,
)
dut.Delete()
# The constructor also accepts None directly, for optionals.
dut = JointSliders(
meshcat=meshcat,
plant=plant,
initial_value=None,
lower_limit=None,
upper_limit=None,
step=None,
)
dut.Delete()
# The constructor has default values, in any case.
dut = JointSliders(meshcat, plant)
# The Run function doesn't crash.
builder.AddSystem(dut)
diagram = builder.Build()
dut.Run(diagram=diagram, timeout=1.0)
# The SetPositions function doesn't crash (Acrobot has two positions).
dut.SetPositions(q=[1, 2])
def add_rope(plant, scene_graph, rope_config, X_W, rope_name="rope"):
parser = Parser(plant, scene_graph)
rope_sdf = generate_rope_sdf_from_config(rope_config, rope_name)
rope_model = parser.AddModelFromString(file_contents=rope_sdf,
file_type="sdf",
model_name=rope_name)
plant.WeldFrames(plant.world_frame(),
plant.GetFrameByName(f"{rope_name}_capsule_1"), X_W)
return rope_model
def test_contact_applet_hydroelastic(self):
"""Check that _ContactApplet doesn't crash when receiving hydroelastic
messages.
"""
# Create the device under test.
dut = mut.Meldis()
meshcat = dut.meshcat
lcm = dut._lcm
# Enqueue a hydroelastic contact message.
sdf_file = FindResourceOrThrow(
"drake/multibody/meshcat/test/hydroelastic.sdf")
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.001)
parser = Parser(plant=plant)
parser.AddModelFromFile(sdf_file)
body1 = plant.GetBodyByName("body1")
body2 = plant.GetBodyByName("body2")
plant.AddJoint(
PrismaticJoint(name="body1",
frame_on_parent=plant.world_body().body_frame(),
frame_on_child=body1.body_frame(),
axis=[0, 0, 1]))
plant.AddJoint(
PrismaticJoint(name="body2",
frame_on_parent=plant.world_body().body_frame(),
frame_on_child=body2.body_frame(),
axis=[1, 0, 0]))
plant.Finalize()
ConnectContactResultsToDrakeVisualizer(builder=builder,
plant=plant,
scene_graph=scene_graph,
lcm=lcm)
diagram = builder.Build()
context = diagram.CreateDefaultContext()
plant.SetPositions(plant.GetMyMutableContextFromRoot(context),
[0.1, 0.3])
diagram.Publish(context)
# The geometry isn't registered until the load is processed.
hydro_path = "/CONTACT_RESULTS/hydroelastic/" + \
"body1.two_bodies::body1_collision+body2"
hydro_path2 = "/CONTACT_RESULTS/hydroelastic/" + \
"body1.two_bodies::body1_collision2+body2"
self.assertEqual(meshcat.HasPath(hydro_path), False)
self.assertEqual(meshcat.HasPath(hydro_path2), False)
# Process the load + draw; contact results should now exist.
lcm.HandleSubscriptions(timeout_millis=1)
dut._invoke_subscriptions()
self.assertEqual(meshcat.HasPath("/CONTACT_RESULTS/hydroelastic"),
True)
self.assertEqual(meshcat.HasPath(hydro_path), True)
self.assertEqual(meshcat.HasPath(hydro_path2), True)
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 main():
args_parser = argparse.ArgumentParser()
args_parser.add_argument("--simulation_sec", type=float, default=np.inf)
args_parser.add_argument("--sim_dt", type=float, default=0.1)
args_parser.add_argument(
"--single_shot",
action="store_true",
help="Test workflow of visulaization through Simulator.Initialize")
args_parser.add_argument("--realtime_rate", type=float, default=1.)
args_parser.add_argument("--num_models", type=int, default=3)
args = args_parser.parse_args()
sdf_file = FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/iiwa7/"
"iiwa7_no_collision.sdf")
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.01)
parser = Parser(plant)
models = []
for i in range(args.num_models):
model_name = f"iiwa{i}"
# TODO(eric.cousineau): This warns about mutating the package path
# multiple times :(
model = parser.AddModelFromFile(sdf_file, model_name)
models.append(model)
base_frame = plant.GetFrameByName("iiwa_link_0", model)
# Translate along x-axis by 1m to separate.
X_WB = RigidTransform([i, 0, 0])
plant.WeldFrames(plant.world_frame(), base_frame, X_WB)
plant.Finalize()
for model in models:
no_control(plant, builder, model)
ConnectDrakeVisualizer(builder, scene_graph)
ConnectRvizVisualizer(builder, scene_graph)
diagram = builder.Build()
simulator = Simulator(diagram)
context = simulator.get_mutable_context()
simulator.set_target_realtime_rate(args.realtime_rate)
# Wait for ROS publishers to wake up :(
time.sleep(0.3)
if args.single_shot:
# To see what 'preview' scripts look like.
# TODO(eric.cousineau): Make this work *robustly* with Rviz. Markers
# still don't always show up :(
simulator.Initialize()
diagram.Publish(context)
else:
while context.get_time() < args.simulation_sec:
# Use increments to permit Ctrl+C to be caught.
simulator.AdvanceTo(context.get_time() + args.sim_dt)
def test_parser_string(self):
"""Checks parsing from a string (not file_name)."""
sdf_file = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
with open(sdf_file, "r") as f:
sdf_contents = f.read()
plant = MultibodyPlant(time_step=0.01)
parser = Parser(plant=plant)
result = parser.AddModelFromString(
file_contents=sdf_contents, file_type="sdf")
self.assertIsInstance(result, ModelInstanceIndex)
def test_multibody_add_joint(self, T):
"""
Tests joint constructors and `AddJoint`.
"""
MultibodyPlant = MultibodyPlant_[T]
RigidTransform = RigidTransform_[T]
RollPitchYaw = RollPitchYaw_[T]
instance_file = FindResourceOrThrow(
"drake/examples/double_pendulum/models/double_pendulum.sdf")
# Add different joints between multiple model instances.
# TODO(eric.cousineau): Remove the multiple instances and use
# programmatically constructed bodies once this API is exposed in
# Python.
num_joints = 2
# N.B. `Parser` only supports `MultibodyPlant_[float]`.
plant_f = MultibodyPlant_[float]()
parser = Parser(plant_f)
instances = []
for i in range(num_joints + 1):
instance = parser.AddModelFromFile(
instance_file, "instance_{}".format(i))
instances.append(instance)
proximal_frame = "base"
distal_frame = "lower_link"
joints_f = [
RevoluteJoint_[float](
name="revolve_things",
frame_on_parent=plant_f.GetBodyByName(
distal_frame, instances[1]).body_frame(),
frame_on_child=plant_f.GetBodyByName(
proximal_frame, instances[2]).body_frame(),
axis=[0, 0, 1],
damping=0.),
WeldJoint_[float](
name="weld_things",
parent_frame_P=plant_f.GetBodyByName(
distal_frame, instances[0]).body_frame(),
child_frame_C=plant_f.GetBodyByName(
proximal_frame, instances[1]).body_frame(),
X_PC=RigidTransform_[float].Identity()),
]
for joint_f in joints_f:
joint_out = plant_f.AddJoint(joint_f)
self.assertIs(joint_f, joint_out)
# The model is now complete.
plant_f.Finalize()
plant = to_type(plant_f, T)
for joint_f in joints_f:
# Not using `joint` because we converted the `plant_f` to `plant`
joint = plant.get_joint(joint_index=joint_f.index())
self._test_joint_api(T, joint)
def _make_diagram(self, *, sdf_filename, visualizer_params, lcm):
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
parser = Parser(plant=plant)
parser.AddModelFromFile(FindResourceOrThrow(sdf_filename))
plant.Finalize()
DrakeVisualizer.AddToBuilder(builder=builder,
scene_graph=scene_graph,
params=visualizer_params,
lcm=lcm)
diagram = builder.Build()
return diagram
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_scene_graph_queries(self, T):
# SceneGraph does not support `Expression` type.
PenetrationAsPointPair = PenetrationAsPointPair_[T]
builder_f = DiagramBuilder_[float]()
# N.B. `Parser` only supports `MultibodyPlant_[float]`.
plant_f, scene_graph_f = AddMultibodyPlantSceneGraph(builder_f)
parser = Parser(plant=plant_f, scene_graph=scene_graph_f)
parser.AddModelFromFile(
FindResourceOrThrow(
"drake/bindings/pydrake/multibody/test/two_bodies.sdf"))
plant_f.Finalize()
diagram_f = builder_f.Build()
# Do conversion.
diagram = to_type(diagram_f, T)
plant = diagram.GetSubsystemByName(plant_f.get_name())
scene_graph = diagram.GetSubsystemByName(scene_graph_f.get_name())
# The model `two_bodies` has two (implicitly) floating bodies that are
# placed in the same position. The default state would be for these two
# bodies to be coincident, and thus collide.
context = diagram.CreateDefaultContext()
sg_context = diagram.GetMutableSubsystemContext(scene_graph, context)
query_object = scene_graph.get_query_output_port().Eval(sg_context)
# Implicitly require that this should be size 1.
point_pair, = query_object.ComputePointPairPenetration()
self.assertIsInstance(point_pair, PenetrationAsPointPair_[float])
signed_distance_pair, = query_object.\
ComputeSignedDistancePairwiseClosestPoints()
self.assertIsInstance(signed_distance_pair, SignedDistancePair_[T])
signed_distance_to_point = query_object.\
ComputeSignedDistanceToPoint(p_WQ=np.ones(3))
self.assertEqual(len(signed_distance_to_point), 2)
self.assertIsInstance(signed_distance_to_point[0],
SignedDistanceToPoint_[T])
self.assertIsInstance(signed_distance_to_point[1],
SignedDistanceToPoint_[T])
inspector = query_object.inspector()
def get_body_from_frame_id(frame_id):
# Get body from frame id, and check inverse method.
body = plant.GetBodyFromFrameId(frame_id)
self.assertEqual(
plant.GetBodyFrameIdIfExists(body.index()), frame_id)
return body
bodies = {get_body_from_frame_id(inspector.GetFrameId(id_))
for id_ in [point_pair.id_A, point_pair.id_B]}
self.assertSetEqual(
bodies,
{plant.GetBodyByName("body1"), plant.GetBodyByName("body2")})
def add_object_from_file(self, object_name, object_path):
parser = Parser(self._mbp, self._sg)
self._model_ids[object_name] = parser.AddModelFromFile(object_path, object_name)
# this should be masked
self._model_names_to_mask.append(object_name)
def finalize_func():
state_output_port = self._mbp.get_state_output_port(self._model_ids[object_name])
port_state_output_name = object_name + '_state_output'
self._port_names.append(port_state_output_name)
self._builder.ExportOutput(state_output_port, port_state_output_name)
self._finalize_functions.append(finalize_func)
def test_contact_applet_point_pair(self):
"""Check that _ContactApplet doesn't crash when receiving point
contact messages.
"""
# Create the device under test.
dut = mut.Meldis()
meshcat = dut.meshcat
lcm = dut._lcm
# Enqueue a point contact result message.
sdf_file = FindResourceOrThrow(
"drake/examples/manipulation_station/models/sphere.sdf")
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.001)
parser = Parser(plant=plant)
sphere1_model = parser.AddModelFromFile(sdf_file, "sphere1")
sphere2_model = parser.AddModelFromFile(sdf_file, "sphere2")
body1 = plant.GetBodyByName("base_link", sphere1_model)
body2 = plant.GetBodyByName("base_link", sphere2_model)
plant.AddJoint(
PrismaticJoint(name="sphere1_x",
frame_on_parent=plant.world_body().body_frame(),
frame_on_child=body1.body_frame(),
axis=[1, 0, 0]))
plant.AddJoint(
PrismaticJoint(name="sphere2_x",
frame_on_parent=plant.world_body().body_frame(),
frame_on_child=body2.body_frame(),
axis=[1, 0, 0]))
plant.Finalize()
ConnectContactResultsToDrakeVisualizer(builder=builder,
plant=plant,
scene_graph=scene_graph,
lcm=lcm)
diagram = builder.Build()
context = diagram.CreateDefaultContext()
plant.SetPositions(plant.GetMyMutableContextFromRoot(context),
[-0.03, 0.03])
diagram.Publish(context)
# The geometry isn't registered until the load is processed.
pair_path = "/CONTACT_RESULTS/point/base_link(2)+base_link(3)"
self.assertEqual(meshcat.HasPath(pair_path), False)
# Process the load + draw; make sure the geometry exists now.
lcm.HandleSubscriptions(timeout_millis=0)
dut._invoke_subscriptions()
self.assertEqual(meshcat.HasPath(pair_path), True)
def run_pendulum_example(duration=1.0, playback=True, show=True):
"""
Runs a simulation of a pendulum
Arguments:
duration: Simulation duration (sec).
playback: Enable pyplot animations to be produced.
"""
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
parser = Parser(plant)
parser.AddModelFromFile(
FindResourceOrThrow("drake/examples/pendulum/Pendulum.urdf"))
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("base"))
plant.Finalize()
pose_bundle_output_port = scene_graph.get_pose_bundle_output_port()
T_VW = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]])
visualizer = builder.AddSystem(
PlanarSceneGraphVisualizer(scene_graph,
T_VW=T_VW,
xlim=[-1.2, 1.2],
ylim=[-1.2, 1.2],
show=show))
builder.Connect(pose_bundle_output_port, visualizer.get_input_port(0))
if playback:
visualizer.start_recording()
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.Initialize()
simulator.set_target_realtime_rate(1.0)
# Fix the input port to zero
plant_context = diagram.GetMutableSubsystemContext(
plant, simulator.get_mutable_context())
plant.get_actuation_input_port().FixValue(plant_context,
np.zeros(plant.num_actuators()))
plant_context.SetContinuousState([0.5, 0.1])
simulator.AdvanceTo(duration)
if playback:
visualizer.stop_recording()
ani = visualizer.get_recording_as_animation()
return ani
else:
return None
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 setUp(self):
builder_f = DiagramBuilder()
self.plant_f, self.scene_graph_f = AddMultibodyPlantSceneGraph(
builder_f, MultibodyPlant(time_step=0.01))
Parser(self.plant_f).AddModelFromFile(FindResourceOrThrow(
"drake/bindings/pydrake/multibody/test/two_bodies.sdf"))
self.plant_f.Finalize()
diagram_f = builder_f.Build()
diagram_ad = diagram_f.ToAutoDiffXd()
plant_ad = diagram_ad.GetSubsystemByName(self.plant_f.get_name())
TypeVariables = namedtuple(
"TypeVariables",
("plant", "plant_context", "body1_frame", "body2_frame"))
def make_type_variables(plant_T, diagram_T):
diagram_context_T = diagram_T.CreateDefaultContext()
return TypeVariables(
plant=plant_T,
plant_context=diagram_T.GetMutableSubsystemContext(
plant_T, diagram_context_T),
body1_frame=plant_T.GetBodyByName("body1").body_frame(),
body2_frame=plant_T.GetBodyByName("body2").body_frame())
self.variables_f = make_type_variables(self.plant_f, diagram_f)
self.variables_ad = make_type_variables(plant_ad, diagram_ad)
def testConnectMeschatVisualizer(self):
"""Cart-Pole with simple geometry."""
file_name = FindResourceOrThrow(
"drake/examples/multibody/cart_pole/cart_pole.sdf")
builder = DiagramBuilder()
cart_pole, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
Parser(plant=cart_pole).AddModelFromFile(file_name)
cart_pole.Finalize()
visualizer = ConnectMeshcatVisualizer(builder=builder,
scene_graph=scene_graph,
zmq_url=ZMQ_URL,
open_browser=False)
self.assertIsInstance(visualizer, MeshcatVisualizer)
vis2 = ConnectMeshcatVisualizer(
builder=builder,
scene_graph=scene_graph,
output_port=scene_graph.get_pose_bundle_output_port(),
zmq_url=ZMQ_URL,
open_browser=False)
vis2.set_name("vis2")
self.assertIsInstance(vis2, MeshcatVisualizer)
diagram = builder.Build()
context = diagram.CreateDefaultContext()
diagram.Publish(context)
def setUp(self):
builder = DiagramBuilder()
self.plant, self.scene_graph = AddMultibodyPlantSceneGraph(
builder, MultibodyPlant(time_step=0.01))
Parser(self.plant).AddModelFromFile(FindResourceOrThrow(
"drake/bindings/pydrake/multibody/test/two_bodies.sdf"))
self.plant.Finalize()
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
plant_context = diagram.GetMutableSubsystemContext(
self.plant, diagram_context)
self.body1_frame = self.plant.GetBodyByName("body1").body_frame()
self.body2_frame = self.plant.GetBodyByName("body2").body_frame()
self.ik_two_bodies = ik.InverseKinematics(
plant=self.plant, plant_context=plant_context)
# Test non-SceneGraph constructor.
ik.InverseKinematics(plant=self.plant)
self.prog = self.ik_two_bodies.get_mutable_prog()
self.q = self.ik_two_bodies.q()
# Test constructor without joint limits
ik.InverseKinematics(plant=self.plant, with_joint_limits=False)
ik.InverseKinematics(
plant=self.plant, plant_context=plant_context,
with_joint_limits=False)
def squaredNorm(x):
return np.array([x[0] ** 2 + x[1] ** 2 + x[2] ** 2 + x[3] ** 2])
self.prog.AddConstraint(
squaredNorm, [1], [1], self._body1_quat(self.q))
self.prog.AddConstraint(
squaredNorm, [1], [1], self._body2_quat(self.q))
self.prog.SetInitialGuess(self._body1_quat(self.q), [1, 0, 0, 0])
self.prog.SetInitialGuess(self._body2_quat(self.q), [1, 0, 0, 0])
def test_texture_override(self):
"""Draws a textured box to test the texture override pathway."""
object_file_path = FindResourceOrThrow(
"drake/systems/sensors/test/models/box_with_mesh.sdf")
# Find the texture path just to ensure it exists and
# we're testing the code path we want to.
FindResourceOrThrow("drake/systems/sensors/test/models/meshes/box.png")
builder = DiagramBuilder()
plant = MultibodyPlant(0.002)
_, scene_graph = AddMultibodyPlantSceneGraph(builder, plant)
object_model = Parser(plant=plant).AddModelFromFile(object_file_path)
plant.Finalize()
# Add meshcat visualizer.
viz = builder.AddSystem(
MeshcatVisualizer(scene_graph, zmq_url=None, open_browser=False))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
viz.get_input_port(0))
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.AdvanceTo(1.0)
def test_cart_pole(self):
"""Cart-Pole with simple geometry."""
file_name = FindResourceOrThrow(
"drake/examples/multibody/cart_pole/cart_pole.sdf")
builder = DiagramBuilder()
cart_pole, scene_graph = AddMultibodyPlantSceneGraph(builder)
Parser(plant=cart_pole).AddModelFromFile(file_name)
cart_pole.AddForceElement(UniformGravityFieldElement())
cart_pole.Finalize()
assert cart_pole.geometry_source_is_registered()
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()
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.StepTo(.1)
def test_multibody_state_access(self):
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant()
Parser(plant).AddModelFromFile(file_name)
plant.Finalize()
context = plant.CreateDefaultContext()
nq = 2
nv = 2
self.assertEqual(plant.num_positions(), nq)
self.assertEqual(plant.num_velocities(), nv)
q0 = np.array([3.14, 2.])
v0 = np.array([-0.5, 1.])
x0 = np.concatenate([q0, v0])
# The default state is all values set to zero.
x = plant.GetPositionsAndVelocities(context)
self.assertTrue(np.allclose(x, np.zeros(4)))
# Write into a mutable reference to the state vector.
x_ref = plant.GetMutablePositionsAndVelocities(context)
x_ref[:] = x0
# Verify that positions and velocities were set correctly.
self.assertTrue(np.allclose(plant.GetPositions(context), q0))
self.assertTrue(np.allclose(plant.GetVelocities(context), v0))
# Verify we did modify the state stored in context.
x = plant.GetPositionsAndVelocities(context)
self.assertTrue(np.allclose(x, x0))
# Now set positions and velocities independently and check them.
zeros_2 = np.zeros([2, 1])
x_ref.fill(0)
plant.SetPositions(context, q0)
self.assertTrue(np.allclose(plant.GetPositions(context), q0))
self.assertTrue(np.allclose(plant.GetVelocities(context), zeros_2))
x_ref.fill(0)
plant.SetVelocities(context, v0)
self.assertTrue(np.allclose(plant.GetPositions(context), zeros_2))
self.assertTrue(np.allclose(plant.GetVelocities(context), v0))
# Now test SetPositionsAndVelocities().
x_ref.fill(0)
plant.SetPositionsAndVelocities(context, x0)
self.assertTrue(
np.allclose(plant.GetPositionsAndVelocities(context), x0))
# Test existence of context resetting methods.
plant.SetDefaultState(context, state=context.get_mutable_state())
# Test existence of limits.
self.assertEqual(plant.GetPositionLowerLimits().shape, (nq, ))
self.assertEqual(plant.GetPositionUpperLimits().shape, (nq, ))
self.assertEqual(plant.GetVelocityLowerLimits().shape, (nv, ))
self.assertEqual(plant.GetVelocityUpperLimits().shape, (nv, ))
self.assertEqual(plant.GetAccelerationLowerLimits().shape, (nv, ))
self.assertEqual(plant.GetAccelerationUpperLimits().shape, (nv, ))
def test_parser(self):
# Calls every combination of argments for the Parser methods and
# inspects their return type.
sdf_file = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
urdf_file = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.urdf")
for dut, file_name, model_name, result_dim in (
(Parser.AddModelFromFile, sdf_file, None, int),
(Parser.AddModelFromFile, sdf_file, "", int),
(Parser.AddModelFromFile, sdf_file, "a", int),
(Parser.AddModelFromFile, urdf_file, None, int),
(Parser.AddModelFromFile, urdf_file, "", int),
(Parser.AddModelFromFile, urdf_file, "a", int),
(Parser.AddAllModelsFromFile, sdf_file, None, list),
(Parser.AddAllModelsFromFile, urdf_file, None, list),
):
plant = MultibodyPlant(time_step=0.01)
parser = Parser(plant=plant)
if model_name is None:
result = dut(parser, file_name=file_name)
else:
result = dut(parser, file_name=file_name,
model_name=model_name)
if result_dim is int:
self.assertIsInstance(result, ModelInstanceIndex)
else:
assert result_dim is list
self.assertIsInstance(result, list)
self.assertIsInstance(result[0], ModelInstanceIndex)
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)
