def publish(self, curTime, comPosition, comAccel):
# fill the stabilizer message properly
self.stabMsg.header.stamp = curTime
self.X_0_baselink = list(self.robot.mbc.bodyPosW)[0]
self.stabMsg.base_link = transform.toTransform(self.X_0_baselink)
self.zmp_world = rbd.computeCentroidalZMP(self.robot.mbc, comPosition,
comAccel, 0.)
self.X_0_zmp = sva.PTransform(self.zmp_world)
self.X_zmp = self.X_0_zmp * self.X_0_baselink.inv()
self.stabMsg.zmp.x = self.X_zmp.translation().x()
self.stabMsg.zmp.y = self.X_zmp.translation().y()
self.stabMsg.zmp.z = self.X_zmp.translation().z()
self.rosPub.publish(self.stabMsg)
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)
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()
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)