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()) ]