def visualize(urdf, xtraj):
plant = MultibodyPlant(time_step=0.0)
scenegraph = SceneGraph()
plant.RegisterAsSourceForSceneGraph(self.scenegraph)
model_index = Parser(plant).AddModelFromFile(FindResource(urdf))
builder = DiagramBuilder()
builder.AddSystem(scenegraph)
plant.Finalize()
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 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 main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"--target_realtime_rate",
type=float,
default=1.0,
help="Desired rate relative to real time. See documentation for "
"Simulator::set_target_realtime_rate() for details.")
parser.add_argument("--simulation_time",
type=float,
default=10.0,
help="Desired duration of the simulation in seconds.")
parser.add_argument(
"--time_step",
type=float,
default=0.,
help="If greater than zero, the plant is modeled as a system with "
"discrete updates and period equal to this time_step. "
"If 0, the plant is modeled as a continuous system.")
args = parser.parse_args()
file_name = FindResourceOrThrow(
"drake/examples/multibody/cart_pole/cart_pole.sdf")
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
cart_pole = builder.AddSystem(MultibodyPlant(time_step=args.time_step))
cart_pole.RegisterAsSourceForSceneGraph(scene_graph)
Parser(plant=cart_pole).AddModelFromFile(file_name)
cart_pole.AddForceElement(UniformGravityFieldElement())
cart_pole.Finalize()
assert cart_pole.geometry_source_is_registered()
builder.Connect(scene_graph.get_query_output_port(),
cart_pole.get_geometry_query_input_port())
builder.Connect(
cart_pole.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(cart_pole.get_source_id()))
ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph)
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.set_target_realtime_rate(args.target_realtime_rate)
simulator.Initialize()
simulator.StepTo(args.simulation_time)
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 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_connect_contact_results(self):
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
builder = DiagramBuilder()
plant = builder.AddSystem(MultibodyPlant(0.001))
Parser(plant).AddModelFromFile(file_name)
plant.Finalize()
publisher = ConnectContactResultsToDrakeVisualizer(builder, plant)
self.assertIsInstance(publisher, LcmPublisherSystem)
def setUpClass(cls):
"""Find and load the sliding_block urdf file"""
urdf_file = FindResource("systems/urdf/sliding_block.urdf")
cls.plant = MultibodyPlant(0.0)
cls.block = Parser(cls.plant).AddModelFromFile(urdf_file)
body_inds = cls.plant.GetBodyIndices(cls.block)
base_frame = cls.plant.get_body(body_inds[0]).body_frame()
world_frame = cls.plant.world_frame()
cls.plant.WeldFrames(world_frame, base_frame, RigidTransform())
cls.plant.Finalize()
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()
assert cart_pole.geometry_source_is_registered()
# Note: pass window=None argument to confirm kwargs are passed
# through to meshcat.Visualizer.
visualizer = builder.AddSystem(
MeshcatVisualizer(scene_graph,
zmq_url=ZMQ_URL,
open_browser=False,
window=None,
delete_prefix_on_load=True))
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.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)
visualizer.set_planar_viewpoint(camera_position=[0, -1, 0],
camera_focus=[0, 0, 0],
xmin=-2,
xmax=2,
ymin=-1,
ymax=2)
visualizer.start_recording()
simulator.AdvanceTo(.1)
visualizer.stop_recording()
# Should have animation "clips" for both bodies.
# See https://github.com/rdeits/meshcat-python/blob/c4ef22c84336d6a8eaab682f73bb47cfca5d5779/src/meshcat/animation.py#L100 # noqa
self.assertEqual(len(visualizer._animation.clips), 2)
# After .1 seconds, we should have had 4 publish events.
self.assertEqual(visualizer._recording_frame_num, 4)
visualizer.publish_recording(play=True, repetitions=1)
visualizer.reset_recording()
self.assertEqual(len(visualizer._animation.clips), 0)
visualizer.delete_prefix()
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 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 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_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 _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 _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_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()
tree = plant.tree()
self.assertEqual(plant.num_positions(), 2)
self.assertEqual(plant.num_velocities(), 2)
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 = tree.GetPositionsAndVelocities(context)
self.assertTrue(np.allclose(x, np.zeros(4)))
# Write into a mutable reference to the state vector.
x_ref = tree.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 = tree.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(tree.GetPositionsAndVelocities(context), x0))
# Test existence of context resetting methods.
plant.SetDefaultContext(context)
plant.SetDefaultState(context, state=context.get_mutable_state())
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_multibody_dynamics(self):
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant()
Parser(plant).AddModelFromFile(file_name)
plant.Finalize()
context = plant.CreateDefaultContext()
tree = plant.tree()
H = tree.CalcMassMatrixViaInverseDynamics(context)
Cv = tree.CalcBiasTerm(context)
self.assertTrue(H.shape == (2, 2))
self.assertTrue(Cv.shape == (2, ))
def test_contact_results_to_lcm(self):
# ContactResultsToLcmSystem
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant_[float]()
Parser(plant).AddModelFromFile(file_name)
plant.Finalize()
contact_results_to_lcm = ContactResultsToLcmSystem(plant)
context = contact_results_to_lcm.CreateDefaultContext()
context.FixInputPort(0, AbstractValue.Make(ContactResults_[float]()))
output = contact_results_to_lcm.AllocateOutput()
contact_results_to_lcm.CalcOutput(context, output)
result = output.get_data(0)
self.assertIsInstance(result, AbstractValue)
def test_usage_no_all(self):
from pydrake.common import FindResourceOrThrow
from pydrake.multibody.parsing import Parser
from pydrake.multibody.plant import AddMultibodyPlantSceneGraph
from pydrake.systems.analysis import Simulator
from pydrake.systems.framework import DiagramBuilder
builder = DiagramBuilder()
plant, _ = AddMultibodyPlantSceneGraph(builder)
Parser(plant).AddModelFromFile(
FindResourceOrThrow("drake/examples/pendulum/Pendulum.urdf"))
plant.Finalize()
diagram = builder.Build()
simulator = Simulator(diagram)
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_inverse_dynamics(self):
sdf_path = FindResourceOrThrow(
"drake/manipulation/models/"
"iiwa_description/sdf/iiwa14_no_collision.sdf")
plant = MultibodyPlant(time_step=0.01)
Parser(plant).AddModelFromFile(sdf_path)
plant.WeldFrames(plant.world_frame(),
plant.GetFrameByName("iiwa_link_0"))
plant.Finalize()
# Just test that the constructor doesn't throw.
controller = InverseDynamics(
plant=plant,
mode=InverseDynamics.InverseDynamicsMode.kGravityCompensation)
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 test_mbp_overloads(self):
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant(0.0)
Parser(plant).AddModelFromFile(file_name)
plant.Finalize()
context = plant.CreateDefaultContext()
frame = plant.GetFrameByName("Link2")
parameters = mut.DifferentialInverseKinematicsParameters(2, 2)
mut.DoDifferentialInverseKinematics(plant, context, np.zeros(6), frame,
parameters)
mut.DoDifferentialInverseKinematics(plant, context, RigidTransform(),
frame, parameters)
def test_contact(self):
# PenetrationAsContactPair
point_pair = PenetrationAsPointPair()
self.assertTrue(isinstance(point_pair.id_A, GeometryId))
self.assertTrue(isinstance(point_pair.id_B, GeometryId))
self.assertTrue(point_pair.p_WCa.shape == (3, ))
self.assertTrue(point_pair.p_WCb.shape == (3, ))
self.assertTrue(isinstance(point_pair.depth, float))
# PointPairContactInfo
id_A = BodyIndex(0)
id_B = BodyIndex(1)
contact_info = PointPairContactInfo(bodyA_index=id_A,
bodyB_index=id_B,
f_Bc_W=np.array([0, 0, 1]),
p_WC=np.array([0, 0, 0]),
separation_speed=0,
slip_speed=0,
point_pair=point_pair)
self.assertTrue(isinstance(contact_info.bodyA_index(), BodyIndex))
self.assertTrue(isinstance(contact_info.bodyB_index(), BodyIndex))
self.assertTrue(contact_info.contact_force().shape == (3, ))
self.assertTrue(contact_info.contact_point().shape == (3, ))
self.assertTrue(isinstance(contact_info.slip_speed(), float))
self.assertIsInstance(contact_info.point_pair(),
PenetrationAsPointPair)
# ContactResults
contact_results = ContactResults()
contact_results.AddContactInfo(contact_info)
self.assertTrue(contact_results.num_contacts() == 1)
self.assertTrue(
isinstance(contact_results.contact_info(0), PointPairContactInfo))
# ContactResultsToLcmSystem
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant()
Parser(plant).AddModelFromFile(file_name)
plant.Finalize()
contact_results_to_lcm = ContactResultsToLcmSystem(plant)
context = contact_results_to_lcm.CreateDefaultContext()
context.FixInputPort(0, AbstractValue.Make(contact_results))
output = contact_results_to_lcm.AllocateOutput()
contact_results_to_lcm.CalcOutput(context, output)
result = output.get_data(0)
self.assertIsInstance(result, AbstractValue)
