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 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
def test_load(self):
epsilon = random()
osc = Oscillator('my oscillator')
osc.setEpsilon(epsilon)
self.assertEqual(osc.getEpsilon(), epsilon)
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
def __init__(self, prefix="", time_period=0.001):
#
# save arguments
#
self.time_period = time_period
self.prefix = prefix
#
# Position
#
for dof in ['x', 'y', 'z']:
# Setup the oscillation entity
self.__dict__['osc_pos_' + dof] = Oscillator(
self.prefix + "osc_pos_" + dof)
self.__dict__['osc_pos_' + dof].setTimePeriod(time_period)
#
# Velocity
#
for dof in ['x', 'y', 'z']:
# Setup the oscillation entity
self.__dict__['osc_vel_' + dof] = Oscillator(
self.prefix + "osc_vel_" + dof)
self.__dict__['osc_vel_' + dof].setTimePeriod(time_period)
# Fully plug the derivatives of above oscillator
# magnitude
self.__dict__['osc_vel_magnitude_' + dof] = Multiply_of_double(
self.prefix + 'osc_vel_magnitude_' + dof)
plug(self.__dict__['osc_pos_' + dof].omega,
self.__dict__['osc_vel_magnitude_' + dof].sin(0))
plug(self.__dict__['osc_pos_' + dof].magnitude,
self.__dict__['osc_vel_magnitude_' + dof].sin(1))
plug(self.__dict__['osc_vel_magnitude_' + dof].sout,
self.__dict__['osc_vel_' + dof].magnitude)
# omega
plug(self.__dict__['osc_pos_' + dof].omega,
self.__dict__['osc_vel_' + dof].omega)
# phase
self.__dict__['osc_vel_phase_' + dof] = Add_of_double(
self.prefix + 'osc_vel_phase_' + dof)
self.pi_by_2 = ConstantDouble(np.pi * 0.5, self.prefix + 'pi_by_2')
plug(self.pi_by_2.sout,
self.__dict__['osc_vel_phase_' + dof].sin(0))
plug(self.__dict__['osc_pos_' + dof].phase,
self.__dict__['osc_vel_phase_' + dof].sin(1))
plug(self.__dict__['osc_vel_phase_' + dof].sout,
self.__dict__['osc_vel_' + dof].phase)
# bias
self.__dict__['osc_vel_' + dof].bias.value = 0.0
# set as activated
self.__dict__['osc_vel_' + dof].setActivated(True)
#
# Create the reference cartesian position and velocity
#
for i, dof in enumerate(['x', 'y', 'z']):
# create a unit vector for each dof
unit_vector = np.array([0.0]*6)
unit_vector[i] = 1.0
self.__dict__['unit_vector_' + dof] = ConstantVector(
unit_vector, self.prefix + 'unit_vector_' + dof)
# multiply the unit vector with the reference oscilator
# position
self.__dict__['des_pos_' + dof] = Multiply_double_vector(
self.prefix + 'des_pos_' + dof)
plug(self.__dict__['osc_pos_' + dof].sout,
self.__dict__['des_pos_' + dof].sin1)
plug(self.__dict__['unit_vector_' + dof].sout,
self.__dict__['des_pos_' + dof].sin2)
# velocity
self.__dict__['des_vel_' + dof] = Multiply_double_vector(
self.prefix + 'des_vel_' + dof)
plug(self.__dict__['osc_vel_' + dof].sout,
self.__dict__['des_vel_' + dof].sin1)
plug(self.__dict__['unit_vector_' + dof].sout,
self.__dict__['des_vel_' + dof].sin2)
#
# Finalize by summing up the 3 cartesian positions and velocities
#
# position
self.add_des_pos_xy = Add_of_vector(self.prefix + 'add_des_pos_xy')
plug(self.des_pos_x.sout, self.add_des_pos_xy.sin(0))
plug(self.des_pos_y.sout, self.add_des_pos_xy.sin(1))
self.add_des_pos = Add_of_vector(self.prefix + 'add_des_pos')
plug(self.add_des_pos_xy.sout, self.add_des_pos.sin(0))
plug(self.des_pos_z.sout, self.add_des_pos.sin(1))
# velocity
self.add_des_vel_xy = Add_of_vector(self.prefix + 'add_des_vel_xy')
plug(self.des_vel_x.sout, self.add_des_vel_xy.sin(0))
plug(self.des_vel_y.sout, self.add_des_vel_xy.sin(1))
self.add_des_vel = Add_of_vector(self.prefix + 'add_des_vel')
plug(self.add_des_vel_xy.sout, self.add_des_vel.sin(0))
plug(self.des_vel_z.sout, self.add_des_vel.sin(1))
# output signals
self.des_pos = self.add_des_pos.sout
self.des_vel = self.add_des_vel.sout
def initOscillator(self):
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 = 0.0
self.oscillatorPitch.omega.value = 0.75
self.oscillatorYaw = Oscillator('oscillatorYaw')
self.oscillatorYaw.setContinuous(True)
self.oscillatorYaw.setActivated(True)
self.oscillatorYaw.setTimePeriod(self.robot.timeStep)
self.oscillatorYaw.setActivated(False)
self.oscillatorYaw.magnitude.value = 0.1
self.oscillatorYaw.phase.value = 1.57
self.oscillatorYaw.omega.value = 0.75
self.minuspitch = Multiply_double_vector('minuspitch')
self.minuspitch.sin1.value = -1
plug (self.oscillatorPitch.vectorSout, self.minuspitch.sin2)
self.chestyaw = Multiply_double_vector('chestyaw')
self.chestyaw.sin1.value = -1
plug (self.oscillatorYaw.vectorSout, self.chestyaw.sin2)
self.stack1 = Stack_of_vector('Stack1')
self.stack1.sin1.value = (0.0,)*6
plug ( self.oscillatorYaw.vectorSout, self.stack1.sin2 )
self.stack1.selec1(0,6)
self.stack1.selec2(0,1)
self.stack2 = Stack_of_vector('Stack2')
plug ( self.stack1.sout, self.stack2.sin1 )
self.stack2.sin2.value = (0.0,)
self.stack2.selec1(0,7)
self.stack2.selec2(0,1)
self.stack3 = Stack_of_vector('Stack3')
plug ( self.stack2.sout, self.stack3.sin1 )
plug ( self.oscillatorPitch.vectorSout, self.stack3.sin2 )
self.stack3.selec1(0,8)
self.stack3.selec2(0,1)
self.stack4 = Stack_of_vector('Stack4')
plug ( self.stack3.sout, self.stack4.sin1 )
self.stack4.sin2.value=(0.0,0.0,0.0)
self.stack4.selec1(0,9)
self.stack4.selec2(0,3)
self.stack5 = Stack_of_vector('Stack5')
plug ( self.stack4.sout, self.stack5.sin1 )
plug ( self.oscillatorYaw.vectorSout, self.stack5.sin2 )
self.stack5.selec1(0,12)
self.stack5.selec2(0,1)
self.stack6 = Stack_of_vector('Stack6')
plug ( self.stack5.sout, self.stack6.sin1 )
self.stack6.sin2.value = (0.0,)
self.stack6.selec1(0,13)
self.stack6.selec2(0,1)
self.stack7 = Stack_of_vector('Stack7')
plug ( self.stack6.sout, self.stack7.sin1 )
plug ( self.minuspitch.sout, self.stack7.sin2 )
self.stack7.selec1(0,14)
self.stack7.selec2(0,1)
self.stack8 = Stack_of_vector('Stack8')
plug ( self.stack7.sout, self.stack8.sin1 )
self.stack8.sin2.value=(0.0,0.0,0.0)
self.stack8.selec1(0,15)
self.stack8.selec2(0,3)
self.stack9 = Stack_of_vector('Stack9')
plug ( self.stack8.sout, self.stack9.sin1 )
plug ( self.chestyaw.sout, self.stack9.sin2 )
self.stack9.selec1(0,18)
self.stack9.selec2(0,1)
self.stack10 = Stack_of_vector('Stack10')
plug ( self.stack9.sout, self.stack10.sin1 )
self.stack10.sin2.value = (0.0,)*17
self.stack10.selec1(0,19)
self.stack10.selec2(0,17)
self.postureRef = self.stack10.sout
