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 run_callback(self):
hj1_q = self.robot().mbc.q[self.hj1_index][0]
self.v3d_data = eigen.Vector3d.Random()
self.d_data += 1.0
self.dv_data = [self.d_data] * 3
self.theta += 0.005
self.quat_data = eigen.Quaterniond(sva.RotZ(self.theta))
return True
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 run_callback(self):
hj1_q = self.robot().mbc.q[self.hj1_index][0]
self.v3d_data = eigen.Vector3d.Random()
self.d_data += 1.0
self.dv_data = [self.d_data] * 3
self.theta += 0.005
self.quat_data = eigen.Quaterniond(sva.RotZ(self.theta))
assert (self.observerPipeline("FirstPipeline").success())
if abs(self.d_data - 2.0) < 1e-6:
self.removeAnchorFrameCallback("KinematicAnchorFrame::{}".format(
self.robot().name()))
self.addAnchorFrameCallback(
"KinematicAnchorFrame::{}".format(self.robot().name()),
self.anchorFrameCallback)
return True
def reset_callback(self, data):
self.comTask.reset()
self.robots().robot(1).posW(
sva.PTransformd(sva.RotZ(math.pi), eigen.Vector3d(0.7, 0.5, 0)))
self.doorKinematics = mc_solver.KinematicsConstraint(
self.robots(), 1, self.qpsolver.timeStep)
self.qpsolver.addConstraintSet(self.doorKinematics)
self.doorPosture = mc_tasks.PostureTask(self.qpsolver, 1, 5.0, 1000.0)
self.qpsolver.addTask(self.doorPosture)
self.handTask = mc_tasks.SurfaceTransformTask("RightGripper",
self.robots(), 0, 5.0,
1000.0)
self.qpsolver.addTask(self.handTask)
self.handTask.target(
sva.PTransformd(eigen.Vector3d(0, 0, -0.025)) *
self.robots().robot(1).surfacePose("Handle"))
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 __init__(self, rm, dt):
self.qpsolver.addConstraintSet(self.dynamicsConstraint)
self.qpsolver.addConstraintSet(self.contactConstraint)
self.qpsolver.addTask(self.postureTask)
self.positionTask = mc_tasks.PositionTask("R_WRIST_Y_S", self.robots(),
0)
self.qpsolver.addTask(self.positionTask)
self.qpsolver.setContacts([
mc_rbdyn.Contact(self.robots(), "LeftFoot", "AllGround"),
mc_rbdyn.Contact(self.robots(), "RightFoot", "AllGround")
])
self.hj1_name = "NECK_P"
self.hj1_index = self.robot().jointIndexByName(self.hj1_name)
self.hj1_q = 0.5
# Stuff to log
self.v3d_data = eigen.Vector3d.Random()
self.logger().addLogEntry("PYTHONV3D", lambda: self.v3d_data)
self.d_data = 0.0
self.dv_data = [self.d_data] * 3
self.theta = 0
self.quat_data = eigen.Quaterniond(sva.RotZ(self.theta))
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)