def test(self):
mb1, mbc1Init = arms.makeZXZArm(True, sva.PTransformd(sva.RotZ(-math.pi/4), eigen.Vector3d(-0.5, 0, 0)))
rbdyn.forwardKinematics(mb1, mbc1Init)
rbdyn.forwardVelocity(mb1, mbc1Init)
mb2, mbc2Init = arms.makeZXZArm(False, sva.PTransformd(sva.RotZ(math.pi/2), eigen.Vector3d(0.5, 0, 0)))
rbdyn.forwardKinematics(mb2, mbc2Init)
rbdyn.forwardVelocity(mb2, mbc2Init)
if not LEGACY:
mbs = rbdyn.MultiBodyVector([mb1, mb2])
mbcs = rbdyn.MultiBodyConfigVector([mbc1Init, mbc2Init])
else:
mbs = [mb1, mb2]
mbcs = [rbdyn.MultiBodyConfig(mbc1Init), rbdyn.MultiBodyConfig(mbc2Init)]
solver = tasks.qp.QPSolver()
if not LEGACY:
posture1Task = tasks.qp.PostureTask(mbs, 0, mbc1Init.q, 0.1, 10)
posture2Task = tasks.qp.PostureTask(mbs, 1, mbc2Init.q, 0.1, 10)
else:
posture1Task = tasks.qp.PostureTask(mbs, 0, rbdList(mbc1Init.q), 2, 1)
posture2Task = tasks.qp.PostureTask(mbs, 1, rbdList(mbc2Init.q), 2, 1)
mrtt = tasks.qp.MultiRobotTransformTask(mbs, 0, 1, "b3", "b3", sva.PTransformd(sva.RotZ(-math.pi/8)), sva.PTransformd.Identity(), 100, 1000)
if not LEGACY:
mrtt.dimWeight(eigen.VectorXd(0, 0, 1, 1, 1, 0))
else:
mrtt.dimWeight(eigen.Vector6d(0, 0, 1, 1, 1, 0))
solver.addTask(posture1Task)
solver.addTask(posture2Task)
solver.addTask(mrtt)
solver.nrVars(mbs, [], [])
solver.updateConstrSize()
# 3 dof + 9 dof
self.assertEqual(solver.nrVars(), 3 + 9)
for i in range(2000):
if not LEGACY:
self.assertTrue(solver.solve(mbs, mbcs))
else:
self.assertTrue(solver.solveNoMbcUpdate(mbs, mbcs))
solver.updateMbc(mbcs[0], 0)
solver.updateMbc(mbcs[1], 1)
for i in range(2):
rbdyn.eulerIntegration(mbs[i], mbcs[i], 0.001)
rbdyn.forwardKinematics(mbs[i], mbcs[i])
rbdyn.forwardVelocity(mbs[i], mbcs[i])
self.assertAlmostEqual(mrtt.eval().norm(), 0, delta = 1e-3)
solver.removeTask(posture1Task)
solver.removeTask(posture2Task)
solver.removeTask(mrtt)
def ReemC():
"""
Return the ReemC MultiBodyGraph, MultiBody, the zeroed MultiBodyConfig
and the mesh dictionary.
"""
rbd.copy_reg_pickle()
reemcDataPath = os.path.join(os.path.dirname(__file__), 'reemc_data')
filePath = os.path.join(reemcDataPath, 'reemc_full.pkl')
with open(filePath, 'r') as reemCData:
mb, meshDict = pickle.load(reemCData)
mbc = rbd.MultiBodyConfig(mb)
mbc.zero(mb)
mbg = mbgFromMb(mb)
def fixMeshPath(path):
return os.path.join(reemcDataPath, path)
meshDictFixPath = dict(
map(
lambda (key, (path, X, scale)):
(key, (fixMeshPath(path), X, scale)), meshDict.items()))
return mbg, mb, mbc, meshDictFixPath
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 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
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)
mbc = rbdyn.MultiBodyConfig(mb)
mbc.zero(mb)
rbdyn.forwardKinematics(mb, mbc)
print(mbc.bodyPosW[-1])
def test(self):
mb1, mbc1Init = arms.makeZXZArm(
True, sva.PTransformd(eigen.Vector3d(-0.5, 0, 0)))
rbdyn.forwardKinematics(mb1, mbc1Init)
rbdyn.forwardVelocity(mb1, mbc1Init)
mb2, mbc2Init = arms.makeZXZArm(
True, sva.PTransformd(eigen.Vector3d(0.5, 0, 0)))
rbdyn.forwardKinematics(mb2, mbc2Init)
rbdyn.forwardVelocity(mb2, mbc2Init)
if not LEGACY:
X_0_b1 = sva.PTransformd(mbc1Init.bodyPosW[-1])
X_0_b2 = sva.PTransformd(mbc2Init.bodyPosW[-1])
else:
X_0_b1 = sva.PTransformd(list(mbc1Init.bodyPosW)[-1])
X_0_b2 = sva.PTransformd(list(mbc2Init.bodyPosW)[-1])
X_b1_b2 = X_0_b2 * X_0_b1.inv()
if not LEGACY:
mbs = rbdyn.MultiBodyVector([mb1, mb2])
mbcs = rbdyn.MultiBodyConfigVector([mbc1Init, mbc2Init])
else:
mbs = [mb1, mb2]
mbcs = [
rbdyn.MultiBodyConfig(mbc1Init),
rbdyn.MultiBodyConfig(mbc2Init)
]
nrGen = 3
solver = tasks.qp.QPSolver()
contVec = [
tasks.qp.UnilateralContact(0, 1, "b3", "b3",
[eigen.Vector3d.Zero()],
sva.RotX(math.pi / 2), X_b1_b2, nrGen,
0.7)
]
if not LEGACY:
posture1Task = tasks.qp.PostureTask(mbs, 0, mbc1Init.q, 2, 1)
posture2Task = tasks.qp.PostureTask(mbs, 1, mbc2Init.q, 2, 1)
else:
posture1Task = tasks.qp.PostureTask(mbs, 0, rbdList(mbc1Init.q), 2,
1)
posture2Task = tasks.qp.PostureTask(mbs, 1, rbdList(mbc2Init.q), 2,
1)
comD = (rbdyn.computeCoM(mb1, mbc1Init) + rbdyn.computeCoM(
mb2, mbc2Init)) / 2 + eigen.Vector3d(0, 0, 0.5)
multiCoM = tasks.qp.MultiCoMTask(mbs, [0, 1], comD, 10, 500)
multiCoM.updateInertialParameters(mbs)
contCstrSpeed = tasks.qp.ContactSpeedConstr(0.001)
solver.addTask(posture1Task)
solver.addTask(posture2Task)
solver.nrVars(mbs, contVec, [])
solver.addTask(mbs, multiCoM)
contCstrSpeed.addToSolver(mbs, solver)
solver.updateConstrSize()
self.assertEqual(solver.nrVars(), 3 + 3 + 1 * nrGen)
for i in range(2000):
if not LEGACY:
self.assertTrue(solver.solve(mbs, mbcs))
else:
self.assertTrue(solver.solveNoMbcUpdate(mbs, mbcs))
solver.updateMbc(mbcs[0], 0)
solver.updateMbc(mbcs[1], 1)
for i in range(2):
rbdyn.eulerIntegration(mbs[i], mbcs[i], 0.001)
rbdyn.forwardKinematics(mbs[i], mbcs[i])
rbdyn.forwardVelocity(mbs[i], mbcs[i])
# Check that the link hold
if not LEGACY:
X_0_b1_post = mbcs[0].bodyPosW[-1]
X_0_b2_post = mbcs[1].bodyPosW[-1]
else:
X_0_b1_post = list(mbcs[0].bodyPosW)[-1]
X_0_b2_post = list(mbcs[1].bodyPosW)[-1]
X_b1_b2_post = X_0_b2 * X_0_b1.inv()
self.assertAlmostEqual(
(X_b1_b2.matrix() - X_b1_b2_post.matrix()).norm(),
0,
delta=1e-5)
self.assertAlmostEqual(multiCoM.speed().norm(), 0, delta=1e-3)
contCstrSpeed.removeFromSolver(solver)
solver.removeTask(posture1Task)
solver.removeTask(posture2Task)
solver.removeTask(multiCoM)
def test(self):
mb1, mbc1Init = arms.makeZXZArm()
mb2, mbc2Init = arms.makeZXZArm()
rbdyn.forwardKinematics(mb1, mbc1Init)
rbdyn.forwardVelocity(mb1, mbc1Init)
rbdyn.forwardKinematics(mb2, mbc2Init)
rbdyn.forwardVelocity(mb2, mbc2Init)
if not LEGACY:
X_0_b1 = sva.PTransformd(mbc1Init.bodyPosW[-1])
X_0_b2 = sva.PTransformd(mbc2Init.bodyPosW[-1])
else:
X_0_b1 = sva.PTransformd(list(mbc1Init.bodyPosW)[-1])
X_0_b2 = sva.PTransformd(list(mbc2Init.bodyPosW)[-1])
X_b1_b2 = X_0_b2 * X_0_b1.inv()
if not LEGACY:
mbs = rbdyn.MultiBodyVector([mb1, mb2])
mbcs = rbdyn.MultiBodyConfigVector([mbc1Init, mbc2Init])
else:
mbs = [mb1, mb2]
mbcs = [
rbdyn.MultiBodyConfig(mbc1Init),
rbdyn.MultiBodyConfig(mbc2Init)
]
# Test ContactAccConstr contraint and test PositionTask on the second robot
solver = tasks.qp.QPSolver()
contVec = [
tasks.qp.UnilateralContact(0, 1, "b3", "b3",
[eigen.Vector3d.Zero()],
sva.RotX(math.pi / 2), X_b1_b2, 3,
math.tan(math.pi / 4))
]
oriD = sva.RotZ(math.pi / 4)
if not LEGACY:
posD = oriD * mbc2Init.bodyPosW[-1].translation()
else:
posD = oriD * list(mbc2Init.bodyPosW)[-1].translation()
posTask = tasks.qp.PositionTask(mbs, 1, "b3", posD)
posTaskSp = tasks.qp.SetPointTask(mbs, 1, posTask, 1000, 1)
contCstrAcc = tasks.qp.ContactAccConstr()
contCstrAcc.addToSolver(solver)
solver.addTask(posTaskSp)
solver.nrVars(mbs, contVec, [])
solver.updateConstrSize()
self.assertEqual(solver.nrVars(), 3 + 3 + 3)
for i in range(1000):
if not LEGACY:
self.assertTrue(solver.solve(mbs, mbcs))
else:
self.assertTrue(solver.solveNoMbcUpdate(mbs, mbcs))
solver.updateMbc(mbcs[0], 0)
solver.updateMbc(mbcs[1], 1)
for i in range(2):
rbdyn.eulerIntegration(mbs[i], mbcs[i], 0.001)
rbdyn.forwardKinematics(mbs[i], mbcs[i])
rbdyn.forwardVelocity(mbs[i], mbcs[i])
# Check that the link hold
if not LEGACY:
X_0_b1_post = mbcs[0].bodyPosW[-1]
X_0_b2_post = mbcs[1].bodyPosW[-1]
else:
X_0_b1_post = list(mbcs[0].bodyPosW)[-1]
X_0_b2_post = list(mbcs[1].bodyPosW)[-1]
X_b1_b2_post = X_0_b2 * X_0_b1.inv()
self.assertAlmostEqual(
(X_b1_b2.matrix() - X_b1_b2_post.matrix()).norm(),
0,
delta=1e-5)
self.assertAlmostEqual(posTask.eval().norm(), 0, delta=1e-5)
contCstrAcc.removeFromSolver(solver)
solver.removeTask(posTaskSp)
# Test ContactSpeedConstr constraint and OrientationTask on the second robot
if not LEGACY:
mbcs = rbdyn.MultiBodyConfigVector([mbc1Init, mbc2Init])
else:
mbcs = [
rbdyn.MultiBodyConfig(mbc1Init),
rbdyn.MultiBodyConfig(mbc2Init)
]
oriTask = tasks.qp.OrientationTask(mbs, 1, "b3", oriD)
oriTaskSp = tasks.qp.SetPointTask(mbs, 1, oriTask, 1000, 1)
contCstrSpeed = tasks.qp.ContactSpeedConstr(0.001)
contCstrSpeed.addToSolver(solver)
solver.addTask(oriTaskSp)
solver.nrVars(mbs, contVec, [])
solver.updateConstrSize()
for i in range(1000):
if not LEGACY:
self.assertTrue(solver.solve(mbs, mbcs))
else:
self.assertTrue(solver.solveNoMbcUpdate(mbs, mbcs))
solver.updateMbc(mbcs[0], 0)
solver.updateMbc(mbcs[1], 1)
for i in range(2):
rbdyn.eulerIntegration(mbs[i], mbcs[i], 0.001)
rbdyn.forwardKinematics(mbs[i], mbcs[i])
rbdyn.forwardVelocity(mbs[i], mbcs[i])
# Check that the link hold
if not LEGACY:
X_0_b1_post = mbcs[0].bodyPosW[-1]
X_0_b2_post = mbcs[1].bodyPosW[-1]
else:
X_0_b1_post = list(mbcs[0].bodyPosW)[-1]
X_0_b2_post = list(mbcs[1].bodyPosW)[-1]
X_b1_b2_post = X_0_b2 * X_0_b1.inv()
self.assertAlmostEqual(
(X_b1_b2.matrix() - X_b1_b2_post.matrix()).norm(),
0,
delta=1e-5)
self.assertAlmostEqual(oriTask.eval().norm(), 0, delta=1e-5)
def test(self):
mb1, mbc1Init = arms.makeZXZArm()
rbdyn.forwardKinematics(mb1, mbc1Init)
rbdyn.forwardVelocity(mb1, mbc1Init)
mb2, mbc2Init = arms.makeZXZArm(False)
if not LEGACY:
mb2InitPos = mbc1Init.bodyPosW[-1].translation()
else:
mb2InitPos = list(mbc1Init.bodyPosW)[-1].translation()
mb2InitOri = eigen.Quaterniond(sva.RotY(math.pi / 2))
if not LEGACY:
mbc2Init.q[0] = [
mb2InitOri.w(),
mb2InitOri.x(),
mb2InitOri.y(),
mb2InitOri.z(),
mb2InitPos.x(),
mb2InitPos.y() + 1,
mb2InitPos.z()
]
mbc2Init.q[0] = [
mb2InitOri.w(),
mb2InitOri.x(),
mb2InitOri.y(),
mb2InitOri.z(),
mb2InitPos.x(),
mb2InitPos.y() + 1,
mb2InitPos.z()
]
rbdyn.forwardKinematics(mb2, mbc2Init)
rbdyn.forwardVelocity(mb2, mbc2Init)
if not LEGACY:
X_0_b1 = sva.PTransformd(mbc1Init.bodyPosW[-1])
X_0_b2 = sva.PTransformd(mbc2Init.bodyPosW[-1])
else:
X_0_b1 = sva.PTransformd(list(mbc1Init.bodyPosW)[-1])
X_0_b2 = sva.PTransformd(list(mbc2Init.bodyPosW)[-1])
X_b1_b2 = X_0_b2 * X_0_b1.inv()
if not LEGACY:
mbs = rbdyn.MultiBodyVector([mb1, mb2])
mbcs = rbdyn.MultiBodyConfigVector([mbc1Init, mbc2Init])
else:
mbs = [mb1, mb2]
mbcs = [
rbdyn.MultiBodyConfig(mbc1Init),
rbdyn.MultiBodyConfig(mbc2Init)
]
# Test ContactAccConstr constraint and PositionTask on the second robot
solver = tasks.qp.QPSolver()
points = [
eigen.Vector3d(0.1, 0, 0.1),
eigen.Vector3d(0.1, 0, -0.1),
eigen.Vector3d(-0.1, 0, -0.1),
eigen.Vector3d(-0.1, 0, 0.1),
]
biPoints = [
eigen.Vector3d.Zero(),
eigen.Vector3d.Zero(),
eigen.Vector3d.Zero(),
eigen.Vector3d.Zero(),
]
nrGen = 4
biFrames = [
sva.RotX(math.pi / 4),
sva.RotX(3 * math.pi / 4),
sva.RotX(math.pi / 4) * sva.RotY(math.pi / 2),
sva.RotX(3 * math.pi / 4) * sva.RotY(math.pi / 2),
]
# The fixed robot can pull the other
contVecFail = [
tasks.qp.UnilateralContact(0, 1, "b3", "b0", points,
sva.RotX(-math.pi / 2), X_b1_b2, nrGen,
0.7)
]
# The fixed robot can push the other
contVec = [
tasks.qp.UnilateralContact(0, 1, "b3", "b0", points,
sva.RotX(math.pi / 2), X_b1_b2, nrGen,
0.7)
]
# The fixed robot has non coplanar force apply on the other
contVecBi = [
tasks.qp.BilateralContact(tasks.qp.ContactId(0, 1, "b3", "b0"),
biPoints, biFrames, X_b1_b2, nrGen, 1)
]
if not LEGACY:
posture1Task = tasks.qp.PostureTask(mbs, 0, mbc1Init.q, 2, 1)
posture2Task = tasks.qp.PostureTask(mbs, 1, mbc2Init.q, 2, 1)
else:
posture1Task = tasks.qp.PostureTask(mbs, 0, rbdList(mbc1Init.q), 2,
1)
posture2Task = tasks.qp.PostureTask(mbs, 1, rbdList(mbc2Init.q), 2,
1)
contCstrSpeed = tasks.qp.ContactSpeedConstr(0.001)
Inf = float("inf")
torqueMin1 = [[], [-Inf], [-Inf], [-Inf]]
torqueMax1 = [[], [Inf], [Inf], [Inf]]
torqueMin2 = [[0, 0, 0, 0, 0, 0], [-Inf], [-Inf], [-Inf]]
torqueMax2 = [[0, 0, 0, 0, 0, 0], [Inf], [Inf], [Inf]]
motion1 = tasks.qp.MotionConstr(
mbs, 0, tasks.TorqueBound(torqueMin1, torqueMax1))
motion2 = tasks.qp.MotionConstr(
mbs, 1, tasks.TorqueBound(torqueMin2, torqueMax2))
plCstr = tasks.qp.PositiveLambda()
motion1.addToSolver(solver)
motion2.addToSolver(solver)
plCstr.addToSolver(solver)
contCstrSpeed.addToSolver(solver)
solver.addTask(posture1Task)
solver.addTask(posture2Task)
# Check the impossible motion
solver.nrVars(mbs, contVecFail, [])
solver.updateConstrSize()
self.assertEqual(solver.nrVars(), 3 + 9 + 4 * nrGen)
self.assertFalse(solver.solve(mbs, mbcs))
# Check the unilateral motion
if not LEGACY:
mbcs = rbdyn.MultiBodyConfigVector([mbc1Init, mbc2Init])
else:
mbcs = [
rbdyn.MultiBodyConfig(mbc1Init),
rbdyn.MultiBodyConfig(mbc2Init)
]
solver.nrVars(mbs, contVec, [])
solver.updateConstrSize()
for i in range(1000):
if not LEGACY:
self.assertTrue(solver.solve(mbs, mbcs))
else:
self.assertTrue(solver.solveNoMbcUpdate(mbs, mbcs))
solver.updateMbc(mbcs[0], 0)
solver.updateMbc(mbcs[1], 1)
for i in range(2):
rbdyn.eulerIntegration(mbs[i], mbcs[i], 0.001)
rbdyn.forwardKinematics(mbs[i], mbcs[i])
rbdyn.forwardVelocity(mbs[i], mbcs[i])
# Check that the link hold
if not LEGACY:
X_0_b1_post = mbcs[0].bodyPosW[-1]
X_0_b2_post = mbcs[1].bodyPosW[-1]
else:
X_0_b1_post = list(mbcs[0].bodyPosW)[-1]
X_0_b2_post = list(mbcs[1].bodyPosW)[-1]
X_b1_b2_post = X_0_b2 * X_0_b1.inv()
self.assertAlmostEqual(
(X_b1_b2.matrix() - X_b1_b2_post.matrix()).norm(),
0,
delta=1e-5)
# Force in the world frame must be the same
f1 = contVec[0].force(solver.lambdaVec(0), contVec[0].r1Cone)
f2 = contVec[0].force(solver.lambdaVec(0), contVec[0].r2Cone)
self.assertAlmostEqual((f1 + f2).norm(), 0, delta=1e-5)
# Check the bilateral motion
if not LEGACY:
mbcs = rbdyn.MultiBodyConfigVector([mbc1Init, mbc2Init])
else:
mbcs = [
rbdyn.MultiBodyConfig(mbc1Init),
rbdyn.MultiBodyConfig(mbc2Init)
]
solver.nrVars(mbs, contVec, [])
solver.updateConstrSize()
self.assertEqual(solver.nrVars(), 3 + 9 + 4 * nrGen)
for i in range(1000):
if not LEGACY:
self.assertTrue(solver.solve(mbs, mbcs))
else:
self.assertTrue(solver.solveNoMbcUpdate(mbs, mbcs))
solver.updateMbc(mbcs[0], 0)
solver.updateMbc(mbcs[1], 1)
for i in range(2):
rbdyn.eulerIntegration(mbs[i], mbcs[i], 0.001)
rbdyn.forwardKinematics(mbs[i], mbcs[i])
rbdyn.forwardVelocity(mbs[i], mbcs[i])
# Check that the link hold
if not LEGACY:
X_0_b1_post = mbcs[0].bodyPosW[-1]
X_0_b2_post = mbcs[1].bodyPosW[-1]
else:
X_0_b1_post = list(mbcs[0].bodyPosW)[-1]
X_0_b2_post = list(mbcs[1].bodyPosW)[-1]
X_b1_b2_post = X_0_b2 * X_0_b1.inv()
self.assertAlmostEqual(
(X_b1_b2.matrix() - X_b1_b2_post.matrix()).norm(),
0,
delta=1e-5)
# Force in the world frame must be the same
f1 = contVec[0].force(solver.lambdaVec(0), contVec[0].r1Cone)
f2 = contVec[0].force(solver.lambdaVec(0), contVec[0].r2Cone)
self.assertAlmostEqual((f1 + f2).norm(), 0, delta=1e-5)
plCstr.removeFromSolver(solver)
motion2.removeFromSolver(solver)
motion1.removeFromSolver(solver)
contCstrSpeed.removeFromSolver(solver)
solver.removeTask(posture1Task)
solver.removeTask(posture2Task)
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