def test(self): w = eigen.Vector3d.Random() * 100 v = eigen.Vector3d.Random() * 100 n = eigen.Vector3d.Random() * 100 f = eigen.Vector3d.Random() * 100 mVec = sva.MotionVecd(w, v) fVec = sva.ForceVecd(n, f) mm = mVec.vector() mf = fVec.vector() self.assertAlmostEqual(mVec.dot(fVec) - (mm.transpose() * mf)[0], 0, delta=TOL) w2 = eigen.Vector3d.Random() * 100 v2 = eigen.Vector3d.Random() * 100 mVec2 = sva.MotionVecd(w2, v2) mm2 = mVec2.vector() crossM = mVec.cross(mVec2) self.assertTrue(isinstance(crossM, sva.MotionVecd)) self.assertAlmostEqual( (crossM.vector() - sva.vector6ToCrossMatrix(mm) * mm2).norm(), 0, delta=TOL) crossF = mVec.crossDual(fVec) self.assertTrue(isinstance(crossF, sva.ForceVecd)) self.assertAlmostEqual( (crossF.vector() - sva.vector6ToCrossDualMatrix(mm) * mf).norm(), 0, delta=TOL)
def test(self): create_random_mv = lambda: sva.MotionVecd( eigen.Vector3d().Random() * 100, eigen.Vector3d().Random() * 100) # Check empty creation and filling v1 = sva.MotionVecdVector() for i in range(100): v1.append(create_random_mv()) assert (len(v1) == 100) # Check creation from a single MotionVecd object v2 = sva.MotionVecdVector(create_random_mv()) assert (len(v2) == 1) # Check creation from a list v3 = sva.MotionVecdVector([create_random_mv() for i in range(100)]) assert (len(v3) == 100) # Check creation from a lot of MotionVecd v3 = sva.MotionVecdVector(*[create_random_mv() for i in range(100)]) assert (len(v3) == 100) # Check access mv = create_random_mv() v4 = sva.MotionVecdVector([mv] * 100) assert (all([mv == vi for vi in v4]))
def test(self): M = eigen.Matrix3d([[1, 2, 3],[2, 1, 4], [3, 4, 1]]) H = eigen.Matrix3d.Random() I = eigen.Matrix3d([[1, 2, 3],[2, 1, 4], [3, 4, 1]]) ab = sva.ABInertiad(M, H, I) ab6d = ab.matrix() mass = 1. h = eigen.Vector3d.Random() * 100 rb = sva.RBInertiad(mass, h, I) rb6d = rb.matrix() w = eigen.Vector3d.Random() v = eigen.Vector3d.Random() mVec = sva.MotionVecd(w, v) mVec6d = mVec.vector() abRes = ab + rb abRes6d = ab6d + rb6d self.assertTrue(isinstance(abRes, sva.ABInertiad)) self.assertAlmostEqual((abRes6d - abRes.matrix()).norm(), 0, delta = TOL) self.assertTrue(isUpperNull(abRes.lowerTriangularMassMatrix())) self.assertTrue(isUpperNull(abRes.lowerTriangularInertia())) fVec = ab*mVec fVec6d = ab6d*mVec6d self.assertTrue(isinstance(fVec, sva.ForceVecd)) self.assertAlmostEqual((fVec6d - fVec.vector()).norm(), 0, delta = TOL)
def test(self): w = eigen.Vector3d.Random() v = eigen.Vector3d.Random() vec = sva.ImpedanceVecd(w, v) z = vec.vector() self.assertEqual(w, vec.angular()) self.assertEqual(v, vec.linear()) self.assertEqual( z, eigen.Vector6d(w.x(), w.y(), w.z(), v.x(), v.y(), v.z())) self.assertEqual((5. * vec).vector(), 5. * z) self.assertEqual((vec * 5.).vector(), 5. * z) self.assertEqual((vec / 5.).vector(), z / 5.) vec *= 5. self.assertEqual(vec.vector(), 5. * z) vec /= 5. [self.assertAlmostEqual(x, 0, delta=TOL) for x in vec.vector() - z] w2 = eigen.Vector3d.Random() v2 = eigen.Vector3d.Random() vec2 = sva.ImpedanceVecd(w2, v2) z2 = vec2.vector() self.assertAlmostEqual(((vec + vec2).vector() - (z + z2)).norm(), 0, delta=TOL) vec_pluseq = sva.ImpedanceVecd(vec) self.assertEqual(vec_pluseq, vec) vec_pluseq += vec2 self.assertEqual(vec_pluseq, vec + vec2) self.assertEqual(vec, vec) self.assertTrue(not (vec != vec)) w = eigen.Vector3d.Random() v = eigen.Vector3d.Random() mv = sva.MotionVecd(eigen.Vector3d.Random(), eigen.Vector3d.Random()) fv = vec * mv self.assertTrue(isinstance(fv, sva.ForceVecd)) res = eigen.Vector6d( [x * y for x, y in zip(vec.vector(), mv.vector())]) self.assertEqual(res, fv.vector()) fv2 = mv * vec self.assertEqual(fv, fv2) hv = sva.ImpedanceVecd(11., 42.) self.assertEqual(hv.angular(), eigen.Vector3d(11., 11., 11.)) self.assertEqual(hv.linear(), eigen.Vector3d(42., 42., 42.)) z = sva.ImpedanceVecd(0., 0.) self.assertEqual(sva.ImpedanceVecd.Zero(), z)
def test(self): w = eigen.Vector3d.Random() v = eigen.Vector3d.Random() vec = sva.MotionVecd(w, v) m = vec.vector() self.assertEqual(w, vec.angular()) self.assertEqual(v, vec.linear()) self.assertEqual(m, eigen.Vector6d(list(w) + list(v))) self.assertEqual((5. * vec).vector(), 5. * m) self.assertEqual((vec * 5.).vector(), 5. * m) self.assertEqual((vec / 5.).vector(), m / 5) self.assertEqual((-vec).vector(), -m) w2 = eigen.Vector3d.Random() v2 = eigen.Vector3d.Random() vec2 = sva.MotionVecd(w2, v2) m2 = vec2.vector() self.assertEqual((vec + vec2).vector(), (m + m2)) self.assertEqual((vec - vec2).vector(), (m - m2)) vec_pluseq = sva.MotionVecd(vec) vec_pluseq += vec2 self.assertEqual(vec_pluseq, vec + vec2) vec_minuseq = sva.MotionVecd(vec) vec_minuseq -= vec2 self.assertEqual(vec_minuseq, vec - vec2) self.assertEqual(vec, vec) self.assertNotEqual(vec, -vec) self.assertTrue(vec != (-vec)) self.assertTrue(not (vec != vec)) z = sva.MotionVecd([0, 0, 0], [0, 0, 0]) self.assertEqual(sva.MotionVecd.Zero(), z)
def test(self): mass = 1. I = eigen.Matrix3d([[1, 2, 3],[2, 1, 4], [3, 4, 1]]) h = eigen.Vector3d.Random() rb = sva.RBInertiad(mass, h, I) rb6d = rb.matrix() w = eigen.Vector3d.Random() v = eigen.Vector3d.Random() mVec = sva.MotionVecd(w, v) mVec6d = mVec.vector() fVec = rb*mVec fVec6d = rb6d*mVec6d self.assertTrue(isinstance(fVec, sva.ForceVecd)) self.assertAlmostEqual((fVec6d - fVec.vector()).norm(), 0, delta = TOL)
def test(self): # register custom pickle function sva.copy_reg_pickle() mv = sva.MotionVecd(e3.Vector6d.Random()) fv = sva.ForceVecd(e3.Vector6d.Random()) pt = sva.PTransformd(e3.Quaterniond(e3.Vector4d.Random().normalized()), e3.Vector3d.Random()) rb = sva.RBInertiad(3., e3.Vector3d.Random(), e3.Matrix3d.Random()) ab = sva.ABInertiad(e3.Matrix3d.Random(), e3.Matrix3d.Random(), e3.Matrix3d.Random()) def test_pickle(v): pickled = pickle.dumps(v) v2 = pickle.loads(pickled) self.assertEqual(v, v2) test_pickle(mv) test_pickle(fv) test_pickle(pt) test_pickle(rb) test_pickle(ab)
def test(self): Eq = eigen.AngleAxisd(np.pi / 2, eigen.Vector3d.UnitX()) r = eigen.Vector3d.Random() * 100 pt = sva.PTransformd(Eq, r) ptInv = pt.inv() pt6d = pt.matrix() ptInv6d = ptInv.matrix() ptDual6d = pt.dualMatrix() M = eigen.Matrix3d([[1, 2, 3], [2, 1, 4], [3, 4, 1]]) H = eigen.Matrix3d.Random() * 100 I = eigen.Matrix3d([[1, 2, 3], [2, 1, 4], [3, 4, 1]]) ab = sva.ABInertiad(M, H, I) ab6d = ab.matrix() mass = 1. h = eigen.Vector3d.Random() * 100 rb = sva.RBInertiad(mass, h, I) rb6d = rb.matrix() w = eigen.Vector3d.Random() * 100 v = eigen.Vector3d.Random() * 100 n = eigen.Vector3d.Random() * 100 f = eigen.Vector3d.Random() * 100 mVec = sva.MotionVecd(w, v) mVec6d = mVec.vector() fVec = sva.ForceVecd(n, f) fVec6d = fVec.vector() mvRes1 = pt * mVec mvRes16d = pt6d * mVec6d self.assertAlmostEqual((mvRes1.vector() - mvRes16d).norm(), 0, delta=TOL) self.assertAlmostEqual((mvRes1.angular() - pt.angularMul(mVec)).norm(), 0, delta=TOL) self.assertAlmostEqual((mvRes1.linear() - pt.linearMul(mVec)).norm(), 0, delta=TOL) # Note: the C++ version would test the vectorized version here but this is not supported by the Python bindings mvRes2 = pt.invMul(mVec) mvRes26d = ptInv6d * mVec6d self.assertAlmostEqual((mvRes2.vector() - mvRes26d).norm(), 0, delta=TOL) self.assertAlmostEqual( (mvRes2.angular() - pt.angularInvMul(mVec)).norm(), 0, delta=TOL) self.assertAlmostEqual( (mvRes2.linear() - pt.linearInvMul(mVec)).norm(), 0, delta=TOL) # Same note about the vectorized version fvRes1 = pt.dualMul(fVec) fvRes16d = ptDual6d * fVec6d self.assertAlmostEqual((fvRes1.vector() - fvRes16d).norm(), 0, delta=TOL) self.assertAlmostEqual( (fvRes1.couple() - pt.coupleDualMul(fVec)).norm(), 0, delta=TOL) self.assertAlmostEqual((fvRes1.force() - pt.forceDualMul(fVec)).norm(), 0, delta=TOL) # Same note about the vectorized version fvRes2 = pt.transMul(fVec) fvRes26d = pt6d.transpose() * fVec6d self.assertAlmostEqual((fvRes2.vector() - fvRes26d).norm(), 0, delta=TOL) self.assertAlmostEqual( (fvRes2.couple() - pt.coupleTransMul(fVec)).norm(), 0, delta=TOL) self.assertAlmostEqual( (fvRes2.force() - pt.forceTransMul(fVec)).norm(), 0, delta=TOL) # Same note about the vectorized version rbRes1 = pt.dualMul(rb) rbRes16d = ptDual6d * rb6d * ptInv6d self.assertAlmostEqual((rbRes1.matrix() - rbRes16d).norm(), 0, delta=TOL) rbRes2 = pt.transMul(rb) rbRes26d = pt6d.transpose() * rb6d * pt6d self.assertAlmostEqual((rbRes2.matrix() - rbRes26d).norm(), 0, delta=TOL) abRes1 = pt.dualMul(ab) abRes16d = ptDual6d * ab6d * ptInv6d self.assertAlmostEqual((abRes1.matrix() - abRes16d).norm(), 0, delta=TOL) abRes2 = pt.transMul(ab) abRes26d = pt6d.transpose() * ab6d * pt6d self.assertAlmostEqual((abRes2.matrix() - abRes26d).norm(), 0, delta=TOL)
rbd.forwardKinematics(mb, mbc) X_s = sva.PTransformd(sva.RotY(-np.pi / 2.), e.Vector3d(0.1, 0., 0.)) mbv = MultiBodyViz(mbg, mb, endEffectorDict={'b4': (X_s, 0.1, (0., 1., 0.))}) # test MultiBodyViz from tvtk.tools import ivtk viewer = ivtk.viewer() viewer.size = (640, 480) mbv.addActors(viewer.scene) mbv.display(mb, mbc) # test axis from axis import Axis a1 = Axis(text='test', length=0.2) a1.addActors(viewer.scene) a1.X = sva.PTransformd(sva.RotX(np.pi / 2.), e.Vector3d.UnitX()) # test vector6d from vector6d import ForceVecViz, MotionVecViz M = sva.MotionVecd(e.Vector3d(0.2, 0.1, 0.), e.Vector3d(0.1, 0., 0.2)) F = sva.ForceVecd(e.Vector3d(-0.2, -0.1, 0.), e.Vector3d(-0.1, 0., -0.2)) MV = MotionVecViz(M, a1.X) FV = ForceVecViz( F, sva.PTransformd(sva.RotX(np.pi / 2.), e.Vector3d.UnitX() * 1.4)) MV.addActors(viewer.scene) FV.addActors(viewer.scene)