appli.startTracer()
appli.nextStep()
appli.nextStep(3)
appli.nextStep(2)
# Perturbation Generator on control
perturbatorControl = VectorPerturbationsGenerator('perturbatedControl')
perturbatorControl.setSinLessMode(True)
vect1 = perturbatorControl.sin
vect1.value = (0,0,0,0,0)
plug (perturbatorControl.sout,stabilizer.perturbationAcc)
appli.robot.addTrace( perturbatorControl.name, 'sout')
perturbatorControl.perturbation.value=(1,1,0,1,1)
perturbatorControl.selec.value = '11111'
perturbatorControl.setMode(2)
perturbatorControl.activate(False)
# Perturbation Generator on task
perturbatorTask = VectorPerturbationsGenerator('perturbatedTask')
perturbatorTask.setSinLessMode(True)
vect = perturbatorTask.sin
vect.value = (0,0,0,0,0)
plug (perturbatorTask.sout,stabilizer.perturbationVel)
appli.robot.addTrace( perturbatorTask.name, 'sout')
perturbatorTask.perturbation.value=(1,0,0,0,0)
perturbatorTask.selec.value = '11111'
perturbatorTask.setMode(0)
perturbatorTask.setPeriod(0)
perturbatorTask.activate(False)
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')
inputs = est2.signal('input')
contactNbr = est2.signal('contactNbr')
# Definition des contacts
contactNbr.value = 2
est2.setContactModel(1)
rFootPos = MatrixHomoToPoseUTheta('rFootFramePos')
lFootPos = MatrixHomoToPoseUTheta('lFootFramePos')
