robot.after.addSignal(contactLF.task.name+'.error')
robot.after.addSignal('dyn.rf')
robot.after.addSignal('dyn.lf')
robot.after.addSignal('dyn.chest')
robot.after.addSignal('dyn.com')
robot.after.addSignal('sot.forcesNormal')
robot.after.addSignal('dyn.waist')
robot.after.addSignal(taskLim.task.name+'.normalizedPosition')
#-----------------------------------------------------------------------------
# --- RUN --------------------------------------------------------------------
#-----------------------------------------------------------------------------
sot.clear()
contact(contactLF)
contact(contactRF)
''' To avoid problems with solverMotionReduced '''
if (cmp(sot.className,'SolverMotionReduced')==0):
sot._RF_ddx3.value = ((0.,0.,0.,),)
sot._LF_ddx3.value = ((0.,0.,0.,),)
sot.push(taskLim.task.name)
plug(robot.state,sot.position)
q0 = robot.state.value
chest0 = matrix(dyn.chest.value)[0:3,3]
taskCom.feature.selec.value = "11"
taskCom.gain.setByPoint(100,10,0.005,0.8)
