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 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 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.Finalize()
visualizer = builder.AddSystem(MeshcatVisualizer(scene_graph,
zmq_url=ZMQ_URL,
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.AdvanceTo(.1)
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.Finalize()
visualizer = builder.AddSystem(
MeshcatVisualizer(scene_graph, zmq_url=ZMQ_URL,
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.AdvanceTo(.1)
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.Finalize()
assert cart_pole.geometry_source_is_registered()
visualizer = builder.AddSystem(MeshcatVisualizer(scene_graph,
zmq_url=ZMQ_URL,
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.AdvanceTo(.1)
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
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 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_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_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_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 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_add_default_visualization(self):
"""Exercises AddDefaultVisualization.
"""
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
plant.Finalize()
config = mut.AddDefaultVisualization(builder=builder)
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 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 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_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 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_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 DepthCameraDemoSystem():
builder = DiagramBuilder()
# Create the physics engine + scene graph.
plant, scene_graph = AddMultibodyPlantSceneGraph(builder)
# Add a single object into it.
Parser(plant, scene_graph).AddModelFromFile(
FindResourceOrThrow(
"drake/manipulation/models/ycb/sdf/006_mustard_bottle.sdf"))
# Add a rendering engine
renderer = "my_renderer"
scene_graph.AddRenderer(renderer,
MakeRenderEngineVtk(RenderEngineVtkParams()))
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))
# Add a camera to the environment.
pose = RigidTransform(RollPitchYaw(-0.2, 0.2, 0), [-.1, -0.1, -.5])
properties = DepthCameraProperties(width=640,
height=480,
fov_y=np.pi / 4.0,
renderer_name=renderer,
z_near=0.1,
z_far=10.0)
camera = builder.AddSystem(
RgbdSensor(parent_id=scene_graph.world_frame_id(),
X_PB=pose,
properties=properties,
show_window=False))
camera.set_name("rgbd_sensor")
builder.Connect(scene_graph.get_query_output_port(),
camera.query_object_input_port())
# Export the camera outputs
builder.ExportOutput(camera.color_image_output_port(), "color_image")
builder.ExportOutput(camera.depth_image_32F_output_port(), "depth_image")
# Add a system to convert the camera output into a point cloud
to_point_cloud = builder.AddSystem(
DepthImageToPointCloud(camera_info=camera.depth_camera_info(),
fields=BaseField.kXYZs | BaseField.kRGBs))
builder.Connect(camera.depth_image_32F_output_port(),
to_point_cloud.depth_image_input_port())
builder.Connect(camera.color_image_output_port(),
to_point_cloud.color_image_input_port())
# Export the point cloud output.
builder.ExportOutput(to_point_cloud.point_cloud_output_port(),
"point_cloud")
diagram = builder.Build()
diagram.set_name("depth_camera_demo_system")
return diagram
def simple_mbp():
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder)
Parser(plant).AddModelFromFile("common/test/models/double_pendulum.sdf")
plant.Finalize()
ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph)
diagram = builder.Build()
return plant, scene_graph, diagram
def make_plant(model_directive_path):
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder)
InsertCorrectLoadingMethodHere(model_directive_path, plant)
plant.Finalize()
ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph)
diagram = builder.Build()
return plant, scene_graph, diagram
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 parse_model_and_create_context(file):
"""Tests a model by loading parsing it with a SceneGraph connected,
building the relevant diagram, and allocating its default context."""
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder)
Parser(plant).AddModelFromFile(file)
plant.Finalize()
diagram = builder.Build()
diagram.CreateDefaultContext()
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_arbitrary_multibody_stuff(self, *, finalize=True, **kwargs):
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder,
time_step=0.01)
util.add_arbitrary_multibody_stuff(plant, **kwargs)
if finalize:
plant.Finalize()
diagram = builder.Build()
return plant, scene_graph, diagram
else:
return plant, scene_graph, builder
def test_usage_all(self):
from pydrake.all import (AddMultibodyPlantSceneGraph, DiagramBuilder,
FindResourceOrThrow, Parser, Simulator)
builder = DiagramBuilder()
plant, _ = AddMultibodyPlantSceneGraph(builder)
Parser(plant).AddModelFromFile(
FindResourceOrThrow("drake/examples/pendulum/Pendulum.urdf"))
plant.Finalize()
diagram = builder.Build()
simulator = Simulator(diagram)
def __init__(self, config):
Diagram.__init__(self)
dt = config["mbp_dt"]
self._builder = DiagramBuilder()
self._mbp, self._sg = AddMultibodyPlantSceneGraph(self._builder, dt)
self._finalize_functions = []
self._finalized = False
self._rgbd_sensors = dict()
self._renderer_name = None
def make_plant_and_context(floating_base=True, springs=True):
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
if (springs):
addCassieMultibody(plant, scene_graph, floating_base, cassie_urdf,
True, True)
else:
addCassieMultibody(plant, scene_graph, floating_base,
cassie_urdf_no_springs, False, True)
plant.Finalize()
return plant, plant.CreateDefaultContext()
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 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_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 setUp(self):
file_name = FindResourceOrThrow(
"drake/bindings/pydrake/multibody/test/two_bodies.sdf")
builder = DiagramBuilder()
self.plant, _ = AddMultibodyPlantSceneGraph(
builder, MultibodyPlant(time_step=0.01))
model_instance = Parser(self.plant).AddModelFromFile(file_name)
self.plant.Finalize()
self.diagram = builder.Build()
self.diagram_context = self.diagram.CreateDefaultContext()
self.plant_context = self.diagram.GetMutableSubsystemContext(
self.plant, self.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=self.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()
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])
class TestInverseKinematics(unittest.TestCase):
"""
This test reflects inverse_kinematics_test.cc
"""
def setUp(self):
file_name = FindResourceOrThrow(
"drake/bindings/pydrake/multibody/test/two_bodies.sdf")
builder = DiagramBuilder()
self.plant, _ = AddMultibodyPlantSceneGraph(
builder, MultibodyPlant(time_step=0.01))
model_instance = Parser(self.plant).AddModelFromFile(file_name)
self.plant.Finalize()
self.diagram = builder.Build()
self.diagram_context = self.diagram.CreateDefaultContext()
self.plant_context = self.diagram.GetMutableSubsystemContext(
self.plant, self.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=self.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()
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 _body1_quat(self, q):
# TODO(eric.cousineau): Replace with state indexing.
return q[0:4]
def _body1_xyz(self, q):
# TODO(eric.cousineau): Replace with state indexing.
return q[4:7]
def _body2_quat(self, q):
# TODO(eric.cousineau): Replace with state indexing.
return q[7:11]
def _body2_xyz(self, q):
# TODO(eric.cousineau): Replace with state indexing.
return q[11:14]
def test_AddPositionConstraint(self):
p_BQ = np.array([0.2, 0.3, 0.5])
p_AQ_lower = np.array([-0.1, -0.2, -0.3])
p_AQ_upper = np.array([-0.05, -0.12, -0.28])
self.ik_two_bodies.AddPositionConstraint(
frameB=self.body1_frame, p_BQ=p_BQ,
frameA=self.body2_frame,
p_AQ_lower=p_AQ_lower, p_AQ_upper=p_AQ_upper)
result = mp.Solve(self.prog)
self.assertTrue(result.is_success())
q_val = result.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body1_pos = self._body1_xyz(q_val)
body2_quat = self._body2_quat(q_val)
body2_pos = self._body2_xyz(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
p_AQ = body2_rotmat.transpose().dot(
body1_rotmat.dot(p_BQ) + body1_pos - body2_pos)
self.assertTrue(np.greater(p_AQ, p_AQ_lower -
1E-6 * np.ones((3, 1))).all())
self.assertTrue(np.less(p_AQ, p_AQ_upper +
1E-6 * np.ones((3, 1))).all())
with warnings.catch_warnings(record=True) as w:
warnings.simplefilter('always', DrakeDeprecationWarning)
self.assertEqual(
self.prog.Solve(), mp.SolutionResult.kSolutionFound)
self.assertTrue(np.allclose(
self.prog.GetSolution(self.q), q_val))
self.assertEqual(len(w), 2)
def test_AddOrientationConstraint(self):
theta_bound = 0.2 * math.pi
R_AbarA = RotationMatrix(quaternion=Quaternion(0.5, -0.5, 0.5, 0.5))
R_BbarB = RotationMatrix(
quaternion=Quaternion(1.0 / 3, 2.0 / 3, 0, 2.0 / 3))
self.ik_two_bodies.AddOrientationConstraint(
frameAbar=self.body1_frame, R_AbarA=R_AbarA,
frameBbar=self.body2_frame, R_BbarB=R_BbarB,
theta_bound=theta_bound)
result = mp.Solve(self.prog)
self.assertTrue(result.is_success())
q_val = result.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body2_quat = self._body2_quat(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
R_AbarBbar = body1_rotmat.transpose().dot(body2_rotmat)
R_AB = R_AbarA.matrix().transpose().dot(
R_AbarBbar.dot(R_BbarB.matrix()))
self.assertGreater(R_AB.trace(), 1 + 2 * math.cos(theta_bound) - 1E-6)
with warnings.catch_warnings(record=True) as w:
warnings.simplefilter('always', DrakeDeprecationWarning)
self.assertEqual(
self.prog.Solve(), mp.SolutionResult.kSolutionFound)
q_val = self.prog.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body2_quat = self._body2_quat(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
R_AbarBbar = body1_rotmat.transpose().dot(body2_rotmat)
R_AB = R_AbarA.matrix().transpose().dot(
R_AbarBbar.dot(R_BbarB.matrix()))
self.assertGreater(R_AB.trace(),
1 + 2 * math.cos(theta_bound) - 1E-6)
self.assertEqual(len(w), 2)
def test_AddGazeTargetConstraint(self):
p_AS = np.array([0.1, 0.2, 0.3])
n_A = np.array([0.3, 0.5, 1.2])
p_BT = np.array([1.1, 0.2, 1.5])
cone_half_angle = 0.2 * math.pi
self.ik_two_bodies.AddGazeTargetConstraint(
frameA=self.body1_frame, p_AS=p_AS, n_A=n_A,
frameB=self.body2_frame, p_BT=p_BT,
cone_half_angle=cone_half_angle)
result = mp.Solve(self.prog)
self.assertTrue(result.is_success())
q_val = result.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body1_pos = self._body1_xyz(q_val)
body2_quat = self._body2_quat(q_val)
body2_pos = self._body2_xyz(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
p_WS = body1_pos + body1_rotmat.dot(p_AS)
p_WT = body2_pos + body2_rotmat.dot(p_BT)
p_ST_W = p_WT - p_WS
n_W = body1_rotmat.dot(n_A)
self.assertGreater(p_ST_W.dot(n_W), np.linalg.norm(
p_ST_W) * np.linalg.norm(n_W) * math.cos(cone_half_angle) - 1E-6)
with warnings.catch_warnings(record=True) as w:
warnings.simplefilter('always', DrakeDeprecationWarning)
self.assertEqual(
self.prog.Solve(), mp.SolutionResult.kSolutionFound)
q_val = self.prog.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body1_pos = self._body1_xyz(q_val)
body2_quat = self._body2_quat(q_val)
body2_pos = self._body2_xyz(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
p_WS = body1_pos + body1_rotmat.dot(p_AS)
p_WT = body2_pos + body2_rotmat.dot(p_BT)
p_ST_W = p_WT - p_WS
n_W = body1_rotmat.dot(n_A)
self.assertGreater(p_ST_W.dot(n_W), np.linalg.norm(
p_ST_W) * np.linalg.norm(n_W) *
math.cos(cone_half_angle) - 1E-6)
self.assertEqual(len(w), 2)
def test_AddAngleBetweenVectorsConstraint(self):
na_A = np.array([0.2, -0.4, 0.9])
nb_B = np.array([1.4, -0.1, 1.8])
angle_lower = 0.2 * math.pi
angle_upper = 0.2 * math.pi
self.ik_two_bodies.AddAngleBetweenVectorsConstraint(
frameA=self.body1_frame, na_A=na_A,
frameB=self.body2_frame, nb_B=nb_B,
angle_lower=angle_lower, angle_upper=angle_upper)
result = mp.Solve(self.prog)
self.assertTrue(result.is_success())
q_val = result.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body2_quat = self._body2_quat(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
na_W = body1_rotmat.dot(na_A)
nb_W = body2_rotmat.dot(nb_B)
angle = math.acos(na_W.transpose().dot(nb_W) /
(np.linalg.norm(na_W) * np.linalg.norm(nb_W)))
self.assertLess(math.fabs(angle - angle_lower), 1E-6)
with warnings.catch_warnings(record=True) as w:
warnings.simplefilter('always', DrakeDeprecationWarning)
self.assertEqual(
self.prog.Solve(), mp.SolutionResult.kSolutionFound)
self.assertTrue(np.allclose(
self.prog.GetSolution(self.q), q_val))
self.assertEqual(len(w), 2)
def test_AddMinimumDistanceConstraint(self):
ik = self.ik_two_bodies
W = self.plant.world_frame()
B1 = self.body1_frame
B2 = self.body2_frame
min_distance = 0.1
tol = 1e-2
radius1 = 0.1
radius2 = 0.2
ik.AddMinimumDistanceConstraint(minimal_distance=min_distance)
context = self.plant.CreateDefaultContext()
R_I = np.eye(3)
self.plant.SetFreeBodyPose(
context, B1.body(), Isometry3(R_I, [0, 0, 0.01]))
self.plant.SetFreeBodyPose(
context, B2.body(), Isometry3(R_I, [0, 0, -0.01]))
def get_min_distance_actual():
X = partial(self.plant.CalcRelativeTransform, context)
distance = norm(X(W, B1).translation() - X(W, B2).translation())
return distance - radius1 - radius2
self.assertLess(get_min_distance_actual(), min_distance - tol)
self.prog.SetInitialGuess(ik.q(), self.plant.GetPositions(context))
result = mp.Solve(self.prog)
self.assertTrue(result.is_success())
q_val = result.GetSolution(ik.q())
self.plant.SetPositions(context, q_val)
self.assertGreater(get_min_distance_actual(), min_distance - tol)
with warnings.catch_warnings(record=True) as w:
warnings.simplefilter('always', DrakeDeprecationWarning)
self.assertEqual(
self.prog.Solve(), mp.SolutionResult.kSolutionFound)
self.assertTrue(np.allclose(
self.prog.GetSolution(ik.q()), q_val))
self.assertEqual(len(w), 2)
