def test_add_joint(self):
model = pin.Model()
idx = 0
idx = model.addJoint(idx, pin.JointModelRY(), pin.SE3.Identity(),
'joint_' + str(idx + 1))
MAX_EFF = 100.
MAX_VEL = 10.
MIN_POS = -1.
MAX_POS = 1.
me = np.matrix([MAX_EFF])
mv = np.matrix([MAX_VEL])
lb = np.matrix([MIN_POS])
ub = np.matrix([MAX_POS])
idx = model.addJoint(idx, pin.JointModelRY(), pin.SE3.Identity(),
'joint_' + str(idx + 1), me, mv, lb, ub)
self.assertEqual(model.nbodies, 1)
self.assertEqual(model.njoints, 3)
self.assertEqual(model.nq, 2)
self.assertEqual(model.nv, 2)
self.assertEqual(float(model.effortLimit[1]), MAX_EFF)
self.assertEqual(float(model.velocityLimit[1]), MAX_VEL)
self.assertEqual(float(model.lowerPositionLimit[1]), MIN_POS)
self.assertEqual(float(model.upperPositionLimit[1]), MAX_POS)
def test_add_joint(self):
model = pin.Model()
idx = 0
idx = model.addJoint(idx, pin.JointModelRY(), pin.SE3.Identity(), 'joint_'+str(idx+1))
MAX_EFF = 100.
MAX_VEL = 10.
MIN_POS = -1.
MAX_POS = 1.
me = np.matrix([MAX_EFF])
mv = np.matrix([MAX_VEL])
lb = np.matrix([MIN_POS])
ub = np.matrix([MAX_POS])
idx = model.addJoint(idx, pin.JointModelRY(), pin.SE3.Identity(), 'joint_'+str(idx+1),me,mv,lb,ub)
self.assertEqual(model.nbodies, 1)
self.assertEqual(model.njoints, 3)
self.assertEqual(model.nq, 2)
self.assertEqual(model.nv, 2)
self.assertEqual(float(model.effortLimit[1]), MAX_EFF)
self.assertEqual(float(model.velocityLimit[1]), MAX_VEL)
self.assertEqual(float(model.lowerPositionLimit[1]), MIN_POS)
self.assertEqual(float(model.upperPositionLimit[1]), MAX_POS)
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 placement(x=0, y=0, z=0, rx=0, ry=0, rz=0):
m = se3.SE3.Identity()
m.translation = np.matrix([[float(i)] for i in [x, y, z]])
m.rotation *= rotate('x', rx)
m.rotation *= rotate('y', ry)
m.rotation *= rotate('z', rz)
return m
def placement(x=0, y=0, z=0, rx=0, ry=0, rz=0):
m = pin.SE3.Identity()
m.translation = np.matrix([[float(i)] for i in [x, y, z]])
m.rotation *= rotate('x', rx)
m.rotation *= rotate('y', ry)
m.rotation *= rotate('z', rz)
return m
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 = 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_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')
def test_gravity(self):
self.assertApprox(self.model.gravity.np,
np.matrix('0 0 -9.81 0 0 0').T)
def test_gravity(self):
self.assertApprox(self.model.gravity.np, np.matrix('0 0 -9.81 0 0 0').T)
