appli.robot.addTrace( est3.name,'inovation' )
appli.robot.addTrace( est3.name,'prediction' )
#appli.robot.addTrace( est3.name, 'forcesAndMoments')
appli.robot.addTrace( est3.name, 'flexibilityComputationTime')
#appli.robot.addTrace( est3.name,'flexThetaU' )
#appli.robot.addTrace( est3.name,'flexOmega' )
zmpestimated3 = ZmpFromForces('zmpestimated3')
firstContactForceEst = Selec_of_vector('firstContactForceEst3')
secondContactForceEst = Selec_of_vector('secondContactForceEst3')
firstContactForceEst.selec(0,6)
secondContactForceEst.selec(6,12)
plug ( est3.forcesAndMoments, firstContactForceEst.sin)
plug ( est3.forcesAndMoments, secondContactForceEst.sin)
plug (firstContactForceEst.sout , zmpestimated3.force_0)
plug (secondContactForceEst.sout, zmpestimated3.force_1)
plug (robot.frames['rightFootForceSensor'].position , zmpestimated3.sensorPosition_1)
plug (robot.frames['leftFootForceSensor'].position, zmpestimated3.sensorPosition_0)
appli.robot.addTrace( zmpestimated3.name, 'zmp')
est3.setMeasurementNoiseCovariance(matrixToTuple(np.diag((1e8,)*3+(1e8,)*3)))
est3.setForceVariance(perturbation)
est3.setWithForceSensors(True)
appli.startTracer()
appli.comRef.value = (0.01, 0.0, 0.80)
