2 import pinocchio
as pin
5 from test_case
import PinocchioTestCase
as TestCase
14 self.
R, _, _ = npl.svd(self.
R)
16 self.
m = pin.SE3(self.
R, self.
p)
19 R, p, m = self.
R, self.
p, self.
m
21 [np.hstack([R, pin.skew(p).dot(R)]), np.hstack([
zero([3, 3]), R])]
23 self.assertApprox(m.action, X)
25 [np.hstack([R, np.expand_dims(p, 1)]), np.array([[0.0, 0.0, 0.0, 1.0]])]
27 self.assertApprox(m.homogeneous, M)
29 self.assertApprox((m * m2).homogeneous, m.homogeneous.dot(m2.homogeneous))
30 self.assertApprox((~m).homogeneous, npl.inv(m.homogeneous))
33 self.assertApprox(m * p, m.rotation.dot(p) + m.translation)
35 m.actInv(p), m.rotation.T.dot(p) - m.rotation.T.dot(m.translation)
42 with self.assertRaises(Exception):
44 with self.assertRaises(Exception):
46 with self.assertRaises(
53 self.assertApprox(pin.Motion.Zero().vector,
zero(6))
54 v = pin.Motion.Random()
55 self.assertApprox((m * v).vector, m.action.dot(v.vector))
56 self.assertApprox((m.actInv(v)).vector, npl.inv(m.action).dot(v.vector))
59 self.assertApprox(v.vector, np.concatenate([vv, vw]))
60 self.assertApprox((v ^ v).vector,
zero(6))
64 self.assertApprox(pin.Force.Zero().vector,
zero(6))
65 f = pin.Force.Random()
68 self.assertApprox(f.vector, np.concatenate([ff, ft]))
70 self.assertApprox((m * f).vector, npl.inv(m.action.T).dot(f.vector))
71 self.assertApprox((m.actInv(f)).vector, m.action.T.dot(f.vector))
72 v = pin.Motion.Random()
73 f = pin.Force(np.concatenate([v.vector[3:], v.vector[:3]]))
74 self.assertApprox((v ^ f).vector,
zero(6))
78 Y1 = pin.Inertia.Random()
79 Y2 = pin.Inertia.Random()
81 self.assertApprox(Y1.matrix() + Y2.matrix(), Y.matrix())
82 v = pin.Motion.Random()
83 self.assertApprox((Y * v).vector, Y.matrix().dot(v.vector))
86 m.inverse().action.T.dot(Y.matrix()).dot(m.inverse().action),
89 (m.actInv(Y)).matrix(), m.action.T.dot(Y.matrix()).dot(m.action)
93 m = pin.Motion.Random()
94 f = pin.Force.Random()
95 self.assertApprox(m ^ m, m.cross(m))
96 self.assertApprox(m ^ f, m.cross(f))
97 with self.assertRaises(TypeError):
101 if __name__ ==
"__main__":