def initFeetTask(robot):
robot.selecFeet = Selector('selecFeet',
['matrixHomo','leftfootref', \
robot.dynamic.signal(robot.OperationalPointsMap['left-ankle']),\
robot.pg.leftfootref], \
['matrixHomo','rightfootref', \
robot.dynamic.signal(robot.OperationalPointsMap['right-ankle']), \
robot.pg.rightfootref])
plug(robot.pg.inprocess, robot.selecFeet.selec)
robot.tasks['right-ankle'].controlGain.value = 200
robot.tasks['left-ankle'].controlGain.value = 200
def initZMPRef(robot):
# --- Selector of Com Ref: when robot.pg is stopped, pg.inprocess becomes 0
robot.comSelector = Selector('comSelector',
['vector', 'ref', robot.com, robot.pg.comref])
plug(robot.pg.inprocess, robot.comSelector.selec)
selecSupportFoot = Selector(
'selecSupportFoot',
['matrixHomo', 'pg_H_sf', robot.pg.rightfootref, robot.pg.leftfootref],
['matrixHomo', 'wa_H_sf', robot.geom.rf2, robot.geom.lf2])
robot.addTrace(robot.pg.name, 'rightfootref')
robot.addTrace(robot.pg.name, 'leftfootref')
robot.addTrace(robot.pg.name, 'comref')
robot.addTrace(robot.pg.name, 'zmpref')
robot.addTrace(robot.pg.name, 'inprocess')
robot.addTrace(robot.device.name, 'forceLLEG')
robot.addTrace(robot.device.name, 'forceRLEG')
plug(robot.pg.SupportFoot, selecSupportFoot.selec)
sf_H_wa = Inverse_of_matrixHomo('sf_H_wa')
plug(selecSupportFoot.wa_H_sf, sf_H_wa.sin)
pg_H_wa = Multiply_of_matrixHomo('pg_H_wa')
plug(selecSupportFoot.pg_H_sf, pg_H_wa.sin1)
plug(sf_H_wa.sout, pg_H_wa.sin2)
# --- Compute the ZMP ref in the Waist reference frame.
wa_H_pg = Inverse_of_matrixHomo('wa_H_pg')
plug(pg_H_wa.sout, wa_H_pg.sin)
wa_zmp = Multiply_matrixHomo_vector('wa_zmp')
plug(wa_H_pg.sout, wa_zmp.sin1)
plug(robot.pg.zmpref, wa_zmp.sin2)
# Connect the ZMPref to OpenHRP in the waist reference frame.
robot.pg.parseCmd(':SetZMPFrame world')
plug(robot.pg.zmpref, robot.device.zmp)
robot.addTrace(robot.device.name, 'zmp')
robot.addTrace(pg_H_wa.name, 'sout')
def initFeetTask(robot):
robot.selecFeet = Selector('selecFeet',
['matrixHomo','leftfootref', \
robot.dynamic.signal('left-ankle'),\
robot.pg.leftfootref], \
['matrixHomo','rightfootref', \
robot.dynamic.signal('right-ankle'), \
robot.pg.rightfootref])
plug(robot.pg.inprocess, robot.selecFeet.selec)
robot.tasks['right-ankle'].controlGain.value = 180
robot.tasks['left-ankle'].controlGain.value = 180
print "After Task for Right and Left Feet"
pg.initState()
# --- PG INIT FRAMES ---
geom = Dynamic("geom")
geom.setFiles(modelDir, modelName[robotName], specificitiesPath, jointRankPath)
geom.parse()
geom.createOpPoint('rf', 'right-ankle')
geom.createOpPoint('lf', 'left-ankle')
plug(dyn.position, geom.position)
geom.ffposition.value = 6 * (0, )
geom.velocity.value = robotDim * (0, )
geom.acceleration.value = robotDim * (0, )
# --- Selector of Com Ref: when pg is stopped, pg.inprocess becomes 0
comRef = Selector('comRef', ['vector', 'ref', dyn.com, pg.comref])
plug(pg.inprocess, comRef.selec)
selecSupportFoot = Selector(
'selecSupportFoot',
['matrixHomo', 'pg_H_sf', pg.rightfootref, pg.leftfootref],
['matrixHomo', 'wa_H_sf', geom.rf, geom.lf])
plug(pg.SupportFoot, selecSupportFoot.selec)
sf_H_wa = Inverse_of_matrixHomo('sf_H_wa')
plug(selecSupportFoot.wa_H_sf, sf_H_wa.sin)
pg_H_wa = Multiply_of_matrixHomo('pg_H_wa')
plug(selecSupportFoot.pg_H_sf, pg_H_wa.sin1)
plug(sf_H_wa.sout, pg_H_wa.sin2)
# --- Compute the ZMP ref in the Waist reference frame.
wa_H_pg = Inverse_of_matrixHomo('wa_H_pg')
def __init__(self, dyn):
self.pg = PatternGenerator('pg')
modelDir = dyn.getProperty('vrmlDirectory')
modelName = dyn.getProperty('vrmlMainFile')
specificitiesPath = dyn.getProperty('xmlSpecificityFile')
jointRankPath = dyn.getProperty('xmlRankFile')
robotDim = len(dyn.position.value)
# print(modelDir,modelName,specificitiesPath,jointRankPath,robotDim)
self.pg.setVrmlDir(modelDir + '/')
self.pg.setVrml(modelName)
self.pg.setXmlSpec(specificitiesPath)
self.pg.setXmlRank(jointRankPath)
self.pg.buildModel()
# Standard initialization
self.pg.parseCmd(":samplingperiod 0.005")
self.pg.parseCmd(":previewcontroltime 1.6")
self.pg.parseCmd(":comheight 0.814")
self.pg.parseCmd(":omega 0.0")
self.pg.parseCmd(":stepheight 0.05")
self.pg.parseCmd(":singlesupporttime 0.780")
self.pg.parseCmd(":doublesupporttime 0.020")
self.pg.parseCmd(":armparameters 0.5")
self.pg.parseCmd(":LimitsFeasibility 0.0")
self.pg.parseCmd(":ZMPShiftParameters 0.015 0.015 0.015 0.015")
self.pg.parseCmd(":TimeDistributeParameters 2.0 3.5 1.0 3.0")
self.pg.parseCmd(":UpperBodyMotionParameters 0.0 -0.5 0.0")
self.pg.parseCmd(":comheight 0.814")
self.pg.parseCmd(":SetAlgoForZmpTrajectory Morisawa")
plug(dyn.position, self.pg.position)
plug(dyn.com, self.pg.com)
self.pg.motorcontrol.value = robotDim * (0, )
self.pg.zmppreviouscontroller.value = (0, 0, 0)
self.pg.initState()
# --- PG INIT FRAMES ---
self.geom = Dynamic("geom")
self.geom.setFiles(modelDir, modelName, specificitiesPath,
jointRankPath)
self.geom.parse()
self.geom.createOpPoint('rf', 'right-ankle')
self.geom.createOpPoint('lf', 'left-ankle')
plug(dyn.position, self.geom.position)
self.geom.ffposition.value = 6 * (0, )
self.geom.velocity.value = robotDim * (0, )
self.geom.acceleration.value = robotDim * (0, )
# --- Selector of Com Ref: when pg is stopped, pg.inprocess becomes 0
self.comRef = Selector('comRef',
['vector', 'ref', dyn.com, self.pg.comref])
plug(self.pg.inprocess, self.comRef.selec)
self.selecSupportFoot = Selector('selecSupportFoot', [
'matrixHomo', 'pg_H_sf', self.pg.rightfootref, self.pg.leftfootref
], ['matrixHomo', 'wa_H_sf', self.geom.rf, self.geom.lf])
plug(self.pg.SupportFoot, self.selecSupportFoot.selec)
self.sf_H_wa = Inverse_of_matrixHomo('sf_H_wa')
plug(self.selecSupportFoot.wa_H_sf, self.sf_H_wa.sin)
self.pg_H_wa = Multiply_of_matrixHomo('pg_H_wa')
plug(self.selecSupportFoot.pg_H_sf, self.pg_H_wa.sin1)
plug(self.sf_H_wa.sout, self.pg_H_wa.sin2)
# --- Compute the ZMP ref in the Waist reference frame.
self.wa_H_pg = Inverse_of_matrixHomo('wa_H_pg')
plug(self.pg_H_wa.sout, self.wa_H_pg.sin)
self.wa_zmp = Multiply_matrixHomo_vector('wa_zmp')
plug(self.wa_H_pg.sout, self.wa_zmp.sin1)
plug(self.pg.zmpref, self.wa_zmp.sin2)
# --- Build the converter object for the waist task
self.waistReferenceVector = Stack_of_vector('waistReferenceVector')
plug(self.pg.initwaistposref, self.waistReferenceVector.sin1)
# plug(self.pg.initwaistattref,self.waistReferenceVector.sin2)
plug(self.pg.comattitude, self.waistReferenceVector.sin2)
self.waistReferenceVector.selec1(0, 3)
self.waistReferenceVector.selec2(0, 3)
self.waistReference = PoseRollPitchYawToMatrixHomo('waistReference')
plug(self.waistReferenceVector.sout, self.waistReference.sin)