def sine_generator(amplitude, omega, phase, bias, entityName): # generates a y = a*sin(W.t + phi) osc_pos = Oscillator(entityName + "_pos") osc_pos.setTimePeriod(0.001) plug(omega, osc_pos.omega) plug(amplitude, osc_pos.magnitude) plug(phase, osc_pos.phase) # osc_pos.phase.value = phase osc_pos.bias.value = bias osc_vel = Oscillator(entityName + '_vel') osc_vel.setTimePeriod(0.001) plug(osc_pos.omega, osc_vel.omega) plug(osc_pos.magnitude, osc_vel.magnitude) plug(add_doub_doub_2(osc_pos.phase, pi, entityName + "phase_add"), osc_vel.phase) osc_vel.bias.value = 0 unit_vector_pos = ConstantVector( [0.0, 0.0, 1.0, 0.0, 0.0, 0.0], entityName + "_unit_vector_pos") unit_vector_vel = ConstantVector( [0.0, 0.0, 1.0, 0.0, 0.0, 0.0], entityName + "_unit_vector_vel") pos_traj = mul_double_vec_2( osc_pos.sout, unit_vector_pos.sout, entityName + "_des_position") vel_traj = mul_double_vec_2( osc_vel.sout, unit_vector_vel.sout, entityName + "_des_velocity") return pos_traj, vel_traj
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)
def circular_trajectory_generator(radius_x, radius_z, omega, phase, bias, entityName): """ generates a circular circular_trajectory_generator """ ## Position################################################################ osc_x = Oscillator(entityName + "_posx") osc_x.setTimePeriod(0.001) plug(omega, osc_x.omega) plug(radius_x, osc_x.magnitude) plug(phase, osc_x.phase) osc_x.bias.value = 0.0 osc_xd = Oscillator(entityName + '_velx') osc_xd.setTimePeriod(0.001) plug(osc_x.omega, osc_xd.omega) plug(mul_doub_doub(osc_x.omega, osc_x.magnitude, "dx"), osc_xd.magnitude) #plug(osc_x.magnitude, osc_xd.magnitude) plug(add_doub_doub_2(osc_x.phase, pi, entityName + "phase_addx"), osc_xd.phase) osc_xd.bias.value = 0 osc_z = Oscillator(entityName + "_posz") osc_z.setTimePeriod(0.001) plug(omega, osc_z.omega) plug(radius_z, osc_z.magnitude) plug(add_doub_doub_2(osc_x.phase, pi, entityName + "phase_addx"), osc_z.phase) osc_z.bias.value = bias osc_zd = Oscillator(entityName + '_velz') osc_zd.setTimePeriod(0.001) plug(osc_z.omega, osc_zd.omega) plug(mul_doub_doub(osc_z.omega, osc_z.magnitude, "dz"), osc_zd.magnitude) # plug(osc_z.magnitude, osc_zd.magnitude) plug(add_doub_doub_2(osc_z.phase, pi, entityName + "phase_addz"), osc_zd.phase) osc_zd.bias.value = 0 unit_vector_x = ConstantVector( [1.0, 0.0, 0.0, 0.0, 0.0, 0.0], entityName + "unit_vector_x") unit_vector_z = ConstantVector( [0.0, 0.0, 1.0, 0.0, 0.0, 0.0], entityName + "unit_vector_z") pos_des_x = mul_double_vec_2( osc_x.sout, unit_vector_x.sout, entityName + "sine_des_position_x") pos_des_z = mul_double_vec_2( osc_z.sout, unit_vector_z.sout, entityName + "sine_des_position_z") pos_des = add_vec_vec(pos_des_x, pos_des_z, entityName + "_des_pos") unit_vector_xd = ConstantVector( [1.0, 0.0, 0.0, 0.0, 0.0, 0.0], entityName + "unit_vector_xd") unit_vector_zd = ConstantVector( [0.0, 0.0, 1.0, 0.0, 0.0, 0.0], entityName + "unit_vector_zd") vel_des_x = mul_double_vec_2( osc_xd.sout, unit_vector_xd.sout, entityName + "sine_des_velocity_x") vel_des_z = mul_double_vec_2( osc_zd.sout, unit_vector_zd.sout, entityName + "sine_des_velocity_z") vel_des = add_vec_vec(vel_des_x, vel_des_z, entityName + "_des_vel") return pos_des, vel_des