pg.parseCmd(":singlesupporttime 0.780") pg.parseCmd(":doublesupporttime 0.020") pg.parseCmd(":armparameters 0.5") pg.parseCmd(":LimitsFeasibility 0.0") pg.parseCmd(":ZMPShiftParameters 0.015 0.015 0.015 0.015") pg.parseCmd(":TimeDistributeParameters 2.0 3.5 1.0 3.0") pg.parseCmd(":UpperBodyMotionParameters 0.0 -0.5 0.0") pg.parseCmd(":comheight 0.814") pg.parseCmd(":SetAlgoForZmpTrajectory Morisawa") plug(dyn.position, pg.position) plug(dyn.com, pg.com) pg.motorcontrol.value = robotDim * (0, ) pg.zmppreviouscontroller.value = (0, 0, 0) 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)
class MetaPG: 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) def plugZmp(self, robot): # Connect the ZMPref to OpenHRP in the waist reference frame. self.pg.parseCmd(':SetZMPFrame world') plug(self.wa_zmp.sout, robot.zmp) def plugWaistTask(self, taskWaist): plug(self.waistReference.sout, taskWaist.featureDes.position) taskWaist.feature.selec.value = '111100' def plugComTask(self, taskCom): plug(self.comRef.ref, taskCom.featureDes.errorIN) plug(self.pg.dcomref, taskCom.featureDes.errordotIN) taskCom.task = TaskPD('taskComPD') taskCom.task.add(taskCom.feature.name) # This next line is not very nice. The principle should be reported in Task. plug(taskCom.feature.errordot, taskCom.task.errorDot) plug(taskCom.task.error, taskCom.gain.error) plug(taskCom.gain.gain, taskCom.task.controlGain) taskCom.gain.setConstant(40) taskCom.task.setBeta(-1) def startHerdt(self, xyconstraint=True): self.pg.parseCmd(':SetAlgoForZmpTrajectory Herdt') self.pg.parseCmd(':doublesupporttime 0.1') self.pg.parseCmd(':singlesupporttime 0.7') # When velocity reference is at zero, the robot stops all motion after n steps self.pg.parseCmd(':numberstepsbeforestop 2') # Set constraints on XY if xyconstraint: self.pg.parseCmd(':setfeetconstraint XY 0.09 0.06') # The next command must be runned after a OpenHRP.inc ... ??? # Start the robot with a speed of 0.1 m/0.8 s. self.pg.parseCmd(':HerdtOnline 0.1 0.0 0.0')
featurePosture.selec.value = toFlags(range(6, 36)) sot.clear() for task in [taskWaist, taskRF, taskLF]: task.feature.position.recompute(0) task.feature.keep() task.feature.selec.value = '111111' sot.push(task.task.name) taskWaist.ref = matrixToTuple(eye(4)) taskWaist.feature.selec.value = '111011' taskWaist.gain.setByPoint(18, 0.1, 0.005, 0.8) attime(200, (lambda: sot.rm(taskWaist.task.name), 'Rm waist'), (lambda: pg.initState(), 'Init PG'), (lambda: pg.parseCmd(":stepseq " + seqpart), 'Push step list'), (lambda: sot.push(taskCom.name), 'Push COM'), (lambda: plug(pg.rightfootref, taskRF.featureDes.position), 'Plug RF'), (lambda: plug(pg.leftfootref, taskLF.featureDes.position), 'plug LF')) #attime(700,lambda: sot.push(taskPosture.name),'Add Posture') #attime(900,lambda: sot.rm(taskCom.name),'rm com') #attime(1200,lambda: dump(),'dump!') #attime(1200,lambda: sys.exit(),'Bye bye') attime(1200, dump, stop) go() ''' sot.clear()
pg.parseCmd(":singlesupporttime 0.780") pg.parseCmd(":doublesupporttime 0.020") pg.parseCmd(":armparameters 0.5") pg.parseCmd(":LimitsFeasibility 0.0") pg.parseCmd(":ZMPShiftParameters 0.015 0.015 0.015 0.015") pg.parseCmd(":TimeDistributeParameters 2.0 3.5 1.0 3.0") pg.parseCmd(":UpperBodyMotionParameters 0.0 -0.5 0.0") pg.parseCmd(":comheight 0.814") pg.parseCmd(":SetAlgoForZmpTrajectory Morisawa") plug(dyn.position,pg.position) plug(dyn.com,pg.com) pg.motorcontrol.value = robotDim*(0,) pg.zmppreviouscontroller.value = (0,0,0) 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])
class MetaPG: 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) def plugZmp(self, robot): # Connect the ZMPref to OpenHRP in the waist reference frame. self.pg.parseCmd(":SetZMPFrame world") plug(self.wa_zmp.sout, robot.zmp) def plugWaistTask(self, taskWaist): plug(self.waistReference.sout, taskWaist.featureDes.position) taskWaist.feature.selec.value = "111100" def plugComTask(self, taskCom): plug(self.comRef.ref, taskCom.featureDes.errorIN) plug(self.pg.dcomref, taskCom.featureDes.errordotIN) taskCom.task = TaskPD("taskComPD") taskCom.task.add(taskCom.feature.name) # This next line is not very nice. The principle should be reported in Task. plug(taskCom.feature.errordot, taskCom.task.errorDot) plug(taskCom.task.error, taskCom.gain.error) plug(taskCom.gain.gain, taskCom.task.controlGain) taskCom.gain.setConstant(40) taskCom.task.setBeta(-1) def startHerdt(self, xyconstraint=True): self.pg.parseCmd(":SetAlgoForZmpTrajectory Herdt") self.pg.parseCmd(":doublesupporttime 0.1") self.pg.parseCmd(":singlesupporttime 0.7") # When velocity reference is at zero, the robot stops all motion after n steps self.pg.parseCmd(":numberstepsbeforestop 2") # Set constraints on XY if xyconstraint: self.pg.parseCmd(":setfeetconstraint XY 0.09 0.06") # The next command must be runned after a OpenHRP.inc ... ??? # Start the robot with a speed of 0.1 m/0.8 s. self.pg.parseCmd(":HerdtOnline 0.1 0.0 0.0")
featurePosture.selec.value = toFlags( range(6,36) ) sot.clear() for task in [taskWaist, taskRF, taskLF]: task.feature.position.recompute(0) task.feature.keep() task.feature.selec.value = '111111' sot.push(task.task.name) taskWaist.ref = matrixToTuple(eye(4)) taskWaist.feature.selec.value = '111011' taskWaist.gain.setByPoint(18,0.1,0.005,0.8) attime(200 ,(lambda : sot.rm(taskWaist.task.name),'Rm waist') ,(lambda : pg.initState(),'Init PG') ,(lambda : pg.parseCmd(":stepseq " + seqpart),'Push step list') ,(lambda : sot.push(taskCom.name),'Push COM') ,(lambda : plug(pg.rightfootref,taskRF.featureDes.position),'Plug RF') ,(lambda : plug(pg.leftfootref,taskLF.featureDes.position),'plug LF' ) ) #attime(700,lambda: sot.push(taskPosture.name),'Add Posture') #attime(900,lambda: sot.rm(taskCom.name),'rm com') #attime(1200,lambda: dump(),'dump!') #attime(1200,lambda: sys.exit(),'Bye bye') attime(1200,dump,stop) go()