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)