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


eigenpy
Author(s): Justin Carpentier, Nicolas Mansard
autogenerated on Sat Apr 17 2021 02:37:59