5 import pinocchio
as pin
8 from test_case
import PinocchioTestCase
as TestCase
12 m = pin.SE3.Identity()
13 m.translation = np.array([1., 2., 3.])
14 m.rotation = np.array([[1., 0., 0.],[0., 0., -1.],[0., 1., 0.]])
15 self.assertApprox(pin.SE3ToXYZQUAT(m).T, [1., 2., 3., sqrt(2) / 2, 0, 0, sqrt(2) / 2])
16 self.assertApprox(pin.XYZQUATToSE3([1., 2., 3., sqrt(2) / 2, 0, 0, sqrt(2) / 2]), m)
19 self.assertFalse(isapprox(1, 2))
20 self.assertTrue(isapprox(1, 2, 10))
21 self.assertFalse(isapprox([1e10, 1e-7], [1.00001e10, 1e-8]))
22 self.assertTrue(isapprox([1e10, 1e-8], [1.00001e10, 1e-9], 1e-5))
24 if __name__ ==
'__main__':
def test_se3ToXYZQUAT_XYZQUATToSe3(self)