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')
inputs = est2.signal('input')
contactNbr = est2.signal('contactNbr')
# Definition des contacts
contactNbr.value = 2
est2.setContactModel(1)
appli.robot.addTrace( zmpEst.name, 'zmp')
appli.robot.addTrace( stabilizer.name, 'stateError')
appli.robot.addTrace( stabilizer.name, 'state')
appli.robot.addTrace( stabilizer.name, 'control')
appli.robot.addTrace( stabilizer.name, 'task')
appli.robot.addTrace( stabilizer.name, 'integratedTask')
#appli.robot.addTrace( robot.device.name, 'control')
#appli.robot.addTrace( robot.device.name, 'state')
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)
stab.constantInertia(True)
plug(stab.control,model.control)
plug(model.com,stab.com)
plug(model.waistHomo,stab.waistHomo)
plug(model.flexOriVect,stab.flexOriVect)
plug(model.comDot,stab.comDot)
plug(model.waistAngVel,stab.waistAngVel)
plug(model.flexAngVelVect,stab.flexAngVelVect)
plug(model.contact1Pos,stab.leftFootPosition)
plug(model.contact2Pos,stab.rightFootPosition)
plug(model.contact1Forces,stab.force_lf)
plug(model.contact2Forces,stab.force_rf)
plug(model.angularMomentum,stab.angularmomentum)
perturbator = VectorPerturbationsGenerator('perturbation')
perturbator.setSinLessMode(True)
vect = perturbator.sin
vect.value = stab.comRef.value
plug (perturbator.sout,stab.perturbationAcc)
perturbator.perturbation.value=(10,0,0)
perturbator.selec.value = '111'
perturbator.setMode(0)
perturbator.setPeriod(1500)
perturbator.activate(True)
stab.start()
stepTime = 7.5
step2Time=15
step3Time=22.5
