def test(self):
theta2d = eigen.Vector2d.Random() * 10
theta = theta2d[0]
self.assertAlmostEqual(
(sva.RotX(theta) -
eigen.AngleAxisd(-theta, eigen.Vector3d.UnitX()).matrix()).norm(),
0,
delta=TOL)
self.assertAlmostEqual(
(sva.RotY(theta) -
eigen.AngleAxisd(-theta, eigen.Vector3d.UnitY()).matrix()).norm(),
0,
delta=TOL)
self.assertAlmostEqual(
(sva.RotZ(theta) -
eigen.AngleAxisd(-theta, eigen.Vector3d.UnitZ()).matrix()).norm(),
0,
delta=TOL)
def test(self):
res = eigen.Vector3d()
res = sva.rotationError(eigen.Matrix3d.Identity(), sva.RotX(np.pi / 2))
self.assertAlmostEqual((res - eigen.Vector3d(np.pi / 2, 0, 0)).norm(),
0,
delta=TOL)
res = sva.rotationError(eigen.Matrix3d.Identity(), sva.RotY(np.pi / 2))
self.assertAlmostEqual((res - eigen.Vector3d(0, np.pi / 2, 0)).norm(),
0,
delta=TOL)
res = sva.rotationError(eigen.Matrix3d.Identity(), sva.RotZ(np.pi / 2))
self.assertAlmostEqual((res - eigen.Vector3d(0, 0, np.pi / 2)).norm(),
0,
delta=TOL)
res = sva.rotationError(sva.RotZ(np.pi / 4), sva.RotZ(np.pi / 2))
self.assertAlmostEqual((res - eigen.Vector3d(0, 0, np.pi / 4)).norm(),
0,
delta=TOL)
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)
import eigen as e
import sva
from robots import TutorialTree
mbg, mb, mbc = TutorialTree()
q = map(list, mbc.q)
q[1] = [np.pi / 2.]
q[2] = [-np.pi / 4.]
q[3] = [-np.pi / 2.]
q[4] = [0.5]
mbc.q = q
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)
