def initOscillator(self):
self.oscillatorRoll = Oscillator('oscillatorRoll')
self.oscillatorRoll.setContinuous(True)
self.oscillatorRoll.setActivated(True)
self.oscillatorRoll.setTimePeriod(self.robot.timeStep)
self.oscillatorRoll.setActivated(False)
self.oscillatorRoll.magnitude.value = 0.1
self.oscillatorRoll.phase.value = 0.0
self.oscillatorRoll.omega.value = 0.75
self.oscillatorPitch = Oscillator('oscillatorPitch')
self.oscillatorPitch.setContinuous(True)
self.oscillatorPitch.setActivated(True)
self.oscillatorPitch.setTimePeriod(self.robot.timeStep)
self.oscillatorPitch.setActivated(False)
self.oscillatorPitch.magnitude.value = 0.1
self.oscillatorPitch.phase.value = 1.57
self.oscillatorPitch.omega.value = 0.75
self.stackRP = Stack_of_vector('StackOscRollPitch')
plug ( self.oscillatorRoll.vectorSout, self.stackRP.sin1 )
plug ( self.oscillatorPitch.vectorSout, self.stackRP.sin2 )
self.stackRP.selec1(0,1)
self.stackRP.selec2(0,1)
self.stackRPY = Stack_of_vector('StackOscRollPitchYaw')
plug ( self.stackRP.sout, self.stackRPY.sin1 )
self.stackRPY.sin2.value = (0.0,)
self.stackRPY.selec1(0,2)
self.stackRPY.selec2(0,1)
self.stackPoseRPY = Stack_of_vector('StackOscPoseRollPitchYaw')
self.stackPoseRPY.sin1.value = (0.0,0.0,0.0)
plug ( self.stackRPY.sout, self.stackPoseRPY.sin2 )
self.stackPoseRPY.selec1(0,3)
self.stackPoseRPY.selec2(0,3)
self.poseRPYaw2Homo = PoseRollPitchYawToMatrixHomo('OscPoseRPYaw2Homo')
plug ( self.stackPoseRPY.sout , self.poseRPYaw2Homo.sin)
self.headRef = Multiply_of_matrixHomo('headRef')
self.headRef.sin1.value = self.robot.dynamic.signal('gaze').value
plug( self.poseRPYaw2Homo.sout, self.headRef.sin2)
plug( self.headRef.sout, self.features['gaze'].reference)
self.chestRef = Multiply_of_matrixHomo('chestRef')
self.chestRef.sin1.value = self.robot.dynamic.signal('chest').value
plug( self.poseRPYaw2Homo.sout, self.chestRef.sin2)
plug( self.chestRef.sout, self.features['chest'].reference)
self.waistRef = Multiply_of_matrixHomo('waistRef')
self.waistRef.sin1.value = self.robot.dynamic.signal('waist').value
plug( self.poseRPYaw2Homo.sout, self.waistRef.sin2)
plug( self.waistRef.sout, self.features['waist'].reference)
def createContact(self,name, prl):
self.contactOpPoint = OpPointModifier(name+'_opPoint')
self.contactOpPoint.setEndEffector(False)
self.contactOpPoint.setTransformation(matrixToTuple(prl))
plug (self.robot.dynamic.chest,self.contactOpPoint.positionIN)
self.contactPos = MatrixHomoToPose(name+'_pos')
plug(self.contactOpPoint.position, self.contactPos.sin)
self.contact = Stack_of_vector (name)
plug(self.contactPos.sout,self.contact.sin1)
self.contact.sin2.value = (0,0,0)
self.contact.selec1 (0, 3)
self.contact.selec2 (0, 3)
return (self.contactOpPoint,self.contactPos,self.contact)
def __init__(self,robot,taskname = 'com-stabilized'):
from dynamic_graph.sot.application.state_observation.initializations.hrp2_model_base_flex_estimator import HRP2ModelBaseFlexEstimator
HRP2LQRDecoupledStabilizer.__init__(self,taskname)
robot.dynamic.com.recompute(0)
robot.dynamic.Jcom.recompute(0)
plug (robot.dynamic.com, self.com)
plug (robot.dynamic.Jcom, self.Jcom)
plug (robot.device.forceLLEG,self.force_lf)
plug (robot.device.forceRLEG,self.force_rf)
plug (robot.frames['rightFootForceSensor'].position,self.rightFootPosition)
plug (robot.frames['leftFootForceSensor'].position,self.leftFootPosition)
#TODO ///// SELEC ??
self.estimator = HRP2ModelBaseFlexEstimator(robot, taskname+"Estimator")
plug (self.nbSupport,self.estimator.contactNbr)
self.estimator.setContactModel(1)
self.contacts = Stack_of_vector (taskname+'contacts')
plug(self.supportPos1,self.contacts.sin1)
plug(self.supportPos2,self.contacts.sin2)
self.contacts.selec1 (0, 6)
self.contacts.selec2 (0, 6)
plug(self.contacts.sout, self.estimator.inputVector.contactsPosition)
plug(self.estimator.flexTransformationMatrix, self.stateFlex )
plug(self.estimator.flexOmega, self.stateFlexDot )
plug(self.estimator.flexOmegaDot, self.stateFlexDDot )
self.comVectorPV = Stack_of_vector (taskname+'ComVectorPV')
self.comVector = Stack_of_vector (taskname+'ComVector')
self.comVectorPV.selec1 (0, 3)
self.comVectorPV.selec2 (0, 3)
self.comVector.selec1 (0, 6)
self.comVector.selec2 (0, 3)
plug(robot.dynamic.com, self.comVectorPV.sin1)
plug(self.comdot, self.comVectorPV.sin2)
plug(self.comVectorPV.sout, self.comVector.sin1)
plug(self.comddot, self.comVector.sin2)
def __init__(self, robot, name='flextimator'):
DGIMUModelFreeFlexEstimation.__init__(self,name)
self.setSamplingPeriod(0.005)
self.robot = robot
self.sensorStack = Stack_of_vector (name+'Sensors')
plug(robot.device.accelerometer,self.sensorStack.sin1)
plug(robot.device.gyrometer,self.sensorStack.sin2)
self.sensorStack.selec1 (0, 3)
self.sensorStack.selec2 (0, 3)
plug(self.sensorStack.sout,self.measurement);
self.inputPos = MatrixHomoToPoseUTheta(name+'InputPosition')
plug(robot.frames['accelerometer'].position,self.inputPos.sin)
robot.dynamic.createJacobian('ChestJ_OpPoint','chest')
self.imuOpPoint = OpPointModifier('IMU_oppoint')
self.imuOpPoint.setEndEffector(False)
self.imuOpPoint.setTransformation(matrixToTuple(np.linalg.inv(np.matrix(self.robot.dynamic.chest.value))*np.matrix(self.robot.frames['accelerometer'].position.value)))
plug (robot.dynamic.chest,self.imuOpPoint.positionIN)
plug (robot.dynamic.signal('ChestJ_OpPoint'),self.imuOpPoint.jacobianIN)
self.inputVel = Multiply_matrix_vector(name+'InputVelocity')
plug(self.imuOpPoint.jacobian,self.inputVel.sin1)
plug(robot.device.velocity,self.inputVel.sin2)
self.inputPosVel = Stack_of_vector (name+'InputPosVel')
plug(self.inputPos.sout,self.inputPosVel.sin1)
plug(self.inputVel.sout,self.inputPosVel.sin2)
self.inputPosVel.selec1 (0, 6)
self.inputPosVel.selec2 (0, 6)
self.inputVector = PositionStateReconstructor (name+'EstimatorInput')
plug(self.inputPosVel.sout,self.inputVector.sin)
self.inputVector.inputFormat.value = '001111'
self.inputVector.outputFormat.value = '011111'
self.inputVector.setFiniteDifferencesInterval(2)
plug(self.inputVector.sout,self.input)
robot.flextimator = self
def createContact(self,name, prl):
self.contactOpPoint = OpPointModifier(name+'_opPoint')
self.contactOpPoint.setEndEffector(False)
self.contactOpPoint.setTransformation(matrixToTuple(prl))
plug (self.robot.dynamic.chest,self.contactOpPoint.positionIN)
self.contactPos = MatrixHomoToPose(name+'_pos')
plug(self.contactOpPoint.position, self.contactPos.sin)
self.contact = Stack_of_vector (name)
plug(self.contactPos.sout,self.contact.sin1)
self.contact.sin2.value = (0,0,0)
self.contact.selec1 (0, 3)
self.contact.selec2 (0, 3)
return (self.contactOpPoint,self.contactPos,self.contact)
def __init__(self, robot, name='flextimator2'):
DGIMUModelBaseFlexEstimation.__init__(self,name)
self.setSamplingPeriod(0.005)
self.robot = robot
# Definition of IMU vector
self.sensorStack = Stack_of_vector (name+'Sensors')
plug(self.robot.device.accelerometer,self.sensorStack.sin1)
plug(self.robot.device.gyrometer,self.sensorStack.sin2)
self.sensorStack.selec1 (0, 3)
self.sensorStack.selec2 (0, 3)
# Calibration
self.calibration= Calibrate('calibration')
plug(self.sensorStack.sout,self.calibration.imuIn)
plug(self.robot.dynamic.com,self.calibration.comIn)
plug(self.calibration.imuOut,self.measurement)
self.inputPos = MatrixHomoToPoseUTheta(name+'InputPosition')
plug(robot.frames['accelerometer'].position,self.inputPos.sin)
self.robot.dynamic.createJacobian('ChestJ_OpPoint','chest')
self.imuOpPoint = OpPointModifier('IMU_oppoint')
self.imuOpPoint.setEndEffector(False)
self.imuOpPoint.setTransformation(matrixToTuple(np.linalg.inv(np.matrix(self.robot.dynamic.chest.value))*np.matrix(self.robot.frames['accelerometer'].position.value)))
plug (self.robot.dynamic.chest,self.imuOpPoint.positionIN)
plug (self.robot.dynamic.signal('ChestJ_OpPoint'),self.imuOpPoint.jacobianIN)
self.inputVel = Multiply_matrix_vector(name+'InputVelocity')
plug(self.imuOpPoint.jacobian,self.inputVel.sin1)
plug(self.robot.device.velocity,self.inputVel.sin2)
self.inputPosVel = Stack_of_vector (name+'InputPosVel')
plug(self.inputPos.sout,self.inputPosVel.sin1)
plug(self.inputVel.sout,self.inputPosVel.sin2)
self.inputPosVel.selec1 (0, 6)
self.inputPosVel.selec2 (0, 6)
self.IMUVector = PositionStateReconstructor (name+'EstimatorInput')
plug(self.inputPosVel.sout,self.IMUVector.sin)
self.IMUVector.inputFormat.value = '001111'
self.IMUVector.outputFormat.value = '011111'
self.IMUVector.setFiniteDifferencesInterval(2)
# Definition of inertia, angular momentum and derivatives
self.robot.dynamic.inertia.recompute(0)
self.inertia=self.robot.dynamic.inertia #(48.2378,48.2378,2.87339,0,0,0) #
self.dotInertia=(0,0,0,0,0,0)
self.zeroMomentum=(0,0,0,0,0,0)
# Waist position
self.robot.dynamic.waist.recompute(0)
#self.robot.dynamic.chest.recompute(1)
#self.robot.dynamic.com.recompute(1)
self.positionWaist=self.robot.dynamic.waist
# Definition of com and derivatives
self.com=self.calibration.comOut #self.robot.dynamic.com
self.DCom = Multiply_matrix_vector(name+'DCom')
self.robot.dynamic.Jcom.recompute(0)
plug(self.robot.dynamic.Jcom,self.DCom.sin1)
plug(self.robot.device.velocity,self.DCom.sin2)
self.comVectorIn = Stack_of_vector (name+'ComVectorIn')
plug(self.calibration.comOut,self.comVectorIn.sin1)
plug(self.DCom.sout,self.comVectorIn.sin2)
self.comVectorIn.selec1 (0, 3)
self.comVectorIn.selec2 (0, 3)
self.comVector = PositionStateReconstructor (name+'ComVector')
plug(self.comVectorIn.sout,self.comVector.sin)
self.comVector.inputFormat.value = '000101'
self.comVector.outputFormat.value = '010101'
# Concatenate with InputReconstructor entity
self.inputVector=InputReconstructor(name+'inputVector')
plug(self.comVector.sout,self.inputVector.comVector)
plug(self.inertia,self.inputVector.inertia)
self.inputVector.dinertia.value=self.dotInertia
plug(self.positionWaist,self.inputVector.positionWaist)
self.inputVector.setSamplingPeriod(robot.timeStep)
self.inputVector.setFDInertiaDot(True)
plug(self.robot.dynamic.angularmomentum,self.inputVector.angMomentum)
self.angMomDerivator = Derivator_of_Vector('angMomDerivator')
plug(self.robot.dynamic.angularmomentum,self.angMomDerivator.sin)
self.angMomDerivator.dt.value = self.robot.timeStep
plug(self.angMomDerivator.sout,self.inputVector.dangMomentum)
#self.inputVector.dangMomentum.value = self.zeroMomentum
plug(self.IMUVector.sout,self.inputVector.imuVector)
plug(self.contactNbr,self.inputVector.nbContacts)
# plug(self.contacts,self.inputVector.contactsPosition)
plug(self.inputVector.input,self.input)
self.robot.flextimator = self
kfe=40000
kfv=600
kte=600
ktv=60
self.setKfe(matrixToTuple(np.diag((kfe,kfe,kfe))))
self.setKfv(matrixToTuple(np.diag((kfv,kfv,kfv))))
self.setKte(matrixToTuple(np.diag((kte,kte,kte))))
self.setKtv(matrixToTuple(np.diag((ktv,ktv,ktv))))
def __init__(self, robot, name='flextimatorEncoders'):
DGIMUModelBaseFlexEstimation.__init__(self, name)
self.setSamplingPeriod(0.005)
self.robot = robot
# Covariances
self.setProcessNoiseCovariance(
matrixToTuple(
np.diag((1e-8, ) * 12 + (1e-4, ) * 3 + (1e-4, ) * 3 +
(1e-4, ) * 3 + (1e-4, ) * 3 + (1.e-2, ) * 6 +
(1e-15, ) * 2 + (1.e-8, ) * 3)))
self.setMeasurementNoiseCovariance(
matrixToTuple(np.diag((1e-3, ) * 3 + (1e-6, ) * 3)))
self.setUnmodeledForceVariance(1e-13)
self.setForceVariance(1e-4)
self.setAbsolutePosVariance(1e-4)
# Contact model definition
self.setContactModel(1)
self.setKfe(matrixToTuple(np.diag((40000, 40000, 40000))))
self.setKfv(matrixToTuple(np.diag((600, 600, 600))))
self.setKte(matrixToTuple(np.diag((600, 600, 600))))
self.setKtv(matrixToTuple(np.diag((60, 60, 60))))
#Estimator interface
self.interface = EstimatorInterface(name + "EstimatorInterface")
self.interface.setLeftHandSensorTransformation((0., 0., 1.57))
self.interface.setRightHandSensorTransformation((0., 0., 1.57))
self.interface.setFDInertiaDot(True)
# State and measurement definition
self.interface.setWithUnmodeledMeasurements(False)
self.interface.setWithModeledForces(True)
self.interface.setWithAbsolutePose(False)
self.setWithComBias(False)
# Contacts forces
plug(self.robot.device.forceLLEG, self.interface.force_lf)
plug(self.robot.device.forceRLEG, self.interface.force_rf)
plug(self.robot.device.forceLARM, self.interface.force_lh)
plug(self.robot.device.forceRARM, self.interface.force_rh)
# Selecting robotState
self.robot.device.robotState.value = 46 * (0., )
self.robotState = Selec_of_vector('robotState')
plug(self.robot.device.robotState, self.robotState.sin)
self.robotState.selec(0, 36)
# Reconstruction of the position of the free flyer from encoders
# Create dynamic with the free flyer at the origin of the control frame
self.robot.dynamicOdo = self.createDynamic(self.robotState.sout,
'_dynamicOdo')
self.robot.dynamicOdo.inertia.recompute(1)
self.robot.dynamicOdo.waist.recompute(1)
# Reconstruction of the position of the contacts in dynamicOdo
self.leftFootPosOdo = Multiply_of_matrixHomo(name + "leftFootPosOdo")
plug(self.robot.dynamicOdo.signal('left-ankle'),
self.leftFootPosOdo.sin1)
self.leftFootPosOdo.sin2.value = self.robot.forceSensorInLeftAnkle
self.rightFootPosOdo = Multiply_of_matrixHomo(name + "rightFootPosOdo")
plug(self.robot.dynamicOdo.signal('right-ankle'),
self.rightFootPosOdo.sin1)
self.rightFootPosOdo.sin2.value = self.robot.forceSensorInRightAnkle
# Odometry
self.odometry = Odometry(name + 'odometry')
plug(self.robot.frames['leftFootForceSensor'].position,
self.odometry.leftFootPositionRef)
plug(self.robot.frames['rightFootForceSensor'].position,
self.odometry.rightFootPositionRef)
plug(self.rightFootPosOdo.sout, self.odometry.rightFootPositionIn)
plug(self.leftFootPosOdo.sout, self.odometry.leftFootPositionIn)
plug(self.robot.device.forceLLEG, self.odometry.force_lf)
plug(self.robot.device.forceRLEG, self.odometry.force_rf)
self.odometry.setLeftFootPosition(
self.robot.frames['leftFootForceSensor'].position.value)
self.odometry.setRightFootPosition(
self.robot.frames['rightFootForceSensor'].position.value)
plug(self.interface.stackOfSupportContacts,
self.odometry.stackOfSupportContacts)
# Create dynamicEncoders
self.robotStateWoFF = Selec_of_vector('robotStateWoFF')
plug(self.robot.device.robotState, self.robotStateWoFF.sin)
self.robotStateWoFF.selec(6, 36)
self.stateEncoders = Stack_of_vector(name + 'stateEncoders')
plug(self.odometry.freeFlyer, self.stateEncoders.sin1)
plug(self.robotStateWoFF.sout, self.stateEncoders.sin2)
self.stateEncoders.selec1(0, 6)
self.stateEncoders.selec2(0, 30)
self.robot.dynamicEncoders = self.createDynamic(
self.stateEncoders.sout, '_dynamicEncoders')
# self.robot.dynamicEncoders=self.createDynamic(self.robotState.sout,'_dynamicEncoders')
# plug(self.odometry.freeFlyer,self.robot.dynamicEncoders.ffposition)
# self.robot.dynamicEncoders=self.createDynamic(self.robot.device.state,'_dynamicEncoders')
# Reconstruction of the position of the contacts in dynamicEncoders
self.leftFootPos = Multiply_of_matrixHomo("leftFootPos")
plug(self.robot.dynamicEncoders.signal('left-ankle'),
self.leftFootPos.sin1)
self.leftFootPos.sin2.value = self.robot.forceSensorInLeftAnkle
self.rightFootPos = Multiply_of_matrixHomo("rightFootPos")
plug(self.robot.dynamicEncoders.signal('right-ankle'),
self.rightFootPos.sin1)
self.rightFootPos.sin2.value = self.robot.forceSensorInRightAnkle
# Contacts velocities
self.leftFootVelocity = Multiply_matrix_vector('leftFootVelocity')
plug(self.robot.frames['leftFootForceSensor'].jacobian,
self.leftFootVelocity.sin1)
plug(self.robot.dynamicEncoders.velocity, self.leftFootVelocity.sin2)
self.rightFootVelocity = Multiply_matrix_vector('rightFootVelocity')
plug(self.robot.frames['rightFootForceSensor'].jacobian,
self.rightFootVelocity.sin1)
plug(self.robot.dynamicEncoders.velocity, self.rightFootVelocity.sin2)
# Contacts positions and velocities
plug(self.leftFootPos.sout, self.interface.position_lf)
plug(self.rightFootPos.sout, self.interface.position_rf)
plug(self.leftFootVelocity.sout, self.interface.velocity_lf)
plug(self.rightFootVelocity.sout, self.interface.velocity_rf)
plug(self.robot.dynamicEncoders.signal('right-wrist'),
self.interface.position_lh)
plug(self.robot.dynamicEncoders.signal('left-wrist'),
self.interface.position_rh)
# Compute contacts number
plug(self.interface.supportContactsNbr, self.contactNbr)
# Contacts model and config
plug(self.interface.contactsModel, self.contactsModel)
self.setWithConfigSignal(True)
plug(self.interface.config, self.config)
# Drift
self.drift = DriftFromMocap(name + 'Drift')
# Compute measurement vector
plug(self.robot.device.accelerometer, self.interface.accelerometer)
plug(self.robot.device.gyrometer, self.interface.gyrometer)
plug(self.drift.driftVector, self.interface.drift)
plug(self.interface.measurement, self.measurement)
# Input reconstruction
# IMU Vector
# Creating an operational point for the IMU
self.robot.dynamicEncoders.createJacobian(name + 'ChestJ_OpPoint',
'chest')
self.imuOpPoint = OpPointModifier(name + 'IMU_oppoint')
self.imuOpPoint.setTransformation(
matrixToTuple(
np.linalg.inv(np.matrix(
self.robot.dynamicEncoders.chest.value)) *
np.matrix(self.robot.frames['accelerometer'].position.value)))
self.imuOpPoint.setEndEffector(False)
plug(self.robot.dynamicEncoders.chest, self.imuOpPoint.positionIN)
plug(self.robot.dynamicEncoders.signal(name + 'ChestJ_OpPoint'),
self.imuOpPoint.jacobianIN)
# IMU position
self.PosAccelerometer = Multiply_of_matrixHomo(name +
"PosAccelerometer")
plug(self.robot.dynamicEncoders.chest, self.PosAccelerometer.sin1)
self.PosAccelerometer.sin2.value = matrixToTuple(
self.robot.accelerometerPosition)
self.inputPos = MatrixHomoToPoseUTheta(name + 'InputPosition')
plug(self.PosAccelerometer.sout, self.inputPos.sin)
# IMU velocity
self.inputVel = Multiply_matrix_vector(name + 'InputVelocity')
plug(self.imuOpPoint.jacobian, self.inputVel.sin1)
plug(self.robot.dynamicEncoders.velocity, self.inputVel.sin2)
# Concatenate
self.inputPosVel = Stack_of_vector(name + 'InputPosVel')
plug(self.inputPos.sout, self.inputPosVel.sin1)
plug(self.inputVel.sout, self.inputPosVel.sin2)
self.inputPosVel.selec1(0, 6)
self.inputPosVel.selec2(0, 6)
# IMU Vector
self.IMUVector = PositionStateReconstructor(name + 'EstimatorInput')
plug(self.inputPosVel.sout, self.IMUVector.sin)
self.IMUVector.inputFormat.value = '001111'
self.IMUVector.outputFormat.value = '011111'
self.IMUVector.setFiniteDifferencesInterval(2)
# CoM and derivatives
self.comIn = self.robot.dynamicEncoders.com
self.comVector = PositionStateReconstructor(name + 'ComVector')
plug(self.comIn, self.comVector.sin)
self.comVector.inputFormat.value = '000001'
self.comVector.outputFormat.value = '010101'
self.comVector.setFiniteDifferencesInterval(20)
# Compute derivative of Angular Momentum
self.angMomDerivator = Derivator_of_Vector(name + 'angMomDerivator')
plug(self.robot.dynamicEncoders.angularmomentum,
self.angMomDerivator.sin)
self.angMomDerivator.dt.value = self.robot.timeStep
# Concatenate with interace estimator
plug(self.comVector.sout, self.interface.comVector)
plug(self.robot.dynamicEncoders.inertia, self.interface.inertia)
plug(self.robot.dynamicEncoders.angularmomentum,
self.interface.angMomentum)
plug(self.angMomDerivator.sout, self.interface.dangMomentum)
self.interface.dinertia.value = (0, 0, 0, 0, 0, 0)
plug(self.robot.dynamicEncoders.waist, self.interface.positionWaist)
plug(self.IMUVector.sout, self.interface.imuVector)
plug(self.interface.input, self.input)
self.robot.flextimator = self
class HRP2ModelBaseFlexEstimatorIMUForceEncoders(DGIMUModelBaseFlexEstimation):
def __init__(self, robot, name='flextimatorEncoders'):
DGIMUModelBaseFlexEstimation.__init__(self, name)
self.setSamplingPeriod(0.005)
self.robot = robot
# Covariances
self.setProcessNoiseCovariance(
matrixToTuple(
np.diag((1e-8, ) * 12 + (1e-4, ) * 3 + (1e-4, ) * 3 +
(1e-4, ) * 3 + (1e-4, ) * 3 + (1.e-2, ) * 6 +
(1e-15, ) * 2 + (1.e-8, ) * 3)))
self.setMeasurementNoiseCovariance(
matrixToTuple(np.diag((1e-3, ) * 3 + (1e-6, ) * 3)))
self.setUnmodeledForceVariance(1e-13)
self.setForceVariance(1e-4)
self.setAbsolutePosVariance(1e-4)
# Contact model definition
self.setContactModel(1)
self.setKfe(matrixToTuple(np.diag((40000, 40000, 40000))))
self.setKfv(matrixToTuple(np.diag((600, 600, 600))))
self.setKte(matrixToTuple(np.diag((600, 600, 600))))
self.setKtv(matrixToTuple(np.diag((60, 60, 60))))
#Estimator interface
self.interface = EstimatorInterface(name + "EstimatorInterface")
self.interface.setLeftHandSensorTransformation((0., 0., 1.57))
self.interface.setRightHandSensorTransformation((0., 0., 1.57))
self.interface.setFDInertiaDot(True)
# State and measurement definition
self.interface.setWithUnmodeledMeasurements(False)
self.interface.setWithModeledForces(True)
self.interface.setWithAbsolutePose(False)
self.setWithComBias(False)
# Contacts forces
plug(self.robot.device.forceLLEG, self.interface.force_lf)
plug(self.robot.device.forceRLEG, self.interface.force_rf)
plug(self.robot.device.forceLARM, self.interface.force_lh)
plug(self.robot.device.forceRARM, self.interface.force_rh)
# Selecting robotState
self.robot.device.robotState.value = 46 * (0., )
self.robotState = Selec_of_vector('robotState')
plug(self.robot.device.robotState, self.robotState.sin)
self.robotState.selec(0, 36)
# Reconstruction of the position of the free flyer from encoders
# Create dynamic with the free flyer at the origin of the control frame
self.robot.dynamicOdo = self.createDynamic(self.robotState.sout,
'_dynamicOdo')
self.robot.dynamicOdo.inertia.recompute(1)
self.robot.dynamicOdo.waist.recompute(1)
# Reconstruction of the position of the contacts in dynamicOdo
self.leftFootPosOdo = Multiply_of_matrixHomo(name + "leftFootPosOdo")
plug(self.robot.dynamicOdo.signal('left-ankle'),
self.leftFootPosOdo.sin1)
self.leftFootPosOdo.sin2.value = self.robot.forceSensorInLeftAnkle
self.rightFootPosOdo = Multiply_of_matrixHomo(name + "rightFootPosOdo")
plug(self.robot.dynamicOdo.signal('right-ankle'),
self.rightFootPosOdo.sin1)
self.rightFootPosOdo.sin2.value = self.robot.forceSensorInRightAnkle
# Odometry
self.odometry = Odometry(name + 'odometry')
plug(self.robot.frames['leftFootForceSensor'].position,
self.odometry.leftFootPositionRef)
plug(self.robot.frames['rightFootForceSensor'].position,
self.odometry.rightFootPositionRef)
plug(self.rightFootPosOdo.sout, self.odometry.rightFootPositionIn)
plug(self.leftFootPosOdo.sout, self.odometry.leftFootPositionIn)
plug(self.robot.device.forceLLEG, self.odometry.force_lf)
plug(self.robot.device.forceRLEG, self.odometry.force_rf)
self.odometry.setLeftFootPosition(
self.robot.frames['leftFootForceSensor'].position.value)
self.odometry.setRightFootPosition(
self.robot.frames['rightFootForceSensor'].position.value)
plug(self.interface.stackOfSupportContacts,
self.odometry.stackOfSupportContacts)
# Create dynamicEncoders
self.robotStateWoFF = Selec_of_vector('robotStateWoFF')
plug(self.robot.device.robotState, self.robotStateWoFF.sin)
self.robotStateWoFF.selec(6, 36)
self.stateEncoders = Stack_of_vector(name + 'stateEncoders')
plug(self.odometry.freeFlyer, self.stateEncoders.sin1)
plug(self.robotStateWoFF.sout, self.stateEncoders.sin2)
self.stateEncoders.selec1(0, 6)
self.stateEncoders.selec2(0, 30)
self.robot.dynamicEncoders = self.createDynamic(
self.stateEncoders.sout, '_dynamicEncoders')
# self.robot.dynamicEncoders=self.createDynamic(self.robotState.sout,'_dynamicEncoders')
# plug(self.odometry.freeFlyer,self.robot.dynamicEncoders.ffposition)
# self.robot.dynamicEncoders=self.createDynamic(self.robot.device.state,'_dynamicEncoders')
# Reconstruction of the position of the contacts in dynamicEncoders
self.leftFootPos = Multiply_of_matrixHomo("leftFootPos")
plug(self.robot.dynamicEncoders.signal('left-ankle'),
self.leftFootPos.sin1)
self.leftFootPos.sin2.value = self.robot.forceSensorInLeftAnkle
self.rightFootPos = Multiply_of_matrixHomo("rightFootPos")
plug(self.robot.dynamicEncoders.signal('right-ankle'),
self.rightFootPos.sin1)
self.rightFootPos.sin2.value = self.robot.forceSensorInRightAnkle
# Contacts velocities
self.leftFootVelocity = Multiply_matrix_vector('leftFootVelocity')
plug(self.robot.frames['leftFootForceSensor'].jacobian,
self.leftFootVelocity.sin1)
plug(self.robot.dynamicEncoders.velocity, self.leftFootVelocity.sin2)
self.rightFootVelocity = Multiply_matrix_vector('rightFootVelocity')
plug(self.robot.frames['rightFootForceSensor'].jacobian,
self.rightFootVelocity.sin1)
plug(self.robot.dynamicEncoders.velocity, self.rightFootVelocity.sin2)
# Contacts positions and velocities
plug(self.leftFootPos.sout, self.interface.position_lf)
plug(self.rightFootPos.sout, self.interface.position_rf)
plug(self.leftFootVelocity.sout, self.interface.velocity_lf)
plug(self.rightFootVelocity.sout, self.interface.velocity_rf)
plug(self.robot.dynamicEncoders.signal('right-wrist'),
self.interface.position_lh)
plug(self.robot.dynamicEncoders.signal('left-wrist'),
self.interface.position_rh)
# Compute contacts number
plug(self.interface.supportContactsNbr, self.contactNbr)
# Contacts model and config
plug(self.interface.contactsModel, self.contactsModel)
self.setWithConfigSignal(True)
plug(self.interface.config, self.config)
# Drift
self.drift = DriftFromMocap(name + 'Drift')
# Compute measurement vector
plug(self.robot.device.accelerometer, self.interface.accelerometer)
plug(self.robot.device.gyrometer, self.interface.gyrometer)
plug(self.drift.driftVector, self.interface.drift)
plug(self.interface.measurement, self.measurement)
# Input reconstruction
# IMU Vector
# Creating an operational point for the IMU
self.robot.dynamicEncoders.createJacobian(name + 'ChestJ_OpPoint',
'chest')
self.imuOpPoint = OpPointModifier(name + 'IMU_oppoint')
self.imuOpPoint.setTransformation(
matrixToTuple(
np.linalg.inv(np.matrix(
self.robot.dynamicEncoders.chest.value)) *
np.matrix(self.robot.frames['accelerometer'].position.value)))
self.imuOpPoint.setEndEffector(False)
plug(self.robot.dynamicEncoders.chest, self.imuOpPoint.positionIN)
plug(self.robot.dynamicEncoders.signal(name + 'ChestJ_OpPoint'),
self.imuOpPoint.jacobianIN)
# IMU position
self.PosAccelerometer = Multiply_of_matrixHomo(name +
"PosAccelerometer")
plug(self.robot.dynamicEncoders.chest, self.PosAccelerometer.sin1)
self.PosAccelerometer.sin2.value = matrixToTuple(
self.robot.accelerometerPosition)
self.inputPos = MatrixHomoToPoseUTheta(name + 'InputPosition')
plug(self.PosAccelerometer.sout, self.inputPos.sin)
# IMU velocity
self.inputVel = Multiply_matrix_vector(name + 'InputVelocity')
plug(self.imuOpPoint.jacobian, self.inputVel.sin1)
plug(self.robot.dynamicEncoders.velocity, self.inputVel.sin2)
# Concatenate
self.inputPosVel = Stack_of_vector(name + 'InputPosVel')
plug(self.inputPos.sout, self.inputPosVel.sin1)
plug(self.inputVel.sout, self.inputPosVel.sin2)
self.inputPosVel.selec1(0, 6)
self.inputPosVel.selec2(0, 6)
# IMU Vector
self.IMUVector = PositionStateReconstructor(name + 'EstimatorInput')
plug(self.inputPosVel.sout, self.IMUVector.sin)
self.IMUVector.inputFormat.value = '001111'
self.IMUVector.outputFormat.value = '011111'
self.IMUVector.setFiniteDifferencesInterval(2)
# CoM and derivatives
self.comIn = self.robot.dynamicEncoders.com
self.comVector = PositionStateReconstructor(name + 'ComVector')
plug(self.comIn, self.comVector.sin)
self.comVector.inputFormat.value = '000001'
self.comVector.outputFormat.value = '010101'
self.comVector.setFiniteDifferencesInterval(20)
# Compute derivative of Angular Momentum
self.angMomDerivator = Derivator_of_Vector(name + 'angMomDerivator')
plug(self.robot.dynamicEncoders.angularmomentum,
self.angMomDerivator.sin)
self.angMomDerivator.dt.value = self.robot.timeStep
# Concatenate with interace estimator
plug(self.comVector.sout, self.interface.comVector)
plug(self.robot.dynamicEncoders.inertia, self.interface.inertia)
plug(self.robot.dynamicEncoders.angularmomentum,
self.interface.angMomentum)
plug(self.angMomDerivator.sout, self.interface.dangMomentum)
self.interface.dinertia.value = (0, 0, 0, 0, 0, 0)
plug(self.robot.dynamicEncoders.waist, self.interface.positionWaist)
plug(self.IMUVector.sout, self.interface.imuVector)
plug(self.interface.input, self.input)
self.robot.flextimator = self
# Create a dynamic ######################################
def createCenterOfMassFeatureAndTask(self,
dynamicTmp,
featureName,
featureDesName,
taskName,
selec='111',
ingain=1.):
dynamicTmp.com.recompute(0)
dynamicTmp.Jcom.recompute(0)
featureCom = FeatureGeneric(featureName)
plug(dynamicTmp.com, featureCom.errorIN)
plug(dynamicTmp.Jcom, featureCom.jacobianIN)
featureCom.selec.value = selec
featureComDes = FeatureGeneric(featureDesName)
featureComDes.errorIN.value = dynamicTmp.com.value
featureCom.setReference(featureComDes.name)
taskCom = Task(taskName)
taskCom.add(featureName)
gainCom = GainAdaptive('gain' + taskName)
gainCom.setConstant(ingain)
plug(gainCom.gain, taskCom.controlGain)
plug(taskCom.error, gainCom.error)
return (featureCom, featureComDes, taskCom, gainCom)
def createOperationalPointFeatureAndTask(self,
dynamicTmp,
operationalPointName,
featureName,
taskName,
ingain=.2):
jacobianName = 'J{0}'.format(operationalPointName)
dynamicTmp.signal(operationalPointName).recompute(0)
dynamicTmp.signal(jacobianName).recompute(0)
feature = \
FeaturePosition(featureName,
dynamicTmp.signal(operationalPointName),
dynamicTmp.signal(jacobianName),
dynamicTmp.signal(operationalPointName).value)
task = Task(taskName)
task.add(featureName)
gain = GainAdaptive('gain' + taskName)
gain.setConstant(ingain)
plug(gain.gain, task.controlGain)
plug(task.error, gain.error)
return (feature, task, gain)
def createDynamic(self, state, name):
# Create dynamic
self.dynamicTmp = self.robot.loadModelFromJrlDynamics(
self.robot.name + name, self.robot.modelDir, self.robot.modelName,
self.robot.specificitiesPath, self.robot.jointRankPath,
DynamicHrp2_14)
self.dynamicTmp.dimension = self.dynamicTmp.getDimension()
if self.dynamicTmp.dimension != len(self.robot.halfSitting):
raise RuntimeError(
"Dimension of half-sitting: {0} differs from dimension of robot: {1}"
.format(len(self.halfSitting), self.dynamicTmp.dimension))
# Pluging position
plug(state, self.dynamicTmp.position)
self.derivative = True
# Pluging velocity
self.robot.enableVelocityDerivator = self.derivative
if self.robot.enableVelocityDerivator:
self.dynamicTmp.velocityDerivator = Derivator_of_Vector(
'velocityDerivator')
self.dynamicTmp.velocityDerivator.dt.value = self.robot.timeStep
plug(state, self.dynamicTmp.velocityDerivator.sin)
plug(self.dynamicTmp.velocityDerivator.sout,
self.dynamicTmp.velocity)
else:
self.dynamicTmp.velocity.value = self.dynamicTmp.dimension * (0., )
# Pluging acceleration
self.robot.enableAccelerationDerivator = self.derivative
if self.robot.enableAccelerationDerivator:
self.dynamicTmp.accelerationDerivator = Derivator_of_Vector(
'accelerationDerivator')
self.dynamicTmp.accelerationDerivator.dt.value = self.robot.timeStep
plug(self.dynamicTmp.velocityDerivator.sout,
self.dynamicTmp.accelerationDerivator.sin)
plug(self.dynamicTmp.accelerationDerivator.sout,
self.dynamicTmp.acceleration)
else:
self.dynamicTmp.acceleration.value = self.dynamicTmp.dimension * (
0., )
# # --- center of mass ------------
# (self.featureCom, self.featureComDes, self.taskCom, self.gainCom) = \
# self.createCenterOfMassFeatureAndTask\
# (self.dynamicTmp, '{0}_feature_com'.format(self.robot.name),
# '{0}_feature_ref_com'.format(self.robot.name),
# '{0}_task_com'.format(self.robot.name))
# --- operational points tasks -----
self.robot.features = dict()
self.robot.tasks = dict()
self.robot.gains = dict()
for op in self.robot.OperationalPoints:
opName = op + name
self.dynamicTmp.createOpPoint(op, op)
(self.robot.features[opName], self.robot.tasks[opName], self.robot.gains[opName]) = \
self.createOperationalPointFeatureAndTask(self.dynamicTmp, op,
'{0}_feature_{1}'.format(self.robot.name, opName),
'{0}_task_{1}'.format(self.robot.name, opName))
# define a member for each operational point
w = op.split('-')
memberName = w[0]
for i in w[1:]:
memberName += i.capitalize()
setattr(self, memberName, self.robot.features[opName])
# self.robot.tasks ['com'] = self.taskCom
# self.robot.features ['com'] = self.featureCom
# self.robot.gains['com'] = self.gainCom
self.robot.features['waist' + name].selec.value = '011100'
return self.dynamicTmp
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)
sensorStack = Stack_of_vector("sv1")
plug(robot.device.accelerometer, sensorStack.sin1)
plug(robot.device.gyrometer, sensorStack.sin2)
sensorStack.selec1(0, 3)
sensorStack.selec2(0, 3)
plug(sensorStack.sout, meas)
inputPos = MatrixHomoToPoseUTheta("inputPosition")
plug(robot.frames["accelerometer"].position, inputPos.sin)
inputVector = sotso.PositionStateReconstructor("estimatorInput")
robot.dynamic.createJacobian("jchest", "chest")
class HRP2ModelBaseFlexEstimatorIMUForce(DGIMUModelBaseFlexEstimation):
def __init__(self, robot, name="flextimator"):
DGIMUModelBaseFlexEstimation.__init__(self, name)
self.setSamplingPeriod(0.005)
self.robot = robot
# Covariances
self.setProcessNoiseCovariance(
matrixToTuple(
np.diag(
(1e-8,) * 12
+ (1e-4,) * 3
+ (1e-4,) * 3
+ (1e-4,) * 3
+ (1e-4,) * 3
+ (1.0e-2,) * 6
+ (1e-15,) * 2
+ (1.0e-8,) * 3
)
)
)
self.setMeasurementNoiseCovariance(matrixToTuple(np.diag((1e-3,) * 3 + (1e-6,) * 3)))
self.setUnmodeledForceVariance(1e-13)
self.setForceVariance(1e-4)
self.setAbsolutePosVariance(1e-4)
# Contact model definition
self.setKfe(matrixToTuple(np.diag((40000, 40000, 40000))))
self.setKfv(matrixToTuple(np.diag((600, 600, 600))))
self.setKte(matrixToTuple(np.diag((600, 600, 600))))
self.setKtv(matrixToTuple(np.diag((60, 60, 60))))
# Estimator interface
self.interface = EstimatorInterface(name + "EstimatorInterface")
self.interface.setLeftHandSensorTransformation((0.0, 0.0, 1.57))
self.interface.setRightHandSensorTransformation((0.0, 0.0, 1.57))
# State and measurement definition
self.interface.setWithUnmodeledMeasurements(False)
self.interface.setWithModeledForces(True)
self.interface.setWithAbsolutePose(False)
self.setWithComBias(False)
# Contacts velocities
self.leftFootVelocity = Multiply_matrix_vector("leftFootVelocity")
plug(self.robot.frames["leftFootForceSensor"].jacobian, self.leftFootVelocity.sin1)
plug(self.robot.device.velocity, self.leftFootVelocity.sin2)
self.rightFootVelocity = Multiply_matrix_vector("rightFootVelocity")
plug(self.robot.frames["rightFootForceSensor"].jacobian, self.rightFootVelocity.sin1)
plug(self.robot.device.velocity, self.rightFootVelocity.sin2)
self.interface.setFDInertiaDot(True)
# Contacts forces, positions and velocities
# Feet
plug(self.robot.device.forceLLEG, self.interface.force_lf)
plug(self.robot.device.forceRLEG, self.interface.force_rf)
plug(self.robot.frames["leftFootForceSensor"].position, self.interface.position_lf)
plug(self.robot.frames["rightFootForceSensor"].position, self.interface.position_rf)
plug(self.leftFootVelocity.sout, self.interface.velocity_lf)
plug(self.rightFootVelocity.sout, self.interface.velocity_rf)
# Hands
plug(self.robot.device.forceLARM, self.interface.force_lh)
plug(self.robot.device.forceRARM, self.interface.force_rh)
plug(self.robot.dynamic.signal("right-wrist"), self.interface.position_lh)
plug(self.robot.dynamic.signal("left-wrist"), self.interface.position_rh)
# Strings
self.Peg = (0, 0, 4.60) # Position of the anchorage in the global frame
self.Prl1 = np.matrix(
[[1, 0, 0, 0 - 3.19997004e-02], [0, 1, 0, 0.15 - 0], [0, 0, 1, 1.28 - 1], [0, 0, 0, 1]]
) # Positions of the contacts on the robot (in the local frame) with respect to the chest
self.Prl2 = np.matrix([[1, 0, 0, 0 - 3.19997004e-02], [0, 1, 0, -0.15 - 0], [0, 0, 1, 1.28 - 1], [0, 0, 0, 1]])
(self.contact1OpPoint, self.contact1Pos, self.contact1) = self.createContact("contact1", self.Prl1, self.Peg)
(self.contact2OpPoint, self.contact2Pos, self.contact2) = self.createContact("contact2", self.Prl2, self.Peg)
plug(self.contact1.sout, self.interface.position_ls)
plug(self.contact2.sout, self.interface.position_rs)
# Contacts model and config
plug(self.interface.contactsModel, self.contactsModel)
self.setWithConfigSignal(True)
plug(self.interface.config, self.config)
# Mocap signal
self.ros = RosExport("rosExportMocap")
self.ros.add("matrixHomoStamped", "chest", "/evart/hrp2_head_sf/hrp2_head_sf")
# Filtering
self.mocapFilter = MocapDataFilter("MocapDataFilter")
plug(self.ros.signal("chest"), self.mocapFilter.sin)
self.mocapSignal = self.mocapFilter.sout
# Drift
self.drift = DriftFromMocap(name + "Drift")
plug(self.mocapSignal, self.drift.limbGlobal)
plug(self.robot.dynamic.chest, self.drift.limbLocal)
self.drift.init()
# Measurement reconstruction
plug(self.robot.device.accelerometer, self.interface.accelerometer)
plug(self.robot.device.gyrometer, self.interface.gyrometer)
plug(self.drift.driftVector, self.interface.drift)
plug(self.interface.measurement, self.measurement)
# Input reconstruction
# IMU Vector
self.inputPos = MatrixHomoToPoseUTheta(name + "InputPosition")
plug(robot.frames["accelerometer"].position, self.inputPos.sin)
self.robot.dynamic.createJacobian(name + "ChestJ_OpPoint", "chest")
self.imuOpPoint = OpPointModifier(name + "IMU_oppoint")
self.imuOpPoint.setEndEffector(False)
self.imuOpPoint.setTransformation(
matrixToTuple(
np.linalg.inv(np.matrix(self.robot.dynamic.chest.value))
* np.matrix(self.robot.frames["accelerometer"].position.value)
)
)
plug(self.robot.dynamic.chest, self.imuOpPoint.positionIN)
plug(self.robot.dynamic.signal(name + "ChestJ_OpPoint"), self.imuOpPoint.jacobianIN)
self.inputVel = Multiply_matrix_vector(name + "InputVelocity")
plug(self.imuOpPoint.jacobian, self.inputVel.sin1)
plug(self.robot.device.velocity, self.inputVel.sin2)
self.inputPosVel = Stack_of_vector(name + "InputPosVel")
plug(self.inputPos.sout, self.inputPosVel.sin1)
plug(self.inputVel.sout, self.inputPosVel.sin2)
self.inputPosVel.selec1(0, 6)
self.inputPosVel.selec2(0, 6)
self.IMUVector = PositionStateReconstructor(name + "EstimatorInput")
plug(self.inputPosVel.sout, self.IMUVector.sin)
self.IMUVector.inputFormat.value = "001111"
self.IMUVector.outputFormat.value = "011111"
self.IMUVector.setFiniteDifferencesInterval(1)
self.inputPosVel.sout.recompute(0)
self.IMUVector.setLastVector(self.inputPosVel.sout.value + (0.0,) * 6)
# CoM and derivatives
self.com = self.robot.dynamic.com
self.DCom = Multiply_matrix_vector(name + "DCom")
plug(self.robot.dynamic.Jcom, self.DCom.sin1)
plug(self.robot.device.velocity, self.DCom.sin2)
self.comVectorIn = Stack_of_vector(name + "ComVectorIn")
plug(self.com, self.comVectorIn.sin1)
plug(self.DCom.sout, self.comVectorIn.sin2)
self.comVectorIn.selec1(0, 3)
self.comVectorIn.selec2(0, 3)
self.comVector = PositionStateReconstructor(name + "ComVector")
plug(self.comVectorIn.sout, self.comVector.sin)
self.comVector.inputFormat.value = "000101"
self.comVector.outputFormat.value = "010101"
self.comVector.setFiniteDifferencesInterval(1)
self.DCom.sout.recompute(0)
self.comVector.setLastVector(self.com.value + (0.0,) * 15) # (0.,)*3+self.DCom.sout.value+(0.,)*9)
# Compute derivative of Angular Momentum
self.angMomDerivator = Derivator_of_Vector(name + "angMomDerivator")
plug(self.robot.dynamic.angularmomentum, self.angMomDerivator.sin)
self.angMomDerivator.dt.value = self.robot.timeStep
# self.angMomDerivator = PositionStateReconstructor (name+'angMomDerivator')
# plug(self.robot.dynamic.angularmomentum,self.angMomDerivator.sin)
# self.angMomDerivator.inputFormat.value = '000001'
# self.angMomDerivator.outputFormat.value = '000100'
# self.angMomDerivator.setFiniteDifferencesInterval(2)
# self.robot.dynamic.angularmomentum.recompute(0)
# self.angMomDerivator.setLastVector(self.robot.dynamic.angularmomentum.value+(0.,)*15)
# Concatenate with interface estimator
plug(self.comVector.sout, self.interface.comVector)
plug(self.robot.dynamic.inertia, self.interface.inertia)
self.interface.dinertia.value = (0, 0, 0, 0, 0, 0)
plug(self.robot.dynamic.angularmomentum, self.interface.angMomentum)
plug(self.angMomDerivator.sout, self.interface.dangMomentum)
plug(self.robot.dynamic.waist, self.interface.positionWaist)
plug(self.IMUVector.sout, self.interface.imuVector)
plug(self.interface.input, self.input)
plug(self.interface.modeledContactsNbr, self.contactNbr)
self.robot.flextimator = self
def initAbsolutePoses(self):
self.drift.init()
def createContact(self, name, prl, peg):
self.contactOpPoint = OpPointModifier(name + "_opPoint")
self.contactOpPoint.setEndEffector(False)
self.contactOpPoint.setTransformation(matrixToTuple(prl))
plug(self.robot.dynamic.chest, self.contactOpPoint.positionIN)
self.contactPos = MatrixHomoToPose(name + "_pos")
plug(self.contactOpPoint.position, self.contactPos.sin)
self.contact = Stack_of_vector(name)
self.contact.sin1.value = peg
plug(self.contactPos.sout, self.contact.sin2)
self.contact.selec1(0, 3)
self.contact.selec2(0, 3)
return (self.contactOpPoint, self.contactPos, self.contact)
# plug encoder velocities (with different base vel) to balance controller
from dynamic_graph.sot.core import Stack_of_vector, Selec_of_vector
robot.v = Stack_of_vector('v')
plug(robot.base_estimator.v_flex, robot.v.sin1)
plug(robot.filters.estimator_kin.dx, robot.v.sin2)
robot.v.selec1(0, 6)
robot.v.selec2(0, 30)
plug(robot.v.sout, robot.inv_dyn.v)
# Compute finite differences of base position
from dynamic_graph.sot.torque_control.utils.filter_utils import create_chebi2_lp_filter_Wn_03_N_4
#robot.q_fd = create_chebi2_lp_filter_Wn_03_N_4('q_filter', robot.timeStep, 36)
from dynamic_graph.sot.torque_control.filter_differentiator import FilterDifferentiator
robot.q_fd = FilterDifferentiator('q_filter')
robot.q_fd.init(robot.timeStep, 36, (1., 0.), (1., 0.))
plug(robot.base_estimator.q, robot.q_fd.x)
create_topic(robot.ros, robot.q_fd.dx, 'q_fd')
# Replace force sensor filters
from dynamic_graph.sot.torque_control.utils.filter_utils import create_butter_lp_filter_Wn_05_N_3, create_chebi2_lp_filter_Wn_03_N_4
robot.force_LF_filter = create_chebi2_lp_filter_Wn_03_N_4(
'force_LF_filter', robot.timeStep, 6)
robot.force_RF_filter = create_chebi2_lp_filter_Wn_03_N_4(
'force_RF_filter', robot.timeStep, 6)
plug(robot.device.forceLLEG, robot.force_LF_filter.x)
plug(robot.force_LF_filter.x_filtered, robot.base_estimator.forceLLEG)
plug(robot.force_LF_filter.dx, robot.base_estimator.dforceLLEG)
plug(robot.device.forceRLEG, robot.force_RF_filter.x)
def create_balance_controller(device,
floatingBase,
estimator,
torque_ctrl,
traj_gen,
com_traj_gen,
urdfFileName,
dt=0.001,
ff_locator=None):
ctrl = InverseDynamicsBalanceController("invDynBalCtrl")
if (floatingBase != None):
from dynamic_graph.sot.core import Stack_of_vector
base6d_encoders = Stack_of_vector('base6d_encoders')
plug(floatingBase.soutPos, base6d_encoders.sin1)
base6d_encoders.selec1(0, 6)
plug(device.robotState, base6d_encoders.sin2)
base6d_encoders.selec2(6, 36)
plug(base6d_encoders.sout, ctrl.q)
v = Stack_of_vector('v')
plug(floatingBase.soutVel, v.sin1)
v.selec1(0, 6)
plug(estimator.jointsVelocities, v.sin2)
v.selec2(6, 36)
plug(v.sout, ctrl.v)
else:
plug(ff_locator.base6dFromFoot_encoders, ctrl.q)
plug(ff_locator.v, ctrl.v)
plug(traj_gen.q, ctrl.posture_ref_pos)
plug(traj_gen.dq, ctrl.posture_ref_vel)
plug(traj_gen.ddq, ctrl.posture_ref_acc)
# plug(estimator.contactWrenchRightSole, ctrl.wrench_right_foot);
# plug(estimator.contactWrenchLeftSole, ctrl.wrench_left_foot);
plug(ctrl.tau_des, torque_ctrl.jointsTorquesDesired)
plug(ctrl.tau_des, estimator.tauDes)
import balance_ctrl_conf as conf
plug(com_traj_gen.x, ctrl.com_ref_pos)
plug(com_traj_gen.dx, ctrl.com_ref_vel)
plug(com_traj_gen.ddx, ctrl.com_ref_acc)
ctrl.rotor_inertias.value = conf.ROTOR_INERTIAS
ctrl.gear_ratios.value = conf.GEAR_RATIOS
ctrl.contact_normal.value = (0.0, 0.0, 1.0)
ctrl.contact_points.value = conf.RIGHT_FOOT_CONTACT_POINTS
ctrl.f_min.value = conf.fMin
ctrl.mu.value = conf.mu[0]
ctrl.weight_contact_forces.value = (1e2, 1e2, 1e0, 1e3, 1e3, 1e3)
ctrl.kp_com.value = 3 * (conf.kp_com, )
ctrl.kd_com.value = 3 * (conf.kd_com, )
ctrl.kp_constraints.value = 6 * (conf.kp_constr, )
ctrl.kd_constraints.value = 6 * (conf.kd_constr, )
ctrl.kp_posture.value = 30 * (conf.kp_posture, )
ctrl.kd_posture.value = 30 * (conf.kd_posture, )
ctrl.kp_pos.value = 30 * (conf.kp_pos, )
ctrl.kd_pos.value = 30 * (conf.kd_pos, )
ctrl.w_com.value = conf.w_com
ctrl.w_forces.value = conf.w_forces
ctrl.w_posture.value = conf.w_posture
ctrl.w_base_orientation.value = conf.w_base_orientation
ctrl.w_torques.value = conf.w_torques
ctrl.init(dt, urdfFileName)
return ctrl
contactNbr.value = 2
est1.setContactModel(2)
# Position of the anchorage in the global frame
Peg = (0,0,4.60)
# Positions of the contacts on the robot (in the local frame) with respect to the chest
Prl1 = np.matrix([[1,0,0,0-3.19997004e-02],[0,1,0,0.15-0],[0,0,1,1.28-1],[0,0,0,1]])
Prl2 = np.matrix([[1,0,0,0-3.19997004e-02],[0,1,0,-0.15-0],[0,0,1,1.28-1],[0,0,0,1]])
# contact 1
contact1OpPoint = OpPointModifier('contact1_oppoint')
contact1OpPoint.setEndEffector(False)
contact1OpPoint.setTransformation(matrixToTuple(Prl1))
plug (robot.dynamic.chest,contact1OpPoint.positionIN)
contact1Pos = MatrixHomoToPose('contact1Pos')
plug(contact1OpPoint.position, contact1Pos.sin)
contact1 = Stack_of_vector ('contact1')
contact1.sin1.value = Peg
plug(contact1Pos.sout,contact1.sin2)
#contact1.sin2.value = (0,-0.15,1.28)
contact1.selec1 (0, 3)
contact1.selec2 (0, 3)
# contact 2
contact2OpPoint = OpPointModifier('contact2_oppoint')
contact2OpPoint.setEndEffector(False)
contact2OpPoint.setTransformation(matrixToTuple(Prl2))
plug (robot.dynamic.chest,contact2OpPoint.positionIN)
contact2Pos = MatrixHomoToPose('contact2Pos')
plug(contact2OpPoint.position, contact2Pos.sin)
contact2 = Stack_of_vector ('contact2')
import sys
# --- ROBOT DYNAMIC SIMULATION -------------------------------------------------
from dynamic_graph.sot.hrp2_14.robot import Robot
robot = Robot( 'robot' )
# --- LINK ROBOT VIEWER -------------------------------------------------------
from dynamic_graph.sot.core.utils.viewer_helper import addRobotViewer
addRobotViewer(robot.device,small=True,verbose=False)
robot.timeStep=5e-3
usingRobotViewer = True
from dynamic_graph.sot.core import Stack_of_vector
acc = Stack_of_vector('acc')
gyr = Stack_of_vector('gyr')
acc.selec1(0,2)
acc.selec2(0,1)
gyr.selec1(0,2)
gyr.selec2(0,1)
acc.sin1.value=(0.0,0.0)
acc.sin2.value=(9.8,)
gyr.sin1.value=(0.0,0.0)
gyr.sin2.value=(0.0,)
robot.device.accelerometer = acc.sout
robot.device.gyrometer = gyr.sout
def __init__(self, robot, name='flextimator', useMocap=True, dt=0.005):
DGIMUModelBaseFlexEstimation.__init__(self,name)
self.setSamplingPeriod(dt)
self.robot = robot
initDevice(self.robot)
computeDynamic(self.robot,0)
# Covariances
self.setProcessNoiseCovariance(matrixToTuple(np.diag((1e-8,)*12+(1e-4,)*3+(1e-4,)*3+(1e-4,)*3+(1e-4,)*3+(1.e-2,)*6+(1e-15,)*2+(1.e-8,)*3)))
self.setMeasurementNoiseCovariance(matrixToTuple(np.diag((1e-3,)*3+(1e-6,)*3)))
self.setUnmodeledForceVariance(1e-13)
self.setForceVariance(1e-4)
self.setAbsolutePosVariance(1e-4)
# Contact model definition
# self.setKfe(matrixToTuple(np.diag((40000,40000,40000))))
self.setKfe(matrixToTuple(np.diag((150000,150000,150000))))
self.setKfv(matrixToTuple(np.diag((600,600,600))))
self.setKte(matrixToTuple(np.diag((600,600,600))))
self.setKtv(matrixToTuple(np.diag((10,10,10))))
self.setKfeRopes(matrixToTuple(np.diag((10000,10000,10000))))
self.setKfvRopes(matrixToTuple(np.diag((300,300,800))))
self.setKteRopes(matrixToTuple(np.diag((600,600,600))))
self.setKtvRopes(matrixToTuple(np.diag((60,60,60))))
# Estimator interface
self.interface=EstimatorInterface(name+"EstimatorInterface")
self.interface.setSamplingPeriod(dt)
self.interface.setLeftHandSensorTransformation((0.,0.,1.57))
self.interface.setRightHandSensorTransformation((0.,0.,1.57))
# State and measurement definition
self.interface.setWithUnmodeledMeasurements(False)
self.interface.setWithModeledForces(True)
self.interface.setWithAbsolutePose(False)
self.setWithComBias(False)
# Contacts velocities
self.leftFootVelocity = Multiply_matrix_vector ('leftFootVelocity')
plug(self.robot.frames['leftFootForceSensor'].jacobian,self.leftFootVelocity.sin1)
plug(self.robot.device.velocity,self.leftFootVelocity.sin2)
self.rightFootVelocity = Multiply_matrix_vector ('rightFootVelocity')
plug(self.robot.frames['rightFootForceSensor'].jacobian,self.rightFootVelocity.sin1)
plug(self.robot.device.velocity,self.rightFootVelocity.sin2)
self.interface.setFDInertiaDot(True)
# Contacts forces, positions and velocities
# Feet
plug (self.robot.device.forceLLEG,self.interface.force_lf)
plug (self.robot.device.forceRLEG,self.interface.force_rf)
plug (self.robot.frames['leftFootForceSensor'].position,self.interface.position_lf)
plug (self.robot.frames['rightFootForceSensor'].position,self.interface.position_rf)
plug (self.leftFootVelocity.sout,self.interface.velocity_lf)
plug (self.rightFootVelocity.sout,self.interface.velocity_rf)
# Hands
plug (self.robot.device.forceLARM,self.interface.force_lh)
plug (self.robot.device.forceRARM,self.interface.force_rh)
plug (self.robot.dynamic.signal('right-wrist'),self.interface.position_lh)
plug (self.robot.dynamic.signal('left-wrist'),self.interface.position_rh)
# Strings
self.Peg = (0,0,4.60) # Position of the anchorage in the global frame
self.setPe(self.Peg)
self.Prl1 = np.matrix([[1,0,0,0-3.19997004e-02],[0,1,0,0.15-0],[0,0,1,1.28-1],[0,0,0,1]]) # Positions of the contacts on the robot (in the local frame) with respect to the chest
self.Prl2 = np.matrix([[1,0,0,0-3.19997004e-02],[0,1,0,-0.15-0],[0,0,1,1.28-1],[0,0,0,1]])
(self.contact1OpPoint,self.contact1Pos,self.contact1)=self.createContact('contact1', self.Prl1)
(self.contact2OpPoint,self.contact2Pos,self.contact2)=self.createContact('contact2', self.Prl2)
plug(self.contact1.sout,self.interface.position_ls)
plug(self.contact2.sout,self.interface.position_rs)
# Contacts model and config
plug(self.interface.contactsModel,self.contactsModel)
self.setWithConfigSignal(True)
plug(self.interface.config,self.config)
if(useMocap):
# Mocap signal
self.ros = RosExport('rosExportMocap')
self.ros.add('matrixHomoStamped', "chest", "/evart/hrp2_14_head/hrp2_14_head")
# Filtering
from dynamic_graph.sot.tools import MocapDataFilter
self.mocapFilter = MocapDataFilter('MocapDataFilter')
plug(self.ros.signal('chest'),self.mocapFilter.sin)
self.mocapSignal = self.mocapFilter.sout
# Drift
self.drift = DriftFromMocap(name+'Drift')
plug(self.mocapSignal,self.drift.limbGlobal)
plug(self.robot.dynamic.chest,self.drift.limbLocal)
self.drift.init()
plug(self.drift.driftInvVector,self.interface.drift)
# Measurement reconstruction
plug(self.robot.device.accelerometer,self.interface.accelerometer)
plug(self.robot.device.gyrometer,self.interface.gyrometer)
plug(self.interface.measurement,self.measurement)
# Input reconstruction
# IMU Vector
self.inputPos = MatrixHomoToPoseUTheta(name+'InputPosition')
plug(robot.frames['accelerometer'].position,self.inputPos.sin)
self.robot.dynamic.createJacobian(name+'ChestJ_OpPoint','chest')
self.imuOpPoint = OpPointModifier(name+'IMU_oppoint')
self.imuOpPoint.setEndEffector(False)
self.imuOpPoint.setTransformation(matrixToTuple(np.linalg.inv(np.matrix(self.robot.dynamic.chest.value))*np.matrix(self.robot.frames['accelerometer'].position.value)))
plug (self.robot.dynamic.chest,self.imuOpPoint.positionIN)
plug (self.robot.dynamic.signal(name+'ChestJ_OpPoint'),self.imuOpPoint.jacobianIN)
self.inputVel = Multiply_matrix_vector(name+'InputVelocity')
plug(self.imuOpPoint.jacobian,self.inputVel.sin1)
plug(self.robot.device.velocity,self.inputVel.sin2)
self.inputPosVel = Stack_of_vector (name+'InputPosVel')
plug(self.inputPos.sout,self.inputPosVel.sin1)
plug(self.inputVel.sout,self.inputPosVel.sin2)
self.inputPosVel.selec1 (0, 6)
self.inputPosVel.selec2 (0, 6)
self.IMUVector = PositionStateReconstructor (name+'EstimatorInput')
self.IMUVector.setSamplingPeriod(dt)
plug(self.inputPosVel.sout,self.IMUVector.sin)
self.IMUVector.inputFormat.value = '001111'
self.IMUVector.outputFormat.value = '011111'
self.IMUVector.setFiniteDifferencesInterval(1)
self.inputPosVel.sout.recompute(0)
self.IMUVector.setLastVector(self.inputPosVel.sout.value+(0.,)*6)
# CoM and derivatives
self.com=self.robot.dynamic.com
self.DCom = Multiply_matrix_vector(name+'DCom')
plug(self.robot.dynamic.Jcom,self.DCom.sin1)
plug(self.robot.device.velocity,self.DCom.sin2)
self.comVectorIn = Stack_of_vector (name+'ComVectorIn')
plug(self.com,self.comVectorIn.sin1)
plug(self.DCom.sout,self.comVectorIn.sin2)
self.comVectorIn.selec1 (0, 3)
self.comVectorIn.selec2 (0, 3)
self.comVector = PositionStateReconstructor (name+'ComVector')
self.comVector.setSamplingPeriod(dt)
plug(self.comVectorIn.sout,self.comVector.sin)
self.comVector.inputFormat.value = '000101'
self.comVector.outputFormat.value = '010101'
self.comVector.setFiniteDifferencesInterval(1)
self.DCom.sout.recompute(0)
self.comVector.setLastVector(self.com.value+(0.,)*15)#(0.,)*3+self.DCom.sout.value+(0.,)*9)
# Compute derivative of Angular Momentum
self.angMomDerivator = Derivator_of_Vector(name+'angMomDerivator')
plug(self.robot.dynamic.angularmomentum,self.angMomDerivator.sin)
self.angMomDerivator.dt.value = dt
# self.angMomDerivator = PositionStateReconstructor (name+'angMomDerivator')
# self.angMomDerivator.setSamplingPeriod(dt)
# plug(self.robot.dynamic.angularmomentum,self.angMomDerivator.sin)
# self.angMomDerivator.inputFormat.value = '000001'
# self.angMomDerivator.outputFormat.value = '000100'
# self.angMomDerivator.setFiniteDifferencesInterval(2)
# self.robot.dynamic.angularmomentum.recompute(0)
# self.angMomDerivator.setLastVector(self.robot.dynamic.angularmomentum.value+(0.,)*15)
# Concatenate with interface estimator
plug(self.comVector.sout,self.interface.comVector)
plug(self.robot.dynamic.inertia,self.interface.inertia)
self.interface.dinertia.value=(0,0,0,0,0,0)
plug(self.robot.dynamic.angularmomentum,self.interface.angMomentum)
plug(self.angMomDerivator.sout,self.interface.dangMomentum)
plug(self.robot.dynamic.waist,self.interface.positionWaist)
plug(self.IMUVector.sout,self.interface.imuVector)
plug(self.interface.input,self.input)
plug (self.interface.modeledContactsNbr,self.contactNbr)
self.robot.flextimator = self
contact1 = est.signal('contact1')
contact1.value = (0,0,0)
contact2 = est.signal('contact2')
contact2.value = (0,0,0)
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)
sensorStack = Stack_of_vector ('sv1')
plug(robot.device.accelerometer,sensorStack.sin1)
plug(robot.device.gyrometer,sensorStack.sin2)
sensorStack.selec1 (0, 3)
sensorStack.selec2 (0, 3)
plug(sensorStack.sout,meas);
imuPos = MatrixHomoToPose('imuPos')
imuOri = MatrixToUTheta('imuOri')
imuRotM = HomoToRotation('imuRot')
plug(robot.frames['accelerometer'].position,imuPos.sin)
plug(robot.frames['accelerometer'].position,imuRotM.sin)
plug(imuRotM.sout,imuOri.sin)
class HRP2ModelBaseFlexEstimatorIMUForceEncoders(DGIMUModelBaseFlexEstimation):
def __init__(self, robot, name='flextimatorEncoders'):
DGIMUModelBaseFlexEstimation.__init__(self,name)
self.setSamplingPeriod(0.005)
self.robot = robot
# Covariances
self.setProcessNoiseCovariance(matrixToTuple(np.diag((1e-8,)*12+(1e-4,)*3+(1e-4,)*3+(1e-4,)*3+(1e-4,)*3+(1.e-2,)*6+(1e-15,)*2+(1.e-8,)*3)))
self.setMeasurementNoiseCovariance(matrixToTuple(np.diag((1e-3,)*3+(1e-6,)*3)))
self.setUnmodeledForceVariance(1e-13)
self.setForceVariance(1e-4)
self.setAbsolutePosVariance(1e-4)
# Contact model definition
self.setContactModel(1)
self.setKfe(matrixToTuple(np.diag((40000,40000,40000))))
self.setKfv(matrixToTuple(np.diag((600,600,600))))
self.setKte(matrixToTuple(np.diag((600,600,600))))
self.setKtv(matrixToTuple(np.diag((60,60,60))))
#Estimator interface
self.interface=EstimatorInterface(name+"EstimatorInterface")
self.interface.setLeftHandSensorTransformation((0.,0.,1.57))
self.interface.setRightHandSensorTransformation((0.,0.,1.57))
self.interface.setFDInertiaDot(True)
# State and measurement definition
self.interface.setWithUnmodeledMeasurements(False)
self.interface.setWithModeledForces(True)
self.interface.setWithAbsolutePose(False)
self.setWithComBias(False)
# Contacts forces
plug (self.robot.device.forceLLEG,self.interface.force_lf)
plug (self.robot.device.forceRLEG,self.interface.force_rf)
plug (self.robot.device.forceLARM,self.interface.force_lh)
plug (self.robot.device.forceRARM,self.interface.force_rh)
# Selecting robotState
self.robot.device.robotState.value=46*(0.,)
self.robotState = Selec_of_vector('robotState')
plug(self.robot.device.robotState,self.robotState.sin)
self.robotState.selec(0,36)
# Reconstruction of the position of the free flyer from encoders
# Create dynamic with the free flyer at the origin of the control frame
self.robot.dynamicOdo=self.createDynamic(self.robotState.sout,'_dynamicOdo')
self.robot.dynamicOdo.inertia.recompute(1)
self.robot.dynamicOdo.waist.recompute(1)
# Reconstruction of the position of the contacts in dynamicOdo
self.leftFootPosOdo=Multiply_of_matrixHomo(name+"leftFootPosOdo")
plug(self.robot.dynamicOdo.signal('left-ankle'),self.leftFootPosOdo.sin1)
self.leftFootPosOdo.sin2.value=self.robot.forceSensorInLeftAnkle
self.rightFootPosOdo=Multiply_of_matrixHomo(name+"rightFootPosOdo")
plug(self.robot.dynamicOdo.signal('right-ankle'),self.rightFootPosOdo.sin1)
self.rightFootPosOdo.sin2.value=self.robot.forceSensorInRightAnkle
# Odometry
self.odometry=Odometry (name+'odometry')
plug (self.robot.frames['leftFootForceSensor'].position,self.odometry.leftFootPositionRef)
plug (self.robot.frames['rightFootForceSensor'].position,self.odometry.rightFootPositionRef)
plug (self.rightFootPosOdo.sout,self.odometry.rightFootPositionIn)
plug (self.leftFootPosOdo.sout,self.odometry.leftFootPositionIn)
plug (self.robot.device.forceLLEG,self.odometry.force_lf)
plug (self.robot.device.forceRLEG,self.odometry.force_rf)
self.odometry.setLeftFootPosition(self.robot.frames['leftFootForceSensor'].position.value)
self.odometry.setRightFootPosition(self.robot.frames['rightFootForceSensor'].position.value)
plug(self.interface.stackOfSupportContacts,self.odometry.stackOfSupportContacts)
# Create dynamicEncoders
self.robotStateWoFF = Selec_of_vector('robotStateWoFF')
plug(self.robot.device.robotState,self.robotStateWoFF.sin)
self.robotStateWoFF.selec(6,36)
self.stateEncoders = Stack_of_vector (name+'stateEncoders')
plug(self.odometry.freeFlyer,self.stateEncoders.sin1)
plug(self.robotStateWoFF.sout,self.stateEncoders.sin2)
self.stateEncoders.selec1 (0, 6)
self.stateEncoders.selec2 (0, 30)
self.robot.dynamicEncoders=self.createDynamic(self.stateEncoders.sout,'_dynamicEncoders')
# self.robot.dynamicEncoders=self.createDynamic(self.robotState.sout,'_dynamicEncoders')
# plug(self.odometry.freeFlyer,self.robot.dynamicEncoders.ffposition)
# self.robot.dynamicEncoders=self.createDynamic(self.robot.device.state,'_dynamicEncoders')
# Reconstruction of the position of the contacts in dynamicEncoders
self.leftFootPos=Multiply_of_matrixHomo("leftFootPos")
plug(self.robot.dynamicEncoders.signal('left-ankle'),self.leftFootPos.sin1)
self.leftFootPos.sin2.value=self.robot.forceSensorInLeftAnkle
self.rightFootPos=Multiply_of_matrixHomo("rightFootPos")
plug(self.robot.dynamicEncoders.signal('right-ankle'),self.rightFootPos.sin1)
self.rightFootPos.sin2.value=self.robot.forceSensorInRightAnkle
# Contacts velocities
self.leftFootVelocity = Multiply_matrix_vector ('leftFootVelocity')
plug(self.robot.frames['leftFootForceSensor'].jacobian,self.leftFootVelocity.sin1)
plug(self.robot.dynamicEncoders.velocity,self.leftFootVelocity.sin2)
self.rightFootVelocity = Multiply_matrix_vector ('rightFootVelocity')
plug(self.robot.frames['rightFootForceSensor'].jacobian,self.rightFootVelocity.sin1)
plug(self.robot.dynamicEncoders.velocity,self.rightFootVelocity.sin2)
# Contacts positions and velocities
plug (self.leftFootPos.sout,self.interface.position_lf)
plug (self.rightFootPos.sout,self.interface.position_rf)
plug (self.leftFootVelocity.sout,self.interface.velocity_lf)
plug (self.rightFootVelocity.sout,self.interface.velocity_rf)
plug (self.robot.dynamicEncoders.signal('right-wrist'),self.interface.position_lh)
plug (self.robot.dynamicEncoders.signal('left-wrist'),self.interface.position_rh)
# Compute contacts number
plug (self.interface.supportContactsNbr,self.contactNbr)
# Contacts model and config
plug(self.interface.contactsModel,self.contactsModel)
self.setWithConfigSignal(True)
plug(self.interface.config,self.config)
# Drift
self.drift = DriftFromMocap(name+'Drift')
# Compute measurement vector
plug(self.robot.device.accelerometer,self.interface.accelerometer)
plug(self.robot.device.gyrometer,self.interface.gyrometer)
plug(self.drift.driftVector,self.interface.drift)
plug(self.interface.measurement,self.measurement)
# Input reconstruction
# IMU Vector
# Creating an operational point for the IMU
self.robot.dynamicEncoders.createJacobian(name+'ChestJ_OpPoint','chest')
self.imuOpPoint = OpPointModifier(name+'IMU_oppoint')
self.imuOpPoint.setTransformation(matrixToTuple(np.linalg.inv(np.matrix(self.robot.dynamicEncoders.chest.value))*np.matrix(self.robot.frames['accelerometer'].position.value)))
self.imuOpPoint.setEndEffector(False)
plug (self.robot.dynamicEncoders.chest,self.imuOpPoint.positionIN)
plug (self.robot.dynamicEncoders.signal(name+'ChestJ_OpPoint'),self.imuOpPoint.jacobianIN)
# IMU position
self.PosAccelerometer=Multiply_of_matrixHomo(name+"PosAccelerometer")
plug(self.robot.dynamicEncoders.chest,self.PosAccelerometer.sin1)
self.PosAccelerometer.sin2.value=matrixToTuple(self.robot.accelerometerPosition)
self.inputPos = MatrixHomoToPoseUTheta(name+'InputPosition')
plug(self.PosAccelerometer.sout,self.inputPos.sin)
# IMU velocity
self.inputVel = Multiply_matrix_vector(name+'InputVelocity')
plug(self.imuOpPoint.jacobian,self.inputVel.sin1)
plug(self.robot.dynamicEncoders.velocity,self.inputVel.sin2)
# Concatenate
self.inputPosVel = Stack_of_vector (name+'InputPosVel')
plug(self.inputPos.sout,self.inputPosVel.sin1)
plug(self.inputVel.sout,self.inputPosVel.sin2)
self.inputPosVel.selec1 (0, 6)
self.inputPosVel.selec2 (0, 6)
# IMU Vector
self.IMUVector = PositionStateReconstructor (name+'EstimatorInput')
plug(self.inputPosVel.sout,self.IMUVector.sin)
self.IMUVector.inputFormat.value = '001111'
self.IMUVector.outputFormat.value = '011111'
self.IMUVector.setFiniteDifferencesInterval(2)
# CoM and derivatives
self.comIn=self.robot.dynamicEncoders.com
self.comVector = PositionStateReconstructor (name+'ComVector')
plug(self.comIn,self.comVector.sin)
self.comVector.inputFormat.value = '000001'
self.comVector.outputFormat.value = '010101'
self.comVector.setFiniteDifferencesInterval(20)
# Compute derivative of Angular Momentum
self.angMomDerivator = Derivator_of_Vector(name+'angMomDerivator')
plug(self.robot.dynamicEncoders.angularmomentum,self.angMomDerivator.sin)
self.angMomDerivator.dt.value = self.robot.timeStep
# Concatenate with interace estimator
plug(self.comVector.sout,self.interface.comVector)
plug(self.robot.dynamicEncoders.inertia,self.interface.inertia)
plug(self.robot.dynamicEncoders.angularmomentum,self.interface.angMomentum)
plug(self.angMomDerivator.sout,self.interface.dangMomentum)
self.interface.dinertia.value=(0,0,0,0,0,0)
plug(self.robot.dynamicEncoders.waist,self.interface.positionWaist)
plug(self.IMUVector.sout,self.interface.imuVector)
plug(self.interface.input,self.input)
self.robot.flextimator = self
# Create a dynamic ######################################
def createCenterOfMassFeatureAndTask(self,
dynamicTmp,
featureName, featureDesName,
taskName,
selec = '111',
ingain = 1.):
dynamicTmp.com.recompute(0)
dynamicTmp.Jcom.recompute(0)
featureCom = FeatureGeneric(featureName)
plug(dynamicTmp.com, featureCom.errorIN)
plug(dynamicTmp.Jcom, featureCom.jacobianIN)
featureCom.selec.value = selec
featureComDes = FeatureGeneric(featureDesName)
featureComDes.errorIN.value = dynamicTmp.com.value
featureCom.setReference(featureComDes.name)
taskCom = Task(taskName)
taskCom.add(featureName)
gainCom = GainAdaptive('gain'+taskName)
gainCom.setConstant(ingain)
plug(gainCom.gain, taskCom.controlGain)
plug(taskCom.error, gainCom.error)
return (featureCom, featureComDes, taskCom, gainCom)
def createOperationalPointFeatureAndTask(self,
dynamicTmp,
operationalPointName,
featureName,
taskName,
ingain = .2):
jacobianName = 'J{0}'.format(operationalPointName)
dynamicTmp.signal(operationalPointName).recompute(0)
dynamicTmp.signal(jacobianName).recompute(0)
feature = \
FeaturePosition(featureName,
dynamicTmp.signal(operationalPointName),
dynamicTmp.signal(jacobianName),
dynamicTmp.signal(operationalPointName).value)
task = Task(taskName)
task.add(featureName)
gain = GainAdaptive('gain'+taskName)
gain.setConstant(ingain)
plug(gain.gain, task.controlGain)
plug(task.error, gain.error)
return (feature, task, gain)
def createDynamic(self,state,name) :
# Create dynamic
self.dynamicTmp = self.robot.loadModelFromJrlDynamics(
self.robot.name + name,
self.robot.modelDir,
self.robot.modelName,
self.robot.specificitiesPath,
self.robot.jointRankPath,
DynamicHrp2_14)
self.dynamicTmp.dimension = self.dynamicTmp.getDimension()
if self.dynamicTmp.dimension != len(self.robot.halfSitting):
raise RuntimeError("Dimension of half-sitting: {0} differs from dimension of robot: {1}".format (len(self.halfSitting), self.dynamicTmp.dimension))
# Pluging position
plug(state, self.dynamicTmp.position)
self.derivative=True
# Pluging velocity
self.robot.enableVelocityDerivator = self.derivative
if self.robot.enableVelocityDerivator:
self.dynamicTmp.velocityDerivator = Derivator_of_Vector('velocityDerivator')
self.dynamicTmp.velocityDerivator.dt.value = self.robot.timeStep
plug(state, self.dynamicTmp.velocityDerivator.sin)
plug(self.dynamicTmp.velocityDerivator.sout, self.dynamicTmp.velocity)
else:
self.dynamicTmp.velocity.value = self.dynamicTmp.dimension*(0.,)
# Pluging acceleration
self.robot.enableAccelerationDerivator = self.derivative
if self.robot.enableAccelerationDerivator:
self.dynamicTmp.accelerationDerivator = Derivator_of_Vector('accelerationDerivator')
self.dynamicTmp.accelerationDerivator.dt.value = self.robot.timeStep
plug(self.dynamicTmp.velocityDerivator.sout, self.dynamicTmp.accelerationDerivator.sin)
plug(self.dynamicTmp.accelerationDerivator.sout, self.dynamicTmp.acceleration)
else:
self.dynamicTmp.acceleration.value = self.dynamicTmp.dimension*(0.,)
# # --- center of mass ------------
# (self.featureCom, self.featureComDes, self.taskCom, self.gainCom) = \
# self.createCenterOfMassFeatureAndTask\
# (self.dynamicTmp, '{0}_feature_com'.format(self.robot.name),
# '{0}_feature_ref_com'.format(self.robot.name),
# '{0}_task_com'.format(self.robot.name))
# --- operational points tasks -----
self.robot.features = dict()
self.robot.tasks = dict()
self.robot.gains = dict()
for op in self.robot.OperationalPoints:
opName= op + name
self.dynamicTmp.createOpPoint(op, op)
(self.robot.features[opName], self.robot.tasks[opName], self.robot.gains[opName]) = \
self.createOperationalPointFeatureAndTask(self.dynamicTmp, op,
'{0}_feature_{1}'.format(self.robot.name, opName),
'{0}_task_{1}'.format(self.robot.name, opName))
# define a member for each operational point
w = op.split('-')
memberName = w[0]
for i in w[1:]:
memberName += i.capitalize()
setattr(self, memberName, self.robot.features[opName])
# self.robot.tasks ['com'] = self.taskCom
# self.robot.features ['com'] = self.featureCom
# self.robot.gains['com'] = self.gainCom
self.robot.features['waist'+name].selec.value = '011100'
return self.dynamicTmp
class HandCompensaterOscillator(Application):
threePhaseScrew = True
tracesRealTime = True
def __init__(self,robot,twoHands = True,trunkStabilize = True):
Application.__init__(self,robot)
self.twoHands = twoHands
self.trunkStabilize = trunkStabilize
self.robot = robot
plug(self.robot.dynamic.com,self.robot.device.zmp)
self.sot = self.solver.sot
self.createTasks()
self.initTasks()
self.initTaskGains()
self.initOscillator()
self.initialStack()
# --- TASKS --------------------------------------------------------------------
# --- TASKS --------------------------------------------------------------------
# --- TASKS --------------------------------------------------------------------
def createTasks(self):
(self.tasks['trunk'],self.gains['trunk'])= createTrunkTask (self.robot, self, 'Tasktrunk')
self.taskbalance = self.tasks['balance']
self.taskRH = self.tasks['right-wrist']
self.taskLH = self.tasks['left-wrist']
self.taskPosture = self.tasks['posture']
self.taskTrunk = self.tasks['trunk']
self.taskHalfStitting = MetaTaskPosture(self.robot.dynamic,'halfsitting')
#initialization is separated from the creation of the tasks because if we want to switch
#to second order controlm the initialization will remain while the creation is
#changed
def initTasks(self):
self.initTaskTrunk()
self.initTaskPosture()
#self.initTaskHalfSitting()
self.initTaskCompensate()
#def initTaskHalfSitting(self):
# self.taskHalfStitting.gotoq(None,self.robot.halfSitting)
def initTaskTrunk(self):
# --- BALANCE ---
self.features['chest'].frame('desired')
self.features['waist'].frame('desired')
self.features['gaze'].frame('desired')
#self.taskChest.feature.selec.value = '111111'
self.features['chest'].selec.value = '111000'
self.features['waist'].selec.value = '111100'
self.features['gaze'].selec.value = '111000'
self.featureCom.selec.value = '011'
def setOsciFreq(self, f):
self.oscillatorRoll.omega.value = f * 3.1415
self.oscillatorPitch.omega.value = f * 3.1415
def setOsciMagni(self,m):
self.oscillatorRoll.magnitude.value = m
self.oscillatorPitch.magnitude.value = m
def initOscillator(self):
self.oscillatorRoll = Oscillator('oscillatorRoll')
self.oscillatorRoll.setContinuous(True)
self.oscillatorRoll.setActivated(True)
self.oscillatorRoll.setTimePeriod(self.robot.timeStep)
self.oscillatorRoll.setActivated(False)
self.oscillatorRoll.magnitude.value = 0.1
self.oscillatorRoll.phase.value = 0.0
self.oscillatorRoll.omega.value = 0.75
self.oscillatorPitch = Oscillator('oscillatorPitch')
self.oscillatorPitch.setContinuous(True)
self.oscillatorPitch.setActivated(True)
self.oscillatorPitch.setTimePeriod(self.robot.timeStep)
self.oscillatorPitch.setActivated(False)
self.oscillatorPitch.magnitude.value = 0.1
self.oscillatorPitch.phase.value = 1.57
self.oscillatorPitch.omega.value = 0.75
self.stackRP = Stack_of_vector('StackOscRollPitch')
plug ( self.oscillatorRoll.vectorSout, self.stackRP.sin1 )
plug ( self.oscillatorPitch.vectorSout, self.stackRP.sin2 )
self.stackRP.selec1(0,1)
self.stackRP.selec2(0,1)
self.stackRPY = Stack_of_vector('StackOscRollPitchYaw')
plug ( self.stackRP.sout, self.stackRPY.sin1 )
self.stackRPY.sin2.value = (0.0,)
self.stackRPY.selec1(0,2)
self.stackRPY.selec2(0,1)
self.stackPoseRPY = Stack_of_vector('StackOscPoseRollPitchYaw')
self.stackPoseRPY.sin1.value = (0.0,0.0,0.0)
plug ( self.stackRPY.sout, self.stackPoseRPY.sin2 )
self.stackPoseRPY.selec1(0,3)
self.stackPoseRPY.selec2(0,3)
self.poseRPYaw2Homo = PoseRollPitchYawToMatrixHomo('OscPoseRPYaw2Homo')
plug ( self.stackPoseRPY.sout , self.poseRPYaw2Homo.sin)
self.headRef = Multiply_of_matrixHomo('headRef')
self.headRef.sin1.value = self.robot.dynamic.signal('gaze').value
plug( self.poseRPYaw2Homo.sout, self.headRef.sin2)
plug( self.headRef.sout, self.features['gaze'].reference)
self.chestRef = Multiply_of_matrixHomo('chestRef')
self.chestRef.sin1.value = self.robot.dynamic.signal('chest').value
plug( self.poseRPYaw2Homo.sout, self.chestRef.sin2)
plug( self.chestRef.sout, self.features['chest'].reference)
self.waistRef = Multiply_of_matrixHomo('waistRef')
self.waistRef.sin1.value = self.robot.dynamic.signal('waist').value
plug( self.poseRPYaw2Homo.sout, self.waistRef.sin2)
plug( self.waistRef.sout, self.features['waist'].reference)
def initTaskPosture(self):
# --- LEAST NORM
weight_ff = 0
weight_leg = 3
weight_knee = 5
weight_chest = 1
weight_chesttilt = 10
weight_head = 0.3
weight_arm = 1
weight = diag( (weight_ff,)*6 + (weight_leg,)*12 + (weight_chest,)*2 + (weight_head,)*2 + (weight_arm,)*14)
weight[9,9] = weight_knee
weight[15,15] = weight_knee
weight[19,19] = weight_chesttilt
#weight = weight[6:,:]
self.featurePosture.jacobianIN.value = matrixToTuple(weight)
self.featurePostureDes.errorIN.value = self.robot.halfSitting
mask = '1'*36
# mask = 6*'0'+12*'0'+4*'1'+14*'0'
# mask = '000000000000111100000000000000000000000000'
# robot.dynamic.displaySignals ()
# robot.dynamic.Jchest.value
self.features['posture'].selec.value = mask
def initTaskGains(self, setup = "medium"):
if setup == "medium":
self.gains['balance'].setConstant(10)
self.gains['trunk'].setConstant(10)
self.gains['right-wrist'].setByPoint(4,0.2,0.01,0.8)
self.gains['left-wrist'].setByPoint(4,0.2,0.01,0.8)
self.taskHalfStitting.gain.setByPoint(2,0.2,0.01,0.8)
# --- SOLVER ----------------------------------------------------------------
def push(self,task,feature=None,keep=False):
if isinstance(task,str): taskName=task
elif "task" in task.__dict__: taskName=task.task.name
else: taskName=task.name
if taskName not in toList(self.sot):
self.sot.push(taskName)
if taskName!="posture" and "posture" in toList(self.sot):
self.sot.down("posture")
if keep: feature.keep()
def rm(self,task):
if isinstance(task,str): taskName=task
elif "task" in task.__dict__: taskName=task.task.name
else: taskName=task.name
if taskName in toList(self.sot): self.sot.remove(taskName)
# --- DISPLAY -------------------------------------------------------------
def initDisplay(self):
self.robot.device.viewer.updateElementConfig('red',[0,0,-1,0,0,0])
self.robot.device.viewer.updateElementConfig('yellow',[0,0,-1,0,0,0])
def updateDisplay(self):
'''Display the various objects corresponding to the calcul graph. '''
None
# --- TRACES -----------------------------------------------------------
def withTraces(self):
if self.tracesRealTime:
self.robot.tracerSize = 2**26
self.robot.initializeTracer()
else:
self.robot.tracer = Tracer('trace')
self.robot.device.after.addSignal('{0}.triger'.format(self.robot.tracer.name))
self.robot.tracer.open('/tmp/','','.dat')
#self.robot.tracer.add( self.taskRH.task.name+'.error','erh' )
def stopTracer(self):
self.robot.stopTracer()
def dumpTracer(self):
self.robot.tracer.dump()
def startTracer(self):
self.robot.startTracer()
# --- RUN --------------------------------------------------------------
def initialStack(self):
self.sot.clear()
self.push(self.tasks['balance'])
self.push(self.taskTrunk)
#self.push(self.taskPosture)
def moveToInit(self):
'''Go to initial pose.'''
#gotoNd(self.taskRH,(0.3,-0.2,1.1,0,-pi/2,0),'111001')
#gotoNd(self.taskLH,(0.3,0.2,1.1,0,-pi/2,0),'111001')
change6dPositionReference(self.taskRH,self.features['right-wrist'],\
self.gains['right-wrist'],\
(0.3,-0.3,1.1,0,-pi/2,0),'111111')
self.push(self.taskRH)
if self.twoHands:
change6dPositionReference(self.taskLH,self.features['left-wrist'],\
self.gains['left-wrist'],\
(0.3,0.3,1.1,0,-pi/2,0),'111111')
self.push(self.taskLH)
None
def goHalfSitting(self):
'''End of application, go to final pose.'''
self.featurePostureDes.errorIN.value = self.robot.halfSitting
self.sot.clear()
self.push(self.tasks['balance'])
self.push(self.taskPosture)
# --- SEQUENCER ---
seqstep = 0
def nextStep(self,step=None):
if step!=None: self.seqstep = step
if self.seqstep==0:
self.moveToInit()
elif self.seqstep==1:
self.startCompensate()
elif self.seqstep==2:
self.startOcillation()
elif self.seqstep==3:
self.stopOcillation()
elif self.seqstep==4:
self.goHalfSitting()
self.seqstep += 1
def __add__(self,i):
self.nextStep()
# COMPENATION ######################################
def initTaskCompensate(self):
# The constraint is:
# cMhref !!=!! cMh = cMcc ccMh
# or written in ccMh
# ccMh !!=!! ccMc cMhref
# c : central frame of the robot
# cc : central frame for the controller (without the flexibility)
# cMcc= flexibility
# ccMc= flexibility inverted
self.transformerR = MovingFrameTransformation('tranformation_right')
self.ccMc = self.transformerR.gMl # inverted flexibility
self.cMrhref = self.transformerR.lM0 # reference position in the world control frame
# You need to set up the inverted flexibility : plug( ..., self.ccMc)
# You need to set up a reference value here: plug( ... ,self.cMhref)
self.ccVc = self.transformerR.gVl # inverted flexibility velocity
self.cVrhref = self.transformerR.lV0 # reference velocity in the world control frame
# You need to set up the inverted flexibility velocity : plug( ..., self.ccVc)
# You need to set up a reference velocity value here: plug( ... ,self.cVhref)
self.ccMrhref = self.transformerR.gM0 # reference matrix h**o in the control frame
self.ccVrhref = self.transformerR.gV0
######
if self.twoHands:
self.transformerL = MovingFrameTransformation('tranformation_left')
self.sym = Multiply_of_matrixHomo('sym')
self.sym.sin1.value =((1, 0, 0, 0), (0, -1, 0, 0), (0, 0, 1, 0), (0, 0, 0, 1))
plug (self.ccMrhref,self.sym.sin2)
self.symVel = Multiply_matrix_vector('symvel')
self.symVel.sin1.value =((1,0,0,0,0,0),(0,-1,0,0,0,0),(0,0,0,1,0,0),(0,0,0,1,0,0),(0,0,0,0,-1,0),(0,0,0,0,0,1))
plug (self.ccVrhref,self.symVel.sin2)
self.cMrhref.value = (matrixToTuple(diag([1,1,1,1])))
self.cVrhref.value = (0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0)
def startCompensate(self):
'''Start to compensate for the hand movements.'''
self.cMrhref.value = self.robot.dynamic.signal('right-wrist').value
self.cVrhref.value = (0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0)
plug(self.ccMrhref,self.features['right-wrist'].reference)
plug(self.ccVrhref,self.features['right-wrist'].velocity)
self.gains['right-wrist'].setByPoint(4,0.2,0.01,0.8)
self.tasks['right-wrist'].setWithDerivative (True)
self.features['right-wrist'].frame('desired')
print matrix(self.cMrhref.value)
if self.twoHands:
self.gains['left-wrist'].setByPoint(4,0.2,0.01,0.8)
plug (self.sym.sout,self.features['left-wrist'].reference)
plug (self.symVel.sout,self.features['left-wrist'].velocity)
self.features['left-wrist'].selec.value='000111'
self.tasks['left-wrist'].setWithDerivative (True)
self.features['left-wrist'].frame('desired')
#self.tasks['gaze'].setWithDerivative (True)
#######
#self.cMlhref.value = self.robot.dynamic.lh.value
#self.cVlhref.value = (0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0)
#print matrix(self.cMlhref.value)
######
def startOcillation(self):
self.oscillatorRoll.setActivated(True)
self.oscillatorPitch.setActivated(True)
def stopOcillation(self):
self.oscillatorRoll.setActivated(False)
self.oscillatorPitch.setActivated(False)
def __init__(self, robot, name='flextimatorEncoders'):
DGIMUModelBaseFlexEstimation.__init__(self,name)
self.setSamplingPeriod(0.005)
self.robot = robot
# Covariances
self.setProcessNoiseCovariance(matrixToTuple(np.diag((1e-8,)*12+(1e-4,)*3+(1e-4,)*3+(1e-4,)*3+(1e-4,)*3+(1.e-2,)*6+(1e-15,)*2+(1.e-8,)*3)))
self.setMeasurementNoiseCovariance(matrixToTuple(np.diag((1e-3,)*3+(1e-6,)*3)))
self.setUnmodeledForceVariance(1e-13)
self.setForceVariance(1e-4)
self.setAbsolutePosVariance(1e-4)
# Contact model definition
self.setContactModel(1)
self.setKfe(matrixToTuple(np.diag((40000,40000,40000))))
self.setKfv(matrixToTuple(np.diag((600,600,600))))
self.setKte(matrixToTuple(np.diag((600,600,600))))
self.setKtv(matrixToTuple(np.diag((60,60,60))))
#Estimator interface
self.interface=EstimatorInterface(name+"EstimatorInterface")
self.interface.setLeftHandSensorTransformation((0.,0.,1.57))
self.interface.setRightHandSensorTransformation((0.,0.,1.57))
self.interface.setFDInertiaDot(True)
# State and measurement definition
self.interface.setWithUnmodeledMeasurements(False)
self.interface.setWithModeledForces(True)
self.interface.setWithAbsolutePose(False)
self.setWithComBias(False)
# Contacts forces
plug (self.robot.device.forceLLEG,self.interface.force_lf)
plug (self.robot.device.forceRLEG,self.interface.force_rf)
plug (self.robot.device.forceLARM,self.interface.force_lh)
plug (self.robot.device.forceRARM,self.interface.force_rh)
# Selecting robotState
self.robot.device.robotState.value=46*(0.,)
self.robotState = Selec_of_vector('robotState')
plug(self.robot.device.robotState,self.robotState.sin)
self.robotState.selec(0,36)
# Reconstruction of the position of the free flyer from encoders
# Create dynamic with the free flyer at the origin of the control frame
self.robot.dynamicOdo=self.createDynamic(self.robotState.sout,'_dynamicOdo')
self.robot.dynamicOdo.inertia.recompute(1)
self.robot.dynamicOdo.waist.recompute(1)
# Reconstruction of the position of the contacts in dynamicOdo
self.leftFootPosOdo=Multiply_of_matrixHomo(name+"leftFootPosOdo")
plug(self.robot.dynamicOdo.signal('left-ankle'),self.leftFootPosOdo.sin1)
self.leftFootPosOdo.sin2.value=self.robot.forceSensorInLeftAnkle
self.rightFootPosOdo=Multiply_of_matrixHomo(name+"rightFootPosOdo")
plug(self.robot.dynamicOdo.signal('right-ankle'),self.rightFootPosOdo.sin1)
self.rightFootPosOdo.sin2.value=self.robot.forceSensorInRightAnkle
# Odometry
self.odometry=Odometry (name+'odometry')
plug (self.robot.frames['leftFootForceSensor'].position,self.odometry.leftFootPositionRef)
plug (self.robot.frames['rightFootForceSensor'].position,self.odometry.rightFootPositionRef)
plug (self.rightFootPosOdo.sout,self.odometry.rightFootPositionIn)
plug (self.leftFootPosOdo.sout,self.odometry.leftFootPositionIn)
plug (self.robot.device.forceLLEG,self.odometry.force_lf)
plug (self.robot.device.forceRLEG,self.odometry.force_rf)
self.odometry.setLeftFootPosition(self.robot.frames['leftFootForceSensor'].position.value)
self.odometry.setRightFootPosition(self.robot.frames['rightFootForceSensor'].position.value)
plug(self.interface.stackOfSupportContacts,self.odometry.stackOfSupportContacts)
# Create dynamicEncoders
self.robotStateWoFF = Selec_of_vector('robotStateWoFF')
plug(self.robot.device.robotState,self.robotStateWoFF.sin)
self.robotStateWoFF.selec(6,36)
self.stateEncoders = Stack_of_vector (name+'stateEncoders')
plug(self.odometry.freeFlyer,self.stateEncoders.sin1)
plug(self.robotStateWoFF.sout,self.stateEncoders.sin2)
self.stateEncoders.selec1 (0, 6)
self.stateEncoders.selec2 (0, 30)
self.robot.dynamicEncoders=self.createDynamic(self.stateEncoders.sout,'_dynamicEncoders')
# self.robot.dynamicEncoders=self.createDynamic(self.robotState.sout,'_dynamicEncoders')
# plug(self.odometry.freeFlyer,self.robot.dynamicEncoders.ffposition)
# self.robot.dynamicEncoders=self.createDynamic(self.robot.device.state,'_dynamicEncoders')
# Reconstruction of the position of the contacts in dynamicEncoders
self.leftFootPos=Multiply_of_matrixHomo("leftFootPos")
plug(self.robot.dynamicEncoders.signal('left-ankle'),self.leftFootPos.sin1)
self.leftFootPos.sin2.value=self.robot.forceSensorInLeftAnkle
self.rightFootPos=Multiply_of_matrixHomo("rightFootPos")
plug(self.robot.dynamicEncoders.signal('right-ankle'),self.rightFootPos.sin1)
self.rightFootPos.sin2.value=self.robot.forceSensorInRightAnkle
# Contacts velocities
self.leftFootVelocity = Multiply_matrix_vector ('leftFootVelocity')
plug(self.robot.frames['leftFootForceSensor'].jacobian,self.leftFootVelocity.sin1)
plug(self.robot.dynamicEncoders.velocity,self.leftFootVelocity.sin2)
self.rightFootVelocity = Multiply_matrix_vector ('rightFootVelocity')
plug(self.robot.frames['rightFootForceSensor'].jacobian,self.rightFootVelocity.sin1)
plug(self.robot.dynamicEncoders.velocity,self.rightFootVelocity.sin2)
# Contacts positions and velocities
plug (self.leftFootPos.sout,self.interface.position_lf)
plug (self.rightFootPos.sout,self.interface.position_rf)
plug (self.leftFootVelocity.sout,self.interface.velocity_lf)
plug (self.rightFootVelocity.sout,self.interface.velocity_rf)
plug (self.robot.dynamicEncoders.signal('right-wrist'),self.interface.position_lh)
plug (self.robot.dynamicEncoders.signal('left-wrist'),self.interface.position_rh)
# Compute contacts number
plug (self.interface.supportContactsNbr,self.contactNbr)
# Contacts model and config
plug(self.interface.contactsModel,self.contactsModel)
self.setWithConfigSignal(True)
plug(self.interface.config,self.config)
# Drift
self.drift = DriftFromMocap(name+'Drift')
# Compute measurement vector
plug(self.robot.device.accelerometer,self.interface.accelerometer)
plug(self.robot.device.gyrometer,self.interface.gyrometer)
plug(self.drift.driftVector,self.interface.drift)
plug(self.interface.measurement,self.measurement)
# Input reconstruction
# IMU Vector
# Creating an operational point for the IMU
self.robot.dynamicEncoders.createJacobian(name+'ChestJ_OpPoint','chest')
self.imuOpPoint = OpPointModifier(name+'IMU_oppoint')
self.imuOpPoint.setTransformation(matrixToTuple(np.linalg.inv(np.matrix(self.robot.dynamicEncoders.chest.value))*np.matrix(self.robot.frames['accelerometer'].position.value)))
self.imuOpPoint.setEndEffector(False)
plug (self.robot.dynamicEncoders.chest,self.imuOpPoint.positionIN)
plug (self.robot.dynamicEncoders.signal(name+'ChestJ_OpPoint'),self.imuOpPoint.jacobianIN)
# IMU position
self.PosAccelerometer=Multiply_of_matrixHomo(name+"PosAccelerometer")
plug(self.robot.dynamicEncoders.chest,self.PosAccelerometer.sin1)
self.PosAccelerometer.sin2.value=matrixToTuple(self.robot.accelerometerPosition)
self.inputPos = MatrixHomoToPoseUTheta(name+'InputPosition')
plug(self.PosAccelerometer.sout,self.inputPos.sin)
# IMU velocity
self.inputVel = Multiply_matrix_vector(name+'InputVelocity')
plug(self.imuOpPoint.jacobian,self.inputVel.sin1)
plug(self.robot.dynamicEncoders.velocity,self.inputVel.sin2)
# Concatenate
self.inputPosVel = Stack_of_vector (name+'InputPosVel')
plug(self.inputPos.sout,self.inputPosVel.sin1)
plug(self.inputVel.sout,self.inputPosVel.sin2)
self.inputPosVel.selec1 (0, 6)
self.inputPosVel.selec2 (0, 6)
# IMU Vector
self.IMUVector = PositionStateReconstructor (name+'EstimatorInput')
plug(self.inputPosVel.sout,self.IMUVector.sin)
self.IMUVector.inputFormat.value = '001111'
self.IMUVector.outputFormat.value = '011111'
self.IMUVector.setFiniteDifferencesInterval(2)
# CoM and derivatives
self.comIn=self.robot.dynamicEncoders.com
self.comVector = PositionStateReconstructor (name+'ComVector')
plug(self.comIn,self.comVector.sin)
self.comVector.inputFormat.value = '000001'
self.comVector.outputFormat.value = '010101'
self.comVector.setFiniteDifferencesInterval(20)
# Compute derivative of Angular Momentum
self.angMomDerivator = Derivator_of_Vector(name+'angMomDerivator')
plug(self.robot.dynamicEncoders.angularmomentum,self.angMomDerivator.sin)
self.angMomDerivator.dt.value = self.robot.timeStep
# Concatenate with interace estimator
plug(self.comVector.sout,self.interface.comVector)
plug(self.robot.dynamicEncoders.inertia,self.interface.inertia)
plug(self.robot.dynamicEncoders.angularmomentum,self.interface.angMomentum)
plug(self.angMomDerivator.sout,self.interface.dangMomentum)
self.interface.dinertia.value=(0,0,0,0,0,0)
plug(self.robot.dynamicEncoders.waist,self.interface.positionWaist)
plug(self.IMUVector.sout,self.interface.imuVector)
plug(self.interface.input,self.input)
self.robot.flextimator = self
def __init__(self, robot, name='flextimator', useMocap=True, dt=0.005):
DGIMUModelBaseFlexEstimation.__init__(self,name)
self.setSamplingPeriod(dt)
self.robot = robot
initDevice(self.robot)
computeDynamic(self.robot,0)
# Covariances
self.setProcessNoiseCovariance(matrixToTuple(np.diag((1e-8,)*12+(1e-4,)*3+(1e-4,)*3+(1e-4,)*3+(1e-4,)*3+(1.e-2,)*6+(1e-15,)*2+(1.e-8,)*3)))
self.setMeasurementNoiseCovariance(matrixToTuple(np.diag((1e-3,)*3+(1e-6,)*3)))
self.setUnmodeledForceVariance(1e-13)
self.setForceVariance(1e-4)
self.setAbsolutePosVariance(1e-4)
# Contact model definition
# self.setKfe(matrixToTuple(np.diag((40000,40000,40000))))
self.setKfe(matrixToTuple(np.diag((150000,150000,150000))))
self.setKfv(matrixToTuple(np.diag((600,600,600))))
self.setKte(matrixToTuple(np.diag((600,600,600))))
self.setKtv(matrixToTuple(np.diag((10,10,10))))
self.setKfeCordes(matrixToTuple(np.diag((10000,10000,10000))))
self.setKfvCordes(matrixToTuple(np.diag((300,300,800))))
self.setKteCordes(matrixToTuple(np.diag((600,600,600))))
self.setKtvCordes(matrixToTuple(np.diag((60,60,60))))
# Estimator interface
self.interface=EstimatorInterface(name+"EstimatorInterface")
self.interface.setSamplingPeriod(dt)
self.interface.setLeftHandSensorTransformation((0.,0.,1.57))
self.interface.setRightHandSensorTransformation((0.,0.,1.57))
# State and measurement definition
self.interface.setWithUnmodeledMeasurements(False)
self.interface.setWithModeledForces(True)
self.interface.setWithAbsolutePose(False)
self.setWithComBias(False)
# Contacts velocities
self.leftFootVelocity = Multiply_matrix_vector ('leftFootVelocity')
plug(self.robot.frames['leftFootForceSensor'].jacobian,self.leftFootVelocity.sin1)
plug(self.robot.device.velocity,self.leftFootVelocity.sin2)
self.rightFootVelocity = Multiply_matrix_vector ('rightFootVelocity')
plug(self.robot.frames['rightFootForceSensor'].jacobian,self.rightFootVelocity.sin1)
plug(self.robot.device.velocity,self.rightFootVelocity.sin2)
self.interface.setFDInertiaDot(True)
# Contacts forces, positions and velocities
# Feet
plug (self.robot.device.forceLLEG,self.interface.force_lf)
plug (self.robot.device.forceRLEG,self.interface.force_rf)
plug (self.robot.frames['leftFootForceSensor'].position,self.interface.position_lf)
plug (self.robot.frames['rightFootForceSensor'].position,self.interface.position_rf)
plug (self.leftFootVelocity.sout,self.interface.velocity_lf)
plug (self.rightFootVelocity.sout,self.interface.velocity_rf)
# Hands
plug (self.robot.device.forceLARM,self.interface.force_lh)
plug (self.robot.device.forceRARM,self.interface.force_rh)
plug (self.robot.dynamic.signal('right-wrist'),self.interface.position_lh)
plug (self.robot.dynamic.signal('left-wrist'),self.interface.position_rh)
# Strings
self.Peg = (0,0,4.60) # Position of the anchorage in the global frame
self.setPe(self.Peg)
self.Prl1 = np.matrix([[1,0,0,0-3.19997004e-02],[0,1,0,0.15-0],[0,0,1,1.28-1],[0,0,0,1]]) # Positions of the contacts on the robot (in the local frame) with respect to the chest
self.Prl2 = np.matrix([[1,0,0,0-3.19997004e-02],[0,1,0,-0.15-0],[0,0,1,1.28-1],[0,0,0,1]])
(self.contact1OpPoint,self.contact1Pos,self.contact1)=self.createContact('contact1', self.Prl1)
(self.contact2OpPoint,self.contact2Pos,self.contact2)=self.createContact('contact2', self.Prl2)
plug(self.contact1.sout,self.interface.position_ls)
plug(self.contact2.sout,self.interface.position_rs)
# Contacts model and config
plug(self.interface.contactsModel,self.contactsModel)
self.setWithConfigSignal(True)
plug(self.interface.config,self.config)
if(useMocap):
# Mocap signal
self.ros = RosExport('rosExportMocap')
self.ros.add('matrixHomoStamped', "chest", "/evart/hrp2_14_head/hrp2_14_head")
# Filtering
from dynamic_graph.sot.tools import MocapDataFilter
self.mocapFilter = MocapDataFilter('MocapDataFilter')
plug(self.ros.signal('chest'),self.mocapFilter.sin)
self.mocapSignal = self.mocapFilter.sout
# Drift
self.drift = DriftFromMocap(name+'Drift')
plug(self.mocapSignal,self.drift.limbGlobal)
plug(self.robot.dynamic.chest,self.drift.limbLocal)
self.drift.init()
plug(self.drift.driftInvVector,self.interface.drift)
# Measurement reconstruction
plug(self.robot.device.accelerometer,self.interface.accelerometer)
plug(self.robot.device.gyrometer,self.interface.gyrometer)
plug(self.interface.measurement,self.measurement)
# Input reconstruction
# IMU Vector
self.inputPos = MatrixHomoToPoseUTheta(name+'InputPosition')
plug(robot.frames['accelerometer'].position,self.inputPos.sin)
self.robot.dynamic.createJacobian(name+'ChestJ_OpPoint','chest')
self.imuOpPoint = OpPointModifier(name+'IMU_oppoint')
self.imuOpPoint.setEndEffector(False)
self.imuOpPoint.setTransformation(matrixToTuple(np.linalg.inv(np.matrix(self.robot.dynamic.chest.value))*np.matrix(self.robot.frames['accelerometer'].position.value)))
plug (self.robot.dynamic.chest,self.imuOpPoint.positionIN)
plug (self.robot.dynamic.signal(name+'ChestJ_OpPoint'),self.imuOpPoint.jacobianIN)
self.inputVel = Multiply_matrix_vector(name+'InputVelocity')
plug(self.imuOpPoint.jacobian,self.inputVel.sin1)
plug(self.robot.device.velocity,self.inputVel.sin2)
self.inputPosVel = Stack_of_vector (name+'InputPosVel')
plug(self.inputPos.sout,self.inputPosVel.sin1)
plug(self.inputVel.sout,self.inputPosVel.sin2)
self.inputPosVel.selec1 (0, 6)
self.inputPosVel.selec2 (0, 6)
self.IMUVector = PositionStateReconstructor (name+'EstimatorInput')
self.IMUVector.setSamplingPeriod(dt)
plug(self.inputPosVel.sout,self.IMUVector.sin)
self.IMUVector.inputFormat.value = '001111'
self.IMUVector.outputFormat.value = '011111'
self.IMUVector.setFiniteDifferencesInterval(1)
self.inputPosVel.sout.recompute(0)
self.IMUVector.setLastVector(self.inputPosVel.sout.value+(0.,)*6)
# CoM and derivatives
self.com=self.robot.dynamic.com
self.DCom = Multiply_matrix_vector(name+'DCom')
plug(self.robot.dynamic.Jcom,self.DCom.sin1)
plug(self.robot.device.velocity,self.DCom.sin2)
self.comVectorIn = Stack_of_vector (name+'ComVectorIn')
plug(self.com,self.comVectorIn.sin1)
plug(self.DCom.sout,self.comVectorIn.sin2)
self.comVectorIn.selec1 (0, 3)
self.comVectorIn.selec2 (0, 3)
self.comVector = PositionStateReconstructor (name+'ComVector')
self.comVector.setSamplingPeriod(dt)
plug(self.comVectorIn.sout,self.comVector.sin)
self.comVector.inputFormat.value = '000101'
self.comVector.outputFormat.value = '010101'
self.comVector.setFiniteDifferencesInterval(1)
self.DCom.sout.recompute(0)
self.comVector.setLastVector(self.com.value+(0.,)*15)#(0.,)*3+self.DCom.sout.value+(0.,)*9)
# Compute derivative of Angular Momentum
self.angMomDerivator = Derivator_of_Vector(name+'angMomDerivator')
plug(self.robot.dynamic.angularmomentum,self.angMomDerivator.sin)
self.angMomDerivator.dt.value = dt
# self.angMomDerivator = PositionStateReconstructor (name+'angMomDerivator')
# self.angMomDerivator.setSamplingPeriod(dt)
# plug(self.robot.dynamic.angularmomentum,self.angMomDerivator.sin)
# self.angMomDerivator.inputFormat.value = '000001'
# self.angMomDerivator.outputFormat.value = '000100'
# self.angMomDerivator.setFiniteDifferencesInterval(2)
# self.robot.dynamic.angularmomentum.recompute(0)
# self.angMomDerivator.setLastVector(self.robot.dynamic.angularmomentum.value+(0.,)*15)
# Concatenate with interface estimator
plug(self.comVector.sout,self.interface.comVector)
plug(self.robot.dynamic.inertia,self.interface.inertia)
self.interface.dinertia.value=(0,0,0,0,0,0)
plug(self.robot.dynamic.angularmomentum,self.interface.angMomentum)
plug(self.angMomDerivator.sout,self.interface.dangMomentum)
plug(self.robot.dynamic.waist,self.interface.positionWaist)
plug(self.IMUVector.sout,self.interface.imuVector)
plug(self.interface.input,self.input)
plug (self.interface.modeledContactsNbr,self.contactNbr)
self.robot.flextimator = self
def initOscillator(self):
self.oscillatorPitch = Oscillator('oscillatorPitch')
self.oscillatorPitch.setContinuous(True)
self.oscillatorPitch.setActivated(True)
self.oscillatorPitch.setTimePeriod(self.robot.timeStep)
self.oscillatorPitch.setActivated(False)
self.oscillatorPitch.magnitude.value = 0.1
self.oscillatorPitch.phase.value = 0.0
self.oscillatorPitch.omega.value = 0.75
self.oscillatorYaw = Oscillator('oscillatorYaw')
self.oscillatorYaw.setContinuous(True)
self.oscillatorYaw.setActivated(True)
self.oscillatorYaw.setTimePeriod(self.robot.timeStep)
self.oscillatorYaw.setActivated(False)
self.oscillatorYaw.magnitude.value = 0.1
self.oscillatorYaw.phase.value = 1.57
self.oscillatorYaw.omega.value = 0.75
self.minuspitch = Multiply_double_vector('minuspitch')
self.minuspitch.sin1.value = -1
plug (self.oscillatorPitch.vectorSout, self.minuspitch.sin2)
self.chestyaw = Multiply_double_vector('chestyaw')
self.chestyaw.sin1.value = -1
plug (self.oscillatorYaw.vectorSout, self.chestyaw.sin2)
self.stack1 = Stack_of_vector('Stack1')
self.stack1.sin1.value = (0.0,)*6
plug ( self.oscillatorYaw.vectorSout, self.stack1.sin2 )
self.stack1.selec1(0,6)
self.stack1.selec2(0,1)
self.stack2 = Stack_of_vector('Stack2')
plug ( self.stack1.sout, self.stack2.sin1 )
self.stack2.sin2.value = (0.0,)
self.stack2.selec1(0,7)
self.stack2.selec2(0,1)
self.stack3 = Stack_of_vector('Stack3')
plug ( self.stack2.sout, self.stack3.sin1 )
plug ( self.oscillatorPitch.vectorSout, self.stack3.sin2 )
self.stack3.selec1(0,8)
self.stack3.selec2(0,1)
self.stack4 = Stack_of_vector('Stack4')
plug ( self.stack3.sout, self.stack4.sin1 )
self.stack4.sin2.value=(0.0,0.0,0.0)
self.stack4.selec1(0,9)
self.stack4.selec2(0,3)
self.stack5 = Stack_of_vector('Stack5')
plug ( self.stack4.sout, self.stack5.sin1 )
plug ( self.oscillatorYaw.vectorSout, self.stack5.sin2 )
self.stack5.selec1(0,12)
self.stack5.selec2(0,1)
self.stack6 = Stack_of_vector('Stack6')
plug ( self.stack5.sout, self.stack6.sin1 )
self.stack6.sin2.value = (0.0,)
self.stack6.selec1(0,13)
self.stack6.selec2(0,1)
self.stack7 = Stack_of_vector('Stack7')
plug ( self.stack6.sout, self.stack7.sin1 )
plug ( self.minuspitch.sout, self.stack7.sin2 )
self.stack7.selec1(0,14)
self.stack7.selec2(0,1)
self.stack8 = Stack_of_vector('Stack8')
plug ( self.stack7.sout, self.stack8.sin1 )
self.stack8.sin2.value=(0.0,0.0,0.0)
self.stack8.selec1(0,15)
self.stack8.selec2(0,3)
self.stack9 = Stack_of_vector('Stack9')
plug ( self.stack8.sout, self.stack9.sin1 )
plug ( self.chestyaw.sout, self.stack9.sin2 )
self.stack9.selec1(0,18)
self.stack9.selec2(0,1)
self.stack10 = Stack_of_vector('Stack10')
plug ( self.stack9.sout, self.stack10.sin1 )
self.stack10.sin2.value = (0.0,)*17
self.stack10.selec1(0,19)
self.stack10.selec2(0,17)
self.postureRef = self.stack10.sout
