def wait_init_position(self, rs):
if rhPosTask.eval().norm() < 0.1 and rhPosTask.speed().norm() < 0.001 and lhPosTask.eval().norm() < 0.1 and lhPosTask.speed().norm() < 0.001:
# move the x and y location of the CoM target to the left foot center
desired_com = rbd.computeCoM(romeo.mb, romeo.mbc)
lFoot_translation = lFoot.X_0_s(romeo).translation()
desired_com[0] = lFoot_translation[0]
desired_com[1] = lFoot_translation[1]
comTask.com(desired_com)
self.checkSequence()
print 'transferring CoM'
def wait_init_position(self, rs):
# if rhPosTask.eval().norm() < 0.1 and rhPosTask.speed().norm() < 0.001 and lhPosTask.eval().norm() < 0.1 and lhPosTask.speed().norm() < 0.001:
if comTask.eval().norm() < 0.1 and comTask.speed().norm() < 0.001 :
# move the x and y location of the CoM target to the left foot center
desired_com = rbd.computeCoM(romeo.mb, romeo.mbc)
#lFoot_translation = lFoot.X_0_s(romeo).translation()
#desired_com[0] = lFoot_translation[0]
#desired_com[1] = lFoot_translation[1]
comTask.com(desired_com)
self.init_goal_com = comTask.com()
# self.imarker_com = AbstractInteractiveMarker('com_goal', self.init_goal_com)
self.checkSequence()
print 'transferring CoM'
def computeAndFill(self, mb, mbc, zmp_des, com_des, footStep_des):
# update Center of Mass state
comPos = rbd.computeCoM(mb, mbc)
comVel = rbd.computeCoMVelocity(mb, mbc)
comAcc = rbd.computeCoMAcceleration(mb, mbc)
# angular frequency of the Linearized Inverted Pendulum Model
omega = np.sqrt(9.81 / comPos[2])
# Zero Moment Point and Capture Point
zmp = rbd.computeCentroidalZMP(mbc, comPos, comAcc, 0.)
capturePoint = comPos + (comVel / omega)
capturePoint[2] = 0.0
delta_corr = zmp
# fill the markers
self.fill(zmp, zmp_des, comPos, com_des, footStep_des, capturePoint,
delta_corr)
lf_pos_goal = lFoot.X_0_s(hrp4).translation() - lFoot.X_b_s.translation()
# lfPosTask, lfPosTaskTr = positionTrackingTask(robots, hrp4_index, 'l_ankle', lf_pos_goal,
# 5., 5., 100000.)
lfPosTask, lfPosTaskTr = positionTask(robots, hrp4_index, 'l_ankle',
lf_pos_goal, 5., 100000.)
lf_ori_goal = lFoot.X_0_s(hrp4).rotation()
lfOriTask, lfOriTaskSp = orientationTask(robots, hrp4_index, 'l_ankle',
lf_ori_goal, 5., 10000.)
torsoOriTask, torsoOriTaskSp =\
orientationTask(robots, hrp4_index, 'torso', list(hrp4.mbc.bodyPosW)[hrp4.bodyIndexByName('torso')].rotation(), 10., 200.)
# orientationTask(robots, hrp4_index, 'torso', Matrix3d.Identity(), 10., 200.)
# orientationTask(robots, hrp4_index, 'torso', list(hrp4.mbc.bodyPosW)[hrp4.bodyIndexByName('torso')].rotation(), 10., 200.)
compp = rbd.computeCoM(hrp4.mb, hrp4.mbc) + Vector3d(0.0, 0, 0)
compp[0] = -0.003653 + 0.0
comTask, comTaskTr = comTrajectoryTask(robots, hrp4_index, compp, 5., 5.,
10000.)
# allow the CoM to move in the Z direction
comTaskTr.dimWeight(toEigenX(np.mat([1., 1., 0.1]).T))
# allow the torso to rotate about the Z world axis
torsoOriTaskSp.dimWeight(toEigenX(np.mat([1., 1., 0.1]).T))
qpsolver.solver.addTask(rfPosTaskTr)
qpsolver.solver.addTask(rfOriTaskSp)
#
qpsolver.solver.addTask(lfPosTaskTr)
qpsolver.solver.addTask(lfOriTaskSp)
def callWPG(self, qpsolver, comTask, comTaskTr, torsoOriTask, \
rfPosTaskTr, lfPosTaskTr, rfOriTask, lfOriTask, c1L, c1R):
self.RFootHelper=task_playback_helper(self.robot, 'r_ankle',rfPosTaskTr)
self.LFootHelper=task_playback_helper(self.robot, 'l_ankle',lfPosTaskTr)
if len(self.zmpcom)>0:
print '\n ========== wPG iteration ', self.wPG_iters
self.wPG_iters += 1
zmp_com_now = self.zmpcom.pop(0)
# take the computed com state of the QP
comPos = rbd.computeCoM(self.robot.mb, self.robot.mbc)
self.pos_com.write(str(comPos[0])+" "+str(comPos[1])+" "+str(comPos[2])+"\n")
# comVel = rbd.computeCoMVelocity(self.robot.mb, self.robot.mbc)
# update state with walking pattern generator
self.comRefPos = Vector3d(zmp_com_now[4]+0.0, zmp_com_now[5], zmp_com_now[6])
# bu0 = 800
# bu1 = 1400
# bu2 = 1400
# bun = 2885
#
# if self.wPG_iters<=bu0:
# self.comRefPos = Vector3d(zmp_com_now[4]+0.015, zmp_com_now[5], zmp_com_now[6])
# elif self.wPG_iters<=bu1:
# self.comRefPos = Vector3d(zmp_com_now[4]+0.015-(self.wPG_iters-bu0)*(0.005/(bu1-bu0)), zmp_com_now[5], zmp_com_now[6])
# elif self.wPG_iters<=bu2:
# self.comRefPos = Vector3d(zmp_com_now[4]+0.01, zmp_com_now[5], zmp_com_now[6])
# elif self.wPG_iters<=bun:
# self.comRefPos = Vector3d(zmp_com_now[4]+0.01, zmp_com_now[5], zmp_com_now[6])
self.comRefVel = Vector3d(zmp_com_now[7], zmp_com_now[8], zmp_com_now[9])
self.comRefAccel = Vector3d(zmp_com_now[10], zmp_com_now[11], zmp_com_now[12])
comTask.com(self.comRefPos)
comTaskTr.refVel(toVecX(self.comRefVel))
comTaskTr.refAccel(toVecX(self.comRefAccel))
self.stateType = zmp_com_now[0] #0=TDS, 1=LSS, 2=RSS
self.zmp_des[0] = zmp_com_now[1]
self.zmp_des[1] = zmp_com_now[2]
#TODO: very unsure of this part
if ((self.stateType != self.previousStateType) and (self.previousStateType==0)):
next_pstep=self.pstep.pop(0)
self.nextStepPos = [next_pstep[0], next_pstep[1]]
self.next_step_angle=next_pstep[2]
# prevents a bug caused by the orientation task dimWeight #TODO: fix properly in Tasks self.initAlpha *
torsoOriTask.orientation(sva.RotZ((self.last_rotation_angle_rfoot + self.last_rotation_angle_lfoot)/2.))
# double support
if self.stateType==0:
print 'state TDS'
if not (self.previousStateType == 0):
qpsolver.setContacts([c1L, c1R])
qpsolver.update()
print '------------updating contact state'
# left single support
elif self.stateType==1:
self.swingFoot = self.rFoot
print 'state LSS'
if (self.previousStateType == 0):
qpsolver.setContacts([c1L])
qpsolver.update()
print '------------updating contact state'
self.last_rotation_angle_rfoot = self.next_step_angle
# right single support
elif self.stateType==2:
self.swingFoot = self.lFoot
print 'state RSS'
if (self.previousStateType == 0):
qpsolver.setContacts([c1R])
qpsolver.update()
print '------------updating contact state'
self.last_rotation_angle_lfoot = self.next_step_angle
rFootPos=Vector3d(zmp_com_now[22],zmp_com_now[23],zmp_com_now[24])
rFootVel=Vector3d(zmp_com_now[25],zmp_com_now[26],zmp_com_now[27])
rFootAcc=Vector3d(zmp_com_now[28],zmp_com_now[29],zmp_com_now[30])
# rFootVel=Vector3d(0,0,0)
# rFootAcc=Vector3d(0,0,0)
# print self.RFootHelper.bodyState.getPosW()
self.pos_r.write(str(self.RFootHelper.bodyState.getPosW()[0])+" "+str(self.RFootHelper.bodyState.getPosW()[1])+" "+str(self.RFootHelper.bodyState.getPosW()[2])+"\n")
self.RFootHelper.update(rFootPos,rFootVel,rFootAcc)
self.rot_r.write(str(self.RFootHelper.bodyState.getOriW())+"\n")
rfOriTask.orientation(sva.RotX(zmp_com_now[34])*sva.RotY(zmp_com_now[35])*sva.RotZ(zmp_com_now[36]))
lFootPos=Vector3d(zmp_com_now[13],zmp_com_now[14],zmp_com_now[15])
lFootVel=Vector3d(zmp_com_now[16],zmp_com_now[17],zmp_com_now[18])
lFootAcc=Vector3d(zmp_com_now[19],zmp_com_now[20],zmp_com_now[21])
# lFootVel=Vector3d(0,0,0)
# lFootAcc=Vector3d(0,0,0)
# print self.LFootHelper.bodyState.getPosW()
self.pos_l.write(str(self.LFootHelper.bodyState.getPosW()[0])+" "+str(self.LFootHelper.bodyState.getPosW()[1])+" "+str(self.LFootHelper.bodyState.getPosW()[2])+"\n")
self.LFootHelper.update(lFootPos,lFootVel,lFootAcc)
self.rot_l.write(str(self.LFootHelper.bodyState.getOriW())+"\n")
lfOriTask.orientation(sva.RotX(zmp_com_now[31])*sva.RotY(zmp_com_now[32])*sva.RotZ(zmp_com_now[33]))
# used to compare if contact state needs updating
self.previousStateType = self.stateType
# print zmp_com_now[22],zmp_com_now[23],zmp_com_now[24]
# print zmp_com_now[25],zmp_com_now[26],zmp_com_now[27]
# print zmp_com_now[28],zmp_com_now[29],zmp_com_now[30]
#
# print zmp_com_now[13],zmp_com_now[14],zmp_com_now[15]
# print zmp_com_now[16],zmp_com_now[17],zmp_com_now[18]
# print zmp_com_now[19],zmp_com_now[20],zmp_com_now[21]
else:
self.pos_r.write(str(self.RFootHelper.bodyState.getPosW()[0])+" "+str(self.RFootHelper.bodyState.getPosW()[1])+" "+str(self.RFootHelper.bodyState.getPosW()[2])+"\n")
self.pos_l.write(str(self.LFootHelper.bodyState.getPosW()[0])+" "+str(self.LFootHelper.bodyState.getPosW()[1])+" "+str(self.LFootHelper.bodyState.getPosW()[2])+"\n")
self.rot_r.write(str(self.RFootHelper.bodyState.getOriW())+"\n")
self.rot_l.write(str(self.LFootHelper.bodyState.getOriW())+"\n")
self.pos_r.close()
self.pos_l.close()
self.rot_r.close()
self.rot_l.close()
self.pos_com.close()
print 'wPG ended with ', self.wPG_iters - 1, ' iterations'
self.hasEnded = True
rh_pos_goal,
5., 1000., rHand.X_b_s.translation())
rhOriTask, rhOriTaskSp = orientationTask(robots, romeo_index, 'r_wrist', rhand_rotation_goal,
5., 100.)
lhPosTask, lhPosTaskSp = positionTask(robots, romeo_index, 'l_wrist',
lh_pos_goal,
5., 1000., lHand.X_b_s.translation())
lhOriTask, lhOriTaskSp = orientationTask(robots, romeo_index, 'l_wrist', lhand_rotation_goal,
5., 100.)
torsoOriTask, torsoOriTaskSp = orientationTask(robots, romeo_index, 'torso',
Matrix3d.Identity(), 10., 10.)
headOriTask, headOriTaskSp = orientationTask(robots, romeo_index, 'HeadRoll_link',
Matrix3d.Identity(), 10., 10.)
# move the CoM to the center of the support, TODO: remove this if a better half-sitting is available
com_init = rbd.computeCoM(romeo.mb, romeo.mbc)
com_init[0] = 0.
com_init[1] = 0.
comTask, comTaskSp = comTask(robots, romeo_index, com_init,
5., 100000.)
# add tasks to the solver
qpsolver.solver.addTask(rhPosTaskSp)
qpsolver.solver.addTask(rhOriTaskSp)
qpsolver.solver.addTask(lhPosTaskSp)
qpsolver.solver.addTask(lhOriTaskSp)
qpsolver.solver.addTask(torsoOriTaskSp)
qpsolver.solver.addTask(headOriTaskSp)
qpsolver.solver.addTask(comTaskSp)
qpsolver.solver.addTask(postureTask1)
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 run(self, rs):
if self.stopCB is not None and self.stopCB.check():
print 'stopping'
self.stopCB = None
self.isRunning = True
self.hsCB = stopMotion(robots, qpsolver, postureTask1, None,
rbdList(hrp4.mbc.q))
self.fsm = self.waitHS
if self.isRunning:
if not qpsolver.run():
print 'FAIL !!!'
self.isRunning = False
return
curTime = rs.header.stamp
# update the center of mass state
rbd.forwardAcceleration(hrp4.mb, hrp4.mbc)
self.com = rbd.computeCoM(hrp4.mb, hrp4.mbc)
self.comA = rbd.computeCoMAcceleration(hrp4.mb, hrp4.mbc)
if self.fsm == self.wPGiteration:
# Update ZMP to be published
self.zmp_d = Vector3d(self.playbackBridge.zmp_des[0],
self.playbackBridge.zmp_des[1], 0.0)
# markers for debugging the walking pattern generator
if self.isWPGMarkerPublished:
self.zmp_actual = rbd.computeCentroidalZMP(
hrp4.mbc, self.com, self.comA, 0.)
# TODO: use the new API for this!
#compute capture point:
omega = np.sqrt(
9.81 /
self.playbackBridge.robot_params.com_height_)
# omega = np.sqrt(9.81/rbd.computeCoM(hrp4.mb, hrp4.mbc)[2])
comVel = rbd.computeCoMVelocity(hrp4.mb, hrp4.mbc)
capturePoint = self.com + (comVel / omega)
capturePoint[2] = 0.0
# robotH = hrp4
# bodyIdxR = robotH.bodyIndexByName('r_ankle')
# posR=(list(robotH.mbc.bodyPosW)[bodyIdxR]).translation()
# rotR=(list(robotH.mbc.bodyPosW)[bodyIdxR]).rotation()
#
# bodyIdxL = robotH.bodyIndexByName('l_ankle')
# posL=(list(robotH.mbc.bodyPosW)[bodyIdxL]).translation()
# rotL=(list(robotH.mbc.bodyPosW)[bodyIdxL]).rotation()
# walking pattern generator RViZ markers
wpg_markers.fill(
self.zmp_actual, self.zmp_d, self.com,
self.playbackBridge.comRefPos, [
self.playbackBridge.nextStepPos[0],
self.playbackBridge.nextStepPos[1], 0.0
], capturePoint)
wpg_markers.publish()
# Publish all
# hrp4Stab.publishZMPDesired(curTime, self.zmp_d)
hrp4Stab.publish(curTime, self.com, self.comA)
hrp4Jsp.publish(curTime)
qpsolver.send(curTime)
self.fsm(rs)
r1SelfCollisionConstraint.addCollisions(robots, cols)
qpsolver.addConstraintSet(contactConstraint)
qpsolver.addConstraintSet(dynamicsConstraint1)
qpsolver.addConstraintSet(r1SelfCollisionConstraint)
# Useful robot surfaces
rFoot = romeo.surfaces['Rfoot']
lFoot = romeo.surfaces['Lfoot']
# Setting up the tasks
postureTask1 = tasks.qp.PostureTask(robots.mbs, romeo_index,
romeo_q, 0.1, 10.)
torsoOriTask, torsoOriTaskSp = orientationTask(robots, romeo_index, 'torso',
Matrix3d.Identity(), 10., 10.)
comTask, comTaskSp = comTask(robots, romeo_index, rbd.computeCoM(romeo.mb, romeo.mbc),
5., 100000.)
# add tasks to the solver
qpsolver.solver.addTask(torsoOriTaskSp)
qpsolver.solver.addTask(comTaskSp)
qpsolver.solver.addTask(postureTask1)
# setup all
c1L = MRContact(romeo_index, env_index,
lFoot, env.surfaces['AllGround'])
c1R = MRContact(romeo_index, env_index,
rFoot, env.surfaces['AllGround'])
qpsolver.setContacts([c1L, c1R])
qpsolver.update()
def g(self, mb, mbc):
return e.toNumpy(rbd.computeCoM(mb, mbc) - self.com_T)
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)
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\
multiTaskIk(mb, mbc, tasks, delta=1., maxIter=200, prec=1e-8):
q_res = q
alphaInfList.append(alphaInf)
print 'iter number', len(alphaInfList)
print 'last alpha norm', alphaInfList[-1]
print
print 'bodyTask error:', bodyTask.g(mb, mbc).T
print 'postureTask error:', postureTask.g(mb, mbc).T
# 5., 1000., rFoot.X_b_s.translation())
# rfOriTask, rfOriTaskSp = orientationTask(robots, romeo_index, 'r_ankle', rf_ori_goal,
# 5., 100.)
rhOriTask, rhOriTaskSp = orientationTask(robots, romeo_index, 'r_wrist', hand_rotation_goal,
2., 100.)
torsoOriTask, torsoOriTaskSp = orientationTask(robots, romeo_index, 'torso',
Matrix3d.Identity(), 1., 15.) #2., 15.
headOriTask, headOriTaskSp = orientationTask(robots, romeo_index, 'HeadRoll_link',
list(romeo.mbc.bodyPosW)[romeo.bodyIndexByName('HeadRoll_link')].rotation(), 10., 10.)
# comTask, comTaskSp = comTask(robots, romeo_index, rbd.computeCoM(romeo.mb, romeo.mbc),
# 5., 100000.)
# print "COM:" , rbd.computeCoM(romeo.mb, romeo.mbc) # : -0.0482481,-2.00112e-09, 0.694905
comTask, comTaskSp = comTask(robots, romeo_index, Vector3d(-0.02, 0, rbd.computeCoM(romeo.mb, romeo.mbc)[2]),
5., 100000.)
# Set up tasks
# Transformation from r_wrist to hand target
trans = (0.033096434903, 0.0486815138012, 0.0318448350088)
quat = (0.577328318474, 0.0275670521388, 0.110292994864, 0.80855891907)
offset_X_b_s = transform.fromTf(trans, quat)
rhPbvsTask, rhPbvsTaskSp = pbvsTask(robots, romeo_index, 'r_wrist',
sva.PTransformd.Identity(),
1.5, 2500., offset_X_b_s) # 1000
# rhPbvsTask, rhPbvsTaskSp = pbvsTask(robots, romeo_index, 'r_wrist',
# sva.PTransformd.Identity(),
# 5., 1000.)
rhPosTask, rhPosTaskSp = positionTask(robots, romeo_index, 'r_wrist',
def callWPG(self, qpsolver, comTask, comTaskTr, torsoOriTask, \
rfPosTaskTr, lfPosTaskTr, rfOriTask, lfOriTask, c1L, c1R,rs):
# raw_input('wait user input')
# self.LoopControlHelper.update(rs)
if self.wPG_iters == 1:
q0 = rbdList(self.LoopControlHelper.robotW.mbc.q)[0]
quatIMU = Quaterniond(q0[0], q0[1], q0[2], q0[3])
quatIMU.normalize()
self.LoopControlHelper.rotInitIMU = quatIMU.toRotationMatrix()
self.LoopControlHelper.rotInitIMUinv = self.LoopControlHelper.rotInitIMU.inverse(
)
self.RFootHelper = task_playback_helper(self.robot, 'r_ankle',
rfPosTaskTr)
self.LFootHelper = task_playback_helper(self.robot, 'l_ankle',
lfPosTaskTr)
# self.RWFootHelper=task_playback_helper(self.robotW, 'r_ankle',rfPosTaskTr)
# self.LWFootHelper=task_playback_helper(self.robotW, 'l_ankle',lfPosTaskTr)
# print self.LFootHelper.bodyState.getOriW()
# print self.LWFootHelper.bodyState.getOriW()
# self.LoopControlHelper.update(rs)
# print 'delta comVel'
# print self.LoopControlHelper.comVelDesired-self.LoopControlHelper.comVelMesured_
# print 'delta comPos'
# print (self.LoopControlHelper.comPosDesired-(self.LoopControlHelper.footRPosFixed+self.LoopControlHelper.footLPosFixed)/2)-(self.LoopControlHelper.comPosMesured_-(self.LoopControlHelper.footRPosMesured_+self.LoopControlHelper.footLPosMesured_)/2)
# print 'delta zmpPos'
# print (self.LoopControlHelper.zmpPosDesired-(self.LoopControlHelper.footRPosFixed+self.LoopControlHelper.footLPosFixed)/2)-(self.LoopControlHelper.zmpPosMesured_-(self.LoopControlHelper.footRPosMesured_+self.LoopControlHelper.footLPosMesured_)/2)
# if self.IsWalkActiv==False:
# self.LoopControlHelper.update(self.robot,self.robotW,
# self.RWFootHelper,self.LWFootHelper,
# rs.wrench[iR],rs.wrench[iL])
# if self.IsWalkActiv:
# nbiter=160
## nbiter=len(self.zmpcom)>0:
# else:
# nbiter=2000
# if self.wPG_iters<nbiter:
if True:
print '\n ========== wPG iteration ', self.wPG_iters
self.wPG_iters += 1
# take the computed com state of the QP
comPos = rbd.computeCoM(self.robot.mb, self.robot.mbc)
# comVel = rbd.computeCoMVelocity(self.robot.mb, self.robot.mbc)
if hasattr(self.LoopControlHelper, 'footRForceMesured'):
# CheckingFloorContact=sqrt(self.LoopControlHelper.footRForceMesured[0]**2+self.LoopControlHelper.footRForceMesured[1]**2+self.LoopControlHelper.footRForceMesured[2]**2+
# (self.LoopControlHelper.footLForceMesured[0]**2+self.LoopControlHelper.footLForceMesured[1]**2+self.LoopControlHelper.footLForceMesured[2]**2))
CheckingFloorContact = sqrt(
(self.LoopControlHelper.footRForceMesured[0] +
self.LoopControlHelper.footLForceMesured[0])**2 +
(self.LoopControlHelper.footRForceMesured[1] +
self.LoopControlHelper.footLForceMesured[1])**2 +
(self.LoopControlHelper.footRForceMesured[2] +
self.LoopControlHelper.footLForceMesured[2])**2)
if CheckingFloorContact < self.LoopControlHelper.M * 9.81 / 2:
self.IsControlLoopActiv = False
if self.IsDebugWalking:
ktoto = 0.05
maxcounterwalk = 500.0
if self.counterWalk > maxcounterwalk:
self.IsWalkActiv = False
# update state with walking pattern generator
if self.IsControlLoopActiv and not self.IsWalkActiv:
self.comRefPos = self.LoopControlHelper.comPosDesired
self.comRefVel = self.LoopControlHelper.comVelDesired
self.comRefAccel = self.LoopControlHelper.comAccDesired
self.zmp_des[0] = self.LoopControlHelper.zmpPosDesired[0]
self.zmp_des[1] = self.LoopControlHelper.zmpPosDesired[1]
self.stateType = 0
if self.counterWalk > maxcounterwalk:
self.comRefPos[1] = ktoto
self.comRefVel[1] = 0
self.comRefAccel[1] = 0
self.zmp_des[1] = ktoto
elif not self.IsControlLoopActiv and self.IsWalkActiv:
# # update state with walking pattern generator
self.comRefPos = self.LoopControlHelper.comPosDesired
self.comRefVel = self.LoopControlHelper.comVelDesired
self.comRefAccel = self.LoopControlHelper.comAccDesired
self.zmp_des[0] = self.LoopControlHelper.zmpPosDesired[0]
self.zmp_des[1] = self.LoopControlHelper.zmpPosDesired[1]
self.stateType = 0
if self.counterWalk is None:
self.counterWalk = 1
if self.counterWalk <= maxcounterwalk:
self.comRefPos[1] = (
1 - cos(self.counterWalk / maxcounterwalk *
np.pi)) / 2 * ktoto
self.comRefVel[1] = (sin(
self.counterWalk / maxcounterwalk *
np.pi)) / 2 * ktoto * np.pi / maxcounterwalk * 0
self.comRefAccel[1] = (cos(
self.counterWalk / maxcounterwalk *
np.pi)) / 2 * ktoto * (np.pi / maxcounterwalk) * (
np.pi / maxcounterwalk) * 0
self.zmp_des[1] = -self.comRefAccel[
1] * 0.78 / 9.81 + self.comRefPos[1]
self.counterWalk += 1
self.LoopControlHelper.comPosDesired[
1] = self.comRefPos[1]
self.LoopControlHelper.zmpPosDesired[1] = self.zmp_des[
1]
elif self.counterWalk > maxcounterwalk:
self.comRefPos[1] = ktoto
self.comRefVel[1] = 0
self.comRefAccel[1] = 0
self.zmp_des[1] = ktoto
self.LoopControlHelper.comPosDesired[
1] = self.comRefPos[1]
self.LoopControlHelper.zmpPosDesired[1] = self.zmp_des[
1]
elif self.IsWalkActiv:
self.comRefPos = self.LoopControlHelper.comPosDesired
self.comRefVel = self.LoopControlHelper.comVelDesired
self.comRefAccel = self.LoopControlHelper.comAccDesired
self.zmp_des[0] = self.LoopControlHelper.zmpPosDesired[0]
self.zmp_des[1] = self.LoopControlHelper.zmpPosDesired[1]
self.stateType = 0
if self.counterWalk is None:
self.counterWalk = 1
if self.counterWalk <= maxcounterwalk:
self.comRefPos[1] = (
1 - cos(self.counterWalk / maxcounterwalk *
np.pi)) / 2 * ktoto
self.comRefVel[1] = (sin(
self.counterWalk / maxcounterwalk *
np.pi)) / 2 * ktoto * np.pi / maxcounterwalk * 0
self.comRefAccel[1] = (cos(
self.counterWalk / maxcounterwalk *
np.pi)) / 2 * ktoto * (np.pi / maxcounterwalk) * (
np.pi / maxcounterwalk) * 0
self.zmp_des[1] = -self.comRefAccel[
1] * 0.78 / 9.81 + self.comRefPos[1]
self.counterWalk += 1
self.LoopControlHelper.comPosDesired[
1] = self.comRefPos[1]
self.LoopControlHelper.zmpPosDesired[1] = self.zmp_des[
1]
elif self.counterWalk > maxcounterwalk:
self.comRefPos[1] = ktoto
self.comRefVel[1] = 0
self.comRefAccel[1] = 0
self.zmp_des[1] = ktoto
self.LoopControlHelper.comPosDesired[
1] = self.comRefPos[1]
self.LoopControlHelper.zmpPosDesired[1] = self.zmp_des[
1]
else:
self.comRefPos = self.LoopControlHelper.comPosDesired
self.comRefVel = self.LoopControlHelper.comVelDesired
self.comRefAccel = self.LoopControlHelper.comAccDesired
self.zmp_des[0] = self.LoopControlHelper.zmpPosDesired[0]
self.zmp_des[1] = self.LoopControlHelper.zmpPosDesired[1]
self.stateType = 0
if self.counterWalk > maxcounterwalk:
self.comRefPos[1] = ktoto
self.comRefVel[1] = 0
self.comRefAccel[1] = 0
self.zmp_des[1] = ktoto
self.LoopControlHelper.comPosDesired[
1] = self.comRefPos[1]
self.LoopControlHelper.zmpPosDesired[1] = self.zmp_des[
1]
else:
ktoto = 0.0
maxcounterwalk = 500.0
if self.IsWalkActiv and len(self.zmpcom) > 0:
zmp_com_now = self.zmpcom.pop(0)
else:
self.IsWalkActiv = False
# update state with walking pattern generator
if self.IsControlLoopActiv and not self.IsWalkActiv:
# self.comRefPos = self.LoopControlHelper.comPosWanted
# self.comRefVel = self.LoopControlHelper.comVelWanted
# self.comRefAccel = self.LoopControlHelper.comAccWanted
self.comRefPos = self.LoopControlHelper.comPosDesired + Vector3d(
ktoto, 0, 0)
self.LoopControlHelper.comVelDesired = Vector3d().Zero()
self.LoopControlHelper.comAccDesire = Vector3d().Zero()
self.comRefVel = self.LoopControlHelper.comVelDesired
self.comRefAccel = self.LoopControlHelper.comAccDesired
# self.zmp_des[0] = self.LoopControlHelper.zmpPosWanted[0]
# self.zmp_des[1] = self.LoopControlHelper.zmpPosWanted[1]
# self.zmp_des[0] = self.LoopControlHelper.zmpPosDesired[0]+ktoto
# self.zmp_des[1] = self.LoopControlHelper.zmpPosDesired[1]
self.zmp_des[0] = self.comRefPos[0]
self.zmp_des[1] = self.comRefPos[1]
# self.stateType = 0
elif self.IsControlLoopActiv and self.IsWalkActiv:
# update state with walking pattern generator
self.LoopControlHelper.comPosDesired = Vector3d(
zmp_com_now[4] + ktoto, zmp_com_now[5], 0.780678)
self.LoopControlHelper.comVelDesired = Vector3d(
zmp_com_now[7], zmp_com_now[8], zmp_com_now[9])
self.LoopControlHelper.comAccDesired = Vector3d(
zmp_com_now[10], zmp_com_now[11], zmp_com_now[12])
self.LoopControlHelper.previousStateType = self.stateType
self.LoopControlHelper.stateType = zmp_com_now[0]
self.LoopControlHelper.zmpPosDesired[
0] = zmp_com_now[1] + ktoto
self.LoopControlHelper.zmpPosDesired[1] = zmp_com_now[2]
self.comRefPos = Vector3d(zmp_com_now[4] + ktoto,
zmp_com_now[5], 0.780678)
self.comRefVel = Vector3d(zmp_com_now[7], zmp_com_now[8],
zmp_com_now[9])
self.comRefAccel = Vector3d(zmp_com_now[10],
zmp_com_now[11],
zmp_com_now[12])
self.previousStateType = self.stateType
self.stateType = zmp_com_now[0] #0=TDS, 1=LSS, 2=RSS
self.zmp_des[0] = zmp_com_now[1] + ktoto
self.zmp_des[1] = zmp_com_now[2]
elif self.IsWalkActiv:
self.LoopControlHelper.comPosDesired = Vector3d(
zmp_com_now[4] + ktoto, zmp_com_now[5], 0.780678)
self.LoopControlHelper.comVelDesired = Vector3d(
zmp_com_now[7], zmp_com_now[8], zmp_com_now[9])
self.LoopControlHelper.comAccDesired = Vector3d(
zmp_com_now[10], zmp_com_now[11], zmp_com_now[12])
self.LoopControlHelper.previousStateType = self.stateType
self.LoopControlHelper.stateType = zmp_com_now[0]
self.LoopControlHelper.zmpPosDesired[
0] = zmp_com_now[1] + ktoto
self.LoopControlHelper.zmpPosDesired[1] = zmp_com_now[2]
# update state with walking pattern generator
self.comRefPos = Vector3d(zmp_com_now[4] + ktoto,
zmp_com_now[5], 0.780678)
self.comRefVel = Vector3d(zmp_com_now[7], zmp_com_now[8],
zmp_com_now[9])
self.comRefAccel = Vector3d(zmp_com_now[10],
zmp_com_now[11],
zmp_com_now[12])
self.previousStateType = self.stateType
self.stateType = zmp_com_now[0] #0=TDS, 1=LSS, 2=RSS
self.zmp_des[0] = zmp_com_now[1] + ktoto
self.zmp_des[1] = zmp_com_now[2]
else:
self.comRefPos = self.LoopControlHelper.comPosDesired
self.LoopControlHelper.comVelDesired = Vector3d().Zero()
self.LoopControlHelper.comAccDesire = Vector3d().Zero()
self.comRefVel = self.LoopControlHelper.comVelDesired
self.comRefAccel = self.LoopControlHelper.comAccDesired
self.zmp_des[0] = self.LoopControlHelper.zmpPosDesired[0]
self.zmp_des[1] = self.LoopControlHelper.zmpPosDesired[1]
self.zmp_des[0] = self.comRefPos[0]
self.zmp_des[1] = self.comRefPos[1]
# self.stateType = 0
if self.counterWalk is None:
self.counterWalk = 1
if self.counterWalk <= maxcounterwalk:
self.comRefPos[0] = (
1 - cos(self.counterWalk / maxcounterwalk *
np.pi)) / 2 * ktoto - 0.0036526
self.comRefVel[0] = (sin(
self.counterWalk / maxcounterwalk *
np.pi)) / 2 * ktoto * np.pi / maxcounterwalk * 0
self.comRefAccel[0] = (cos(
self.counterWalk / maxcounterwalk *
np.pi)) / 2 * ktoto * (np.pi / maxcounterwalk) * (
np.pi / maxcounterwalk) * 0
self.zmp_des[0] = -self.comRefAccel[
0] * 0.78 / 9.81 + self.comRefPos[0]
self.counterWalk += 1
self.LoopControlHelper.comPosDesired[
0] = self.comRefPos[0]
self.LoopControlHelper.zmpPosDesired[0] = self.zmp_des[
0]
self.zmpRPosDesired = Vector3d(
self.LoopControlHelper.zmpPosDesired[0],
-0.0815817, 0)
self.zmpLPosDesired = Vector3d(
self.LoopControlHelper.zmpPosDesired[0], 0.0815817,
0)
#TODO: very unsure of this part
if self.IsWalkActiv:
if ((self.stateType != self.previousStateType)
and (self.previousStateType == 0)):
next_pstep = self.pstep.pop(0)
self.nextStepPos = [next_pstep[0], next_pstep[1]]
self.next_step_angle = next_pstep[2]
if self.stateType == 1:
self.previousRfootPosFixed[
0] = self.LoopControlHelper.footRPosFixed[0]
self.previousRfootPosFixed[
1] = self.LoopControlHelper.footRPosFixed[1]
self.LoopControlHelper.footRPosFixed[0] = next_pstep[0]
self.LoopControlHelper.footRPosFixed[1] = next_pstep[1]
self.LoopControlHelper.footRAnglFixed = next_pstep[2]
else:
self.previousLfootPosFixed[
0] = self.LoopControlHelper.footLPosFixed[0]
self.previousLfootPosFixed[
1] = self.LoopControlHelper.footLPosFixed[1]
self.LoopControlHelper.footLPosFixed[0] = next_pstep[0]
self.LoopControlHelper.footLPosFixed[1] = next_pstep[1]
self.LoopControlHelper.footLAnglFixed = next_pstep[2]
'''
'''
if self.wPG_iters == 2:
self.LoopControlHelper.footRPosFixed = Vector3d(
self.zmpcom[0][22], self.zmpcom[0][23], self.zmpcom[0][24])
self.LoopControlHelper.footLPosFixed = Vector3d(
self.zmpcom[0][13], self.zmpcom[0][14], self.zmpcom[0][15])
self.LoopControlHelper.footRAnglFixed = self.zmpcom[0][36]
self.LoopControlHelper.footLAnglFixed = self.zmpcom[0][33]
self.LoopControlHelper.update(rs, self.IsControlLoopActiv)
if self.FootWidthEnlarge:
maxcounterFootEnlarge = 500.0
if self.counterFootEnlarge is None:
self.counterFootEnlarge = 0
elif self.counterFootEnlarge < maxcounterFootEnlarge:
self.counterFootEnlarge += 1
if self.IsWalkActiv or self.IsControlLoopActiv:
self.FootWidthEnlarge = False
# '''
# close loop in Qp targets update of COM and ZMP
# '''
# if self.IsControlLoopActiv and self.comRefPosInit is None:
# self.comRefPosInit=self.comRefPos
# self.comRefVelInit=self.comRefVel
# self.comRefAccelInit=self.comRefAccel
# self.zmp_desInit=self.zmp_des
# elif not self.IsControlLoopActiv:
# self.comRefPosInit=None
# self.comRefVelInit=None
# self.comRefAccelInit=None
# self.zmp_desInit=None
#
# if self.IsControlLoopActiv:
# self.zmp_desInit[0]=self.LoopControlHelper.zmpPosWantedSatur_[0]
# self.zmp_desInit[1]=self.LoopControlHelper.zmpPosWantedSatur_[1]
#
# self.comRefAccelInit=self.LoopControlHelper.zmpForceWanted_/self.LoopControlHelper.M
# self.comRefAccelInit[2]=0
#
# self.comRefPosInit=self.comRefPos+self.comRefVelInit*1/200+self.comRefAccelInit*(1/200)**2/2
#
# self.comRefVelInit=self.comRefVelInit+self.comRefAccelInit*1/200
#
# self.comRefPos=self.comRefPosInit
# self.comRefVel=self.comRefVelInit
# self.comRefAccel=self.comRefAccelInit
# self.zmp_des=self.comRefPosInit
# elif not self.comRefPosInit is None:
# self.comRefPos=self.comRefPosInit
# self.comRefVel=self.comRefVelInit*0
# self.comRefAccel=self.comRefAccelInit*0
# self.zmp_des=self.comRefPosInit
# if self.wPG_iters==2:
# self.comRefPosInit=self.comRefPos
# self.comRefVelInit=self.comRefVel
# self.comRefAccelInit=self.comRefAccel
# self.zmp_desInit=self.zmp_des
# else:
# self.zmp_desInit[0]=self.LoopControlHelper.zmpPosWantedSatur_[0]
# self.zmp_desInit[1]=self.LoopControlHelper.zmpPosWantedSatur_[1]
#
# self.comRefAccelInit=self.LoopControlHelper.zmpForceWanted_/self.LoopControlHelper.M
# self.comRefAccelInit[2]=0
#
# self.comRefVelInit=self.comRefVelInit+self.comRefAccelInit*1/200
# self.comRefPosInit=self.comRefPos+self.comRefVelInit*1/200
#
# self.comRefPos=self.comRefPosInit
# self.comRefVel=self.comRefVelInit
# self.comRefAccel=self.comRefAccelInit
# self.zmp_des=self.zmp_desInit
comTask.com(self.comRefPos)
comTaskTr.refVel(toVecX(self.comRefVel))
comTaskTr.refAccel(toVecX(self.comRefAccel))
# prevents a bug caused by the orientation task dimWeight #TODO: fix properly in Tasks self.initAlpha *
torsoOriTask.orientation(
sva.RotY(0.14) * sva.RotZ(
(self.last_rotation_angle_rfoot +
self.last_rotation_angle_lfoot) / 2.))
# double support
if self.stateType == 0:
print 'state TDS'
if not (self.previousStateType == 0):
qpsolver.setContacts([c1L, c1R])
qpsolver.update()
print '------------updating contact state'
# left single support
elif self.stateType == 1:
self.swingFoot = self.rFoot
print 'state LSS'
if (self.previousStateType == 0):
qpsolver.setContacts([c1L])
qpsolver.update()
print '------------updating contact state'
self.last_rotation_angle_rfoot = self.next_step_angle
self.previousRFootPos = self.RFootHelper.bodyState.getPosW(
)
# right single support
elif self.stateType == 2:
self.swingFoot = self.lFoot
print 'state RSS'
if (self.previousStateType == 0):
qpsolver.setContacts([c1R])
qpsolver.update()
print '------------updating contact state'
self.last_rotation_angle_lfoot = self.next_step_angle
self.previousLFootPos = self.LFootHelper.bodyState.getPosW(
)
'''
Manage Right Foot tasks
'''
if self.IsControlLoopActiv and not self.IsWalkActiv:
# rFootPos=self.LoopControlHelper.footRPosWanted
rFootPos = self.RFootHelper.bodyState.getPosW() + Vector3d(
self.LoopControlHelper.DeltaDisplR_[0],
self.LoopControlHelper.DeltaDisplR_[1],
self.LoopControlHelper.DeltaDisplR_[2]
) #+self.LoopControlHelper.DeltaDisplR_
elif self.IsControlLoopActiv and self.IsWalkActiv:
# rFootPos=Vector3d(zmp_com_now[22],zmp_com_now[23],zmp_com_now[24])
rFootPos = self.RFootHelper.bodyState.getPosW() + Vector3d(
self.LoopControlHelper.DeltaDisplR_[0],
self.LoopControlHelper.DeltaDisplR_[1],
self.LoopControlHelper.DeltaDisplR_[2]
) #+self.LoopControlHelper.DeltaDisplR_
if not self.IsDebugWalking:
if self.stateType == 1:
rFootPos = self.previousRFootPos - self.previousRfootPosFixed + Vector3d(
zmp_com_now[22], zmp_com_now[23], zmp_com_now[24])
else:
rFootPos = self.RFootHelper.bodyState.getPosW(
) + Vector3d(self.LoopControlHelper.DeltaDisplR_[0],
self.LoopControlHelper.DeltaDisplR_[1],
self.LoopControlHelper.DeltaDisplR_[2])
elif self.IsWalkActiv:
# rFootPos=Vector3d(zmp_com_now[22],zmp_com_now[23],zmp_com_now[24])
## rFootPos=Vector3d(zmp_com_now[22],zmp_com_now[23],0.093)
## rFootVel=Vector3d(zmp_com_now[25],zmp_com_now[26],zmp_com_now[27])
## rFootAcc=Vector3d(zmp_com_now[28],zmp_com_now[29],zmp_com_now[30])
rFootPos = self.RFootHelper.bodyState.getPosW(
) #-Vector3d(+0.005,0,0)
if not self.IsDebugWalking:
if self.stateType == 1:
rFootPos = self.previousRFootPos - self.previousRfootPosFixed + Vector3d(
zmp_com_now[22], zmp_com_now[23], zmp_com_now[24])
else:
rFootPos = self.RFootHelper.bodyState.getPosW()
elif self.FootWidthEnlarge:
rFootPos = self.LoopControlHelper.footRPosInit + (
self.LoopControlHelper.footRPosFixed -
self.LoopControlHelper.footRPosInit
) * self.counterFootEnlarge / maxcounterFootEnlarge
else:
# if self.FootWidthEnlarge:
# rFootPos=Vector3d(self.zmpcom[0][22],max(self.RFootHelper.bodyState.getPosW()[1]-0.001,self.zmpcom[0][23]),self.zmpcom[0][24])
# else:
rFootPos = self.RFootHelper.bodyState.getPosW()
# rFootVel=Vector3d(0,0,0)
# rFootAcc=Vector3d(0,0,0)
# self.RFootHelper.update(rFootPos,rFootVel,rFootAcc)
rfPosTaskTr.position(rFootPos)
if self.IsControlLoopActiv:
rfOriTask.orientation(self.LoopControlHelper.DeltafootROri_ *
self.RFootHelper.bodyState.getOriW())
toto3 = self.RFootHelper.bodyState.getOriW()
self.ROriMarkerMesured = toto3.eulerAngles(0, 1, 2)
toto4 = self.LoopControlHelper.DeltafootROri_ * self.RFootHelper.bodyState.getOriW(
)
self.ROriMarkerWanted = toto4.eulerAngles(0, 1, 2)
elif self.IsWalkActiv:
# rfOriTask.orientation(sva.RotX(zmp_com_now[34])*sva.RotY(zmp_com_now[35])*sva.RotZ(zmp_com_now[36]))
rfOriTask.orientation(self.RFootHelper.bodyState.getOriW())
elif self.FootWidthEnlarge:
rfOriTask.orientation(
sva.RotZ(self.LoopControlHelper.footRAnglFixed *
self.counterFootEnlarge / maxcounterFootEnlarge))
else:
# rfOriTask.orientation(sva.RotX(self.LoopControlHelper.footRAnglDesired[0])*sva.RotY(self.LoopControlHelper.footRAnglDesired[1])*sva.RotZ(self.LoopControlHelper.footRAnglDesired[2]))
rfOriTask.orientation(self.RFootHelper.bodyState.getOriW())
'''
Manage Left Foot tasks
'''
# lFootPos=Vector3d(zmp_com_now[13],zmp_com_now[14],zmp_com_now[15])
# lFootVel=Vector3d(zmp_com_now[16],zmp_com_now[17],zmp_com_now[18])
# lFootAcc=Vector3d(zmp_com_now[19],zmp_com_now[20],zmp_com_now[21])
if self.IsControlLoopActiv and not self.IsWalkActiv:
# lFootPos=self.LoopControlHelper.footLPosWanted
lFootPos = self.LFootHelper.bodyState.getPosW() + Vector3d(
self.LoopControlHelper.DeltaDisplL_[0],
self.LoopControlHelper.DeltaDisplL_[1],
self.LoopControlHelper.DeltaDisplL_[2]
) #+self.LoopControlHelper.DeltaDisplR_
elif self.IsControlLoopActiv and self.IsWalkActiv:
# lFootPos=Vector3d(zmp_com_now[13],zmp_com_now[14],zmp_com_now[15])
lFootPos = self.LFootHelper.bodyState.getPosW() + Vector3d(
self.LoopControlHelper.DeltaDisplL_[0],
self.LoopControlHelper.DeltaDisplL_[1],
self.LoopControlHelper.DeltaDisplL_[2]
) #+self.LoopControlHelper.DeltaDisplR_
if not self.IsDebugWalking:
if self.stateType == 2:
lFootPos = self.previousLFootPos - self.previousLfootPosFixed + Vector3d(
zmp_com_now[13], zmp_com_now[14], zmp_com_now[15])
else:
lFootPos = self.LFootHelper.bodyState.getPosW(
) + Vector3d(self.LoopControlHelper.DeltaDisplL_[0],
self.LoopControlHelper.DeltaDisplL_[1],
self.LoopControlHelper.DeltaDisplL_[2])
elif self.IsWalkActiv:
# lFootPos=Vector3d(zmp_com_now[13],zmp_com_now[14],zmp_com_now[15])
## lFootPos=Vector3d(zmp_com_now[13],zmp_com_now[14],0.093)
## lFootVel=Vector3d(zmp_com_now[16],zmp_com_now[17],zmp_com_now[18])
## lFootAcc=Vector3d(zmp_com_now[19],zmp_com_now[20],zmp_com_now[21])
lFootPos = self.LFootHelper.bodyState.getPosW(
) #-Vector3d(-0.005,0,0)
if not self.IsDebugWalking:
if self.stateType == 2:
lFootPos = self.previousLFootPos - self.previousLfootPosFixed + Vector3d(
zmp_com_now[13], zmp_com_now[14], zmp_com_now[15])
else:
lFootPos = self.LFootHelper.bodyState.getPosW()
elif self.FootWidthEnlarge:
lFootPos = self.LoopControlHelper.footLPosInit + (
self.LoopControlHelper.footLPosFixed -
self.LoopControlHelper.footLPosInit
) * self.counterFootEnlarge / maxcounterFootEnlarge
else:
# if self.FootWidthEnlarge:
# lFootPos=Vector3d(self.zmpcom[0][13],min(self.LFootHelper.bodyState.getPosW()[1]+0.001,self.zmpcom[0][14]),self.zmpcom[0][15])
# else:
lFootPos = self.LFootHelper.bodyState.getPosW()
# lFootVel=Vector3d(0,0,0)
# lFootAcc=Vector3d(0,0,0)
# self.LFootHelper.update(lFootPos,lFootVel,lFootAcc)
lfPosTaskTr.position(lFootPos)
if self.IsControlLoopActiv:
lfOriTask.orientation(self.LoopControlHelper.DeltafootLOri_ *
self.LFootHelper.bodyState.getOriW())
toto1 = self.LFootHelper.bodyState.getOriW()
self.LOriMarkerMesured = toto1.eulerAngles(0, 1, 2)
toto2 = self.LoopControlHelper.DeltafootLOri_ * self.LFootHelper.bodyState.getOriW(
)
self.LOriMarkerWanted = toto2.eulerAngles(0, 1, 2)
elif self.IsWalkActiv:
# lfOriTask.orientation(sva.RotX(zmp_com_now[31])*sva.RotY(zmp_com_now[32])*sva.RotZ(zmp_com_now[33]))
lfOriTask.orientation(self.LFootHelper.bodyState.getOriW())
elif self.FootWidthEnlarge:
lfOriTask.orientation(
sva.RotZ(self.LoopControlHelper.footLAnglFixed *
self.counterFootEnlarge / maxcounterFootEnlarge))
else:
# lfOriTask.orientation(sva.RotX(self.LoopControlHelper.footLAnglDesired[0])*sva.RotY(self.LoopControlHelper.footLAnglDesired[1])*sva.RotZ(self.LoopControlHelper.footLAnglDesired[2]))
lfOriTask.orientation(self.LFootHelper.bodyState.getOriW())
# lfOriTask.orientation(sva.RotX(self.LoopControlHelper.footLAnglDesired[0])*sva.RotY(self.LoopControlHelper.footLAnglDesired[1]+0.00175*self.wPG_iters)*sva.RotZ(self.LoopControlHelper.footLAnglDesired[2]))
# used to compare if contact state needs updating
self.previousStateType = self.stateType
# if self.FootWidthEnlarge==True:
# if self.LFootHelper.bodyState.getPosW()[1]+0.001>=self.zmpcom[0][14] and self.RFootHelper.bodyState.getPosW()[1]-0.001<=self.zmpcom[0][23]:
# self.FootWidthEnlarge=False
else:
print 'wPG ended with ', self.wPG_iters - 1, ' iterations'
self.hasEnded = True
def run(self, rs):
'qp duration'
self.time_qp = time.time() - self.start
start = time.time()
if self.stopCB is not None and self.stopCB.check():
print 'stopping'
self.stopCB = None
self.isRunning = True
self.hsCB = stopMotion(robots, qpsolver, postureTask1, None,
rbdList(hrp4.mbc.q))
self.fsm = self.waitHS
if self.isRunning:
if not qpsolver.run():
print 'FAIL !!!'
self.isRunning = False
return
curTime = rs.header.stamp
# update the center of mass state
rbd.forwardAcceleration(hrp4.mb, hrp4.mbc)
self.com = rbd.computeCoM(hrp4.mb, hrp4.mbc)
self.comA = rbd.computeCoMAcceleration(hrp4.mb, hrp4.mbc)
hrp4W.update(rs)
# print [rs.imu_orientation.w, rs.imu_orientation.x, rs.imu_orientation.y, rs.imu_orientation.z]
# print rbdList(hrp4.mbc.q)[0]
# print rbdList(hrp4W.mbc.bodyPosW)[0].rotation()
# hrp4_q=rbdList(hrp4.mbc.q)
# print hrp4_q[0]
# hrp4W_q=rbdList(hrp4W.hrp4W.mbc.q)
# print hrp4W_q[0]
# i=1
# name=rs.joint_name[i]
# if hrp4.hasJoint(name):
# print hrp4_q[hrp4.jointIndexByName(name)]
# print rs.joint_position[i]
# print hrp4_q[hrp4.jointIndexByName(name)][0]-rs.joint_position[i]
# hrp4_q=rbdList(hrp4.mbc.q)
# for i, name in enumerate(rs.joint_name):
# if hrp4.hasJoint(name):
# print name
# print hrp4_q[hrp4.jointIndexByName(name)]
# print rs.joint_position[i]
# print hrp4_q[hrp4.jointIndexByName(name)][0]-rs.joint_position[i]
if self.fsm == self.wPGiteration:
# Update ZMP to be published
self.zmp_d = Vector3d(self.playbackBridge.zmp_des[0],
self.playbackBridge.zmp_des[1], 0.0)
# markers for debugging the walking pattern generator
if self.isWPGMarkerPublished:
self.zmp_actual = rbd.computeCentroidalZMP(
hrp4.mbc, self.com, self.comA, 0.)
# TODO: use the new API for this!
#compute capture point:
omega = np.sqrt(
9.81 /
self.playbackBridge.robot_params.com_height_)
# omega = np.sqrt(9.81/rbd.computeCoM(hrp4.mb, hrp4.mbc)[2])
comVel = rbd.computeCoMVelocity(hrp4.mb, hrp4.mbc)
capturePoint = self.com + (comVel / omega)
capturePoint[2] = 0.0
# robotH = hrp4
# bodyIdxR = robotH.bodyIndexByName('r_ankle')
# posR=(list(robotH.mbc.bodyPosW)[bodyIdxR]).translation()
# rotR=(list(robotH.mbc.bodyPosW)[bodyIdxR]).rotation()
#
# bodyIdxL = robotH.bodyIndexByName('l_ankle')
# posL=(list(robotH.mbc.bodyPosW)[bodyIdxL]).translation()
# rotL=(list(robotH.mbc.bodyPosW)[bodyIdxL]).rotation()
# walking pattern generator RViZ markers
wpg_markers.fill(
self.zmp_actual, self.zmp_d, self.com,
self.playbackBridge.comRefPos, [
self.playbackBridge.nextStepPos[0],
self.playbackBridge.nextStepPos[1], 0.0
], capturePoint, self.playbackBridge.
LoopControlHelper.zmpPosWanted_,
self.playbackBridge.LoopControlHelper.
zmpPosWantedSatur_, self.playbackBridge.
LoopControlHelper.comPosMesured, self.
playbackBridge.LoopControlHelper.comVelMesured,
self.playbackBridge.LoopControlHelper.
comAccMesured, self.playbackBridge.
LoopControlHelper.zmpPosDesired, self.
playbackBridge.LoopControlHelper.zmpPosMesured,
self.playbackBridge.LoopControlHelper.
zmpRPosMesured, self.playbackBridge.
LoopControlHelper.zmpLPosMesured, self.
playbackBridge.LoopControlHelper.zmpRPosDesired,
self.playbackBridge.LoopControlHelper.
zmpLPosDesired, self.playbackBridge.
LoopControlHelper.zmpRPosWantedSatur_,
self.playbackBridge.LoopControlHelper.
zmpLPosWantedSatur_, self.playbackBridge.
LoopControlHelper.footRPosMesured_, self.
playbackBridge.LoopControlHelper.footLPosMesured_,
self.playbackBridge.ROriMarkerMesured,
self.playbackBridge.LOriMarkerMesured,
self.playbackBridge.ROriMarkerWanted,
self.playbackBridge.LOriMarkerWanted, self.
playbackBridge.LoopControlHelper.zmpPosMesured_,
self.playbackBridge.LoopControlHelper.
zmpRPosMesured_, self.playbackBridge.
LoopControlHelper.zmpLPosMesured_, self.
playbackBridge.LoopControlHelper.comPosMesured_,
self.playbackBridge.LoopControlHelper.DeltaDisplR_,
self.playbackBridge.LoopControlHelper.DeltaDisplL_,
self.playbackBridge.LoopControlHelper.
comPosDesired, self.playbackBridge.
LoopControlHelper.footRForceMesured_,
self.playbackBridge.LoopControlHelper.
footLForceMesured_,
self.playbackBridge.LoopControlHelper.DeltaAnglR_,
self.playbackBridge.LoopControlHelper.DeltaAnglL_,
self.playbackBridge.LoopControlHelper.
zmpRForceWanted_, self.playbackBridge.
LoopControlHelper.zmpLForceWanted_, self.
playbackBridge.LoopControlHelper.comVelMesured_,
self.playbackBridge.LoopControlHelper.
comVelDesired, self.playbackBridge.
LoopControlHelper.capturePointMesured_, self.
playbackBridge.LoopControlHelper.zmpRPosWanted_,
self.playbackBridge.LoopControlHelper.
zmpLPosWanted_, self.playbackBridge.
LoopControlHelper.footRTorquMesured_,
self.playbackBridge.LoopControlHelper.
footLTorquMesured_, self.playbackBridge.
LoopControlHelper.int_delta_comPos)
wpg_markers.publish()
zmp_com_markers.fill(
self.playbackBridge.LoopControlHelper.
comPosDesired,
self.playbackBridge.LoopControlHelper.
zmpPosDesired,
# self.playbackBridge.LoopControlHelper.comPosMesured_,
# self.playbackBridge.LoopControlHelper.zmpPosMesured_,
Vector3d(self.time_qp, self.time_run,
self.time_playback),
Vector3d(
self.playbackBridge.LoopControlHelper.
stateType, self.playbackBridge.stateType, self.
playbackBridge.LoopControlHelper.forceDistrib))
zmp_com_markers.publish()
# Publish all
# hrp4Stab.publishZMPDesired(curTime, self.zmp_d)
hrp4Stab.publish(curTime, self.com, self.comA)
hrp4Jsp.publish(curTime)
qpsolver.fillResult()
q_posture = list(
chain.from_iterable(list(postureTask1.posture())))
qpsolver.qpRes.robots_state.append(Robot(q_posture, [], []))
q_posture = list(postureTask1.eval())
qpsolver.qpRes.robots_state.append(Robot(q_posture, [], []))
# print len(postureTask1.eval())
# print len(rbdList(postureTask1.posture()))
# print len(rbdList(hrp4.mbc.q))
# print hrp4.mb.nrDof()
qpsolver.send(curTime)
self.fsm(rs)
# if not ((self.fsm == self.wait_init_position) or (self.fsm == self.prepareWPG)):
# raw_input('wait user input')
'callrun duration'
self.time_run = time.time() - start
self.start = time.time()
[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\
multiTaskIk(mb, mbc, tasks, delta=1., maxIter=200, prec=1e-8):
q_res = q
alphaInfList.append(alphaInf)
print 'iter number', len(alphaInfList)
print 'last alpha norm', alphaInfList[-1]
print
print 'bodyTask error:', bodyTask.g(mb, mbc).T
print 'postureTask error:', postureTask.g(mb, mbc).T
def g(self, mb, mbc): return e.toNumpy(rbd.computeCoM(mb, mbc) - self.com_T)