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')