class HandCompensaterOscillator(Application):
threePhaseScrew = True
tracesRealTime = True
def __init__(self,robot,twoHands = True,trunkStabilize = True):
Application.__init__(self,robot)
self.twoHands = twoHands
self.trunkStabilize = trunkStabilize
self.robot = robot
plug(self.robot.dynamic.com,self.robot.device.zmp)
self.sot = self.solver.sot
self.createTasks()
self.initTasks()
self.initTaskGains()
self.initOscillator()
self.initialStack()
# --- TASKS --------------------------------------------------------------------
# --- TASKS --------------------------------------------------------------------
# --- TASKS --------------------------------------------------------------------
def createTasks(self):
(self.tasks['trunk'],self.gains['trunk'])= createTrunkTask (self.robot, self, 'Tasktrunk')
self.taskbalance = self.tasks['balance']
self.taskRH = self.tasks['right-wrist']
self.taskLH = self.tasks['left-wrist']
self.taskPosture = self.tasks['posture']
self.taskTrunk = self.tasks['trunk']
self.taskHalfStitting = MetaTaskPosture(self.robot.dynamic,'halfsitting')
#initialization is separated from the creation of the tasks because if we want to switch
#to second order controlm the initialization will remain while the creation is
#changed
def initTasks(self):
self.initTaskTrunk()
self.initTaskPosture()
#self.initTaskHalfSitting()
self.initTaskCompensate()
#def initTaskHalfSitting(self):
# self.taskHalfStitting.gotoq(None,self.robot.halfSitting)
def initTaskTrunk(self):
# --- BALANCE ---
self.features['chest'].frame('desired')
self.features['waist'].frame('desired')
self.features['gaze'].frame('desired')
#self.taskChest.feature.selec.value = '111111'
self.features['chest'].selec.value = '111000'
self.features['waist'].selec.value = '111100'
self.features['gaze'].selec.value = '111000'
self.featureCom.selec.value = '011'
def setOsciFreq(self, f):
self.oscillatorRoll.omega.value = f * 3.1415
self.oscillatorPitch.omega.value = f * 3.1415
def setOsciMagni(self,m):
self.oscillatorRoll.magnitude.value = m
self.oscillatorPitch.magnitude.value = m
def initOscillator(self):
self.oscillatorRoll = Oscillator('oscillatorRoll')
self.oscillatorRoll.setContinuous(True)
self.oscillatorRoll.setActivated(True)
self.oscillatorRoll.setTimePeriod(self.robot.timeStep)
self.oscillatorRoll.setActivated(False)
self.oscillatorRoll.magnitude.value = 0.1
self.oscillatorRoll.phase.value = 0.0
self.oscillatorRoll.omega.value = 0.75
self.oscillatorPitch = Oscillator('oscillatorPitch')
self.oscillatorPitch.setContinuous(True)
self.oscillatorPitch.setActivated(True)
self.oscillatorPitch.setTimePeriod(self.robot.timeStep)
self.oscillatorPitch.setActivated(False)
self.oscillatorPitch.magnitude.value = 0.1
self.oscillatorPitch.phase.value = 1.57
self.oscillatorPitch.omega.value = 0.75
self.stackRP = Stack_of_vector('StackOscRollPitch')
plug ( self.oscillatorRoll.vectorSout, self.stackRP.sin1 )
plug ( self.oscillatorPitch.vectorSout, self.stackRP.sin2 )
self.stackRP.selec1(0,1)
self.stackRP.selec2(0,1)
self.stackRPY = Stack_of_vector('StackOscRollPitchYaw')
plug ( self.stackRP.sout, self.stackRPY.sin1 )
self.stackRPY.sin2.value = (0.0,)
self.stackRPY.selec1(0,2)
self.stackRPY.selec2(0,1)
self.stackPoseRPY = Stack_of_vector('StackOscPoseRollPitchYaw')
self.stackPoseRPY.sin1.value = (0.0,0.0,0.0)
plug ( self.stackRPY.sout, self.stackPoseRPY.sin2 )
self.stackPoseRPY.selec1(0,3)
self.stackPoseRPY.selec2(0,3)
self.poseRPYaw2Homo = PoseRollPitchYawToMatrixHomo('OscPoseRPYaw2Homo')
plug ( self.stackPoseRPY.sout , self.poseRPYaw2Homo.sin)
self.headRef = Multiply_of_matrixHomo('headRef')
self.headRef.sin1.value = self.robot.dynamic.signal('gaze').value
plug( self.poseRPYaw2Homo.sout, self.headRef.sin2)
plug( self.headRef.sout, self.features['gaze'].reference)
self.chestRef = Multiply_of_matrixHomo('chestRef')
self.chestRef.sin1.value = self.robot.dynamic.signal('chest').value
plug( self.poseRPYaw2Homo.sout, self.chestRef.sin2)
plug( self.chestRef.sout, self.features['chest'].reference)
self.waistRef = Multiply_of_matrixHomo('waistRef')
self.waistRef.sin1.value = self.robot.dynamic.signal('waist').value
plug( self.poseRPYaw2Homo.sout, self.waistRef.sin2)
plug( self.waistRef.sout, self.features['waist'].reference)
def initTaskPosture(self):
# --- LEAST NORM
weight_ff = 0
weight_leg = 3
weight_knee = 5
weight_chest = 1
weight_chesttilt = 10
weight_head = 0.3
weight_arm = 1
weight = diag( (weight_ff,)*6 + (weight_leg,)*12 + (weight_chest,)*2 + (weight_head,)*2 + (weight_arm,)*14)
weight[9,9] = weight_knee
weight[15,15] = weight_knee
weight[19,19] = weight_chesttilt
#weight = weight[6:,:]
self.featurePosture.jacobianIN.value = matrixToTuple(weight)
self.featurePostureDes.errorIN.value = self.robot.halfSitting
mask = '1'*36
# mask = 6*'0'+12*'0'+4*'1'+14*'0'
# mask = '000000000000111100000000000000000000000000'
# robot.dynamic.displaySignals ()
# robot.dynamic.Jchest.value
self.features['posture'].selec.value = mask
def initTaskGains(self, setup = "medium"):
if setup == "medium":
self.gains['balance'].setConstant(10)
self.gains['trunk'].setConstant(10)
self.gains['right-wrist'].setByPoint(4,0.2,0.01,0.8)
self.gains['left-wrist'].setByPoint(4,0.2,0.01,0.8)
self.taskHalfStitting.gain.setByPoint(2,0.2,0.01,0.8)
# --- SOLVER ----------------------------------------------------------------
def push(self,task,feature=None,keep=False):
if isinstance(task,str): taskName=task
elif "task" in task.__dict__: taskName=task.task.name
else: taskName=task.name
if taskName not in toList(self.sot):
self.sot.push(taskName)
if taskName!="posture" and "posture" in toList(self.sot):
self.sot.down("posture")
if keep: feature.keep()
def rm(self,task):
if isinstance(task,str): taskName=task
elif "task" in task.__dict__: taskName=task.task.name
else: taskName=task.name
if taskName in toList(self.sot): self.sot.remove(taskName)
# --- DISPLAY -------------------------------------------------------------
def initDisplay(self):
self.robot.device.viewer.updateElementConfig('red',[0,0,-1,0,0,0])
self.robot.device.viewer.updateElementConfig('yellow',[0,0,-1,0,0,0])
def updateDisplay(self):
'''Display the various objects corresponding to the calcul graph. '''
None
# --- TRACES -----------------------------------------------------------
def withTraces(self):
if self.tracesRealTime:
self.robot.tracerSize = 2**26
self.robot.initializeTracer()
else:
self.robot.tracer = Tracer('trace')
self.robot.device.after.addSignal('{0}.triger'.format(self.robot.tracer.name))
self.robot.tracer.open('/tmp/','','.dat')
#self.robot.tracer.add( self.taskRH.task.name+'.error','erh' )
def stopTracer(self):
self.robot.stopTracer()
def dumpTracer(self):
self.robot.tracer.dump()
def startTracer(self):
self.robot.startTracer()
# --- RUN --------------------------------------------------------------
def initialStack(self):
self.sot.clear()
self.push(self.tasks['balance'])
self.push(self.taskTrunk)
#self.push(self.taskPosture)
def moveToInit(self):
'''Go to initial pose.'''
#gotoNd(self.taskRH,(0.3,-0.2,1.1,0,-pi/2,0),'111001')
#gotoNd(self.taskLH,(0.3,0.2,1.1,0,-pi/2,0),'111001')
change6dPositionReference(self.taskRH,self.features['right-wrist'],\
self.gains['right-wrist'],\
(0.3,-0.3,1.1,0,-pi/2,0),'111111')
self.push(self.taskRH)
if self.twoHands:
change6dPositionReference(self.taskLH,self.features['left-wrist'],\
self.gains['left-wrist'],\
(0.3,0.3,1.1,0,-pi/2,0),'111111')
self.push(self.taskLH)
None
def goHalfSitting(self):
'''End of application, go to final pose.'''
self.featurePostureDes.errorIN.value = self.robot.halfSitting
self.sot.clear()
self.push(self.tasks['balance'])
self.push(self.taskPosture)
# --- SEQUENCER ---
seqstep = 0
def nextStep(self,step=None):
if step!=None: self.seqstep = step
if self.seqstep==0:
self.moveToInit()
elif self.seqstep==1:
self.startCompensate()
elif self.seqstep==2:
self.startOcillation()
elif self.seqstep==3:
self.stopOcillation()
elif self.seqstep==4:
self.goHalfSitting()
self.seqstep += 1
def __add__(self,i):
self.nextStep()
# COMPENATION ######################################
def initTaskCompensate(self):
# The constraint is:
# cMhref !!=!! cMh = cMcc ccMh
# or written in ccMh
# ccMh !!=!! ccMc cMhref
# c : central frame of the robot
# cc : central frame for the controller (without the flexibility)
# cMcc= flexibility
# ccMc= flexibility inverted
self.transformerR = MovingFrameTransformation('tranformation_right')
self.ccMc = self.transformerR.gMl # inverted flexibility
self.cMrhref = self.transformerR.lM0 # reference position in the world control frame
# You need to set up the inverted flexibility : plug( ..., self.ccMc)
# You need to set up a reference value here: plug( ... ,self.cMhref)
self.ccVc = self.transformerR.gVl # inverted flexibility velocity
self.cVrhref = self.transformerR.lV0 # reference velocity in the world control frame
# You need to set up the inverted flexibility velocity : plug( ..., self.ccVc)
# You need to set up a reference velocity value here: plug( ... ,self.cVhref)
self.ccMrhref = self.transformerR.gM0 # reference matrix h**o in the control frame
self.ccVrhref = self.transformerR.gV0
######
if self.twoHands:
self.transformerL = MovingFrameTransformation('tranformation_left')
self.sym = Multiply_of_matrixHomo('sym')
self.sym.sin1.value =((1, 0, 0, 0), (0, -1, 0, 0), (0, 0, 1, 0), (0, 0, 0, 1))
plug (self.ccMrhref,self.sym.sin2)
self.symVel = Multiply_matrix_vector('symvel')
self.symVel.sin1.value =((1,0,0,0,0,0),(0,-1,0,0,0,0),(0,0,0,1,0,0),(0,0,0,1,0,0),(0,0,0,0,-1,0),(0,0,0,0,0,1))
plug (self.ccVrhref,self.symVel.sin2)
self.cMrhref.value = (matrixToTuple(diag([1,1,1,1])))
self.cVrhref.value = (0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0)
def startCompensate(self):
'''Start to compensate for the hand movements.'''
self.cMrhref.value = self.robot.dynamic.signal('right-wrist').value
self.cVrhref.value = (0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0)
plug(self.ccMrhref,self.features['right-wrist'].reference)
plug(self.ccVrhref,self.features['right-wrist'].velocity)
self.gains['right-wrist'].setByPoint(4,0.2,0.01,0.8)
self.tasks['right-wrist'].setWithDerivative (True)
self.features['right-wrist'].frame('desired')
print matrix(self.cMrhref.value)
if self.twoHands:
self.gains['left-wrist'].setByPoint(4,0.2,0.01,0.8)
plug (self.sym.sout,self.features['left-wrist'].reference)
plug (self.symVel.sout,self.features['left-wrist'].velocity)
self.features['left-wrist'].selec.value='000111'
self.tasks['left-wrist'].setWithDerivative (True)
self.features['left-wrist'].frame('desired')
#self.tasks['gaze'].setWithDerivative (True)
#######
#self.cMlhref.value = self.robot.dynamic.lh.value
#self.cVlhref.value = (0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0)
#print matrix(self.cMlhref.value)
######
def startOcillation(self):
self.oscillatorRoll.setActivated(True)
self.oscillatorPitch.setActivated(True)
def stopOcillation(self):
self.oscillatorRoll.setActivated(False)
self.oscillatorPitch.setActivated(False)
