import sys
import numpy as np
from dynamic_graph import plug
import dynamic_graph.signal_base as dgsb
from dynamic_graph.sot.core import Stack_of_vector, OpPointModifier, MatrixHomoToPose, MatrixHomoToPoseUTheta,MatrixHomoToPoseRollPitchYaw, Selec_of_vector
from dynamic_graph.sot.application.state_observation.initializations.hrp2_model_base_flex_estimator_imu_force import HRP2ModelBaseFlexEstimatorIMUForce, PositionStateReconstructor
from dynamic_graph.sot.application.stabilizer.scenarii.hand_compensater import HandCompensater
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from dynamic_graph.sot.application.state_observation import InputReconstructor
from dynamic_graph.sot.dynamics.zmp_from_forces import ZmpFromForces
from dynamic_graph.sot.application.stabilizer import VectorPerturbationsGenerator
import time
# Initialisation de l'appli
appli = HandCompensater(robot, True, False)
appli.withTraces()
est= est2 = HRP2ModelBaseFlexEstimatorIMUForce(robot,'forceflextimator')
perturbation = 10;
perturbator = VectorPerturbationsGenerator('perturbator')
perturbator.setMode(2)
perturbator.activate(True)
perturbator.perturbation.value = (perturbation,)*12
perturbator.selec.value = '1'*12
plug(est.sensorStackforce.sout,perturbator.sin)
plug(perturbator.sout,est.sensorStack.sin2)
meas = est2.signal('measurement')
# Launch it with py ../robotViewerLauncher.py +compensater.py +appli.py
import sys
import numpy as np
from dynamic_graph import plug
import dynamic_graph.signal_base as dgsb
from dynamic_graph.sot.core import Stack_of_vector, OpPointModifier, MatrixHomoToPose, MatrixHomoToPoseRollPitchYaw
from dynamic_graph.sot.application.state_observation.initializations.hrp2_model_free_flex_estimator import HRP2ModelFreeFlexEstimator
from dynamic_graph.sot.application.stabilizer.scenarii.hand_compensater import HandCompensater
from dynamic_graph.sot.core.matrix_util import matrixToTuple
import time
appli = HandCompensater(robot, True, True)
appli.withTraces()
est = HRP2ModelFreeFlexEstimator(robot)
meas = est.signal('measurement')
inputs = est.signal('input')
contactNbr = est.signal('contactNbr')
contactNbr.value = 2
contact1 = est.signal('contact1')
contact2 = est.signal('contact2')
rFootPos = MatrixHomoToPose('rFootFrame')
lFootPos = MatrixHomoToPose('lFootFrame')
plug(robot.frames['rightFootForceSensor'].position,rFootPos.sin)
plug(robot.frames['leftFootForceSensor'].position,lFootPos.sin)
plug(rFootPos.sout,contact1)
plug(lFootPos.sout,contact2)
flex=est.signal('flexMatrixInverse')