2 import pinocchio
as pin
5 from copy
import deepcopy, copy
7 ones =
lambda n: np.ones([n, 1]
if isinstance(n, int)
else n)
12 transform = pin.SE3.Identity()
13 self.assertTrue(np.allclose(
zero(3), transform.translation))
14 self.assertTrue(np.allclose(
eye(3), transform.rotation))
15 self.assertTrue(np.allclose(
eye(4), transform.homogeneous))
16 self.assertTrue(np.allclose(
eye(6), transform.action))
18 transform.setIdentity()
19 self.assertTrue(np.allclose(
eye(4), transform.homogeneous))
23 quat = pin.Quaternion(M.rotation)
24 M_from_quat = pin.SE3(quat, M.translation)
25 self.assertTrue(M_from_quat.isApprox(M))
28 transform = pin.SE3.Identity()
29 self.assertTrue(np.allclose(transform.translation,
zero(3)))
32 transform = pin.SE3.Identity()
33 self.assertTrue(np.allclose(transform.rotation,
eye(3)))
36 transform = pin.SE3.Identity()
37 transform.translation =
ones(3)
38 self.assertFalse(np.allclose(transform.translation,
zero(3)))
39 self.assertTrue(np.allclose(transform.translation,
ones(3)))
42 transform = pin.SE3.Identity()
43 transform.rotation =
zero([3, 3])
44 self.assertFalse(np.allclose(transform.rotation,
eye(3)))
45 self.assertTrue(np.allclose(transform.rotation,
zero([3, 3])))
48 amb = pin.SE3.Random()
51 np.allclose(H[0:3, 0:3], amb.rotation)
54 np.allclose(H[0:3, 3], amb.translation)
57 np.allclose(H[3, :], [0.0, 0.0, 0.0, 1.0])
60 amb_from_H = pin.SE3(H)
61 self.assertTrue(amb_from_H.isApprox(amb))
64 amb = pin.SE3.Random()
67 np.allclose(aXb[:3, :3], amb.rotation)
70 np.allclose(aXb[3:, 3:], amb.rotation)
72 tblock = pin.skew(amb.translation).dot(amb.rotation)
74 np.allclose(aXb[:3, 3:], tblock)
77 np.allclose(aXb[3:, :3],
zero([3, 3]))
81 amb = pin.SE3.Random()
84 self.assertTrue(np.allclose(np.linalg.inv(aMb), bma.homogeneous))
87 amb = pin.SE3.Random()
88 bmc = pin.SE3.Random()
95 cMa = np.linalg.inv(aMc)
97 self.assertTrue(np.allclose(aMb.dot(bMc), aMc))
98 self.assertTrue(np.allclose(cma.homogeneous, cMa))
101 amb = pin.SE3.Random()
102 bmc = pin.SE3.Random()
109 cXa = np.linalg.inv(aXc)
111 self.assertTrue(np.allclose(aXb.dot(bXc), aXc))
112 self.assertTrue(np.allclose(cma.action, cXa))
115 amb = pin.SE3.Random()
116 aMb = amb.homogeneous
117 p_homogeneous =
rand(4)
119 p = p_homogeneous[0:3].
copy()
122 self.assertTrue(np.allclose(amb.act(p), (aMb.dot(p_homogeneous))[0:3]))
125 bMa = np.linalg.inv(aMb)
127 self.assertTrue(np.allclose(bma.act(p), (bMa.dot(p_homogeneous))[0:3]))
131 trans = M.translation
132 M.translation[2] = 1.0
134 self.assertTrue(trans[2] == M.translation[2])
138 tq_vec = pin.SE3ToXYZQUAT(m)
139 tq_tup = pin.SE3ToXYZQUATtuple(m)
140 mm_vec = pin.XYZQUATToSE3(tq_vec)
141 mm_tup = pin.XYZQUATToSE3(tq_tup)
142 mm_lis = pin.XYZQUATToSE3(list(tq_tup))
143 return tq_vec, tq_tup, mm_vec, mm_tup, mm_lis
145 m = pin.SE3.Identity()
146 tq_vec, tq_tup, mm_vec, mm_tup, mm_lis = compute(m)
147 self.assertTrue(np.allclose(m.homogeneous, mm_tup.homogeneous))
148 self.assertTrue(np.allclose(m.homogeneous, mm_vec.homogeneous))
149 self.assertTrue(np.allclose(m.homogeneous, mm_lis.homogeneous))
151 self.assertTrue(tq_vec[i] == 0
and tq_tup[i] == 0)
152 self.assertTrue(tq_vec[6] == 1
and tq_tup[6] == 1)
155 tq_vec, tq_tup, mm_vec, mm_tup, mm_lis = compute(m)
156 self.assertTrue(len(tq_vec) == 7)
157 self.assertTrue(len(tq_tup) == 7)
158 for a, b
in zip(tq_vec, tq_tup):
159 self.assertTrue(a == b)
160 self.assertTrue(np.allclose(m.homogeneous, mm_tup.homogeneous))
161 self.assertTrue(mm_vec == mm_tup)
162 self.assertTrue(mm_vec == mm_lis)
167 M_from_h = pin.SE3(h)
168 self.assertTrue(M == M_from_h)
171 for _
in range(100000):
172 r = pin.SE3.Random() * pin.SE3.Random()
174 self.assertTrue(s !=
"")
180 self.assertTrue(M == Mc)
181 self.assertTrue(M == Mdc)
184 self.assertFalse(M == Mc)
186 self.assertFalse(M == Mc)
189 if __name__ ==
"__main__":