unittest/python/utils.py
Go to the documentation of this file.
1 import unittest
2 from math import sqrt
3 
4 import numpy as np
5 import pinocchio as pin
6 from pinocchio.utils import isapprox
7 
8 from test_case import PinocchioTestCase as TestCase
9 
10 class TestUtils(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.]]) # rotate('x', pi / 2)
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)
17 
18  def test_isapprox(self):
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))
23 
24 if __name__ == '__main__':
25  unittest.main()
def test_se3ToXYZQUAT_XYZQUATToSe3(self)


pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:33