unittest/python/utils.py

Go to the documentation of this file.

import unittest
from math import sqrt
 
import numpy as np
import pinocchio as pin
from pinocchio.utils import isapprox
from test_case import PinocchioTestCase as TestCase
 
 
class TestUtils(TestCase):
    def test_se3ToXYZQUAT_XYZQUATToSe3(self):
        m = pin.SE3.Identity()
        m.translation = np.array([1.0, 2.0, 3.0])
        m.rotation = np.array(
            [[1.0, 0.0, 0.0], [0.0, 0.0, -1.0], [0.0, 1.0, 0.0]]
        )  # rotate('x', pi / 2)
        self.assertApprox(
            pin.SE3ToXYZQUAT(m).T, [1.0, 2.0, 3.0, sqrt(2) / 2, 0, 0, sqrt(2) / 2]
        )
        self.assertApprox(
            pin.XYZQUATToSE3([1.0, 2.0, 3.0, sqrt(2) / 2, 0, 0, sqrt(2) / 2]), m
        )
        self.assertApprox(
            pin.XYZQUATToSE3(np.array([1.0, 2.0, 3.0, sqrt(2) / 2, 0, 0, sqrt(2) / 2])),
            m,
        )
        with self.assertRaises(ValueError):
            pin.XYZQUATToSE3([1.0, 2.0, 3.0])
        with self.assertRaises(ValueError):
            pin.XYZQUATToSE3(np.array([1.0, 2.0, 3.0, sqrt(2) / 2]))
 
    def test_isapprox(self):
        self.assertFalse(isapprox(1, 2))
        self.assertTrue(isapprox(1, 2, 10))
        self.assertFalse(isapprox([1e10, 1e-7], [1.00001e10, 1e-8]))
        self.assertTrue(isapprox([1e10, 1e-8], [1.00001e10, 1e-9], 1e-5))
 
 
if __name__ == "__main__":
    unittest.main()