unittest/python/explog.py
Go to the documentation of this file.
1 import unittest
2 import math
3 
4 import numpy as np
5 import pinocchio as pin
6 
7 from pinocchio.utils import rand, zero, eye
8 from pinocchio.explog import exp, log
9 
10 from test_case import PinocchioTestCase as TestCase
11 
12 
13 class TestExpLog(TestCase):
14  def test_exp3(self):
15  v = zero(3)
16  m = pin.exp3(v)
17  self.assertApprox(m, eye(3))
18 
19  def test_Jexp3(self):
20  v = zero(3)
21  m = pin.Jexp3(v)
22  self.assertApprox(m, eye(3))
23 
24  def test_log3(self):
25  m = eye(3)
26  v = pin.log3(m)
27  self.assertApprox(v, zero(3))
28 
29  def test_log3_quat(self):
30  """Test log3 over the quaternions."""
31  from eigenpy import Quaternion
32 
33  quat_v = np.array([0.0, 0.0, 0.0, 1.0])
34  quat = Quaternion(quat_v)
35  v0 = pin.log3(quat)
36  self.assertApprox(v0, zero(3))
37 
38  v1 = pin.log3(quat_v)
39  self.assertApprox(v1, zero(3))
40 
41  def test_exp3_quat(self):
42  self.assertApprox(pin.exp3_quat(zero(3)), np.array([0.0, 0.0, 0.0, 1.0]))
43 
44  def test_Jlog3(self):
45  m = eye(3)
46  J = pin.Jlog3(m)
47  self.assertApprox(J, eye(3))
48 
49  def test_exp6(self):
50  v = pin.Motion.Zero()
51  M = pin.exp6(v)
52  self.assertTrue(M.isIdentity())
53 
54  M2 = pin.exp6(np.array(v))
55  self.assertTrue(M2.isIdentity())
56 
57  def test_Jexp6(self):
58  v = pin.Motion.Zero()
59  J = pin.Jexp6(v)
60  self.assertApprox(J, eye(6))
61 
62  J2 = pin.Jexp6(np.array(v))
63  self.assertApprox(J, J2)
64 
65  def test_log6(self):
66  m = pin.SE3.Identity()
67  v = pin.log6(m)
68  self.assertApprox(v.vector, zero(6))
69 
71  m = eye(4)
72  v = pin.log6(m)
73  self.assertApprox(v.vector, zero(6))
74 
75  def test_log6_quat(self):
76  q0 = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0])
77  v = pin.log6_quat(q0)
78  self.assertApprox(v.vector, zero(6))
79 
80  def test_Jlog6(self):
81  m = pin.SE3.Identity()
82  J = pin.Jlog6(m)
83  self.assertApprox(J, eye(6))
84 
85  def test_skew(self):
86  u = np.random.rand((3))
87  v = np.random.rand((3))
88 
89  u_skew = pin.skew(u)
90  u_unskew = pin.unSkew(u_skew)
91 
92  self.assertApprox(u, u_unskew)
93 
94  v_skew = pin.skew(v)
95  u_v_square = pin.skewSquare(u, v)
96 
97  self.assertApprox(u_v_square, u_skew.dot(v_skew))
98 
99  def test_explog(self):
100  self.assertApprox(exp(42), math.exp(42))
101  self.assertApprox(log(42), math.log(42))
102  self.assertApprox(exp(log(42)), 42)
103  self.assertApprox(log(exp(42)), 42)
104 
105  m = rand(3)
106  self.assertLess(np.linalg.norm(m), np.pi) # necessary for next test
107  self.assertApprox(log(exp(m)), m)
108 
109  m = np.random.rand(3)
110  self.assertLess(np.linalg.norm(m), np.pi) # necessary for next test
111  self.assertApprox(log(exp(m)), m)
112 
113  m = pin.SE3.Random()
114  self.assertApprox(exp(log(m)), m)
115 
116  m = rand(6)
117  self.assertLess(
118  np.linalg.norm(m), np.pi
119  ) # necessary for next test (actually, only angular part)
120  self.assertApprox(log(exp(m)), m)
121 
122  m = np.random.rand(6)
123  self.assertLess(
124  np.linalg.norm(m), np.pi
125  ) # necessary for next test (actually, only angular part)
126  self.assertApprox(log(exp(m)), m)
127 
128  m = eye(4)
129  self.assertApprox(exp(log(m)).homogeneous, m)
130 
131  with self.assertRaises(ValueError):
132  exp(eye(4))
133  with self.assertRaises(ValueError):
134  exp(list(range(3)))
135  with self.assertRaises(ValueError):
136  log(list(range(3)))
137  with self.assertRaises(ValueError):
138  log(zero(5))
139  with self.assertRaises(ValueError):
140  log(zero((3, 1)))
141 
142 
143 if __name__ == "__main__":
144  unittest.main()
explog.TestExpLog.test_Jexp3
def test_Jexp3(self)
Definition: unittest/python/explog.py:19
explog.TestExpLog.test_Jlog6
def test_Jlog6(self)
Definition: unittest/python/explog.py:80
explog.TestExpLog.test_Jlog3
def test_Jlog3(self)
Definition: unittest/python/explog.py:44
explog.TestExpLog.test_Jexp6
def test_Jexp6(self)
Definition: unittest/python/explog.py:57
pinocchio.explog.exp
def exp(x)
Definition: bindings/python/pinocchio/explog.py:13
explog.TestExpLog.test_log3_quat
def test_log3_quat(self)
Definition: unittest/python/explog.py:29
explog.TestExpLog.test_exp6
def test_exp6(self)
Definition: unittest/python/explog.py:49
explog.TestExpLog
Definition: unittest/python/explog.py:13
explog.TestExpLog.test_skew
def test_skew(self)
Definition: unittest/python/explog.py:85
explog.TestExpLog.test_exp3
def test_exp3(self)
Definition: unittest/python/explog.py:14
pinocchio::python::context::Quaternion
Eigen::Quaternion< Scalar, Options > Quaternion
Definition: bindings/python/context/generic.hpp:49
explog.TestExpLog.test_exp3_quat
def test_exp3_quat(self)
Definition: unittest/python/explog.py:41
pinocchio.utils
Definition: bindings/python/pinocchio/utils.py:1
pinocchio.explog
Definition: bindings/python/pinocchio/explog.py:1
pinocchio.utils.eye
def eye(n)
Definition: bindings/python/pinocchio/utils.py:33
explog.TestExpLog.test_log6_quat
def test_log6_quat(self)
Definition: unittest/python/explog.py:75
explog.TestExpLog.test_log3
def test_log3(self)
Definition: unittest/python/explog.py:24
pinocchio.explog.log
def log(x)
Definition: bindings/python/pinocchio/explog.py:29
explog.TestExpLog.test_log6_homogeneous
def test_log6_homogeneous(self)
Definition: unittest/python/explog.py:70
pinocchio.utils.rand
def rand(n)
Definition: bindings/python/pinocchio/utils.py:42
pinocchio.utils.zero
def zero(n)
Definition: bindings/python/pinocchio/utils.py:38
explog.TestExpLog.test_explog
def test_explog(self)
Definition: unittest/python/explog.py:99
explog.TestExpLog.test_log6
def test_log6(self)
Definition: unittest/python/explog.py:65


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