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

eigenpy
Author(s): Justin Carpentier, Nicolas Mansard
autogenerated on Fri Jun 2 2023 02:10:26