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)
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
simuTime =30
dt = 0.005
logState = np.array([])
logState.resize(simuTime/dt,25)
logControl = np.array([])
logControl.resize(simuTime/dt,6)
logFlexAcc = np.array([])
