def __init__(self, plant, scene_graph, diagram):
self.plant = plant
self.scene_graph = scene_graph
self.diagram = diagram
# Assumes single DOF per Joint.
joints = []
for i in range(plant.num_joints()):
joint = plant.get_joint(JointIndex(i))
assert joint.name(), joint.name()
if joint.num_positions() == 0:
continue
assert joint.num_positions() == 1, (joint, joint.name())
assert isinstance(joint, self._acceptable_joints)
joints.append(joint)
self._joint_map = {}
self._joint_names = []
joints = sorted(joints, key=lambda x: x.position_start())
for joint in joints:
self._joint_names.append(joint.name())
self._joint_map[joint.name()] = joint
# Get frames.
nf = plant.num_frames()
self._frames = []
for i in range(nf):
frame = plant.get_frame(FrameIndex(i))
if frame.name() and not frame.name().startswith("_"):
self._frames.append(frame)
assert len(self._frames) > 0
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))