def test_joint_slider(self):
# Simply test to make sure that the UI loads and produces the output.
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant(0.0)
Parser(plant).AddModelFromFile(file_name)
plant.Finalize()
slider = JointSliders(robot=plant,
lower_limit=-5.,
upper_limit=5.,
resolution=0.001,
update_period_sec=0.01,
title='test',
length=300)
slider.window.withdraw() # Don't open a window during testing.
context = slider.CreateDefaultContext()
output = slider.AllocateOutput()
q = [.1, .2]
slider.set_position(q)
slider.set_joint_position(q)
slider.CalcOutput(context, output)
np.testing.assert_array_equal(output.get_vector_data(0).get_value(), q)
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()
H = plant.CalcMassMatrixViaInverseDynamics(context)
Cv = plant.CalcBiasTerm(context)
self.assertTrue(H.shape == (2, 2))
self.assert_sane(H)
self.assertTrue(Cv.shape == (2, ))
self.assert_sane(Cv, nonzero=False)
nv = plant.num_velocities()
vd_d = np.zeros(nv)
tau = plant.CalcInverseDynamics(
context, vd_d, MultibodyForces(plant))
self.assertEqual(tau.shape, (2,))
self.assert_sane(tau, nonzero=False)
# - Existence checks.
self.assertEqual(plant.CalcPotentialEnergy(context), 0)
plant.CalcConservativePower(context)
tau_g = plant.CalcGravityGeneralizedForces(context)
self.assertEqual(tau_g.shape, (nv,))
self.assert_sane(tau_g, nonzero=False)
forces = MultibodyForces(plant=plant)
plant.CalcForceElementsContribution(context=context, forces=forces)
def test_model_instance_port_access(self):
# Create a MultibodyPlant with a kuka arm and a schunk gripper.
# the arm is welded to the world, the gripper is welded to the
# arm's end effector.
wsg50_sdf_path = FindResourceOrThrow(
"drake/manipulation/models/" +
"wsg_50_description/sdf/schunk_wsg_50.sdf")
iiwa_sdf_path = FindResourceOrThrow(
"drake/manipulation/models/" +
"iiwa_description/sdf/iiwa14_no_collision.sdf")
plant = MultibodyPlant()
parser = Parser(plant)
iiwa_model = parser.AddModelFromFile(file_name=iiwa_sdf_path,
model_name='robot')
gripper_model = parser.AddModelFromFile(file_name=wsg50_sdf_path,
model_name='gripper')
plant.Finalize()
# Test that we can get an actuation input port and a continuous state
# output port.
self.assertIsInstance(plant.get_actuation_input_port(iiwa_model),
InputPort)
self.assertIsInstance(
plant.get_continuous_state_output_port(gripper_model), OutputPort)
def test_model_directives(self):
model_dir = os.path.dirname(
FindResourceOrThrow("drake/multibody/parsing/test/"
"process_model_directives_test/package.xml"))
plant = MultibodyPlant(time_step=0.01)
parser = Parser(plant=plant)
parser.package_map().PopulateFromFolder(model_dir)
directives_file = model_dir + "/add_scoped_top.yaml"
directives = LoadModelDirectives(directives_file)
added_models = ProcessModelDirectives(directives=directives,
plant=plant,
parser=parser)
# Check for an instance.
model_names = [model.model_name for model in added_models]
self.assertIn("extra_model", model_names)
plant.GetModelInstanceByName("extra_model")
# Test that other bound symbols exist.
ModelInstanceInfo.model_name
ModelInstanceInfo.model_path
ModelInstanceInfo.parent_frame_name
ModelInstanceInfo.child_frame_name
ModelInstanceInfo.X_PC
ModelInstanceInfo.model_instance
AddFrame.name
AddFrame.X_PF
frame = GetScopedFrameByName(plant, "world")
self.assertIsNotNone(GetScopedFrameName(plant, frame))
def test_model_instance_port_access(self):
# Create a MultibodyPlant with a kuka arm and a schunk gripper.
# the arm is welded to the world, the gripper is welded to the
# arm's end effector.
wsg50_sdf_path = FindResourceOrThrow(
"drake/manipulation/models/" +
"wsg_50_description/sdf/schunk_wsg_50.sdf")
iiwa_sdf_path = FindResourceOrThrow(
"drake/manipulation/models/" +
"iiwa_description/sdf/iiwa14_no_collision.sdf")
plant = MultibodyPlant(time_step=2e-3)
parser = Parser(plant)
iiwa_model = parser.AddModelFromFile(
file_name=iiwa_sdf_path, model_name='robot')
gripper_model = parser.AddModelFromFile(
file_name=wsg50_sdf_path, model_name='gripper')
plant.Finalize()
# Test that we can get an actuation input port and a continuous state
# output port.
self.assertIsInstance(
plant.get_actuation_input_port(iiwa_model), InputPort)
self.assertIsInstance(
plant.get_state_output_port(gripper_model), OutputPort)
self.assertIsInstance(
plant.get_generalized_contact_forces_output_port(
model_instance=gripper_model),
OutputPort)
def test_multibody_state_access(self):
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant()
Parser(plant).AddModelFromFile(file_name)
plant.Finalize()
context = plant.CreateDefaultContext()
nq = 2
nv = 2
self.assertEqual(plant.num_positions(), nq)
self.assertEqual(plant.num_velocities(), nv)
q0 = np.array([3.14, 2.])
v0 = np.array([-0.5, 1.])
x0 = np.concatenate([q0, v0])
# The default state is all values set to zero.
x = plant.GetPositionsAndVelocities(context)
self.assertTrue(np.allclose(x, np.zeros(4)))
# Write into a mutable reference to the state vector.
x_ref = plant.GetMutablePositionsAndVelocities(context)
x_ref[:] = x0
# Verify that positions and velocities were set correctly.
self.assertTrue(np.allclose(plant.GetPositions(context), q0))
self.assertTrue(np.allclose(plant.GetVelocities(context), v0))
# Verify we did modify the state stored in context.
x = plant.GetPositionsAndVelocities(context)
self.assertTrue(np.allclose(x, x0))
# Now set positions and velocities independently and check them.
zeros_2 = np.zeros([2, 1])
x_ref.fill(0)
plant.SetPositions(context, q0)
self.assertTrue(np.allclose(plant.GetPositions(context), q0))
self.assertTrue(np.allclose(plant.GetVelocities(context), zeros_2))
x_ref.fill(0)
plant.SetVelocities(context, v0)
self.assertTrue(np.allclose(plant.GetPositions(context), zeros_2))
self.assertTrue(np.allclose(plant.GetVelocities(context), v0))
# Now test SetPositionsAndVelocities().
x_ref.fill(0)
plant.SetPositionsAndVelocities(context, x0)
self.assertTrue(np.allclose(
plant.GetPositionsAndVelocities(context), x0))
# Test existence of context resetting methods.
plant.SetDefaultState(context, state=context.get_mutable_state())
# Test existence of limits.
self.assertEqual(plant.GetPositionLowerLimits().shape, (nq,))
self.assertEqual(plant.GetPositionUpperLimits().shape, (nq,))
self.assertEqual(plant.GetVelocityLowerLimits().shape, (nv,))
self.assertEqual(plant.GetVelocityUpperLimits().shape, (nv,))
self.assertEqual(plant.GetAccelerationLowerLimits().shape, (nv,))
self.assertEqual(plant.GetAccelerationUpperLimits().shape, (nv,))
def test_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_map_qdot_to_v_and_back(self):
plant = MultibodyPlant()
iiwa_sdf_path = FindResourceOrThrow(
"drake/manipulation/models/"
"iiwa_description/sdf/iiwa14_no_collision.sdf")
# Use floating base to effectively add a quatnerion in the generalized
# quaternion.
iiwa_model = Parser(plant=plant).AddModelFromFile(
file_name=iiwa_sdf_path, model_name='robot')
plant.Finalize()
context = plant.CreateDefaultContext()
# Try mapping velocity to qdot and back.
nq = plant.num_positions()
nv = plant.num_velocities()
q_init = np.linspace(start=1.0, stop=nq, num=nq)
plant.SetPositions(context, q_init)
# Overwrite the (invalid) base coordinates, wherever in `q` they are.
plant.SetFreeBodyPose(
context, plant.GetBodyByName("iiwa_link_0"),
RigidTransform(RollPitchYaw([0.1, 0.2, 0.3]),
p=[0.4, 0.5, 0.6]))
v_expected = np.linspace(start=-1.0, stop=-nv, num=nv)
qdot = plant.MapVelocityToQDot(context, v_expected)
v_remap = plant.MapQDotToVelocity(context, qdot)
self.assertTrue(np.allclose(v_expected, v_remap))
def make_diagram():
# Use a nested function to ensure that all locals get garbage
# collected quickly.
# Construct a trivial plant and ID controller.
# N.B. We explicitly do *not* add this plant to the diagram.
controller_plant = MultibodyPlant(time_step=0.002)
controller_plant.Finalize()
builder = DiagramBuilder()
controller = builder.AddSystem(
InverseDynamicsController(
controller_plant,
kp=[],
ki=[],
kd=[],
has_reference_acceleration=False,
)
)
# Forward ports for ease of testing.
builder.ExportInput(
controller.get_input_port_estimated_state(), "x_estimated")
builder.ExportInput(
controller.get_input_port_desired_state(), "x_desired")
builder.ExportOutput(controller.get_output_port_control(), "u")
diagram = builder.Build()
return diagram
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_multibody_add_frame(self):
plant = MultibodyPlant()
frame = plant.AddFrame(frame=FixedOffsetFrame(
name="frame", P=plant.world_frame(),
X_PF=RigidTransform.Identity(), model_instance=None))
self.assertIsInstance(frame, Frame)
np.testing.assert_equal(
np.eye(4), frame.GetFixedPoseInBodyFrame().GetAsMatrix4())
def test_multibody_add_frame(self):
plant = MultibodyPlant()
frame = plant.AddFrame(frame=FixedOffsetFrame(
name="frame", P=plant.world_frame(),
X_PF=RigidTransform.Identity(), model_instance=None))
self.assertIsInstance(frame, Frame)
np.testing.assert_equal(
np.eye(4), frame.GetFixedPoseInBodyFrame().GetAsMatrix4())
def copy_to_finalized_plant(plant_src):
"""Copies plant_src. This can be used to compute kinematics on a model by
simply finalizing the copied version.
Returns:
plant_dest, src_to_dest
"""
plant_dest = MultibodyPlant(plant_src.time_step())
subgraph = MultibodyPlantSubgraph(get_elements_from_plant(plant_src))
src_to_dest = subgraph.add_to(plant_dest)
plant_dest.Finalize()
return plant_dest, src_to_dest
def test_deprecated_tree_api(self):
plant = MultibodyPlant()
plant.Finalize()
with warnings.catch_warnings(record=True) as w:
warnings.simplefilter('always', DrakeDeprecationWarning)
num_expected_warnings = [0]
def expect_new_warning(msg_part):
num_expected_warnings[0] += 1
self.assertEqual(len(w), num_expected_warnings[0])
self.assertIn(msg_part, str(w[-1].message))
tree = plant.tree()
expect_new_warning("`tree()`")
MobilizerIndex(0)
expect_new_warning("`MobilizerIndex`")
BodyNodeIndex(0)
expect_new_warning("`BodyNodeIndex`")
MultibodyForces(model=tree)
expect_new_warning("`MultibodyForces(plant)`")
element = plant.world_body()
self.assertIsInstance(element.get_parent_tree(), MultibodyTree)
expect_new_warning("`get_parent_tree()`")
# Check old spellings (no deprecation warnings).
self.check_old_spelling_exists(tree.CalcRelativeTransform)
self.check_old_spelling_exists(tree.CalcPointsPositions)
self.check_old_spelling_exists(
tree.CalcFrameGeometricJacobianExpressedInWorld)
self.check_old_spelling_exists(tree.EvalBodyPoseInWorld)
self.check_old_spelling_exists(tree.SetFreeBodyPoseOrThrow)
self.check_old_spelling_exists(tree.SetFreeBodySpatialVelocityOrThrow)
self.check_old_spelling_exists(tree.EvalBodySpatialVelocityInWorld)
self.check_old_spelling_exists(tree.GetPositionsFromArray)
self.check_old_spelling_exists(tree.GetVelocitiesFromArray)
self.check_old_spelling_exists(tree.CalcMassMatrixViaInverseDynamics)
self.check_old_spelling_exists(tree.CalcBiasTerm)
self.check_old_spelling_exists(tree.CalcInverseDynamics)
self.check_old_spelling_exists(tree.num_frames)
self.check_old_spelling_exists(tree.get_body)
self.check_old_spelling_exists(tree.get_joint)
self.check_old_spelling_exists(tree.get_joint_actuator)
self.check_old_spelling_exists(tree.get_frame)
self.check_old_spelling_exists(tree.GetModelInstanceName)
context = plant.CreateDefaultContext()
# All body poses.
X_WB, = tree.CalcAllBodyPosesInWorld(context)
self.assertIsInstance(X_WB, Isometry3)
v_WB, = tree.CalcAllBodySpatialVelocitiesInWorld(context)
self.assertIsInstance(v_WB, SpatialVelocity)
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 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_parser_file(self):
"""Calls every combination of arguments for the Parser methods which
use a file_name (not contents) and inspects their return type.
"""
sdf_file = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
urdf_file = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.urdf")
for dut, file_name, model_name, result_dim in (
(Parser.AddModelFromFile, sdf_file, None, int),
(Parser.AddModelFromFile, sdf_file, "", int),
(Parser.AddModelFromFile, sdf_file, "a", int),
(Parser.AddModelFromFile, urdf_file, None, int),
(Parser.AddModelFromFile, urdf_file, "", int),
(Parser.AddModelFromFile, urdf_file, "a", int),
(Parser.AddAllModelsFromFile, sdf_file, None, list),
(Parser.AddAllModelsFromFile, urdf_file, None, list),
):
plant = MultibodyPlant(time_step=0.01)
parser = Parser(plant=plant)
if model_name is None:
result = dut(parser, file_name=file_name)
else:
result = dut(parser, file_name=file_name,
model_name=model_name)
if result_dim is int:
self.assertIsInstance(result, ModelInstanceIndex)
else:
assert result_dim is list
self.assertIsInstance(result, list)
self.assertIsInstance(result[0], ModelInstanceIndex)
def 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_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)
def testMultibodyPositionToGeometryPose(self):
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant(time_step=0.01)
model_instance = Parser(plant).AddModelFromFile(file_name)
scene_graph = SceneGraph()
plant.RegisterAsSourceForSceneGraph(scene_graph)
plant.Finalize()
to_pose = MultibodyPositionToGeometryPose(plant)
# Check the size of the input.
self.assertEqual(to_pose.get_input_port().size(), 2)
# Just check the spelling of the output port (size is not meaningful
# for Abstract-valued ports).
to_pose.get_output_port()
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_diff_ik_integrator(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")
time_step = 0.1
parameters = mut.DifferentialInverseKinematicsParameters(2, 2)
integrator = mut.DifferentialInverseKinematicsIntegrator(
robot=plant,
frame_E=frame,
time_step=time_step,
parameters=parameters,
robot_context=context)
def test_deprecated_parsing(self):
sdf_file = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant(time_step=0.01)
with warnings.catch_warnings(record=True) as w:
warnings.simplefilter("default", DrakeDeprecationWarning)
result = AddModelFromSdfFile(plant=plant, file_name=sdf_file)
self.assertIsInstance(result, ModelInstanceIndex)
self.assertEqual(len(w), 1)
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 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 _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 test_api(self):
plant = MultibodyPlant(time_step=0.01)
model_instance = Parser(plant).AddModelFromFile(FindResourceOrThrow(
"drake/bindings/pydrake/multibody/test/two_bodies.sdf"))
plant.Finalize()
context = plant.CreateDefaultContext()
options = ik.GlobalInverseKinematics.Options()
global_ik = ik.GlobalInverseKinematics(plant=plant, options=options)
self.assertIsInstance(global_ik.prog(), mp.MathematicalProgram)
self.assertIsInstance(global_ik.get_mutable_prog(),
mp.MathematicalProgram)
body_index_A = plant.GetBodyIndices(model_instance)[0]
body_index_B = plant.GetBodyIndices(model_instance)[1]
self.assertEqual(
global_ik.body_rotation_matrix(body_index=body_index_A).shape,
(3, 3))
self.assertEqual(
global_ik.body_position(body_index=body_index_A).shape, (3, ))
global_ik.AddWorldPositionConstraint(
body_index=body_index_A,
p_BQ=[0, 0, 0],
box_lb_F=[-np.inf, -np.inf, -np.inf],
box_ub_F=[np.inf, np.inf, np.inf],
X_WF=RigidTransform())
global_ik.AddWorldRelativePositionConstraint(
body_index_B=body_index_B,
p_BQ=[0, 0, 0],
body_index_A=body_index_A,
p_AP=[0, 0, 0],
box_lb_F=[-np.inf, -np.inf, -np.inf],
box_ub_F=[np.inf, np.inf, np.inf],
X_WF=RigidTransform())
global_ik.AddWorldOrientationConstraint(
body_index=body_index_A,
desired_orientation=Quaternion(),
angle_tol=np.inf)
global_ik.AddPostureCost(
q_desired=plant.GetPositions(context),
body_position_cost=[1] * plant.num_bodies(),
body_orientation_cost=[1] * plant.num_bodies())
gurobi_solver = GurobiSolver()
if gurobi_solver.available():
global_ik.SetInitialGuess(q=plant.GetPositions(context))
result = gurobi_solver.Solve(global_ik.prog())
self.assertTrue(result.is_success())
global_ik.ReconstructGeneralizedPositionSolution(result=result)
def main():
builder = DiagramBuilder()
sim_plant, scene_graph = AddMultibodyPlantSceneGraph(
builder, MultibodyPlant(mbp_time_step))
load_atlas(sim_plant, add_ground=True)
sim_plant_context = sim_plant.CreateDefaultContext()
controller_plant = MultibodyPlant(mbp_time_step)
load_atlas(controller_plant)
controller = builder.AddSystem(
HumanoidController(controller_plant,
Atlas.CONTACTS_PER_FRAME,
is_wbc=False))
controller.set_name("HumanoidController")
disturber = builder.AddSystem(
ForceDisturber(sim_plant.GetBodyByName("utorso").index(), 4, 0.1, 2))
builder.Connect(disturber.get_output_port(0),
sim_plant.get_applied_spatial_force_input_port())
builder.Connect(sim_plant.get_state_output_port(),
controller.GetInputPort("q_v"))
builder.Connect(controller.GetOutputPort("tau"),
sim_plant.get_actuation_input_port())
ConnectContactResultsToDrakeVisualizer(builder=builder, plant=sim_plant)
ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph)
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
sim_plant_context = diagram.GetMutableSubsystemContext(
sim_plant, diagram_context)
set_atlas_initial_pose(sim_plant, sim_plant_context)
controller_context = diagram.GetMutableSubsystemContext(
controller, diagram_context)
controller.GetInputPort("y_des").FixValue(controller_context,
np.array([0.1, 0.0]))
simulator = Simulator(diagram, diagram_context)
simulator.set_target_realtime_rate(0.04)
simulator.AdvanceTo(20.0)
def test_deprecated_tree_api(self):
plant = MultibodyPlant()
plant.Finalize()
with warnings.catch_warnings(record=True) as w:
warnings.simplefilter('always', DrakeDeprecationWarning)
num_expected_warnings = [0]
def expect_new_warning(msg_part):
num_expected_warnings[0] += 1
self.assertEqual(len(w), num_expected_warnings[0])
self.assertIn(msg_part, str(w[-1].message))
tree = plant.tree()
expect_new_warning("`tree()`")
MobilizerIndex(0)
expect_new_warning("`MobilizerIndex`")
BodyNodeIndex(0)
expect_new_warning("`BodyNodeIndex`")
MultibodyForces(model=tree)
expect_new_warning("`MultibodyForces(plant)`")
element = plant.world_body()
self.assertIsInstance(element.get_parent_tree(), MultibodyTree)
expect_new_warning("`get_parent_tree()`")
# Check old spellings (no deprecation warnings).
self.check_old_spelling_exists(tree.CalcRelativeTransform)
self.check_old_spelling_exists(tree.CalcPointsPositions)
self.check_old_spelling_exists(
tree.CalcFrameGeometricJacobianExpressedInWorld)
self.check_old_spelling_exists(tree.EvalBodyPoseInWorld)
self.check_old_spelling_exists(tree.SetFreeBodyPoseOrThrow)
self.check_old_spelling_exists(tree.SetFreeBodySpatialVelocityOrThrow)
self.check_old_spelling_exists(tree.EvalBodySpatialVelocityInWorld)
self.check_old_spelling_exists(tree.GetPositionsFromArray)
self.check_old_spelling_exists(tree.GetVelocitiesFromArray)
self.check_old_spelling_exists(tree.CalcMassMatrixViaInverseDynamics)
self.check_old_spelling_exists(tree.CalcBiasTerm)
self.check_old_spelling_exists(tree.CalcInverseDynamics)
self.check_old_spelling_exists(tree.num_frames)
self.check_old_spelling_exists(tree.get_body)
self.check_old_spelling_exists(tree.get_joint)
self.check_old_spelling_exists(tree.get_joint_actuator)
self.check_old_spelling_exists(tree.get_frame)
self.check_old_spelling_exists(tree.GetModelInstanceName)
context = plant.CreateDefaultContext()
# All body poses.
X_WB, = tree.CalcAllBodyPosesInWorld(context)
self.assertIsInstance(X_WB, Isometry3)
v_WB, = tree.CalcAllBodySpatialVelocitiesInWorld(context)
self.assertIsInstance(v_WB, SpatialVelocity)
def test_diff_ik_integrator(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")
time_step = 0.1
parameters = mut.DifferentialInverseKinematicsParameters(2, 2)
integrator = mut.DifferentialInverseKinematicsIntegrator(
robot=plant,
frame_E=frame,
time_step=time_step,
parameters=parameters,
robot_context=context,
log_only_when_result_state_changes=True)
integrator.get_mutable_parameters().set_timestep(0.2)
self.assertEqual(integrator.get_parameters().get_timestep(), 0.2)
def CreateIiwaControllerPlant():
# creates plant that includes only the robot, used for controllers.
robot_sdf_path = FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/iiwa7/iiwa7_no_collision.sdf"
)
sim_timestep = 1e-3
plant_robot = MultibodyPlant(sim_timestep)
parser = Parser(plant=plant_robot)
parser.AddModelFromFile(robot_sdf_path)
plant_robot.WeldFrames(A=plant_robot.world_frame(),
B=plant_robot.GetFrameByName("iiwa_link_0"))
plant_robot.mutable_gravity_field().set_gravity_vector([0, 0, 0])
plant_robot.Finalize()
link_frame_indices = []
for i in range(8):
link_frame_indices.append(
plant_robot.GetFrameByName("iiwa_link_" + str(i)).index())
return plant_robot, link_frame_indices
def test_multibody_add_joint(self):
"""
Tests joint constructors and `AddJoint`.
"""
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
plant = MultibodyPlant()
parser = Parser(plant)
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 = [
RevoluteJoint(
name="revolve_things",
frame_on_parent=plant.GetBodyByName(distal_frame,
instances[1]).body_frame(),
frame_on_child=plant.GetBodyByName(proximal_frame,
instances[2]).body_frame(),
axis=[0, 0, 1],
damping=0.),
WeldJoint(
name="weld_things",
parent_frame_P=plant.GetBodyByName(distal_frame,
instances[0]).body_frame(),
child_frame_C=plant.GetBodyByName(proximal_frame,
instances[1]).body_frame(),
X_PC=RigidTransform.Identity()),
]
for joint in joints:
joint_out = plant.AddJoint(joint)
self.assertIs(joint, joint_out)
# The model is now complete.
plant.Finalize()
for joint in joints:
self._test_joint_api(joint)
def test_multibody_dynamics(self):
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant()
Parser(plant).AddModelFromFile(file_name)
# Getting ready for when we set foot on Mars :-).
plant.AddForceElement(UniformGravityFieldElement([0.0, 0.0, -3.71]))
plant.Finalize()
context = plant.CreateDefaultContext()
# Set an arbitrary configuration away from the model's fixed point.
plant.SetPositions(context, [0.1, 0.2])
H = plant.CalcMassMatrixViaInverseDynamics(context)
Cv = plant.CalcBiasTerm(context)
self.assertTrue(H.shape == (2, 2))
self.assert_sane(H)
self.assertTrue(Cv.shape == (2, ))
self.assert_sane(Cv, nonzero=False)
nv = plant.num_velocities()
vd_d = np.zeros(nv)
tau = plant.CalcInverseDynamics(
context, vd_d, MultibodyForces(plant))
self.assertEqual(tau.shape, (2,))
self.assert_sane(tau, nonzero=False)
# - Existence checks.
# Gravity leads to non-zero potential energy.
self.assertNotEqual(plant.CalcPotentialEnergy(context), 0)
plant.CalcConservativePower(context)
tau_g = plant.CalcGravityGeneralizedForces(context)
self.assertEqual(tau_g.shape, (nv,))
self.assert_sane(tau_g, nonzero=True)
forces = MultibodyForces(plant=plant)
plant.CalcForceElementsContribution(context=context, forces=forces)
def main():
parser = argparse.ArgumentParser(
description=__doc__,
formatter_class=argparse.RawDescriptionHelpFormatter)
parser.add_argument(
"filename", type=str,
help="Path to an SDF or URDF file.")
MeshcatVisualizer.add_argparse_argument(parser)
args = parser.parse_args()
filename = args.filename
if not os.path.isfile(filename):
parser.error("File does not exist: {}".format(filename))
# Construct Plant + SceneGraph.
builder = DiagramBuilder()
plant = builder.AddSystem(MultibodyPlant(0.0))
scene_graph = builder.AddSystem(SceneGraph())
plant.RegisterAsSourceForSceneGraph(scene_graph)
builder.Connect(
scene_graph.get_query_output_port(),
plant.get_geometry_query_input_port())
builder.Connect(
plant.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(plant.get_source_id()))
# Load the model file.
Parser(plant).AddModelFromFile(filename)
plant.Finalize()
# Publish to Drake Visualizer.
drake_viz_pub = ConnectDrakeVisualizer(builder, scene_graph)
# Publish to Meshcat.
if args.meshcat is not None:
meshcat_viz = builder.AddSystem(
MeshcatVisualizer(scene_graph, zmq_url=args.meshcat))
builder.Connect(
scene_graph.get_pose_bundle_output_port(),
meshcat_viz.get_input_port(0))
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 __init__(self, scene_graph):
LeafSystem.__init__(self)
assert scene_graph
mbp = MultibodyPlant(0.0)
parser = Parser(mbp, scene_graph)
model_id = parser.AddModelFromFile(
FindResource("models/glider/glider.urdf"))
mbp.Finalize()
self.source_id = mbp.get_source_id()
self.body_frame_id = mbp.GetBodyFrameIdOrThrow(
mbp.GetBodyIndices(model_id)[0])
self.elevator_frame_id = mbp.GetBodyFrameIdOrThrow(
mbp.GetBodyIndices(model_id)[1])
self.DeclareVectorInputPort("state", BasicVector(7))
self.DeclareAbstractOutputPort(
"geometry_pose", lambda: AbstractValue.Make(FramePoseVector()),
self.OutputGeometryPose)
def test_strict(self):
plant = MultibodyPlant(time_step=0.01)
parser = Parser(plant=plant)
model = """<robot name='robot' version='0.99'>
<link name='a'/>
</robot>"""
parser.AddModelFromString(file_contents=model,
file_type='urdf',
model_name='lax')
parser.SetStrictParsing()
with self.assertRaises(RuntimeError) as e:
result = parser.AddModelFromString(file_contents=model,
file_type='urdf',
model_name='strict')
pattern = r'.*version.*ignored.*'
message = str(e.exception)
match = re.match(pattern, message)
self.assertTrue(match, f'"{message}" does not match "{pattern}"')
def test_multibody_dynamics(self):
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant()
Parser(plant).AddModelFromFile(file_name)
# Getting ready for when we set foot on Mars :-).
gravity_vector = np.array([0.0, 0.0, -3.71])
plant.mutable_gravity_field().set_gravity_vector(gravity_vector)
np.testing.assert_equal(plant.gravity_field().gravity_vector(),
gravity_vector)
plant.Finalize()
context = plant.CreateDefaultContext()
# Set an arbitrary configuration away from the model's fixed point.
plant.SetPositions(context, [0.1, 0.2])
M = plant.CalcMassMatrixViaInverseDynamics(context)
Cv = plant.CalcBiasTerm(context)
self.assertTrue(M.shape == (2, 2))
self.assert_sane(M)
self.assertTrue(Cv.shape == (2, ))
self.assert_sane(Cv, nonzero=False)
nv = plant.num_velocities()
vd_d = np.zeros(nv)
tau = plant.CalcInverseDynamics(
context, vd_d, MultibodyForces(plant))
self.assertEqual(tau.shape, (2,))
self.assert_sane(tau, nonzero=False)
# - Existence checks.
# Gravity leads to non-zero potential energy.
self.assertNotEqual(plant.CalcPotentialEnergy(context), 0)
plant.CalcConservativePower(context)
tau_g = plant.CalcGravityGeneralizedForces(context)
self.assertEqual(tau_g.shape, (nv,))
self.assert_sane(tau_g, nonzero=True)
B = plant.MakeActuationMatrix()
np.testing.assert_equal(B, np.array([[0.], [1.]]))
forces = MultibodyForces(plant=plant)
plant.CalcForceElementsContribution(context=context, forces=forces)
# Test generalized forces.
forces.mutable_generalized_forces()[:] = 1
np.testing.assert_equal(forces.generalized_forces(), 1)
forces.SetZero()
np.testing.assert_equal(forces.generalized_forces(), 0)
# Test body force accessors and mutators.
link2 = plant.GetBodyByName("Link2")
self.assertIsInstance(
link2.GetForceInWorld(context, forces), SpatialForce)
forces.SetZero()
F_expected = np.array([1, 2, 3, 4, 5, 6])
link2.AddInForceInWorld(
context, F_Bo_W=SpatialForce(F=F_expected), forces=forces)
np.testing.assert_equal(
link2.GetForceInWorld(context, forces).get_coeffs(), F_expected)
link2.AddInForce(
context, p_BP_E=[0, 0, 0], F_Bp_E=SpatialForce(F=F_expected),
frame_E=plant.world_frame(), forces=forces)
# Also check accumulation.
np.testing.assert_equal(
link2.GetForceInWorld(context, forces).get_coeffs(),
2 * F_expected)
def main():
args_parser = argparse.ArgumentParser(
description=__doc__,
formatter_class=argparse.RawDescriptionHelpFormatter)
args_parser.add_argument(
"filename", type=str,
help="Path to an SDF or URDF file.")
args_parser.add_argument(
"--package_path",
type=str,
default=None,
help="Full path to the root package for reading in SDF resources.")
position_group = args_parser.add_mutually_exclusive_group()
position_group.add_argument(
"--position", type=float, nargs="+", default=[],
help="A list of positions which must be the same length as the number "
"of positions in the sdf model. Note that most models have a "
"floating-base joint by default (unless the sdf explicitly welds "
"the base to the world, and so have 7 positions corresponding to "
"the quaternion representation of that floating-base position.")
position_group.add_argument(
"--joint_position", type=float, nargs="+", default=[],
help="A list of positions which must be the same length as the number "
"of positions ASSOCIATED WITH JOINTS in the sdf model. This "
"does not include, e.g., floating-base coordinates, which will "
"be assigned a default value.")
args_parser.add_argument(
"--test", action='store_true',
help="Disable opening the gui window for testing.")
# TODO(russt): Add option to weld the base to the world pending the
# availability of GetUniqueBaseBody requested in #9747.
args = args_parser.parse_args()
filename = args.filename
if not os.path.isfile(filename):
args_parser.error("File does not exist: {}".format(filename))
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
# Construct a MultibodyPlant.
plant = MultibodyPlant()
plant.RegisterAsSourceForSceneGraph(scene_graph)
# Create the parser.
parser = Parser(plant)
# Get the package pathname.
if args.package_path:
# Verify that package.xml is found in the designated path.
package_path = os.path.abspath(args.package_path)
if not os.path.isfile(os.path.join(package_path, "package.xml")):
parser.error("package.xml not found at: {}".format(package_path))
# Get the package map and populate it using the package path.
package_map = parser.package_map()
package_map.PopulateFromFolder(package_path)
# Add the model from the file and finalize the plant.
parser.AddModelFromFile(filename)
plant.Finalize(scene_graph)
# Add sliders to set positions of the joints.
sliders = builder.AddSystem(JointSliders(robot=plant))
to_pose = builder.AddSystem(MultibodyPositionToGeometryPose(plant))
builder.Connect(sliders.get_output_port(0), to_pose.get_input_port())
builder.Connect(
to_pose.get_output_port(),
scene_graph.get_source_pose_port(plant.get_source_id()))
# Connect this to drake_visualizer.
ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph)
if len(args.position):
sliders.set_position(args.position)
elif len(args.joint_position):
sliders.set_joint_position(args.joint_position)
# Make the diagram and run it.
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_publish_every_time_step(False)
if args.test:
sliders.window.withdraw()
simulator.AdvanceTo(0.1)
else:
simulator.set_target_realtime_rate(1.0)
simulator.AdvanceTo(np.inf)
def test_model_instance_state_access_by_array(self):
# Create a MultibodyPlant with a kuka arm and a schunk gripper.
# the arm is welded to the world, the gripper is welded to the
# arm's end effector.
wsg50_sdf_path = FindResourceOrThrow(
"drake/manipulation/models/" +
"wsg_50_description/sdf/schunk_wsg_50.sdf")
iiwa_sdf_path = FindResourceOrThrow(
"drake/manipulation/models/" +
"iiwa_description/sdf/iiwa14_no_collision.sdf")
timestep = 0.0002
plant = MultibodyPlant(timestep)
parser = Parser(plant)
iiwa_model = parser.AddModelFromFile(
file_name=iiwa_sdf_path, model_name='robot')
gripper_model = parser.AddModelFromFile(
file_name=wsg50_sdf_path, model_name='gripper')
# Weld the base of arm and gripper to reduce the number of states.
X_EeGripper = RigidTransform(
RollPitchYaw(np.pi / 2, 0, np.pi / 2), [0, 0, 0.081])
plant.WeldFrames(
A=plant.world_frame(),
B=plant.GetFrameByName("iiwa_link_0", iiwa_model))
plant.WeldFrames(
A=plant.GetFrameByName("iiwa_link_7", iiwa_model),
B=plant.GetFrameByName("body", gripper_model),
X_AB=X_EeGripper)
plant.Finalize()
# Create a context of the MBP and set the state of the context
# to desired values.
context = plant.CreateDefaultContext()
nq = plant.num_positions()
nq_iiwa = plant.num_positions(iiwa_model)
nv = plant.num_velocities()
nv_iiwa = plant.num_velocities(iiwa_model)
q_iiwa_desired = np.linspace(0, 0.3, 7)
v_iiwa_desired = q_iiwa_desired + 0.4
q_gripper_desired = [0.4, 0.5]
v_gripper_desired = [-1., -2.]
x_desired = np.zeros(nq + nv)
x_desired[0:7] = q_iiwa_desired
x_desired[7:9] = q_gripper_desired
x_desired[nq:nq+7] = v_iiwa_desired
x_desired[nq+7:nq+nv] = v_gripper_desired
x = plant.GetMutablePositionsAndVelocities(context=context)
x[:] = x_desired
q = plant.GetPositions(context=context)
v = plant.GetVelocities(context=context)
# Get state from context.
x = plant.GetPositionsAndVelocities(context=context)
x_tmp = plant.GetMutablePositionsAndVelocities(context=context)
self.assertTrue(np.allclose(x_desired, x_tmp))
# Get positions and velocities of specific model instances
# from the position/velocity vector of the plant.
q_iiwa = plant.GetPositions(context=context, model_instance=iiwa_model)
q_iiwa_array = plant.GetPositionsFromArray(
model_instance=iiwa_model, q=q)
self.assertTrue(np.allclose(q_iiwa, q_iiwa_array))
q_gripper = plant.GetPositions(
context=context, model_instance=gripper_model)
v_iiwa = plant.GetVelocities(
context=context, model_instance=iiwa_model)
v_iiwa_array = plant.GetVelocitiesFromArray(
model_instance=iiwa_model, v=v)
self.assertTrue(np.allclose(v_iiwa, v_iiwa_array))
v_gripper = plant.GetVelocities(
context=context, model_instance=gripper_model)
# Assert that the `GetPositions` and `GetVelocities` return
# the desired values set earlier.
self.assertTrue(np.allclose(q_iiwa_desired, q_iiwa))
self.assertTrue(np.allclose(v_iiwa_desired, v_iiwa))
self.assertTrue(np.allclose(q_gripper_desired, q_gripper))
self.assertTrue(np.allclose(v_gripper_desired, v_gripper))
# Verify that SetPositionsInArray() and SetVelocitiesInArray() works.
plant.SetPositionsInArray(
model_instance=iiwa_model, q_instance=np.zeros(nq_iiwa), q=q)
self.assertTrue(np.allclose(
plant.GetPositionsFromArray(model_instance=iiwa_model, q=q),
np.zeros(nq_iiwa)))
plant.SetVelocitiesInArray(
model_instance=iiwa_model, v_instance=np.zeros(nv_iiwa), v=v)
self.assertTrue(np.allclose(
plant.GetVelocitiesFromArray(model_instance=iiwa_model, v=v),
np.zeros(nv_iiwa)))
# Check actuation.
nu = plant.num_actuated_dofs()
u = np.zeros(nu)
u_iiwa = np.arange(nv_iiwa)
plant.SetActuationInArray(
model_instance=iiwa_model, u_instance=u_iiwa, u=u)
self.assertTrue(np.allclose(u[:7], u_iiwa))
def test_multibody_tree_kinematics(self):
file_name = FindResourceOrThrow(
"drake/examples/double_pendulum/models/double_pendulum.sdf")
plant = MultibodyPlant()
Parser(plant).AddModelFromFile(file_name)
plant.Finalize()
context = plant.CreateDefaultContext()
world_frame = plant.world_frame()
base = plant.GetBodyByName("base")
base_frame = plant.GetFrameByName("base")
X_WL = plant.CalcRelativeTransform(
context, frame_A=world_frame, frame_B=base_frame)
self.assertIsInstance(X_WL, RigidTransform)
p_AQi = plant.CalcPointsPositions(
context=context, frame_B=base_frame,
p_BQi=np.array([[0, 1, 2], [10, 11, 12]]).T,
frame_A=world_frame).T
self.assertTupleEqual(p_AQi.shape, (2, 3))
Jv_WL = plant.CalcFrameGeometricJacobianExpressedInWorld(
context=context, frame_B=base_frame,
p_BoFo_B=[0, 0, 0])
self.assertTupleEqual(Jv_WL.shape, (6, plant.num_velocities()))
nq = plant.num_positions()
nv = plant.num_velocities()
wrt_list = [
(JacobianWrtVariable.kQDot, nq),
(JacobianWrtVariable.kV, nv),
]
for wrt, nw in wrt_list:
Jw_ABp_E = plant.CalcJacobianSpatialVelocity(
context=context, with_respect_to=wrt, frame_B=base_frame,
p_BP=np.zeros(3), frame_A=world_frame,
frame_E=world_frame)
self.assert_sane(Jw_ABp_E)
self.assertEqual(Jw_ABp_E.shape, (6, nw))
# Compute body pose.
X_WBase = plant.EvalBodyPoseInWorld(context, base)
self.assertIsInstance(X_WBase, RigidTransform)
# Set pose for the base.
X_WB_desired = RigidTransform.Identity()
X_WB = plant.CalcRelativeTransform(context, world_frame, base_frame)
plant.SetFreeBodyPose(
context=context, body=base, X_WB=X_WB_desired)
self.assertTrue(np.allclose(X_WB.matrix(), X_WB_desired.matrix()))
# Set a spatial velocity for the base.
v_WB = SpatialVelocity(w=[1, 2, 3], v=[4, 5, 6])
plant.SetFreeBodySpatialVelocity(
context=context, body=base, V_WB=v_WB)
v_base = plant.EvalBodySpatialVelocityInWorld(context, base)
self.assertTrue(np.allclose(v_base.rotational(), v_WB.rotational()))
self.assertTrue(np.allclose(v_base.translational(),
v_WB.translational()))
# Compute accelerations.
vdot = np.zeros(nv)
A_WB_array = plant.CalcSpatialAccelerationsFromVdot(
context=context, known_vdot=vdot)
self.assertEqual(len(A_WB_array), plant.num_bodies())
def test_multibody_plant_api_via_parsing(self):
# TODO(eric.cousineau): Decouple this when construction can be done
# without parsing.
# This a subset of `multibody_plant_sdf_parser_test.cc`.
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant(time_step=0.01)
model_instance = Parser(plant).AddModelFromFile(file_name)
self.assertIsInstance(model_instance, ModelInstanceIndex)
plant.Finalize()
benchmark = MakeAcrobotPlant(AcrobotParameters(), True)
self.assertEqual(plant.num_bodies(), benchmark.num_bodies())
self.assertEqual(plant.num_joints(), benchmark.num_joints())
self.assertEqual(plant.num_actuators(), benchmark.num_actuators())
self.assertEqual(
plant.num_model_instances(), benchmark.num_model_instances() + 1)
self.assertEqual(plant.num_positions(), benchmark.num_positions())
self.assertEqual(
plant.num_positions(model_instance=model_instance),
benchmark.num_positions())
self.assertEqual(
plant.num_velocities(), benchmark.num_velocities())
self.assertEqual(
plant.num_multibody_states(), benchmark.num_multibody_states())
self.assertEqual(
plant.num_actuated_dofs(), benchmark.num_actuated_dofs())
self.assertTrue(plant.is_finalized())
self.assertTrue(plant.HasBodyNamed(name="Link1"))
self.assertTrue(plant.HasBodyNamed(
name="Link1", model_instance=model_instance))
self.assertTrue(plant.HasJointNamed(name="ShoulderJoint"))
self.assertTrue(plant.HasJointNamed(
name="ShoulderJoint", model_instance=model_instance))
shoulder = plant.GetJointByName(name="ShoulderJoint")
self._test_joint_api(shoulder)
np.testing.assert_array_equal(
shoulder.position_lower_limits(), [-np.inf])
np.testing.assert_array_equal(
shoulder.position_upper_limits(), [np.inf])
self.assertIs(shoulder, plant.GetJointByName(
name="ShoulderJoint", model_instance=model_instance))
self._test_joint_actuator_api(
plant.GetJointActuatorByName(name="ElbowJoint"))
self._test_body_api(plant.GetBodyByName(name="Link1"))
self.assertIs(
plant.GetBodyByName(name="Link1"),
plant.GetBodyByName(name="Link1", model_instance=model_instance))
self.assertEqual(len(plant.GetBodyIndices(model_instance)), 2)
self._test_frame_api(plant.GetFrameByName(name="Link1"))
self.assertIs(
plant.GetFrameByName(name="Link1"),
plant.GetFrameByName(name="Link1", model_instance=model_instance))
self.assertEqual(
model_instance, plant.GetModelInstanceByName(name="acrobot"))
self.assertIsInstance(
plant.get_actuation_input_port(), InputPort)
self.assertIsInstance(
plant.get_state_output_port(), OutputPort)
# Smoke test of deprecated methods.
with catch_drake_warnings(expected_count=2):
plant.get_continuous_state_output_port()
plant.get_continuous_state_output_port(model_instance)
self.assertIsInstance(
plant.get_contact_results_output_port(), OutputPort)
self.assertIsInstance(plant.num_frames(), int)
self.assertIsInstance(plant.get_body(body_index=BodyIndex(0)), Body)
self.assertIs(shoulder, plant.get_joint(joint_index=JointIndex(0)))
self.assertIsInstance(plant.get_joint_actuator(
actuator_index=JointActuatorIndex(0)), JointActuator)
self.assertIsInstance(
plant.get_frame(frame_index=FrameIndex(0)), Frame)
self.assertEqual("acrobot", plant.GetModelInstanceName(
model_instance=model_instance))
def test_contact_force(self):
"""A block sitting on a table."""
object_file_path = FindResourceOrThrow(
"drake/examples/manipulation_station/models/061_foam_brick.sdf")
table_file_path = FindResourceOrThrow(
"drake/examples/kuka_iiwa_arm/models/table/"
"extra_heavy_duty_table_surface_only_collision.sdf")
# T: tabletop frame.
X_TObject = RigidTransform([0, 0, 0.2])
builder = DiagramBuilder()
plant = MultibodyPlant(0.002)
_, scene_graph = AddMultibodyPlantSceneGraph(builder, plant)
object_model = Parser(plant=plant).AddModelFromFile(object_file_path)
table_model = Parser(plant=plant).AddModelFromFile(table_file_path)
# Weld table to world.
plant.WeldFrames(
A=plant.world_frame(),
B=plant.GetFrameByName("link", table_model))
plant.Finalize()
# Add meshcat visualizer.
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))
# Add contact visualizer.
contact_viz = builder.AddSystem(
MeshcatContactVisualizer(
meshcat_viz=viz,
force_threshold=0,
contact_force_scale=10,
plant=plant))
contact_input_port = contact_viz.GetInputPort("contact_results")
builder.Connect(
plant.GetOutputPort("contact_results"),
contact_input_port)
builder.Connect(
scene_graph.get_pose_bundle_output_port(),
contact_viz.GetInputPort("pose_bundle"))
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
mbp_context = diagram.GetMutableSubsystemContext(
plant, diagram_context)
X_WT = plant.CalcRelativeTransform(
mbp_context,
plant.world_frame(),
plant.GetFrameByName("top_center"))
plant.SetFreeBodyPose(
mbp_context,
plant.GetBodyByName("base_link", object_model),
X_WT.multiply(X_TObject))
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.AdvanceTo(1.0)
contact_viz_context = (
diagram.GetMutableSubsystemContext(contact_viz, diagram_context))
contact_results = contact_viz.EvalAbstractInput(
contact_viz_context,
contact_input_port.get_index()).get_value()
self.assertGreater(contact_results.num_contacts(), 0)
self.assertEqual(contact_viz._contact_key_counter, 4)
def test_model_instance_state_access(self):
# Create a MultibodyPlant with a kuka arm and a schunk gripper.
# the arm is welded to the world, the gripper is welded to the
# arm's end effector.
wsg50_sdf_path = FindResourceOrThrow(
"drake/manipulation/models/" +
"wsg_50_description/sdf/schunk_wsg_50.sdf")
iiwa_sdf_path = FindResourceOrThrow(
"drake/manipulation/models/" +
"iiwa_description/sdf/iiwa14_no_collision.sdf")
plant = MultibodyPlant()
parser = Parser(plant)
iiwa_model = parser.AddModelFromFile(
file_name=iiwa_sdf_path, model_name='robot')
gripper_model = parser.AddModelFromFile(
file_name=wsg50_sdf_path, model_name='gripper')
# Weld the base of arm and gripper to reduce the number of states.
X_EeGripper = RigidTransform(
RollPitchYaw(np.pi / 2, 0, np.pi / 2), [0, 0, 0.081])
plant.WeldFrames(A=plant.world_frame(),
B=plant.GetFrameByName("iiwa_link_0", iiwa_model))
plant.WeldFrames(
A=plant.GetFrameByName("iiwa_link_7", iiwa_model),
B=plant.GetFrameByName("body", gripper_model),
X_AB=X_EeGripper)
plant.Finalize()
# Create a context of the MBP and set the state of the context
# to desired values.
context = plant.CreateDefaultContext()
nq = plant.num_positions()
nv = plant.num_velocities()
nq_iiwa = 7
nv_iiwa = 7
nq_gripper = 2
nv_gripper = 2
q_iiwa_desired = np.zeros(nq_iiwa)
v_iiwa_desired = np.zeros(nv_iiwa)
q_gripper_desired = np.zeros(nq_gripper)
v_gripper_desired = np.zeros(nv_gripper)
q_iiwa_desired[2] = np.pi/3
q_gripper_desired[0] = 0.1
v_iiwa_desired[1] = 5.0
q_gripper_desired[0] = -0.3
x_iiwa_desired = np.zeros(nq_iiwa + nv_iiwa)
x_iiwa_desired[0:nq_iiwa] = q_iiwa_desired
x_iiwa_desired[nq_iiwa:nq_iiwa+nv_iiwa] = v_iiwa_desired
x_gripper_desired = np.zeros(nq_gripper + nv_gripper)
x_gripper_desired[0:nq_gripper] = q_gripper_desired
x_gripper_desired[nq_gripper:nq_gripper+nv_gripper] = v_gripper_desired
x_desired = np.zeros(nq + nv)
x_desired[0:7] = q_iiwa_desired
x_desired[7:9] = q_gripper_desired
x_desired[nq:nq+7] = v_iiwa_desired
x_desired[nq+7:nq+nv] = v_gripper_desired
# Check SetPositionsAndVelocities() for each model instance.
# Do the iiwa model first.
plant.SetPositionsAndVelocities(context, np.zeros(nq + nv))
self.assertTrue(np.allclose(plant.GetPositionsAndVelocities(context),
np.zeros(nq + nv)))
plant.SetPositionsAndVelocities(context, iiwa_model, x_iiwa_desired)
self.assertTrue(np.allclose(
plant.GetPositionsAndVelocities(context, iiwa_model),
x_iiwa_desired))
self.assertTrue(np.allclose(plant.GetPositionsAndVelocities(
context, gripper_model), np.zeros(nq_gripper + nv_gripper)))
# Do the gripper model.
plant.SetPositionsAndVelocities(context, np.zeros(nq + nv))
self.assertTrue(np.allclose(plant.GetPositionsAndVelocities(context),
np.zeros(nq + nv)))
plant.SetPositionsAndVelocities(
context, gripper_model, x_gripper_desired)
self.assertTrue(np.allclose(
plant.GetPositionsAndVelocities(context, gripper_model),
x_gripper_desired))
self.assertTrue(np.allclose(plant.GetPositionsAndVelocities(
context, iiwa_model), np.zeros(nq_iiwa + nv_iiwa)))
# Check SetPositions() for each model instance.
# Do the iiwa model first.
plant.SetPositionsAndVelocities(context, np.zeros(nq + nv))
self.assertTrue(np.allclose(plant.GetPositionsAndVelocities(context),
np.zeros(nq + nv)))
plant.SetPositions(context, iiwa_model, q_iiwa_desired)
self.assertTrue(np.allclose(
plant.GetPositions(context, iiwa_model), q_iiwa_desired))
self.assertTrue(np.allclose(plant.GetVelocities(
context, iiwa_model), np.zeros(nv_iiwa)))
self.assertTrue(np.allclose(plant.GetPositionsAndVelocities(
context, gripper_model), np.zeros(nq_gripper + nv_gripper)))
# Do the gripper model.
plant.SetPositionsAndVelocities(context, np.zeros(nq + nv))
self.assertTrue(np.allclose(plant.GetPositionsAndVelocities(context),
np.zeros(nq + nv)))
plant.SetPositions(context, gripper_model, q_gripper_desired)
self.assertTrue(np.allclose(
plant.GetPositions(context, gripper_model),
q_gripper_desired))
self.assertTrue(np.allclose(plant.GetVelocities(
context, gripper_model), np.zeros(nq_gripper)))
self.assertTrue(np.allclose(plant.GetPositionsAndVelocities(
context, iiwa_model), np.zeros(nq_iiwa + nv_iiwa)))
# Check SetVelocities() for each model instance.
# Do the iiwa model first.
plant.SetPositionsAndVelocities(context, np.zeros(nq + nv))
self.assertTrue(np.allclose(plant.GetPositionsAndVelocities(context),
np.zeros(nq + nv)))
plant.SetVelocities(context, iiwa_model, v_iiwa_desired)
self.assertTrue(np.allclose(
plant.GetVelocities(context, iiwa_model), v_iiwa_desired))
self.assertTrue(np.allclose(plant.GetPositions(
context, iiwa_model), np.zeros(nq_iiwa)))
self.assertTrue(np.allclose(plant.GetPositionsAndVelocities(
context, gripper_model), np.zeros(nq_gripper + nv_gripper)))
# Do the gripper model.
plant.SetPositionsAndVelocities(context, np.zeros(nq + nv))
self.assertTrue(np.allclose(plant.GetPositionsAndVelocities(context),
np.zeros(nq + nv)))
plant.SetVelocities(context, gripper_model, v_gripper_desired)
self.assertTrue(np.allclose(
plant.GetVelocities(context, gripper_model),
v_gripper_desired))
self.assertTrue(np.allclose(plant.GetPositions(
context, gripper_model), np.zeros(nv_gripper)))
self.assertTrue(np.allclose(plant.GetPositionsAndVelocities(
context, iiwa_model), np.zeros(nq_iiwa + nv_iiwa)))
def test_inverse_dynamics_controller(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()
# We verify the (known) size of the model.
kNumPositions = 7
kNumVelocities = 7
kNumActuators = 7
kStateSize = kNumPositions + kNumVelocities
self.assertEqual(plant.num_positions(), kNumPositions)
self.assertEqual(plant.num_velocities(), kNumVelocities)
self.assertEqual(plant.num_actuators(), kNumActuators)
kp = np.array([1., 2., 3., 4., 5., 6., 7.])
ki = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7])
kd = np.array([.5, 1., 1.5, 2., 2.5, 3., 3.5])
controller = InverseDynamicsController(robot=plant,
kp=kp,
ki=ki,
kd=kd,
has_reference_acceleration=True)
context = controller.CreateDefaultContext()
output = controller.AllocateOutput()
estimated_state_port = 0
desired_state_port = 1
desired_acceleration_port = 2
control_port = 0
self.assertEqual(
controller.get_input_port(desired_acceleration_port).size(),
kNumVelocities)
self.assertEqual(
controller.get_input_port(estimated_state_port).size(), kStateSize)
self.assertEqual(
controller.get_input_port(desired_state_port).size(), kStateSize)
self.assertEqual(
controller.get_output_port(control_port).size(), kNumVelocities)
# Current state.
q = np.array([-0.3, -0.2, -0.1, 0.0, 0.1, 0.2, 0.3])
v = np.array([-0.9, -0.6, -0.3, 0.0, 0.3, 0.6, 0.9])
x = np.concatenate([q, v])
# Reference state and acceleration.
q_r = q + 0.1*np.ones_like(q)
v_r = v + 0.1*np.ones_like(v)
x_r = np.concatenate([q_r, v_r])
vd_r = np.array([1., 2., 3., 4., 5., 6., 7.])
integral_term = np.array([-1., -2., -3., -4., -5., -6., -7.])
vd_d = vd_r + kp*(q_r-q) + kd*(v_r-v) + ki*integral_term
context.FixInputPort(estimated_state_port, BasicVector(x))
context.FixInputPort(desired_state_port, BasicVector(x_r))
context.FixInputPort(desired_acceleration_port, BasicVector(vd_r))
controller.set_integral_value(context, integral_term)
# Set the plant's context.
plant_context = plant.CreateDefaultContext()
x_plant = plant.GetMutablePositionsAndVelocities(plant_context)
x_plant[:] = x
# Compute the expected value of the generalized forces using
# inverse dynamics.
tau_id = plant.CalcInverseDynamics(
plant_context, vd_d, MultibodyForces(plant))
# Verify the result.
controller.CalcOutput(context, output)
self.assertTrue(np.allclose(output.get_vector_data(0).CopyToVector(),
tau_id))
