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.array([MAX_EFF])
mv = np.array([MAX_VEL])
lb = np.array([MIN_POS])
ub = np.array([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, 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_gravity(self):
self.assertApprox(self.model.gravity.np,
np.array([0, 0, -9.81, 0, 0, 0]).T)