5 import pinocchio
as pin
10 from test_case
import PinocchioTestCase
as TestCase
17 self.assertApprox(m,
eye(3))
22 self.assertApprox(m,
eye(3))
27 self.assertApprox(v,
zero(3))
30 """Test log3 over the quaternions."""
31 from eigenpy
import Quaternion
33 quat_v = np.array([0.0, 0.0, 0.0, 1.0])
36 self.assertApprox(v0,
zero(3))
39 self.assertApprox(v1,
zero(3))
42 self.assertApprox(pin.exp3_quat(
zero(3)), np.array([0.0, 0.0, 0.0, 1.0]))
47 self.assertApprox(J,
eye(3))
52 self.assertTrue(M.isIdentity())
54 M2 = pin.exp6(np.array(v))
55 self.assertTrue(M2.isIdentity())
60 self.assertApprox(J,
eye(6))
62 J2 = pin.Jexp6(np.array(v))
63 self.assertApprox(J, J2)
66 m = pin.SE3.Identity()
68 self.assertApprox(v.vector,
zero(6))
73 self.assertApprox(v.vector,
zero(6))
76 q0 = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0])
78 self.assertApprox(v.vector,
zero(6))
81 m = pin.SE3.Identity()
83 self.assertApprox(J,
eye(6))
86 u = np.random.rand((3))
87 v = np.random.rand((3))
90 u_unskew = pin.unSkew(u_skew)
92 self.assertApprox(u, u_unskew)
95 u_v_square = pin.skewSquare(u, v)
97 self.assertApprox(u_v_square, u_skew.dot(v_skew))
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)
106 self.assertLess(np.linalg.norm(m), np.pi)
107 self.assertApprox(
log(
exp(m)), m)
109 m = np.random.rand(3)
110 self.assertLess(np.linalg.norm(m), np.pi)
111 self.assertApprox(
log(
exp(m)), m)
114 self.assertApprox(
exp(
log(m)), m)
118 np.linalg.norm(m), np.pi
120 self.assertApprox(
log(
exp(m)), m)
122 m = np.random.rand(6)
124 np.linalg.norm(m), np.pi
126 self.assertApprox(
log(
exp(m)), m)
129 self.assertApprox(
exp(
log(m)).homogeneous, m)
131 with self.assertRaises(ValueError):
133 with self.assertRaises(ValueError):
135 with self.assertRaises(ValueError):
137 with self.assertRaises(ValueError):
139 with self.assertRaises(ValueError):
143 if __name__ ==
"__main__":