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