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.RBInertia(mass, h, I)
b0 = rbd.Body(rbi, 0, "b0")
b1 = rbd.Body(rbi, 1, "b1")
b2 = rbd.Body(rbi, 2, "b2")
b3 = rbd.Body(rbi, 3, "b3")
b4 = rbd.Body(rbi, 4, "b4")
b5 = rbd.Body(rbi, 5, "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, 0, "j0")
j1 = rbd.Joint(rbd.Joint.Rev, e.Vector3d.UnitY(), True, 1, "j1")
j2 = rbd.Joint(rbd.Joint.Rev, e.Vector3d.UnitZ(), True, 2, "j2")
j3 = rbd.Joint(rbd.Joint.Spherical, True, 3, "j3")
j4 = rbd.Joint(rbd.Joint.Prism, e.Vector3d.UnitY(), True, 4, "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(0, to, 1, fro, 0)
mbg.linkBodies(1, to, 2, fro, 1)
mbg.linkBodies(2, to, 3, fro, 2)
mbg.linkBodies(1, sva.PTransformd(e.Vector3d(0.5, 0., 0.)), 4, fro, 3)
mbg.linkBodies(3, to, 5, fro, 4)
mb = mbg.makeMultiBody(0, True)
mbc = rbd.MultiBodyConfig(mb)
mbc.zero(mb)
return mbg, 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
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)
mbg.addJoint(j2)
to = sva.PTransformd(eigen.Vector3d.UnitY())
from_ = sva.PTransformd(eigen.Vector3d.Zero())
mbg.linkBodies("b0", from_, "b1", from_, "j0")
mbg.linkBodies("b1", to, "b2", from_, "j1")
mbg.linkBodies("b2", to, "b3", from_, "j2")
mb = mbg.makeMultiBody("b0", True)
def test(self):
# regiter custom pickle function
rbd.copy_reg_pickle()
# create a body with random inertia
def makeBody(bName):
I = sva.RBInertiad(e3.Vector3d.Random().x(), e3.Vector3d.Random(),
e3.Matrix3d.Random())
return rbd.Body(I, bName)
body = makeBody('testBody')
jR = rbd.Joint(rbd.Joint.Rev,
e3.Vector3d.Random().normalized(), True, 'jR')
jP = rbd.Joint(rbd.Joint.Prism,
e3.Vector3d.Random().normalized(), False, 'jP')
jS = rbd.Joint(rbd.Joint.Spherical, False, 'jS')
jPla = rbd.Joint(rbd.Joint.Planar, True, 'jPla')
jC = rbd.Joint(rbd.Joint.Cylindrical,
e3.Vector3d.Random().normalized(), False, 'jC')
jFree = rbd.Joint(rbd.Joint.Free, True, 'jFree')
jFix = rbd.Joint(rbd.Joint.Fixed, False, 'jFix')
mb = rbd.MultiBody(
[makeBody('body%s' % i)
for i in range(7)], [jR, jP, jS, jPla, jC, jFree, jFix],
list(range(-1, 6)), list(range(0, 7)), list(range(-1, 6)),
[sva.PTransformd(e3.Vector3d(0., i, 0.)) for i in range(7)])
def test(v, func):
pickled = pickle.dumps(v)
v2 = pickle.loads(pickled)
assert (func(v, v2))
def bodyEq(b1, b2):
return b1.inertia() == b2.inertia() and\
b1.name() == b2.name()
def jointEq(j1, j2):
return j1.type() == j2.type() and\
j1.name() == j2.name() and\
j1.direction() == j2.direction() and\
list(j1.motionSubspace()) == list(j2.motionSubspace())
def multiBodyEq(mb1, mb2):
isEq = True
for b1, b2 in zip(mb1.bodies(), mb2.bodies()):
isEq &= bodyEq(b1, b2)
for j1, j2 in zip(mb1.joints(), mb2.joints()):
isEq &= jointEq(j1, j2)
mb1T = (list(mb1.predecessors()), list(mb1.successors()),
list(mb1.parents()), list(mb1.transforms()))
mb2T = (list(mb2.predecessors()), list(mb2.successors()),
list(mb2.parents()), list(mb2.transforms()))
return isEq and mb1T == mb2T
# body
test(body, bodyEq)
# joints
test(jR, jointEq)
test(jP, jointEq)
test(jS, jointEq)
test(jPla, jointEq)
test(jC, jointEq)
test(jFree, jointEq)
test(jFix, jointEq)
# multiBody
test(mb, multiBodyEq)
def createRobot():
mb, mbc, mbg = rbdyn.MultiBody(), rbdyn.MultiBodyConfig(
), rbdyn.MultiBodyGraph()
limits = mc_rbdyn_urdf.Limits()
visual = {}
collision_tf = {}
I0 = eigen.Matrix3d([[0.1, 0.0, 0.0], [0.0, 0.05, 0.0], [0.0, 0.0, 0.001]])
I1 = eigen.Matrix3d([[1.35, 0.0, 0.0], [0.0, 0.05, 0.0], [0.0, 0.0,
1.251]])
I2 = eigen.Matrix3d([[0.6, 0.0, 0.0], [0.0, 0.05, 0.0], [0.0, 0.0, 0.501]])
I3 = eigen.Matrix3d([[0.475, 0.0, 0.0], [0.0, 0.05, 0.0],
[0.0, 0.0, 0.376]])
I4 = eigen.Matrix3d([[0.1, 0.0, 0.0], [0.0, 0.3, 0.0], [0.0, 0.0, 0.251]])
T0 = sva.PTransformd(eigen.Vector3d(0.1, 0.2, 0.3))
T1 = sva.PTransformd.Identity()
b0 = rbdyn.Body(1., eigen.Vector3d.Zero(), I0, "b0")
b1 = rbdyn.Body(5., eigen.Vector3d(0., 0.5, 0.), I1, "b1")
b2 = rbdyn.Body(2., eigen.Vector3d(0., 0.5, 0.), I2, "b2")
b3 = rbdyn.Body(1.5, eigen.Vector3d(0., 0.5, 0.), I3, "b3")
b4 = rbdyn.Body(1., eigen.Vector3d(0.5, 0., 0.), I4, "b4")
mbg.addBody(b0)
mbg.addBody(b1)
mbg.addBody(b2)
mbg.addBody(b3)
mbg.addBody(b4)
j0 = rbdyn.Joint(rbdyn.Joint.Rev, eigen.Vector3d.UnitX(), True, "j0")
j1 = rbdyn.Joint(rbdyn.Joint.Rev, eigen.Vector3d.UnitY(), True, "j1")
j2 = rbdyn.Joint(rbdyn.Joint.Rev, eigen.Vector3d.UnitZ(), True, "j2")
j3 = rbdyn.Joint(rbdyn.Joint.Rev, eigen.Vector3d.UnitX(), True, "j3")
mbg.addJoint(j0)
mbg.addJoint(j1)
mbg.addJoint(j2)
mbg.addJoint(j3)
to = sva.PTransformd(eigen.Vector3d(0, 1, 0))
from_ = sva.PTransformd.Identity()
mbg.linkBodies("b0", to, "b1", from_, "j0")
mbg.linkBodies("b1", to, "b2", from_, "j1")
mbg.linkBodies("b2", to, "b3", from_, "j2")
mbg.linkBodies("b1",
sva.PTransformd(sva.RotX(1.), eigen.Vector3d(1., 0., 0.)),
"b4", from_, "j3")
mb = mbg.makeMultiBody("b0", True)
mbc = rbdyn.MultiBodyConfig(mb)
mbc.zero(mb)
limits.lower = {
"j0": [-1.],
"j1": [-1.],
"j2": [-1.],
"j3": [-float('Inf')]
}
limits.upper = {"j0": [1.], "j1": [1.], "j2": [1.], "j3": [float('Inf')]}
limits.velocity = {
"j0": [10.],
"j1": [10.],
"j2": [10.],
"j3": [float('Inf')]
}
limits.torque = {
"j0": [50.],
"j1": [50.],
"j2": [50.],
"j3": [float('Inf')]
}
v1 = mc_rbdyn_urdf.Visual()
v1.origin = T0
geometry = mc_rbdyn_urdf.Geometry()
geometry.type = mc_rbdyn_urdf.Geometry.MESH
mesh = mc_rbdyn_urdf.GeometryMesh()
mesh.filename = "test_mesh1.dae"
geometry.data = mesh
v1.geometry = geometry
v2 = mc_rbdyn_urdf.Visual()
v2.origin = T1
mesh.filename = "test_mesh2.dae"
geometry.data = mesh
v2.geometry = geometry
visual = {b"b0": [v1, v2]}
return mb, mbc, mbg, limits, visual, collision_tf
import spacevecalg as sva
import rbdyn as rbd
if __name__ == '__main__':
# regiter custom pickle function
rbd.copy_reg_pickle()
# create a body with random inertia
def makeBody(bId, bName):
I = sva.RBInertiad(e3.Vector3d.Random().x(),
e3.Vector3d.Random(), e3.Matrix3d.Random())
return rbd.Body(I, bId, bName)
body = makeBody(4, 'testBody')
jR = rbd.Joint(rbd.Joint.Rev, e3.Vector3d.Random().normalized(), True, 5, 'jR')
jP = rbd.Joint(rbd.Joint.Prism, e3.Vector3d.Random().normalized(),
False, 10, 'jP')
jS = rbd.Joint(rbd.Joint.Spherical, False, 100, 'jS')
jPla = rbd.Joint(rbd.Joint.Planar, True, 0, 'jPla')
jC = rbd.Joint(rbd.Joint.Cylindrical, e3.Vector3d.Random().normalized(),
False, 50, 'jC')
jFree = rbd.Joint(rbd.Joint.Free, True, 2344, 'jFree')
jFix = rbd.Joint(rbd.Joint.Fixed, False, 3998, 'jFix')
mb = rbd.MultiBody([makeBody(i, 'body%s' % i) for i in xrange(7)],
[jR, jP, jS, jPla, jC, jFree, jFix],
range(-1, 6), range(0, 7), range(-1, 6),
[sva.PTransformd(e3.Vector3d(0.,i,0.)) for i in xrange(7)])