def update_kinematics(self, q, dq): """ Update kinematics using values from physics engine @param q A list of joint angles @param dq A list of joint velocities """ self.kine_dyn.mbc.alpha = [ [], [dq[0]], [dq[1]], [dq[2]], [dq[3]], [dq[4]], [dq[5]], [dq[6]], ] self.update_poses(q) rbd.forwardVelocity(self.kine_dyn.mb, self.kine_dyn.mbc)
def update_kinematics(self, q, dq): """ Update kinematics using values from physics engine @param q A list of joint angles @param dq A list of joint velocities """ # self.kine_dyn.mbc.q = [] # self.kine_dyn.mbc.alpha = [] # self.kine_dyn.mbc.q.append([]) # self.kine_dyn.mbc.alpha.append([]) # for i in range(len(q)): # self.kine_dyn.mbc.q.append([q[i]]) # self.kine_dyn.mbc.alpha.append([dq[i]]) self.kine_dyn.mbc.q = [ [], [q[0]], [q[1]], [q[2]], [q[3]], [q[4]], [q[5]], [q[6]], ] self.kine_dyn.mbc.alpha = [ [], [dq[0]], [dq[1]], [dq[2]], [dq[3]], [dq[4]], [dq[5]], [dq[6]], ] # forward kinematics rbd.forwardKinematics(self.kine_dyn.mb, self.kine_dyn.mbc) rbd.forwardVelocity(self.kine_dyn.mb, self.kine_dyn.mbc)
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)
import spacevecalg as sva from ik_tasks import BodyTask, PostureTask, CoMTask from robots import TutorialTree mbg, mb, mbc = TutorialTree() quat = e.Quaterniond(np.pi/3., e.Vector3d(0.1, 0.5, 0.3).normalized()) mbc.q = [[], [3.*np.pi/4.], [np.pi/3.], [-3.*np.pi/4.], [0.], [quat.w(), quat.x(), quat.y(), quat.z()]] rbd.forwardKinematics(mb, mbc) rbd.forwardVelocity(mb, mbc) # target frame X_O_T = sva.PTransformd(sva.RotY(np.pi/2.), e.Vector3d(1.5, 0.5, 1.)) X_b5_ef = sva.PTransformd(sva.RotX(-np.pi/2.), e.Vector3d(0., 0.2, 0.)) # create the task bodyTask = BodyTask(mb, mbg.bodyIdByName("b5"), X_O_T, X_b5_ef) postureTask = PostureTask(mb, map(list, mbc.q)) comTask = CoMTask(mb, rbd.computeCoM(mb, mbc) + e.Vector3d(0., 0.5, 0.)) tasks = [(100., bodyTask), ((0., 10000., 0.), comTask), (1., postureTask)] q_res = None X_O_p_res = None alphaInfList = [] for iterate, q, alpha, alphaInf in\
def display_helper(self): rbd.eulerIntegration(self.hrp4W.mb, self.hrp4W.mbc, 0.005) rbd.forwardKinematics(self.hrp4W.mb, self.hrp4W.mbc) rbd.forwardVelocity(self.hrp4W.mb, self.hrp4W.mbc) rbd.forwardAcceleration(self.hrp4W.mb, self.hrp4W.mbc) self.hrp4WDisplayer.display()
env = robots.robots[env_index] # compute foot position to be in contact with the ground rbd.forwardKinematics(hrp4.mb, hrp4.mbc) tz = -hrp4.surfaces['LeftFoot'].X_0_s(hrp4).translation().z() tx = -hrp4.surfaces['LeftFoot'].X_0_s( hrp4).translation().x() #zero the feet surface for the wPG hrp4_q = rbdList(hrp4.mbc.q) hrp4_q[0] = [1., 0., 0., 0., tx, 0., tz] hrp4.mbc.q = hrp4_q # print len(rbdList(hrp4.mbc.q)) # compute init fk and fv for r in robots.robots: rbd.forwardKinematics(r.mb, r.mbc) rbd.forwardVelocity(r.mb, r.mbc) hrp4Jsp = JointStatePublisher(hrp4) # create stabilizer helper hrp4Stab = stabilizerMsg(hrp4) # create solver qpsolver = MRQPSolver(robots, timeStep) # add dynamics constraint to QPSolver # Use 50% of the velocity limits cf Sebastien Langagne. contactConstraint = ContactConstraint(timeStep, ContactConstraint.Position) # contactConstraint = ContactConstraint(timeStep, ContactConstraint.Velocity) dynamicsConstraint1 = DynamicsConstraint(robots, hrp4_index,
romeo = robots.robots[romeo_index] env = robots.robots[env_index] # compute foot position to be in contact with the ground rbd.forwardKinematics(romeo.mb, romeo.mbc) tz = -romeo.surfaces['Lfoot'].X_0_s(romeo).translation().z() tx = -romeo.surfaces['Lfoot'].X_0_s(romeo).translation().x() #zero the feet surface romeo_q = rbdList(romeo.mbc.q) romeo_q[0] = [1., 0., 0., 0., tx, 0., tz] romeo.mbc.q = romeo_q # compute init fk and fv for r in robots.robots: rbd.forwardKinematics(r.mb, r.mbc) rbd.forwardVelocity(r.mb, r.mbc) romeoJsp = JointStatePublisher(romeo) # create solver qpsolver = MRQPSolver(robots, timeStep) # add dynamics constraint to QPSolver # Use 50% of the velocity limits cf Sebastien Langagne. contactConstraint = ContactConstraint(timeStep, ContactConstraint.Position) dynamicsConstraint1 = DynamicsConstraint(robots, romeo_index, timeStep, damper=(0.1, 0.01, 0.5), velocityPercent=0.5) kinConstraint1 = KinematicsConstraint(robots, romeo_index, timeStep, damper=(0.1, 0.01, 0.5), velocityPercent=0.5) # Self-collision robot
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)
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 if __name__ == "__main__": nrIter = 10000 mb, mbcInit = makeZXZArm() rbdyn.forwardKinematics(mb, mbcInit) rbdyn.forwardVelocity(mb, mbcInit) mbs = rbdyn.MultiBodyVector([mb]) mbcs = rbdyn.MultiBodyConfigVector([mbcInit]) solver = tasks.qp.QPSolver() solver.nrVars(mbs, [], []) solver.updateConstrSize() posD = eigen.Vector3d(0.707106, 0.707106, 0.0) posTask = tasks.qp.PositionTask(mbs, 0, "b3", posD) posTaskSp = tasks.qp.SetPointTask(mbs, 0, posTask, 10, 1) solver.addTask(posTaskSp)
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)
if __name__ == '__main__': import sys sys.path += [".."] import spacevecalg as sva from ik_tasks import BodyTask, PostureTask, CoMTask from robots import TutorialTree mbg, mb, mbc = TutorialTree() quat = e.Quaterniond(np.pi / 3., e.Vector3d(0.1, 0.5, 0.3).normalized()) mbc.q = [[], [3. * np.pi / 4.], [np.pi / 3.], [-3. * np.pi / 4.], [0.], [quat.w(), quat.x(), quat.y(), quat.z()]] rbd.forwardKinematics(mb, mbc) rbd.forwardVelocity(mb, mbc) # target frame X_O_T = sva.PTransformd(sva.RotY(np.pi / 2.), e.Vector3d(1.5, 0.5, 1.)) X_b5_ef = sva.PTransformd(sva.RotX(-np.pi / 2.), e.Vector3d(0., 0.2, 0.)) # create the task bodyTask = BodyTask(mb, mbg.bodyIdByName("b5"), X_O_T, X_b5_ef) postureTask = PostureTask(mb, map(list, mbc.q)) comTask = CoMTask(mb, rbd.computeCoM(mb, mbc) + e.Vector3d(0., 0.5, 0.)) tasks = [(100., bodyTask), ((0., 10000., 0.), comTask), (1., postureTask)] q_res = None X_O_p_res = None alphaInfList = [] for iterate, q, alpha, alphaInf in\