2 from copy
import copy, deepcopy
5 import pinocchio
as pin
10 return np.ones([n, 1]
if isinstance(n, int)
else n)
15 transform = pin.SE3.Identity()
16 self.assertTrue(np.allclose(
zero(3), transform.translation))
17 self.assertTrue(np.allclose(
eye(3), transform.rotation))
18 self.assertTrue(np.allclose(
eye(4), transform.homogeneous))
19 self.assertTrue(np.allclose(
eye(6), transform.action))
21 transform.setIdentity()
22 self.assertTrue(np.allclose(
eye(4), transform.homogeneous))
26 quat = pin.Quaternion(M.rotation)
27 M_from_quat = pin.SE3(quat, M.translation)
28 self.assertTrue(M_from_quat.isApprox(M))
31 transform = pin.SE3.Identity()
32 self.assertTrue(np.allclose(transform.translation,
zero(3)))
35 transform = pin.SE3.Identity()
36 self.assertTrue(np.allclose(transform.rotation,
eye(3)))
39 transform = pin.SE3.Identity()
40 transform.translation =
ones(3)
41 self.assertFalse(np.allclose(transform.translation,
zero(3)))
42 self.assertTrue(np.allclose(transform.translation,
ones(3)))
45 transform = pin.SE3.Identity()
46 transform.rotation =
zero([3, 3])
47 self.assertFalse(np.allclose(transform.rotation,
eye(3)))
48 self.assertTrue(np.allclose(transform.rotation,
zero([3, 3])))
51 amb = pin.SE3.Random()
54 np.allclose(H[0:3, 0:3], amb.rotation)
57 np.allclose(H[0:3, 3], amb.translation)
60 np.allclose(H[3, :], [0.0, 0.0, 0.0, 1.0])
63 amb_from_H = pin.SE3(H)
64 self.assertTrue(amb_from_H.isApprox(amb))
67 amb = pin.SE3.Random()
70 np.allclose(aXb[:3, :3], amb.rotation)
73 np.allclose(aXb[3:, 3:], amb.rotation)
75 tblock = pin.skew(amb.translation).dot(amb.rotation)
77 np.allclose(aXb[:3, 3:], tblock)
80 np.allclose(aXb[3:, :3],
zero([3, 3]))
84 amb = pin.SE3.Random()
87 self.assertTrue(np.allclose(np.linalg.inv(aMb), bma.homogeneous))
90 amb = pin.SE3.Random()
91 bmc = pin.SE3.Random()
98 cMa = np.linalg.inv(aMc)
100 self.assertTrue(np.allclose(aMb.dot(bMc), aMc))
101 self.assertTrue(np.allclose(cma.homogeneous, cMa))
104 amb = pin.SE3.Random()
105 bmc = pin.SE3.Random()
112 cXa = np.linalg.inv(aXc)
114 self.assertTrue(np.allclose(aXb.dot(bXc), aXc))
115 self.assertTrue(np.allclose(cma.action, cXa))
118 amb = pin.SE3.Random()
119 aMb = amb.homogeneous
120 p_homogeneous =
rand(4)
122 p = p_homogeneous[0:3].
copy()
125 self.assertTrue(np.allclose(amb.act(p), (aMb.dot(p_homogeneous))[0:3]))
128 bMa = np.linalg.inv(aMb)
130 self.assertTrue(np.allclose(bma.act(p), (bMa.dot(p_homogeneous))[0:3]))
134 trans = M.translation
135 M.translation[2] = 1.0
137 self.assertTrue(trans[2] == M.translation[2])
141 tq_vec = pin.SE3ToXYZQUAT(m)
142 tq_tup = pin.SE3ToXYZQUATtuple(m)
143 mm_vec = pin.XYZQUATToSE3(tq_vec)
144 mm_tup = pin.XYZQUATToSE3(tq_tup)
145 mm_lis = pin.XYZQUATToSE3(list(tq_tup))
146 return tq_vec, tq_tup, mm_vec, mm_tup, mm_lis
148 m = pin.SE3.Identity()
149 tq_vec, tq_tup, mm_vec, mm_tup, mm_lis = compute(m)
150 self.assertTrue(np.allclose(m.homogeneous, mm_tup.homogeneous))
151 self.assertTrue(np.allclose(m.homogeneous, mm_vec.homogeneous))
152 self.assertTrue(np.allclose(m.homogeneous, mm_lis.homogeneous))
154 self.assertTrue(tq_vec[i] == 0
and tq_tup[i] == 0)
155 self.assertTrue(tq_vec[6] == 1
and tq_tup[6] == 1)
158 tq_vec, tq_tup, mm_vec, mm_tup, mm_lis = compute(m)
159 self.assertTrue(len(tq_vec) == 7)
160 self.assertTrue(len(tq_tup) == 7)
161 for a, b
in zip(tq_vec, tq_tup):
162 self.assertTrue(a == b)
163 self.assertTrue(np.allclose(m.homogeneous, mm_tup.homogeneous))
164 self.assertTrue(mm_vec == mm_tup)
165 self.assertTrue(mm_vec == mm_lis)
170 M_from_h = pin.SE3(h)
171 self.assertTrue(M == M_from_h)
174 for _
in range(100000):
175 r = pin.SE3.Random() * pin.SE3.Random()
177 self.assertTrue(s !=
"")
183 self.assertTrue(M == Mc)
184 self.assertTrue(M == Mdc)
187 self.assertFalse(M == Mc)
189 self.assertFalse(M == Mc)
192 if __name__ ==
"__main__":