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 
11 class TestUtils(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]]
17  ) # rotate('x', pi / 2)
18  self.assertApprox(
19  pin.SE3ToXYZQUAT(m).T, [1.0, 2.0, 3.0, sqrt(2) / 2, 0, 0, sqrt(2) / 2]
20  )
21  self.assertApprox(
22  pin.XYZQUATToSE3([1.0, 2.0, 3.0, sqrt(2) / 2, 0, 0, sqrt(2) / 2]), m
23  )
24  self.assertApprox(
25  pin.XYZQUATToSE3(np.array([1.0, 2.0, 3.0, sqrt(2) / 2, 0, 0, sqrt(2) / 2])),
26  m,
27  )
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]))
32 
33  def test_isapprox(self):
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))
38 
39 
40 if __name__ == "__main__":
41  unittest.main()
pinocchio.utils
Definition: bindings/python/pinocchio/utils.py:1
utils.TestUtils
Definition: unittest/python/utils.py:11
utils.TestUtils.test_isapprox
def test_isapprox(self)
Definition: unittest/python/utils.py:33
utils.TestUtils.test_se3ToXYZQUAT_XYZQUATToSe3
def test_se3ToXYZQUAT_XYZQUATToSe3(self)
Definition: unittest/python/utils.py:12


pinocchio
Author(s):
autogenerated on Sun Jun 16 2024 02:43:13