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))
Esempio n. 2
0
    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)
Esempio n. 3
0
    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)