def test_se3(self):
R, p, m = self.R, self.p, self.m
X = np.vstack(
[np.hstack([R, skew(p) * R]),
np.hstack([zero([3, 3]), R])])
self.assertApprox(m.action, X)
M = np.vstack([np.hstack([R, p]), np.matrix('0 0 0 1', np.double)])
self.assertApprox(m.homogeneous, M)
m2 = se3.SE3.Random()
self.assertApprox((m * m2).homogeneous, m.homogeneous * m2.homogeneous)
self.assertApprox((~m).homogeneous, npl.inv(m.homogeneous))
p = rand(3)
self.assertApprox(m * p, m.rotation * p + m.translation)
self.assertApprox(m.actInv(p),
m.rotation.T * p - m.rotation.T * m.translation)
p = np.vstack([p, 1])
self.assertApprox(m * p, m.homogeneous * p)
self.assertApprox(m.actInv(p), npl.inv(m.homogeneous) * p)
p = rand(6)
self.assertApprox(m * p, m.action * p)
self.assertApprox(m.actInv(p), npl.inv(m.action) * p)
p = rand(5)
with self.assertRaises(ValueError):
m * p
with self.assertRaises(ValueError):
m.actInv(p)
with self.assertRaises(ValueError):
m.actInv('42')
def test_se3(self):
R, p, m = self.R, self.p, self.m
X = np.vstack([np.hstack([R, skew(p) * R]), np.hstack([zero([3, 3]), R])])
self.assertApprox(m.action, X)
M = np.vstack([np.hstack([R, p]), np.matrix('0 0 0 1', np.double)])
self.assertApprox(m.homogeneous, M)
m2 = se3.SE3.Random()
self.assertApprox((m * m2).homogeneous, m.homogeneous * m2.homogeneous)
self.assertApprox((~m).homogeneous, npl.inv(m.homogeneous))
p = rand(3)
self.assertApprox(m * p, m.rotation * p + m.translation)
self.assertApprox(m.actInv(p), m.rotation.T * p - m.rotation.T * m.translation)
p = np.vstack([p, 1])
self.assertApprox(m * p, m.homogeneous * p)
self.assertApprox(m.actInv(p), npl.inv(m.homogeneous) * p)
p = rand(6)
self.assertApprox(m * p, m.action * p)
self.assertApprox(m.actInv(p), npl.inv(m.action) * p)
p = rand(5)
with self.assertRaises(ValueError):
m * p
with self.assertRaises(ValueError):
m.actInv(p)
with self.assertRaises(ValueError):
m.actInv('42')
def test_se3(self):
R, p, m = self.R, self.p, self.m
X = np.vstack(
[np.hstack([R, pin.skew(p).dot(R)]),
np.hstack([zero([3, 3]), R])])
self.assertApprox(m.action, X)
M = np.vstack([
np.hstack([R, np.expand_dims(p, 1)]),
np.array([[0., 0., 0., 1.]])
])
self.assertApprox(m.homogeneous, M)
m2 = pin.SE3.Random()
self.assertApprox((m * m2).homogeneous,
m.homogeneous.dot(m2.homogeneous))
self.assertApprox((~m).homogeneous, npl.inv(m.homogeneous))
p = rand(3)
self.assertApprox(m * p, m.rotation.dot(p) + m.translation)
self.assertApprox(
m.actInv(p),
m.rotation.T.dot(p) - m.rotation.T.dot(m.translation))
# Currently, the different cases do not throw the same exception type.
# To have a more robust test, only Exception is checked.
# In the comments, the most specific actual exception class at the time of writing
p = rand(5)
with self.assertRaises(Exception): # RuntimeError
m * p
with self.assertRaises(Exception): # RuntimeError
m.actInv(p)
with self.assertRaises(
Exception
): # Boost.Python.ArgumentError (subclass of TypeError)
m.actInv('42')
def test_se3(self):
R, p, m = self.R, self.p, self.m
X = np.vstack([np.hstack([R, skew(p) * R]), np.hstack([zero([3, 3]), R])])
self.assertApprox(m.action, X)
M = np.vstack([np.hstack([R, p]), np.matrix([0., 0., 0., 1.], np.double)])
self.assertApprox(m.homogeneous, M)
m2 = pin.SE3.Random()
self.assertApprox((m * m2).homogeneous, m.homogeneous * m2.homogeneous)
self.assertApprox((~m).homogeneous, npl.inv(m.homogeneous))
p = rand(3)
self.assertApprox(m * p, m.rotation * p + m.translation)
self.assertApprox(m.actInv(p), m.rotation.T * p - m.rotation.T * m.translation)
## not supported
# p = np.vstack([p, 1])
# self.assertApprox(m * p, m.homogeneous * p)
# self.assertApprox(m.actInv(p), npl.inv(m.homogeneous) * p)
## not supported
# p = rand(6)
# self.assertApprox(m * p, m.action * p)
# self.assertApprox(m.actInv(p), npl.inv(m.action) * p)
# Currently, the different cases do not throw the same exception type.
# To have a more robust test, only Exception is checked.
# In the comments, the most specific actual exception class at the time of writing
p = rand(5)
with self.assertRaises(Exception): # RuntimeError
m * p
with self.assertRaises(Exception): # RuntimeError
m.actInv(p)
with self.assertRaises(Exception): # Boost.Python.ArgumentError (subclass of TypeError)
m.actInv('42')
def test_force(self):
m = self.m
self.assertApprox(se3.Force.Zero().vector, zero(6))
f = se3.Force.Random()
ff = f.linear
ft = f.angular
self.assertApprox(f.vector, np.vstack([ff, ft]))
self.assertApprox((m * f).vector, npl.inv(m.action.T) * f.vector)
self.assertApprox((m.actInv(f)).vector, m.action.T * f.vector)
v = se3.Motion.Random()
f = se3.Force(np.vstack([v.vector[3:], v.vector[:3]]))
self.assertApprox((v ** f).vector, zero(6))
def test_force(self):
m = self.m
self.assertApprox(se3.Force.Zero().vector, zero(6))
f = se3.Force.Random()
ff = f.linear
ft = f.angular
self.assertApprox(f.vector, np.vstack([ff, ft]))
self.assertApprox((m * f).vector, npl.inv(m.action.T) * f.vector)
self.assertApprox((m.actInv(f)).vector, m.action.T * f.vector)
v = se3.Motion.Random()
f = se3.Force(np.vstack([v.vector[3:], v.vector[:3]]))
self.assertApprox((v ^ f).vector, zero(6))
def test_motion(self):
m = self.m
self.assertApprox(se3.Motion.Zero().vector, zero(6))
v = se3.Motion.Random()
self.assertApprox((m * v).vector, m.action * v.vector)
self.assertApprox((m.actInv(v)).vector, npl.inv(m.action) * v.vector)
vv = v.linear
vw = v.angular
self.assertApprox(v.vector, np.vstack([vv, vw]))
self.assertApprox((v ** v).vector, zero(6))
def test_motion(self):
m = self.m
self.assertApprox(se3.Motion.Zero().vector, zero(6))
v = se3.Motion.Random()
self.assertApprox((m * v).vector, m.action * v.vector)
self.assertApprox((m.actInv(v)).vector, npl.inv(m.action) * v.vector)
vv = v.linear
vw = v.angular
self.assertApprox(v.vector, np.vstack([vv, vw]))
self.assertApprox((v ^ v).vector, zero(6))
def test_se3(self):
R, p, m = self.R, self.p, self.m
X = np.vstack(
[np.hstack([R, skew(p) * R]),
np.hstack([zero([3, 3]), R])])
self.assertApprox(m.action, X)
M = np.vstack(
[np.hstack([R, p]),
np.matrix([0., 0., 0., 1.], np.double)])
self.assertApprox(m.homogeneous, M)
m2 = se3.SE3.Random()
self.assertApprox((m * m2).homogeneous, m.homogeneous * m2.homogeneous)
self.assertApprox((~m).homogeneous, npl.inv(m.homogeneous))
p = rand(3)
self.assertApprox(m * p, m.rotation * p + m.translation)
self.assertApprox(m.actInv(p),
m.rotation.T * p - m.rotation.T * m.translation)
## not supported
# p = np.vstack([p, 1])
# self.assertApprox(m * p, m.homogeneous * p)
# self.assertApprox(m.actInv(p), npl.inv(m.homogeneous) * p)
## not supported
# p = rand(6)
# self.assertApprox(m * p, m.action * p)
# self.assertApprox(m.actInv(p), npl.inv(m.action) * p)
# Currently, the different cases do not throw the same exception type.
# To have a more robust test, only Exception is checked.
# In the comments, the most specific actual exception class at the time of writing
p = rand(5)
with self.assertRaises(Exception): # RuntimeError
m * p
with self.assertRaises(Exception): # RuntimeError
m.actInv(p)
with self.assertRaises(
Exception
): # Boost.Python.ArgumentError (subclass of TypeError)
m.actInv('42')
