Exemplos de HandCompensater em Python

Exemplo n.º 1

Arquivo: appli_model_base_force_elast_pendulum_two_hands_compensater_hrp.py Projeto: amifsud/sot-stabilizer

import sys
import numpy as np
from dynamic_graph import plug
import dynamic_graph.signal_base as dgsb
from dynamic_graph.sot.core import Stack_of_vector, OpPointModifier, MatrixHomoToPose, MatrixHomoToPoseUTheta,MatrixHomoToPoseRollPitchYaw, Selec_of_vector
from dynamic_graph.sot.application.state_observation.initializations.hrp2_model_base_flex_estimator_imu_force import HRP2ModelBaseFlexEstimatorIMUForce, PositionStateReconstructor 
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')

Exemplo n.º 2

Arquivo: appli_two_hands_compensater_hrp.py Projeto: amifsud/sot-stabilizer

# Launch it with py ../robotViewerLauncher.py +compensater.py +appli.py 
import sys
import numpy as np
from dynamic_graph import plug
import dynamic_graph.signal_base as dgsb
from dynamic_graph.sot.core import Stack_of_vector, OpPointModifier, MatrixHomoToPose, MatrixHomoToPoseRollPitchYaw
from dynamic_graph.sot.application.state_observation.initializations.hrp2_model_free_flex_estimator import HRP2ModelFreeFlexEstimator
from dynamic_graph.sot.application.stabilizer.scenarii.hand_compensater import HandCompensater
from dynamic_graph.sot.core.matrix_util import matrixToTuple
import time

appli = HandCompensater(robot, True, True)
appli.withTraces()

est = HRP2ModelFreeFlexEstimator(robot)

meas = est.signal('measurement')
inputs = est.signal('input')
contactNbr = est.signal('contactNbr')

contactNbr.value = 2
contact1 = est.signal('contact1')
contact2 = est.signal('contact2')
rFootPos = MatrixHomoToPose('rFootFrame')
lFootPos = MatrixHomoToPose('lFootFrame')
plug(robot.frames['rightFootForceSensor'].position,rFootPos.sin)
plug(robot.frames['leftFootForceSensor'].position,lFootPos.sin)
plug(rFootPos.sout,contact1)
plug(lFootPos.sout,contact2)

flex=est.signal('flexMatrixInverse')