test_geometry.py
Go to the documentation of this file.
1 import numpy as np
2 from geometry import (
3  AngleAxis,
4  Quaternion,
5  testInAngleAxis,
6  testInQuaternion,
7  testOutAngleAxis,
8  testOutQuaternion,
9 )
10 from numpy import cos
11 
12 verbose = True
13 
14 
15 def isapprox(a, b, epsilon=1e-6):
16  if issubclass(a.__class__, np.ndarray) and issubclass(b.__class__, np.ndarray):
17  return np.allclose(a, b, epsilon)
18  else:
19  return abs(a - b) < epsilon
20 
21 
22 # --- Quaternion ---------------------------------------------------------------
23 # Coefficient initialisation
24 verbose and print("[Quaternion] Coefficient initialisation")
25 q = Quaternion(1, 2, 3, 4)
26 q.normalize()
27 assert isapprox(np.linalg.norm(q.coeffs()), q.norm())
28 assert isapprox(np.linalg.norm(q.coeffs()), 1)
29 
30 # Coefficient-vector initialisation
31 verbose and print("[Quaternion] Coefficient-vector initialisation")
32 v = np.array([0.5, -0.5, 0.5, 0.5])
33 for k in range(10000):
34  qv = Quaternion(v)
35 assert isapprox(qv.coeffs(), v)
36 
37 # Angle axis initialisation
38 verbose and print("[Quaternion] AngleAxis initialisation")
39 r = AngleAxis(q)
40 q2 = Quaternion(r)
41 assert q == q
42 assert isapprox(q.coeffs(), q2.coeffs())
43 assert q2.isApprox(q2)
44 assert q2.isApprox(q2, 1e-2)
45 
46 Rq = q.matrix()
47 Rr = r.matrix()
48 assert isapprox(Rq.dot(Rq.T), np.eye(3))
49 assert isapprox(Rr, Rq)
50 
51 # Rotation matrix initialisation
52 verbose and print("[Quaternion] Rotation Matrix initialisation")
53 qR = Quaternion(Rr)
54 assert q.isApprox(qR)
55 assert isapprox(q.coeffs(), qR.coeffs())
56 
57 assert isapprox(qR[3], 1.0 / np.sqrt(30))
58 try:
59  qR[5]
60  print("Error, this message should not appear.")
61 except RuntimeError as e:
62  if verbose:
63  print("As expected, caught exception: ", e)
64 
65 # --- Angle Vector ------------------------------------------------
66 r = AngleAxis(0.1, np.array([1, 0, 0], np.double))
67 if verbose:
68  print("Rx(.1) = \n\n", r.matrix(), "\n")
69 assert isapprox(r.matrix()[2, 2], cos(r.angle))
70 assert isapprox(r.axis, np.array([1.0, 0, 0]))
71 assert isapprox(r.angle, 0.1)
72 assert r.isApprox(r)
73 assert r.isApprox(r, 1e-2)
74 
75 r.axis = np.array([0, 1, 0], np.double).T
76 assert isapprox(r.matrix()[0, 0], cos(r.angle))
77 
78 ri = r.inverse()
79 assert isapprox(ri.angle, -0.1)
80 
81 R = r.matrix()
82 r2 = AngleAxis(np.dot(R, R))
83 assert isapprox(r2.angle, r.angle * 2)
84 
85 # --- USER FUNCTIONS -----------------------------------------------------------
87 assert ro.__class__ == AngleAxis
89 assert res == r.angle
90 
92 assert qo.__class__ == Quaternion
93 res = testInQuaternion(q)
94 assert q.norm() == res
print
void print(const Eigen::SparseMatrix< Scalar, Options > &mat)
Definition: sparse_matrix.cpp:50
testOutQuaternion
Eigen::Quaterniond testOutQuaternion()
Definition: geometry.cpp:17
test_geometry.isapprox
def isapprox(a, b, epsilon=1e-6)
Definition: test_geometry.py:15
testInAngleAxis
double testInAngleAxis(Eigen::AngleAxisd aa)
Definition: geometry.cpp:15
testInQuaternion
double testInQuaternion(Eigen::Quaterniond q)
Definition: geometry.cpp:21
testOutAngleAxis
Eigen::AngleAxisd testOutAngleAxis()
Definition: geometry.cpp:11


eigenpy
Author(s): Justin Carpentier, Nicolas Mansard
autogenerated on Sun Apr 14 2024 02:17:00