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))
# 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))
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_continuous_state_output_port(),
OutputPort)
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 get_bodies(mbp):
return [mbp.tree().get_body(BodyIndex(i)) for i in range(mbp.num_bodies())]
def test_type_safe_indices(self):
self.assertEqual(world_index(), BodyIndex(0))
def get_bodies(self):
return [
self.tree.get_body(BodyIndex(i))
for i in range(self.plant.num_bodies())
]
