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
