Example #1
0
    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 __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
Example #4
0
    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.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