Python VectorPerturbationsGenerator.setMode 예제들

예제 #1

파일: appli_model_base_force_elast_pendulum_two_hands_compensater_hrp.py 프로젝트: amifsud/sot-stabilizer

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)
rFootPos = MatrixHomoToPoseUTheta('rFootFramePos')

예제 #2

파일: appli_pg_smc_stabilizer_hrp.py 프로젝트: amifsud/sot-stabilizer

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)

예제 #3

파일: simuNonLinearRotationalTableCartModel.py 프로젝트: amifsud/sot-stabilizer

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
simuTime =30
dt = 0.005

logState = np.array([])
logState.resize(simuTime/dt,25)
logControl = np.array([])
logControl.resize(simuTime/dt,6)