def goto6d(task, position, gain=None):
M = eye(4)
if (len(position) == 3): M[0:3, 3] = position
else: print "Position 6D with rotation ... todo"
task.feature.selec.value = "111111"
if gain != None: task.gain.setConstant(gain)
task.featureDes.position.value = matrixToTuple(M)
def jointPosition(self, joint, t=None):
if t == None: t = self.timeDecimal
p1 = matrix(MocapParser.jointPosition(self, joint, int(floor(t))))
p2 = matrix(MocapParser.jointPosition(self, joint, int(ceil(t))))
dt = self.timeDecimal % 1
p1[0:3, 3] *= (1 - dt)
p1[0:3, 3] += dt * p2[0:3, 3]
return matrixToTuple(p1)
def jointPosition(self,joint,t=None):
if t==None: t=self.timeDecimal
p1=matrix(MocapParser.jointPosition(self,joint,int(floor(t))))
p2=matrix(MocapParser.jointPosition(self,joint,int(ceil(t))))
dt=self.timeDecimal%1
p1[0:3,3]*=(1-dt)
p1[0:3,3]+=dt*p2[0:3,3]
return matrixToTuple(p1)
def goto6d(task, position, gain=None):
M = eye(4)
if len(position) == 3:
M[0:3, 3] = position
else:
print "Position 6D with rotation ... todo"
task.feature.selec.value = "111111"
if gain != None:
task.gain.setConstant(gain)
task.featureDes.position.value = matrixToTuple(M)
def createOpPointModif(self):
self.opPointModif = DynamicOppointModifier('dynopmodif' + self.name)
offset = numpy.eye(4)
offset[0:3, 3] = (0.0, 0.0, 0.0)
self.opPointModif.setTransformation(matrixToTuple(offset))
plug(self.dyn.signal(self.opPoint),
self.opPointModif.signal('positionIN'))
plug(self.dyn.signal('J' + self.opPoint),
self.opPointModif.signal('jacobianIN'))
self.opPointModif.activ = True
def plugEverything(self):
self.dyn.signal(self.opPoint).recompute(0)
self.dyn.signal('J' + self.opPoint).recompute(0)
plug(self.dyn.signal(self.opPoint), self.task.p1)
plug(self.dyn.signal('J' + self.opPoint), self.task.jVel)
if (self.collisionCenter != None):
self.task.p2.value = matrixToTuple(self.collisionCenter)
self.task.di.value = self._di
self.task.ds.value = self._ds
self.task.controlGain.value = self.controlGain
self.task.dt.value = 0.001
def plugEverything(self):
self.dyn.signal(self.opPoint).recompute(0)
self.dyn.signal('J'+self.opPoint).recompute(0)
plug(self.dyn.signal(self.opPoint),self.task.p1)
plug(self.dyn.signal('J'+self.opPoint),self.task.jVel)
if (self.collisionCenter != None):
self.task.p2.value = matrixToTuple(self.collisionCenter)
self.task.di.value = self._di
self.task.ds.value = self._ds
self.task.controlGain.value = self.controlGain
self.task.dt.value = 0.001
@optionalparentheses
def next(): inc()
# --- shortcuts -------------------------------------------------
@optionalparentheses
def q():
print robot.state.__repr__()
@optionalparentheses
def qdot(): print robot.control.__repr__()
@optionalparentheses
def iter(): print 'iter = ',robot.state.time
@optionalparentheses
def status(): print runner.isPlay
@optionalparentheses
def pl(): print matlab( matrixToTuple(zmp.leftSupport()) ).resstr
@optionalparentheses
def pr(): print matlab( matrixToTuple(zmp.rightSupport()) ).resstr
@optionalparentheses
def dump(): history.dumpToOpenHRP('openhrp/screenwash')
#-----------------------------------------------------------------------------
#---- DYN --------------------------------------------------------------------
#-----------------------------------------------------------------------------
modelDir = pkgDataRootDir[robotName]
xmlDir = pkgDataRootDir[robotName]
specificitiesPath = xmlDir + '/HRP2SpecificitiesSmall.xml'
jointRankPath = xmlDir + '/HRP2LinkJointRankSmall.xml'
def pr():
print matlab(matrixToTuple(zmp.rightSupport())).resstr
def pl():
print matlab(matrixToTuple(zmp.leftSupport())).resstr
sot.clear()
contact(contactLF)
contact(contactRF)
taskCom.feature.selec.value = "11"
taskCom.ref = (XCOM, 0.09, 0.8077)
taskCom.gain.setByPoint(200, 10, 0.005, 0.8)
taskrh.feature.selec.value = '111'
taskrh.gain.setConstant(100)
mrh = eye(4)
mrh[0:3, 3] = (0, 0, -0.08)
taskrh.opmodif = matrixToTuple(mrh)
tasklh.feature.selec.value = '111'
tasklh.gain.setByPoint(80, 5, 0.01, .9)
tasklh.opmodif = matrixToTuple(mrh)
#taskHead.gain.setConstant(500)
taskHead.feature.selec.value = '111000'
taskrf.feature.selec.value = '111'
mrf = eye(4)
mrf[0:3, 3] = (0, 0, -0.08)
taskrf.opmodif = matrixToTuple(mrf)
taskrf.feature.frame('desired')
taskrfr.ref = matrixToTuple(eye(4))
@optionalparentheses
def next(): inc()
# --- shortcuts -------------------------------------------------
@optionalparentheses
def q():
print robot.state.__repr__()
@optionalparentheses
def qdot(): print robot.control.__repr__()
@optionalparentheses
def iter(): print 'iter = ',robot.state.time
@optionalparentheses
def status(): print runner.isPlay
@optionalparentheses
def pl(): print matlab( matrixToTuple(zmp.leftSupport()) ).resstr
@optionalparentheses
def pr(): print matlab( matrixToTuple(zmp.rightSupport()) ).resstr
@optionalparentheses
def dump():
history.dumpToOpenHRP('openhrp/screenwash')
attime.addPing( VisualPinger(robot.viewer) )
#-----------------------------------------------------------------------------
#---- DYN --------------------------------------------------------------------
#-----------------------------------------------------------------------------
modelDir = pkgDataRootDir[robotName]
xmlDir = pkgDataRootDir[robotName]
taskCom = TaskDynPD("taskCom")
taskCom.add("featureCom")
plug(dyn.velocity, taskCom.qdot)
taskCom.dt.value = dt
gCom = GainAdaptive("gCom")
plug(taskCom.error, gCom.error)
plug(gCom.gain, taskCom.controlGain)
# --- TASK POSTURE --------------------------------------------------
featurePosture = FeatureGeneric("featurePosture")
featurePostureDes = FeatureGeneric("featurePostureDes")
plug(dyn.position, featurePosture.errorIN)
featurePosture.sdes.value = featurePostureDes.name
featurePosture.jacobianIN.value = matrixToTuple(identity(robotDim))
taskPosture = TaskDynPD("taskPosture")
taskPosture.add("featurePosture")
plug(dyn.velocity, taskPosture.qdot)
taskPosture.dt.value = dt
taskPosture.controlGain.value = 2
def gotoq(qref, selec=None):
featurePostureDes.errorIN.value = vectorToTuple(qref.transpose())
if selec != None:
featurePosture.selec.value = toFlags(selec)
taskCom = TaskDynPD('taskCom')
taskCom.add('featureCom')
plug(dyn.velocity, taskCom.qdot)
taskCom.dt.value = dt
gCom = GainAdaptive('gCom')
plug(taskCom.error, gCom.error)
plug(gCom.gain, taskCom.controlGain)
# --- TASK POSTURE --------------------------------------------------
featurePosture = FeatureGeneric('featurePosture')
featurePostureDes = FeatureGeneric('featurePostureDes')
plug(dyn.position, featurePosture.errorIN)
featurePosture.sdes.value = featurePostureDes.name
featurePosture.jacobianIN.value = matrixToTuple(identity(robotDim))
taskPosture = TaskDynPD('taskPosture')
taskPosture.add('featurePosture')
plug(dyn.velocity, taskPosture.qdot)
taskPosture.dt.value = dt
taskPosture.controlGain.value = 2
def gotoq(qref, selec=None):
featurePostureDes.errorIN.value = vectorToTuple(qref.transpose())
if selec != None: featurePosture.selec.value = toFlags(selec)
qref = zeros((robotDim, 1))
mp.update()
sot.clear()
contact(contactLF)
contact(contactRF)
taskCom.feature.selec.value = "11"
taskCom.ref = ( XCOM, 0.09, 0.8077 )
taskCom.gain.setByPoint(200,10,0.005,0.8)
taskrh.feature.selec.value = '111'
taskrh.gain.setConstant(100)
mrh=eye(4)
mrh[0:3,3] = (0,0,-0.08)
taskrh.opmodif = matrixToTuple(mrh)
tasklh.feature.selec.value = '111'
tasklh.gain.setByPoint(80,5,0.01,.9)
tasklh.opmodif = matrixToTuple(mrh)
#taskHead.gain.setConstant(500)
taskHead.feature.selec.value = '111000'
taskrf.feature.selec.value = '111'
mrf=eye(4)
mrf[0:3,3] = (0,0,-0.08)
taskrf.opmodif = matrixToTuple(mrf)
taskrf.feature.frame('desired')
taskrfr.ref = matrixToTuple(eye(4))
# --- TASK COM ---
featureCom = FeatureGeneric('featureCom')
plug(dyn.com,featureCom.errorIN)
plug(dyn.Jcom,featureCom.jacobianIN)
featureComDes = FeatureGeneric('featureComDes')
featureCom.sdes.value = 'featureComDes'
plug(comRef.ref,featureComDes.errorIN)
featureCom.selec.value = '011'
taskCom = Task('taskCom')
taskCom.add('featureCom')
taskCom.controlGain.value = 40
# --- TASK POSTURE ---
featurePosture = FeatureGeneric('featurePosture')
plug(dyn.position,featurePosture.errorIN)
featurePosture.jacobianIN.value = matrixToTuple(eye(robotDim))
featurePostureDes = FeatureGeneric('featurePostureDes')
featurePosture.sdes.value = 'featurePostureDes'
featurePostureDes.errorIN.value = q[1]
gainPosture = GainAdaptive('gainPosture')
gainPosture.setByPoint(10,0.1,0.1,0.8)
taskPosture = Task('taskPosture')
taskPosture.add('featurePosture')
plug(taskPosture.error,gainPosture.error)
plug(gainPosture.gain,taskPosture.controlGain)
# --- TASK RIGHT FOOT
# Task right hand
#plug(pg.rightfootref,taskRF.featureDes.position)
for task in [ taskWaist, taskChest, taskHead, taskrh, tasklh, taskrf, taskrfz ]:
task.feature.frame('current')
task.gain.setConstant(50)
task.task.dt.value = dt
# --- TASK COM ------------------------------------------------------
taskCom = MetaTaskDynCom(dyn,dt)
# --- TASK POSTURE --------------------------------------------------
featurePosture = FeatureGeneric('featurePosture')
featurePostureDes = FeatureGeneric('featurePostureDes')
plug(dyn.position,featurePosture.errorIN)
featurePosture.setReference(featurePostureDes.name)
featurePosture.jacobianIN.value = matrixToTuple( identity(robotDim) )
taskPosture = TaskDynPD('taskPosture')
taskPosture.add('featurePosture')
plug(dyn.velocity,taskPosture.qdot)
taskPosture.dt.value = dt
taskPosture.controlGain.value = 2
def gotoq( qref,selec=None ):
featurePostureDes.errorIN.value = vectorToTuple( qref.transpose() )
if selec!=None: featurePosture.selec.value = toFlags( selec )
qref = zeros((robotDim,1))
qref[29] = -0.95
gotoq(qref,[29])
# --- TASK COM ---
featureCom = FeatureGeneric('featureCom')
plug(dyn.com, featureCom.errorIN)
plug(dyn.Jcom, featureCom.jacobianIN)
featureComDes = FeatureGeneric('featureComDes')
featureCom.sdes.value = 'featureComDes'
plug(comRef.ref, featureComDes.errorIN)
featureCom.selec.value = '011'
taskCom = Task('taskCom')
taskCom.add('featureCom')
taskCom.controlGain.value = 40
# --- TASK POSTURE ---
featurePosture = FeatureGeneric('featurePosture')
plug(dyn.position, featurePosture.errorIN)
featurePosture.jacobianIN.value = matrixToTuple(eye(robotDim))
featurePostureDes = FeatureGeneric('featurePostureDes')
featurePosture.sdes.value = 'featurePostureDes'
featurePostureDes.errorIN.value = q[1]
gainPosture = GainAdaptive('gainPosture')
gainPosture.setByPoint(10, 0.1, 0.1, 0.8)
taskPosture = Task('taskPosture')
taskPosture.add('featurePosture')
plug(taskPosture.error, gainPosture.error)
plug(gainPosture.gain, taskPosture.controlGain)
# --- TASK RIGHT FOOT
# Task right hand
#plug(pg.rightfootref,taskRF.featureDes.position)
def refresh(self):
if self.started:
M = array(self.sig.value)
M[0:2,3] = self.point()
self.sig.value = matrixToTuple(M)