plug(robot.state, dyn.position) dyn.velocity.value = robotDim * (0., ) dyn.acceleration.value = robotDim * (0., ) dyn.ffposition.unplug() dyn.ffvelocity.unplug() dyn.ffacceleration.unplug() robot.control.unplug() # --- PG --------------------------------------------------------- from dynamic_graph.sot.pattern_generator import PatternGenerator, Selector pg = PatternGenerator('pg') pg.setVrmlDir(modelDir + '/') pg.setVrml(modelName[robotName]) pg.setXmlSpec(specificitiesPath) pg.setXmlRank(jointRankPath) pg.buildModel() # Standard initialization pg.parseCmd(":samplingperiod 0.005") pg.parseCmd(":previewcontroltime 1.6") pg.parseCmd(":comheight 0.814") pg.parseCmd(":omega 0.0") pg.parseCmd(":stepheight 0.05") 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")
plug(robot.state,dyn.position) dyn.velocity.value = robotDim*(0.,) dyn.acceleration.value = robotDim*(0.,) dyn.ffposition.unplug() dyn.ffvelocity.unplug() dyn.ffacceleration.unplug() robot.control.unplug() # --- PG --------------------------------------------------------- from dynamic_graph.sot.pattern_generator import PatternGenerator,Selector pg = PatternGenerator('pg') pg.setVrmlDir(modelDir+'/') pg.setVrml(modelName[robotName]) pg.setXmlSpec(specificitiesPath) pg.setXmlRank(jointRankPath) pg.buildModel() # Standard initialization pg.parseCmd(":samplingperiod 0.005") pg.parseCmd(":previewcontroltime 1.6") pg.parseCmd(":comheight 0.814") pg.parseCmd(":omega 0.0") pg.parseCmd(":stepheight 0.05") 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")
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')
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")