# Position ref main droite
RHandref = MatrixHomoToPoseRollPitchYaw('HomotoPoseRHR')
plug(appli.features['right-wrist'].reference,RHandref.sin)
appli.robot.addTrace( RHandref.name, 'sout')
# Position ref main gauche
LHandref = MatrixHomoToPoseRollPitchYaw('HomotoPoseLHR')
plug(appli.features['left-wrist'].reference,LHandref.sin)
appli.robot.addTrace( LHandref.name, 'sout')
# Position ref main droite
appli.robot.addTrace( 'tranformation_right', 'gV0')
# Position ref main droite
appli.robot.addTrace( 'tranformation_left' , 'gV0')
appli.startTracer()
plug(flex,appli.ccMc)
plug(flexdot,appli.ccVc)
est.setMeasurementNoiseCovariance(matrixToTuple(np.diag((1e-2,)*6)))
est.setVirtualMeasurementsCovariance(1e-4)
appli.gains['trunk'].setConstant(2)
appli.nextStep()
