def test(self): mass = 1. I = eigen.Matrix3d([[1, 2, 3],[2, 1, 4], [3, 4, 1]]) h = eigen.Vector3d.Random() rb2 = sva.RBInertiad(mass, h, I) self.assertEqual(rb2.mass(), mass) self.assertEqual(rb2.momentum(), h) self.assertEqual(rb2.inertia(), I) self.assertTrue(isUpperNull(rb2.lowerTriangularInertia())) rb3 = sva.RBInertiad(mass, h, rb2.lowerTriangularInertia()) self.assertEqual(rb3.mass(), mass) self.assertEqual(rb3.momentum(), h) self.assertEqual(rb3.inertia(), I) self.assertTrue(isUpperNull(rb3.lowerTriangularInertia())) rb4 = rb2 + rb3 self.assertEqual(rb4.mass(), mass + mass) self.assertEqual(rb4.momentum(), h + h) self.assertEqual(rb4.inertia(), I + I) self.assertTrue(isUpperNull(rb4.lowerTriangularInertia())) rb5 = 2.*rb2 self.assertEqual(rb5.mass(), 2.*mass) self.assertEqual(rb5.momentum(), 2.*h) self.assertEqual(rb5.inertia(), 2.*I) self.assertTrue(isUpperNull(rb5.lowerTriangularInertia())) rb6 = rb2*2. self.assertEqual(rb6.mass(), 2.*mass) self.assertEqual(rb6.momentum(), 2.*h) self.assertEqual(rb6.inertia(), 2.*I) self.assertTrue(isUpperNull(rb6.lowerTriangularInertia())) rb7 = rb2 - rb3 self.assertEqual(rb7.mass(), mass - mass) self.assertEqual(rb7.momentum(), h - h) self.assertEqual(rb7.inertia(), I - I) self.assertTrue(isUpperNull(rb7.lowerTriangularInertia())) rb8 = -rb2 self.assertEqual(rb8, rb2*-1) self.assertTrue(isUpperNull(rb8.lowerTriangularInertia())) rb9 = sva.RBInertiad(rb2) rb9 += rb3 self.assertEqual(rb9, rb2 + rb3) self.assertTrue(isUpperNull(rb9.lowerTriangularInertia())) rb10 = sva.RBInertiad(rb2) rb10 -= rb3 self.assertEqual(rb10, rb2 - rb3) self.assertTrue(isUpperNull(rb10.lowerTriangularInertia())) self.assertEqual(rb2, rb2) self.assertNotEqual(rb2, rb6) self.assertTrue(rb2 != rb6) self.assertTrue(not(rb2 != rb2))
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): 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 makeEnv(): mbg = rbdyn.MultiBodyGraph() mass = 1.0 I = eigen.Matrix3d.Identity() h = eigen.Vector3d.Zero() rbi = sva.RBInertiad(mass, h, I) b0 = rbdyn.Body(rbi, "b0") mbg.addBody(b0) mb = mbg.makeMultiBody("b0", True) mbc = rbdyn.MultiBodyConfig(mb) mbc.zero(mb) return mb, mbc
def makeZXZArm(isFixed=True, X_base=sva.PTransformd.Identity()): mbg = rbdyn.MultiBodyGraph() mass = 1.0 I = eigen.Matrix3d.Identity() h = eigen.Vector3d.Zero() rbi = sva.RBInertiad(mass, h, I) b0 = rbdyn.Body(rbi, "b0") b1 = rbdyn.Body(rbi, "b1") b2 = rbdyn.Body(rbi, "b2") b3 = rbdyn.Body(rbi, "b3") mbg.addBody(b0) mbg.addBody(b1) mbg.addBody(b2) mbg.addBody(b3) j0 = rbdyn.Joint(rbdyn.Joint.Rev, eigen.Vector3d.UnitZ(), True, "j0") j1 = rbdyn.Joint(rbdyn.Joint.Rev, eigen.Vector3d.UnitX(), True, "j1") j2 = rbdyn.Joint(rbdyn.Joint.Rev, eigen.Vector3d.UnitZ(), True, "j2") mbg.addJoint(j0) mbg.addJoint(j1) mbg.addJoint(j2) to = sva.PTransformd(eigen.Vector3d(0, 0.5, 0)) _from = sva.PTransformd(eigen.Vector3d(0, 0, 0)) mbg.linkBodies("b0", sva.PTransformd.Identity(), "b1", _from, "j0") mbg.linkBodies("b1", to, "b2", _from, "j1") mbg.linkBodies("b2", to, "b3", _from, "j2") mb = mbg.makeMultiBody("b0", isFixed, X_base) mbc = rbdyn.MultiBodyConfig(mb) mbc.zero(mb) return mb, mbc
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)
#!/usr/bin/env python # -*- coding: utf-8 -*- import eigen import sva import rbdyn mbg = rbdyn.MultiBodyGraph() mass = 1.0 I = eigen.Matrix3d.Identity() h = eigen.Vector3d.Zero() rbi = sva.RBInertiad(mass, h, I) b0 = rbdyn.Body(rbi, "b0") b1 = rbdyn.Body(rbi, "b1") b2 = rbdyn.Body(rbi, "b2") b3 = rbdyn.Body(rbi, "b3") mbg.addBody(b0) mbg.addBody(b1) mbg.addBody(b2) mbg.addBody(b3) j0 = rbdyn.Joint(rbdyn.Joint.Rev, eigen.Vector3d.UnitX(), True, "j0") j1 = rbdyn.Joint(rbdyn.Joint.Rev, eigen.Vector3d.UnitX(), True, "j1") j2 = rbdyn.Joint(rbdyn.Joint.Rev, eigen.Vector3d.UnitX(), True, "j2") mbg.addJoint(j0) mbg.addJoint(j1)
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)
def makeBody(bName): I = sva.RBInertiad(e3.Vector3d.Random().x(), e3.Vector3d.Random(), e3.Matrix3d.Random()) return rbd.Body(I, bName)
def TutorialTree(): """ Return the MultiBodyGraph, MultiBody and the zeroed MultiBodyConfig with the following tree structure: b4 j3 | Spherical Root j0 | j1 j2 j4 ---- b0 ---- b1 ---- b2 ----b3 ----b5 Fixed RevX RevY RevZ PrismZ """ mbg = rbd.MultiBodyGraph() mass = 1. I = e.Matrix3d.Identity() h = e.Vector3d.Zero() rbi = sva.RBInertiad(mass, h, I) b0 = rbd.Body(rbi, "b0") b1 = rbd.Body(rbi, "b1") b2 = rbd.Body(rbi, "b2") b3 = rbd.Body(rbi, "b3") b4 = rbd.Body(rbi, "b4") b5 = rbd.Body(rbi, "b5") mbg.addBody(b0) mbg.addBody(b1) mbg.addBody(b2) mbg.addBody(b3) mbg.addBody(b4) mbg.addBody(b5) j0 = rbd.Joint(rbd.Joint.Rev, e.Vector3d.UnitX(), True, "j0") j1 = rbd.Joint(rbd.Joint.Rev, e.Vector3d.UnitY(), True, "j1") j2 = rbd.Joint(rbd.Joint.Rev, e.Vector3d.UnitZ(), True, "j2") j3 = rbd.Joint(rbd.Joint.Spherical, True, "j3") j4 = rbd.Joint(rbd.Joint.Prism, e.Vector3d.UnitY(), True, "j4") mbg.addJoint(j0) mbg.addJoint(j1) mbg.addJoint(j2) mbg.addJoint(j3) mbg.addJoint(j4) to = sva.PTransformd(e.Vector3d(0., 0.5, 0.)) fro = sva.PTransformd.Identity() mbg.linkBodies("b0", to, "b1", fro, "j0") mbg.linkBodies("b1", to, "b2", fro, "j1") mbg.linkBodies("b2", to, "b3", fro, "j2") mbg.linkBodies("b1", sva.PTransformd(e.Vector3d(0.5, 0., 0.)), "b4", fro, "j3") mbg.linkBodies("b3", to, "b5", fro, "j4") mb = mbg.makeMultiBody("b0", True) mbc = rbd.MultiBodyConfig(mb) mbc.zero(mb) return mbg, mb, mbc