5 import pinocchio
as pin
8 from test_case
import PinocchioTestCase
as TestCase
13 m = pin.SE3.Identity()
14 m.translation = np.array([1.0, 2.0, 3.0])
15 m.rotation = np.array(
16 [[1.0, 0.0, 0.0], [0.0, 0.0, -1.0], [0.0, 1.0, 0.0]]
19 pin.SE3ToXYZQUAT(m).T, [1.0, 2.0, 3.0, sqrt(2) / 2, 0, 0, sqrt(2) / 2]
22 pin.XYZQUATToSE3([1.0, 2.0, 3.0, sqrt(2) / 2, 0, 0, sqrt(2) / 2]), m
25 pin.XYZQUATToSE3(np.array([1.0, 2.0, 3.0, sqrt(2) / 2, 0, 0, sqrt(2) / 2])),
28 with self.assertRaises(ValueError):
29 pin.XYZQUATToSE3([1.0, 2.0, 3.0])
30 with self.assertRaises(ValueError):
31 pin.XYZQUATToSE3(np.array([1.0, 2.0, 3.0, sqrt(2) / 2]))
34 self.assertFalse(isapprox(1, 2))
35 self.assertTrue(isapprox(1, 2, 10))
36 self.assertFalse(isapprox([1e10, 1e-7], [1.00001e10, 1e-8]))
37 self.assertTrue(isapprox([1e10, 1e-8], [1.00001e10, 1e-9], 1e-5))
40 if __name__ ==
"__main__":