示例#1
0
class HRP2ModelBaseFlexEstimatorIMUForce(DGIMUModelBaseFlexEstimation):

    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 initAbsolutePoses(self):
        self.drift.init()


    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)
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
示例#3
0
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 HRP2ModelBaseFlexEstimatorIMUForce(DGIMUModelBaseFlexEstimation):

    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

    def initAbsolutePoses(self):
        self.drift.init()


    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)