Esempio n. 1
0
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)
Esempio n. 2
0
 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)
Esempio n. 3
0
 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)
Esempio n. 4
0
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
Esempio n. 8
0
@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'
Esempio n. 9
0
def pr():
    print matlab(matrixToTuple(zmp.rightSupport())).resstr
Esempio n. 10
0
def pl():
    print matlab(matrixToTuple(zmp.leftSupport())).resstr
Esempio n. 11
0

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))
Esempio n. 12
0
@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]
Esempio n. 13
0
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)

Esempio n. 14
0
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))
Esempio n. 15
0
    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))
Esempio n. 16
0
# --- 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)
Esempio n. 17
0
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])
Esempio n. 18
0
# --- 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)
Esempio n. 19
0
 def refresh(self):
     if self.started:
         M = array(self.sig.value)
         M[0:2,3] = self.point()
         self.sig.value = matrixToTuple(M)