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='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