def test_copying(self):
"""Ensures that index ordering is generally the same when copying a
plant using a subgraph."""
time_step = 0.01
builder_a = DiagramBuilder()
plant_a, scene_graph_a = AddMultibodyPlantSceneGraph(
builder_a, time_step)
util.add_arbitrary_multibody_stuff(plant_a)
# Ensure that this is "physically" valid.
plant_a.Finalize()
if VISUALIZE:
for model in me.get_model_instances(plant_a):
util.no_control(builder_a, plant_a, model)
print("test_composition_array_with_scene_graph")
ConnectDrakeVisualizer(builder_a, scene_graph_a)
diagram = builder_a.Build()
simulator = Simulator(diagram)
simulator.Initialize()
diagram.Publish(simulator.get_context())
simulator.AdvanceTo(1.)
# Checking for determinism, making a slight change to trigger an error.
for slight_difference in [False, True]:
builder_b = DiagramBuilder()
plant_b, scene_graph_b = AddMultibodyPlantSceneGraph(
builder_b, time_step)
util.add_arbitrary_multibody_stuff(
plant_b, slight_difference=slight_difference)
plant_b.Finalize()
if not slight_difference:
util.assert_plant_equals(plant_a, scene_graph_a, plant_b,
scene_graph_b)
else:
with self.assertRaises(AssertionError):
util.assert_plant_equals(plant_a, scene_graph_a, plant_b,
scene_graph_b)
# Check for general ordering with full subgraph "cloning".
builder_b = DiagramBuilder()
plant_b, scene_graph_b = AddMultibodyPlantSceneGraph(
builder_b, time_step)
subgraph_a = mut.MultibodyPlantSubgraph(
mut.get_elements_from_plant(plant_a, scene_graph_a))
subgraph_a.add_to(plant_b, scene_graph_b)
plant_b.Finalize()
util.assert_plant_equals(plant_a, scene_graph_a, plant_b,
scene_graph_b)
def visualize(q, dt=None):
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, MultibodyPlant(0.001))
load_atlas(plant, add_ground=True)
plant_context = plant.CreateDefaultContext()
ConnectContactResultsToDrakeVisualizer(builder=builder, plant=plant)
ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph)
diagram = builder.Build()
if len(q.shape) == 1:
q = np.reshape(q, (1, -1))
for i in range(q.shape[0]):
print(f"knot point: {i}")
diagram_context = diagram.CreateDefaultContext()
plant_context = diagram.GetMutableSubsystemContext(plant, diagram_context)
set_null_input(plant, plant_context)
plant.SetPositions(plant_context, q[i])
simulator = Simulator(diagram, diagram_context)
simulator.set_target_realtime_rate(0.0)
simulator.AdvanceTo(0.0001)
if not dt is None:
time.sleep(5/(np.sum(dt))*dt[i])
else:
time.sleep(0.5)
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 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 test_kuka(self):
"""Kuka IIWA with mesh geometry."""
file_name = FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/sdf/"
"iiwa14_no_collision.sdf")
builder = DiagramBuilder()
kuka, scene_graph = AddMultibodyPlantSceneGraph(builder)
Parser(plant=kuka).AddModelFromFile(file_name)
kuka.AddForceElement(UniformGravityFieldElement())
kuka.Finalize()
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 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 = args_parser.parse_args()
filename, make_parser = parse_filename_and_parser(args_parser, args)
update_visualization, connect_visualizers = parse_visualizers(
args_parser, args)
# Construct Plant + SceneGraph.
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
# Load the model file.
make_parser(plant).AddModelFromFile(filename)
update_visualization(plant, scene_graph)
plant.Finalize()
connect_visualizers(builder, plant, scene_graph)
diagram = builder.Build()
context = diagram.CreateDefaultContext()
# Use Simulator to dispatch initialization events.
# TODO(eric.cousineau): Simplify as part of #10015.
Simulator(diagram).Initialize()
# Publish draw messages with current state.
diagram.Publish(context)
def test_subgraph_add_to_copying(self):
"""Ensures that index ordering is generally the same when copying a
plant using a MultibodyPlantSubgraph.add_to."""
# TODO(eric.cousineau): Increas number of bodies for viz, once
# `create_manual_map` can acommodate it.
plant_a, scene_graph_a, _ = self.make_arbitrary_multibody_stuff(
num_bodies=1)
# Check for general ordering with full subgraph "cloning".
builder_b = DiagramBuilder()
plant_b, scene_graph_b = AddMultibodyPlantSceneGraph(
builder_b, plant_a.time_step())
subgraph_a = mut.MultibodyPlantSubgraph(
mut.get_elements_from_plant(plant_a, scene_graph_a))
a_to_b = subgraph_a.add_to(plant_b, scene_graph_b)
plant_b.Finalize()
util.assert_plant_equals(plant_a, scene_graph_a, plant_b,
scene_graph_b)
a_to_b_expected = self.create_manual_map(plant_a, scene_graph_a,
plant_b, scene_graph_b)
self.assertEqual(a_to_b, a_to_b_expected)
if VISUALIZE:
for model in me.get_model_instances(plant_b):
util.build_with_no_control(builder_b, plant_b, model)
print("test_subgraph_add_to_copying")
DrakeVisualizer.AddToBuilder(builder_b, scene_graph_b)
diagram = builder_b.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.)
simulator.Initialize()
diagram.Publish(simulator.get_context())
simulator.AdvanceTo(1.)
def test_procedural_geometry():
"""
This test ensures we can draw procedurally added primitive
geometry that is added to the world model instance (which has
a slightly different naming scheme than geometry with a
non-default / non-world model instance).
"""
builder = DiagramBuilder()
mbp, scene_graph = AddMultibodyPlantSceneGraph(builder)
world_body = mbp.world_body()
box_shape = Box(1., 2., 3.)
# This rigid body will be added to the world model instance since
# the model instance is not specified.
box_body = mbp.AddRigidBody(
"box",
SpatialInertia(mass=1.0,
p_PScm_E=np.array([0., 0., 0.]),
G_SP_E=UnitInertia(1.0, 1.0, 1.0)))
mbp.WeldFrames(world_body.body_frame(), box_body.body_frame(),
RigidTransform())
mbp.RegisterVisualGeometry(box_body, RigidTransform.Identity(), box_shape,
"ground_vis", np.array([0.5, 0.5, 0.5, 1.]))
mbp.RegisterCollisionGeometry(box_body, RigidTransform.Identity(),
box_shape, "ground_col",
CoulombFriction(0.9, 0.8))
mbp.Finalize()
# frames_to_draw = {"world": {"box"}}
# visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(scene_graph))
# builder.Connect(scene_graph.get_pose_bundle_output_port(),
# visualizer.get_input_port(0))
diagram = builder.Build()
def show_cloud(pc, use_native=False, **kwargs):
# kwargs go to ctor.
builder = DiagramBuilder()
# Add point cloud visualization.
if use_native:
viz = meshcat.Visualizer(zmq_url=ZMQ_URL)
else:
plant, scene_graph = AddMultibodyPlantSceneGraph(builder)
plant.Finalize()
viz = builder.AddSystem(
MeshcatVisualizer(scene_graph,
zmq_url=ZMQ_URL,
open_browser=False))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
viz.get_input_port(0))
pc_viz = builder.AddSystem(
MeshcatPointCloudVisualizer(viz, **kwargs))
# Make sure the system runs.
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
context = diagram.GetMutableSubsystemContext(
pc_viz, diagram_context)
context.FixInputPort(
pc_viz.GetInputPort("point_cloud_P").get_index(),
AbstractValue.Make(pc))
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.StepTo(sim_time)
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 __init__(self, config):
self._config = config
builder = DiagramBuilder()
dt = config['env']['mbp_dt']
mbp, sg = AddMultibodyPlantSceneGraph(builder, MultibodyPlant(dt),
SceneGraph())
parser = Parser(mbp, sg)
# renderer_params = RenderEngineVtkParams()
# renderer_name = "vtk_renderer"
# sg.AddRenderer(renderer_name, MakeRenderEngineVtk(renderer_params))
self._mbp = mbp
self._sg = sg
self._parser = parser
self._builder = builder
self._rgbd_sensors = dict()
self._finalized = False
self._built = False
self._diagram = None
# add rendered
# Add renderer to scene
renderer_params = RenderEngineVtkParams()
self._renderer_name = "vtk_renderer"
sg.AddRenderer(self._renderer_name,
MakeRenderEngineVtk(renderer_params))
def test_deprecated_finalize(self):
builder = DiagramBuilder_[float]()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder)
Parser(plant).AddModelFromFile(FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf"))
with catch_drake_warnings(expected_count=1):
plant.Finalize(scene_graph)
def test_multibody_plant_construction_api(self, T):
# SceneGraph does not support `Expression` type.
DiagramBuilder = DiagramBuilder_[T]
SpatialInertia = SpatialInertia_[float]
RigidTransform = RigidTransform_[T]
CoulombFriction = CoulombFriction_[T]
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder)
spatial_inertia = SpatialInertia()
body = plant.AddRigidBody(name="new_body",
M_BBo_B=spatial_inertia)
new_model_instance = plant.AddModelInstance("new_model_instance")
body = plant.AddRigidBody(name="new_body_2",
M_BBo_B=spatial_inertia,
model_instance=new_model_instance)
box = Box(width=0.5, depth=1.0, height=2.0)
body_X_BG = RigidTransform()
body_friction = CoulombFriction(static_friction=0.6,
dynamic_friction=0.5)
if T == float:
plant.RegisterVisualGeometry(
body=body, X_BG=body_X_BG, shape=box, name="new_body_visual",
diffuse_color=[1., 0.64, 0.0, 0.5])
plant.RegisterCollisionGeometry(
body=body, X_BG=body_X_BG, shape=box,
name="new_body_collision", coulomb_friction=body_friction)
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_add_default_visualization(self):
"""Exercises AddDefaultVisualization.
"""
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
plant.Finalize()
config = mut.AddDefaultVisualization(builder=builder)
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 construct_environment(masses: typing.List, box_sizes: typing.List):
"""
Construct an environment with many free boxes.
@param masses masses[i] is the mass of box i.
@param box_sizes box_sizes[i] is the size of box i.
"""
builder = DiagramBuilder_[AutoDiffXd]()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
# Add the ground as a big box.
ground_box = plant.AddRigidBody(
"ground", SpatialInertia(1, np.array([0, 0, 0]), UnitInertia(1, 1, 1)))
X_WG = RigidTransform([0, 0, -0.05])
ground_geometry_id = plant.RegisterCollisionGeometry(
ground_box, RigidTransform(), Box(10, 10, 0.1), "ground",
CoulombFriction(0.9, 0.8))
plant.RegisterVisualGeometry(
ground_box, RigidTransform(), Box(10, 10, 0.1), "ground",
[0.5, 0.5, 0.5, 1.])
plant.WeldFrames(plant.world_frame(), ground_box.body_frame(), X_WG)
# Add boxes
assert isinstance(masses, list)
assert isinstance(box_sizes, list)
assert len(masses) == len(box_sizes)
num_boxes = len(masses)
boxes = []
boxes_geometry_id = []
for i in range(num_boxes):
box_name = f"box{i}"
boxes.append(plant.AddRigidBody(
box_name, SpatialInertia(
masses[i], np.array([0, 0, 0]), UnitInertia(1, 1, 1))))
box_shape = Box(box_sizes[i][0], box_sizes[i][1], box_sizes[i][2])
boxes_geometry_id.append(plant.RegisterCollisionGeometry(
boxes[i], RigidTransform(), box_shape, f"{box_name}_box",
CoulombFriction(0.9, 0.8)))
plant.RegisterVisualGeometry(
ground_box, RigidTransform(), box_shape, f"{box_name}_box",
[0.5, 0.5, 0.5, 1.])
# Add small spheres at the corners of the box.
vertices = np.array([
[1, 1, 1], [1, 1, -1], [1, -1, 1], [1, -1, -1], [-1, 1, 1],
[-1, -1, 1], [-1, 1, -1], [-1, -1, -1]]) *\
np.tile(box_sizes[i]/2, (8, 1))
sphere_shape = Sphere(0.001)
for j in range(8):
plant.RegisterCollisionGeometry(
boxes[i], RigidTransform(vertices[j]), sphere_shape,
f"{box_name}_sphere{j}", CoulombFriction(0.9, 0.8))
plant.RegisterVisualGeometry(
boxes[i], RigidTransform(vertices[j]), sphere_shape,
f"{box_name}_sphere{j}", [0.5, 0.5, 0.5, 1])
plant.Finalize()
diagram = builder.Build()
return Environment(
plant=plant, scene_graph=scene_graph, diagram=diagram, boxes=boxes,
ground_geometry_id=ground_geometry_id,
boxes_geometry_id=boxes_geometry_id)
def test_allegro_proximity_geometry(self):
"""Allegro hand with visual and collision geometry, drawn in
proximity-geom mode."""
file_name = FindResourceOrThrow(
"drake/manipulation/models/allegro_hand_description/sdf/"
"allegro_hand_description_left.sdf")
builder = DiagramBuilder()
hand, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
Parser(plant=hand).AddModelFromFile(file_name)
hand.Finalize()
visualizer = builder.AddSystem(
MeshcatVisualizer(zmq_url=ZMQ_URL,
open_browser=False,
role=Role.kProximity))
builder.Connect(scene_graph.get_query_output_port(),
visualizer.get_geometry_query_input_port())
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
hand_context = diagram.GetMutableSubsystemContext(
hand, diagram_context)
hand_actuation_port = hand.get_actuation_input_port()
hand_actuation_port.FixValue(hand_context,
np.zeros(hand_actuation_port.size()))
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.AdvanceTo(.1)
def test_iris_cspace(self):
limits_urdf = """
<robot name="limits">
<link name="movable">
<collision>
<geometry><box size="1 1 1"/></geometry>
</collision>
</link>
<joint name="movable" type="prismatic">
<axis xyz="1 0 0"/>
<limit lower="-2" upper="2"/>
<parent link="world"/>
<child link="movable"/>
</joint>
</robot>"""
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
Parser(plant).AddModelFromString(limits_urdf, "urdf")
plant.Finalize()
diagram = builder.Build()
context = diagram.CreateDefaultContext()
options = mut.IrisOptions()
with catch_drake_warnings(expected_count=1):
region = mut.IrisInConfigurationSpace(
plant=plant, context=plant.GetMyContextFromRoot(context),
sample=[0], options=options)
plant.SetPositions(plant.GetMyMutableContextFromRoot(context), [0])
region = mut.IrisInConfigurationSpace(
plant=plant, context=plant.GetMyContextFromRoot(context),
options=options)
self.assertIsInstance(region, mut.ConvexSet)
self.assertEqual(region.ambient_dimension(), 1)
self.assertTrue(region.PointInSet([1.0]))
self.assertFalse(region.PointInSet([3.0]))
def test_textured_meshes(self):
"""Draws a solid green box (the texture map is just a green pixel) to
test the texture override pathway. You should confirm that you see a
green box in the visualizer."""
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.
visualizer = builder.AddSystem(
MeshcatVisualizer(zmq_url=ZMQ_URL, open_browser=False))
builder.Connect(scene_graph.get_query_output_port(),
visualizer.get_geometry_query_input_port())
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_meta_arbitrary_multibody_stuff(self):
"""Ensures that the test model is logical and can be compared /
falsified."""
plant_a, scene_graph_a, _ = self.make_arbitrary_multibody_stuff(
finalize=False)
# Ensure that we span all relevant joint classes.
joint_cls_set = {type(x) for x in me.get_joints(plant_a)}
self.assertEqual(joint_cls_set, set(util.JOINT_CLS_LIST))
# Ensure that we can copy via a subgraph pre-Finalize.
builder_b = DiagramBuilder()
plant_b, scene_graph_b = AddMultibodyPlantSceneGraph(
builder_b, plant_a.time_step())
subgraph_a = mut.MultibodyPlantSubgraph(
mut.get_elements_from_plant(plant_a, scene_graph_a))
subgraph_a.add_to(plant_b, scene_graph_b)
# Check equivalence.
util.assert_plant_equals(plant_a, scene_graph_a, plant_b,
scene_graph_b)
# Ensure that this is "physically" valid.
plant_a.Finalize()
# Checking for determinism, making a slight change to trigger an error.
for slight_difference in [False, True]:
plant_b, scene_graph_b, _ = self.make_arbitrary_multibody_stuff(
slight_difference=slight_difference)
if not slight_difference:
util.assert_plant_equals(plant_a, scene_graph_a, plant_b,
scene_graph_b)
else:
with self.assertRaises(AssertionError):
util.assert_plant_equals(plant_a, scene_graph_a, plant_b,
scene_graph_b)
def test_connect_meshcat_visualizer(self):
"""Cart-Pole with simple geometry. This should produce two cart-poles,
with identical geometry, one under the prefix `vis` and another under
the prefix `vis2`. You should confirm in the visualizer that both
exist (by unchecking one at a time in the MeshCat controls menu)"""
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()
vis = ConnectMeshcatVisualizer(builder=builder,
prefix="vis",
scene_graph=scene_graph,
zmq_url=ZMQ_URL,
open_browser=False)
self.assertIsInstance(vis, MeshcatVisualizer)
vis2 = ConnectMeshcatVisualizer(
builder=builder,
prefix="vis2",
output_port=scene_graph.get_query_output_port(),
zmq_url=ZMQ_URL,
open_browser=False)
vis2.set_name("vis2")
self.assertIsInstance(vis2, MeshcatVisualizer)
diagram = builder.Build()
context = diagram.CreateDefaultContext()
vis.load(vis.GetMyContextFromRoot(context))
vis2.load(vis2.GetMyContextFromRoot(context))
diagram.Publish(context)
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 testConnectPlanarSceneGraphVisualizer(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 = ConnectPlanarSceneGraphVisualizer(
builder=builder, scene_graph=scene_graph, xlim=[0.3, 1.2])
self.assertIsInstance(visualizer, PlanarSceneGraphVisualizer)
# Confirm that arguments are passed through.
self.assertEqual(visualizer.ax.get_xlim(), (0.3, 1.2))
vis2 = ConnectPlanarSceneGraphVisualizer(
builder=builder,
scene_graph=scene_graph,
output_port=scene_graph.get_pose_bundle_output_port())
vis2.set_name("vis2")
self.assertIsInstance(vis2, PlanarSceneGraphVisualizer)
diagram = builder.Build()
context = diagram.CreateDefaultContext()
diagram.Publish(context)
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, 0.0)
Parser(plant=cart_pole).AddModelFromFile(file_name)
cart_pole.Finalize()
self.assertTrue(cart_pole.geometry_source_is_registered())
visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(scene_graph))
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)
vis_context = diagram.GetMutableSubsystemContext(
visualizer, diagram_context)
cart_pole.get_actuation_input_port().FixValue(cart_pole_context, 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.AdvanceTo(.1)
visualizer.draw(vis_context)
self.assertEqual(visualizer.ax.get_title(), "t = 0.1",)
def test_contact_visualizer(self, T):
meshcat = Meshcat()
params = ContactVisualizerParams()
params.publish_period = 0.123
params.default_color = Rgba(0.5, 0.5, 0.5)
params.prefix = "py_visualizer"
params.delete_on_initialization_event = False
params.force_threshold = 0.2
params.newtons_per_meter = 5
params.radius = 0.1
self.assertNotIn("object at 0x", repr(params))
params2 = ContactVisualizerParams(publish_period=0.4)
self.assertEqual(params2.publish_period, 0.4)
vis = ContactVisualizer_[T](meshcat=meshcat, params=params)
vis.Delete()
vis.contact_results_input_port()
builder = DiagramBuilder_[T]()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.01)
plant.Finalize()
ContactVisualizer_[T].AddToBuilder(builder=builder,
plant=plant,
meshcat=meshcat,
params=params)
ContactVisualizer_[T].AddToBuilder(
builder=builder,
contact_results_port=plant.get_contact_results_output_port(),
meshcat=meshcat,
params=params)
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 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 MustardExampleSystem():
builder = DiagramBuilder()
# Create the physics engine + scene graph.
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
parser = Parser(plant)
AddPackagePaths(parser)
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_query_output_port(),
meshcat.get_geometry_query_input_port())
AddRgbdSensors(builder, plant, scene_graph)
diagram = builder.Build()
diagram.set_name("depth_camera_demo_system")
return diagram
