def test_rotation(self): T = ((0., 0., 1., 0.), (0., -1., 0., 0.), (1., 0., 0., 0.), (0., 0., 0., 1.)) op = OpPointModifier('op3') op.setTransformation(T) op.positionIN.value = gaze op.jacobianIN.value = Jgaze op.position.recompute(1) op.jacobian.recompute(1) self.assertEqual(op.getTransformation(), T) # w_M_s = w_M_g * g_M_s w_M_g = np.asmatrix(gaze) g_M_s = np.asmatrix(T) w_M_s_ref = w_M_g * g_M_s w_M_s = np.asmatrix(op.position.value) # Check w_M_s == w_M_s_ref self.assertEqual(np.equal(w_M_s, w_M_s_ref).all(), True) twist = np.matrix([[0., 0., 1., 0., 0., 0.], [0., -1., 0., 0., 0., 0.], [1., 0., 0., 0., 0., 0.], [0., 0., 0., 0., 0., 1.], [0., 0., 0., 0., -1., 0.], [0., 0., 0., 1., 0., 0.]]) J = np.asmatrix(op.jacobian.value) J_ref = twist * Jgaze # Check w_M_s == w_M_s_ref self.assertEqual(np.equal(J, J_ref).all(), True)
def createOpPointModif(self): self.opPointModif = OpPointModifier('opmodif' + self.name) plug(self.dyn.signal(self.opPoint), self.opPointModif.signal('positionIN')) plug(self.dyn.signal('J' + self.opPoint), self.opPointModif.signal('jacobianIN')) self.opPointModif.activ = False
def createFrame(self, frameName, transformation, operationalPoint): frame = OpPointModifier(frameName) frame.setTransformation(transformation) plug(self.dynamic.signal(operationalPoint), frame.positionIN) plug(self.dynamic.signal("J{0}".format(operationalPoint)), frame.jacobianIN) frame.position.recompute(frame.position.time + 1) frame.jacobian.recompute(frame.jacobian.time + 1) return frame
def test_simple(self): op = OpPointModifier('op') op.setTransformation(I4) op.positionIN.value = I4 op.jacobianIN.value = I6 op.position.recompute(0) op.jacobian.recompute(0) self.assertEqual(op.getTransformation(), I4) self.assertEqual(op.position.value, I4) self.assertEqual(op.jacobian.value, I6)
class HRP2MocapToSOT: def __init__(self, robot): self.robot = robot self.ros = RosExport("rosExportMocap") self.ros.add("matrixHomoStamped", "chest", "/evart/hrp2_head_sf/hrp2_head_sf") self.mocapFilter = MocapDataFilter("MocapDataFilter") plug(self.ros.signal("chest"), self.mocapFilter.sin) self.mocapSignal = self.mocapFilter.sout def initialize(self): m = np.matrix( [ [-0.8530, 0.5208, -0.0360, 0.0049], [-0.5206, -0.8537, -0.0146, -0.0078], [-0.0384, 0.0063, 0.9993, 0.2158], [0, 0, 0, 1.0000], ] ) # transformation from the chest-markers frame to the chest frame self.chestMarkersSot = OpPointModifier( "chestMarkersSot" ) # gets the position of the chest-markers frame in sot frame self.chestMarkersSot.setEndEffector(False) self.chestMarkersSot.setTransformation(matrixToTuple(m)) plug(self.robot.dynamic.chest, self.chestMarkersSot.positionIN) self.chestMarkersSot.position.recompute(1) self.mchestsot = np.matrix(self.chestMarkersSot.position.value) self.mchestmocap = np.matrix(self.ros.signal("chest").value) self.mtransformMocap2ISot = ( np.linalg.inv(self.mchestsot) * self.mchestmocap ) # transforms the mocap frame to the initial-sot frame self.chestMarkerMocapInISot = Multiply_of_matrixHomo( "chestMarkerMocap" ) # gets position of chest-markers frame in sot-initial frame self.chestMarkerMocapInISot.sin1.value = matrixToTuple(self.mtransformMocap2ISot) plug(self.mocapSignal, self.chestMarkerMocapInISot.sin2) self.chestMarkersSotInverse = Inverse_of_matrixHomo( "chestMarkersSotInverse" ) # inverse of the homomatrix for position of markers in local sot plug(self.chestMarkersSot.position, self.chestMarkersSotInverse.sin) self.robotPositionISot = Multiply_of_matrixHomo("robotPositionInSot") plug(self.chestMarkerMocapInISot.sout, self.robotPositionISot.sin1) plug(self.chestMarkersSotInverse.sout, self.robotPositionISot.sin2) self.robotPositionInMocap = Multiply_of_matrixHomo("robotPositionInMocap") plug(self.mocapSignal, self.robotPositionInMocap.sin1) plug(self.chestMarkersSotInverse.sout, self.robotPositionInMocap.sin2)
class HRP2MocapToSOT: def __init__(self, robot): self.robot = robot self.ros = RosExport('rosExportMocap') self.ros.add('matrixHomoStamped', "chest", "/evart/hrp2_head_sf/hrp2_head_sf") self.mocapFilter = MocapDataFilter('MocapDataFilter') plug(self.ros.signal('chest'), self.mocapFilter.sin) self.mocapSignal = self.mocapFilter.sout def initialize(self): m = np.matrix([ [-0.8530, 0.5208, -0.0360, 0.0049], [-0.5206, -0.8537, -0.0146, -0.0078], [-0.0384, 0.0063, 0.9993, 0.2158], [0, 0, 0, 1.0000] ]) #transformation from the chest-markers frame to the chest frame self.chestMarkersSot = OpPointModifier( 'chestMarkersSot' ) # gets the position of the chest-markers frame in sot frame self.chestMarkersSot.setEndEffector(False) self.chestMarkersSot.setTransformation(matrixToTuple(m)) plug(self.robot.dynamic.chest, self.chestMarkersSot.positionIN) self.chestMarkersSot.position.recompute(1) self.mchestsot = np.matrix(self.chestMarkersSot.position.value) self.mchestmocap = np.matrix(self.ros.signal('chest').value) self.mtransformMocap2ISot = np.linalg.inv( self.mchestsot ) * self.mchestmocap #transforms the mocap frame to the initial-sot frame self.chestMarkerMocapInISot = Multiply_of_matrixHomo( "chestMarkerMocap" ) #gets position of chest-markers frame in sot-initial frame self.chestMarkerMocapInISot.sin1.value = matrixToTuple( self.mtransformMocap2ISot) plug(self.mocapSignal, self.chestMarkerMocapInISot.sin2) self.chestMarkersSotInverse = Inverse_of_matrixHomo( "chestMarkersSotInverse" ) #inverse of the homomatrix for position of markers in local sot plug(self.chestMarkersSot.position, self.chestMarkersSotInverse.sin) self.robotPositionISot = Multiply_of_matrixHomo("robotPositionInSot") plug(self.chestMarkerMocapInISot.sout, self.robotPositionISot.sin1) plug(self.chestMarkersSotInverse.sout, self.robotPositionISot.sin2) self.robotPositionInMocap = Multiply_of_matrixHomo( "robotPositionInMocap") plug(self.mocapSignal, self.robotPositionInMocap.sin1) plug(self.chestMarkersSotInverse.sout, self.robotPositionInMocap.sin2)
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 HRP2ModelFreeFlexEstimator(DGIMUModelFreeFlexEstimation): def __init__(self, robot, name='flextimator'): DGIMUModelFreeFlexEstimation.__init__(self,name) self.setSamplingPeriod(0.005) self.robot = robot self.sensorStack = Stack_of_vector (name+'Sensors') plug(robot.device.accelerometer,self.sensorStack.sin1) plug(robot.device.gyrometer,self.sensorStack.sin2) self.sensorStack.selec1 (0, 3) self.sensorStack.selec2 (0, 3) plug(self.sensorStack.sout,self.measurement); self.inputPos = MatrixHomoToPoseUTheta(name+'InputPosition') plug(robot.frames['accelerometer'].position,self.inputPos.sin) robot.dynamic.createJacobian('ChestJ_OpPoint','chest') self.imuOpPoint = OpPointModifier('IMU_oppoint') self.imuOpPoint.setEndEffector(False) self.imuOpPoint.setTransformation(matrixToTuple(np.linalg.inv(np.matrix(self.robot.dynamic.chest.value))*np.matrix(self.robot.frames['accelerometer'].position.value))) plug (robot.dynamic.chest,self.imuOpPoint.positionIN) plug (robot.dynamic.signal('ChestJ_OpPoint'),self.imuOpPoint.jacobianIN) self.inputVel = Multiply_matrix_vector(name+'InputVelocity') plug(self.imuOpPoint.jacobian,self.inputVel.sin1) plug(robot.device.velocity,self.inputVel.sin2) self.inputPosVel = Stack_of_vector (name+'InputPosVel') plug(self.inputPos.sout,self.inputPosVel.sin1) plug(self.inputVel.sout,self.inputPosVel.sin2) self.inputPosVel.selec1 (0, 6) self.inputPosVel.selec2 (0, 6) self.inputVector = PositionStateReconstructor (name+'EstimatorInput') plug(self.inputPosVel.sout,self.inputVector.sin) self.inputVector.inputFormat.value = '001111' self.inputVector.outputFormat.value = '011111' self.inputVector.setFiniteDifferencesInterval(2) plug(self.inputVector.sout,self.input) robot.flextimator = self
def initialize(self): m = np.matrix([ [-0.8530, 0.5208, -0.0360, 0.0049], [-0.5206, -0.8537, -0.0146, -0.0078], [-0.0384, 0.0063, 0.9993, 0.2158], [0, 0, 0, 1.0000] ]) #transformation from the chest-markers frame to the chest frame self.chestMarkersSot = OpPointModifier( 'chestMarkersSot' ) # gets the position of the chest-markers frame in sot frame self.chestMarkersSot.setEndEffector(False) self.chestMarkersSot.setTransformation(matrixToTuple(m)) plug(self.robot.dynamic.chest, self.chestMarkersSot.positionIN) self.chestMarkersSot.position.recompute(1) self.mchestsot = np.matrix(self.chestMarkersSot.position.value) self.mchestmocap = np.matrix(self.ros.signal('chest').value) self.mtransformMocap2ISot = np.linalg.inv( self.mchestsot ) * self.mchestmocap #transforms the mocap frame to the initial-sot frame self.chestMarkerMocapInISot = Multiply_of_matrixHomo( "chestMarkerMocap" ) #gets position of chest-markers frame in sot-initial frame self.chestMarkerMocapInISot.sin1.value = matrixToTuple( self.mtransformMocap2ISot) plug(self.mocapSignal, self.chestMarkerMocapInISot.sin2) self.chestMarkersSotInverse = Inverse_of_matrixHomo( "chestMarkersSotInverse" ) #inverse of the homomatrix for position of markers in local sot plug(self.chestMarkersSot.position, self.chestMarkersSotInverse.sin) self.robotPositionISot = Multiply_of_matrixHomo("robotPositionInSot") plug(self.chestMarkerMocapInISot.sout, self.robotPositionISot.sin1) plug(self.chestMarkersSotInverse.sout, self.robotPositionISot.sin2) self.robotPositionInMocap = Multiply_of_matrixHomo( "robotPositionInMocap") plug(self.mocapSignal, self.robotPositionInMocap.sin1) plug(self.chestMarkersSotInverse.sout, self.robotPositionInMocap.sin2)
def test_translation(self): tx = 11.; ty = 22.; tz = 33. T = ((1., 0., 0., tx), (0., 1., 0., ty), (0., 0., 1., tz), (0., 0., 0., 1.)) op = OpPointModifier('op2') op.setTransformation(T) op.positionIN.value = gaze op.jacobianIN.value = Jgaze op.position.recompute(1) op.jacobian.recompute(1) self.assertEqual(op.getTransformation(), T) # w_M_s = w_M_g * g_M_s w_M_g = np.asmatrix(gaze) g_M_s = np.asmatrix(T) w_M_s_ref = w_M_g * g_M_s w_M_s = np.asmatrix(op.position.value) # Check w_M_s == w_M_s_ref self.assertEqual(np.equal(w_M_s, w_M_s_ref).all(), True) twist = np.matrix( [[1., 0., 0., 0., tz,-ty], [0., 1., 0.,-tz, 0., tx], [0., 0., 1., ty,-tx, 0.], [0., 0., 0., 1., 0., 0.], [0., 0., 0., 0., 1., 0.], [0., 0., 0., 0., 0., 1.]]) J = np.asmatrix(op.jacobian.value) J_ref = twist * Jgaze # Check w_M_s == w_M_s_ref self.assertEqual(np.equal(J, J_ref).all(), True)
def test_rotation(self): T = ((0., 0., 1., 0.), (0.,-1., 0., 0.), (1., 0., 0., 0.), (0., 0., 0., 1.)) op = OpPointModifier('op3') op.setTransformation(T) op.positionIN.value = gaze op.jacobianIN.value = Jgaze op.position.recompute(1) op.jacobian.recompute(1) self.assertEqual(op.getTransformation(), T) # w_M_s = w_M_g * g_M_s w_M_g = np.asmatrix(gaze) g_M_s = np.asmatrix(T) w_M_s_ref = w_M_g * g_M_s w_M_s = np.asmatrix(op.position.value) # Check w_M_s == w_M_s_ref self.assertEqual(np.equal(w_M_s, w_M_s_ref).all(), True) twist = np.matrix( [[0., 0., 1., 0., 0., 0.], [0.,-1., 0., 0., 0., 0.], [1., 0., 0., 0., 0., 0.], [0., 0., 0., 0., 0., 1.], [0., 0., 0., 0.,-1., 0.], [0., 0., 0., 1., 0., 0.]]) J = np.asmatrix(op.jacobian.value) J_ref = twist * Jgaze # Check w_M_s == w_M_s_ref self.assertEqual(np.equal(J, J_ref).all(), True)
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
class MetaTaskVisualPoint(object): name = '' opPoint = '' dyn = 0 task = 0 feature = 0 featureDes = 0 proj = 0 def opPointExist(self, opPoint): sigsP = filter(lambda x: x.getName().split(':')[-1] == opPoint, self.dyn.signals()) sigsJ = filter(lambda x: x.getName().split(':')[-1] == 'J' + opPoint, self.dyn.signals()) return len(sigsP) == 1 & len(sigsJ) == 1 def defineDynEntities(self, dyn): self.dyn = dyn def createOpPoint(self, opPoint, opPointRef='right-wrist'): self.opPoint = opPoint if self.opPointExist(opPoint): return self.dyn.createOpPoint(opPoint, opPointRef) def createOpPointModif(self): self.opPointModif = OpPointModifier('opmodif' + self.name) plug(self.dyn.signal(self.opPoint), self.opPointModif.signal('positionIN')) plug(self.dyn.signal('J' + self.opPoint), self.opPointModif.signal('jacobianIN')) self.opPointModif.activ = False def createFeatures(self): self.feature = FeatureVisualPoint('feature' + self.name) self.featureDes = FeatureVisualPoint('feature' + self.name + '_ref') self.feature.selec.value = '11' def createTask(self): self.task = Task('task' + self.name) def createGain(self): self.gain = GainAdaptive('gain' + self.name) self.gain.set(0.1, 0.1, 125e3) def createProj(self): self.proj = VisualPointProjecter('proj' + self.name) def plugEverything(self): self.feature.setReference(self.featureDes.name) plug(self.dyn.signal(self.opPoint), self.proj.signal('transfo')) plug(self.proj.signal('point2D'), self.feature.signal('xy')) plug(self.proj.signal('depth'), self.feature.signal('Z')) plug(self.dyn.signal('J' + self.opPoint), self.feature.signal('Jq')) self.task.add(self.feature.name) plug(self.task.error, self.gain.error) plug(self.gain.gain, self.task.controlGain) def __init__(self, name, dyn, opPoint, opPointRef='right-wrist'): self.name = name self.defineDynEntities(dyn) self.createOpPoint(opPoint, opPointRef) self.createOpPointModif() self.createFeatures() self.createTask() self.createGain() self.createProj() self.plugEverything() @property def ref(self): return self.featureDes.xy.value @ref.setter def ref(self, m): self.featureDes.xy.value = m @property def target(self): return self.proj.point3D @target.setter def target(self, m): self.proj.point3D.value = m @property def opmodif(self): if not self.opPointModif.activ: return False else: return self.opPointModif.getTransformation() @opmodif.setter def opmodif(self, m): if isinstance(m, bool) and not m: plug(self.dyn.signal(self.opPoint), self.proj.signal('transfo')) plug(self.dyn.signal('J' + self.opPoint), self.feature.signal('Jq')) self.opPointModif.activ = False else: if not self.opPointModif.activ: plug(self.opPointModif.signal('position'), self.proj.signal('transfo')) plug(self.opPointModif.signal('jacobian'), self.feature.signal('Jq')) self.opPointModif.setTransformation(m) self.opPointModif.activ = True def goto3D(self, point3D, gain=None, ref2D=None, selec=None): self.target = point3D if ref2D is not None: self.ref = ref2D if selec is not None: self.feature.selec.value = selec setGain(self.gain, gain)
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 createOpPointModifBase(self): self.opPointModifBase = OpPointModifier('opmodifBase' + self.name) plug(self.dyn.signal(self.opPointBase), self.opPointModifBase.signal('positionIN')) plug(self.dyn.signal('J' + self.opPointBase), self.opPointModifBase.signal('jacobianIN')) self.opPointModifBase.activ = False
def __init__(self, robot, name='flextimatorEncoders'): DGIMUModelBaseFlexEstimation.__init__(self, name) self.setSamplingPeriod(0.005) self.robot = robot # Covariances self.setProcessNoiseCovariance( matrixToTuple( np.diag((1e-8, ) * 12 + (1e-4, ) * 3 + (1e-4, ) * 3 + (1e-4, ) * 3 + (1e-4, ) * 3 + (1.e-2, ) * 6 + (1e-15, ) * 2 + (1.e-8, ) * 3))) self.setMeasurementNoiseCovariance( matrixToTuple(np.diag((1e-3, ) * 3 + (1e-6, ) * 3))) self.setUnmodeledForceVariance(1e-13) self.setForceVariance(1e-4) self.setAbsolutePosVariance(1e-4) # Contact model definition self.setContactModel(1) self.setKfe(matrixToTuple(np.diag((40000, 40000, 40000)))) self.setKfv(matrixToTuple(np.diag((600, 600, 600)))) self.setKte(matrixToTuple(np.diag((600, 600, 600)))) self.setKtv(matrixToTuple(np.diag((60, 60, 60)))) #Estimator interface self.interface = EstimatorInterface(name + "EstimatorInterface") self.interface.setLeftHandSensorTransformation((0., 0., 1.57)) self.interface.setRightHandSensorTransformation((0., 0., 1.57)) self.interface.setFDInertiaDot(True) # State and measurement definition self.interface.setWithUnmodeledMeasurements(False) self.interface.setWithModeledForces(True) self.interface.setWithAbsolutePose(False) self.setWithComBias(False) # Contacts forces plug(self.robot.device.forceLLEG, self.interface.force_lf) plug(self.robot.device.forceRLEG, self.interface.force_rf) plug(self.robot.device.forceLARM, self.interface.force_lh) plug(self.robot.device.forceRARM, self.interface.force_rh) # Selecting robotState self.robot.device.robotState.value = 46 * (0., ) self.robotState = Selec_of_vector('robotState') plug(self.robot.device.robotState, self.robotState.sin) self.robotState.selec(0, 36) # Reconstruction of the position of the free flyer from encoders # Create dynamic with the free flyer at the origin of the control frame self.robot.dynamicOdo = self.createDynamic(self.robotState.sout, '_dynamicOdo') self.robot.dynamicOdo.inertia.recompute(1) self.robot.dynamicOdo.waist.recompute(1) # Reconstruction of the position of the contacts in dynamicOdo self.leftFootPosOdo = Multiply_of_matrixHomo(name + "leftFootPosOdo") plug(self.robot.dynamicOdo.signal('left-ankle'), self.leftFootPosOdo.sin1) self.leftFootPosOdo.sin2.value = self.robot.forceSensorInLeftAnkle self.rightFootPosOdo = Multiply_of_matrixHomo(name + "rightFootPosOdo") plug(self.robot.dynamicOdo.signal('right-ankle'), self.rightFootPosOdo.sin1) self.rightFootPosOdo.sin2.value = self.robot.forceSensorInRightAnkle # Odometry self.odometry = Odometry(name + 'odometry') plug(self.robot.frames['leftFootForceSensor'].position, self.odometry.leftFootPositionRef) plug(self.robot.frames['rightFootForceSensor'].position, self.odometry.rightFootPositionRef) plug(self.rightFootPosOdo.sout, self.odometry.rightFootPositionIn) plug(self.leftFootPosOdo.sout, self.odometry.leftFootPositionIn) plug(self.robot.device.forceLLEG, self.odometry.force_lf) plug(self.robot.device.forceRLEG, self.odometry.force_rf) self.odometry.setLeftFootPosition( self.robot.frames['leftFootForceSensor'].position.value) self.odometry.setRightFootPosition( self.robot.frames['rightFootForceSensor'].position.value) plug(self.interface.stackOfSupportContacts, self.odometry.stackOfSupportContacts) # Create dynamicEncoders self.robotStateWoFF = Selec_of_vector('robotStateWoFF') plug(self.robot.device.robotState, self.robotStateWoFF.sin) self.robotStateWoFF.selec(6, 36) self.stateEncoders = Stack_of_vector(name + 'stateEncoders') plug(self.odometry.freeFlyer, self.stateEncoders.sin1) plug(self.robotStateWoFF.sout, self.stateEncoders.sin2) self.stateEncoders.selec1(0, 6) self.stateEncoders.selec2(0, 30) self.robot.dynamicEncoders = self.createDynamic( self.stateEncoders.sout, '_dynamicEncoders') # self.robot.dynamicEncoders=self.createDynamic(self.robotState.sout,'_dynamicEncoders') # plug(self.odometry.freeFlyer,self.robot.dynamicEncoders.ffposition) # self.robot.dynamicEncoders=self.createDynamic(self.robot.device.state,'_dynamicEncoders') # Reconstruction of the position of the contacts in dynamicEncoders self.leftFootPos = Multiply_of_matrixHomo("leftFootPos") plug(self.robot.dynamicEncoders.signal('left-ankle'), self.leftFootPos.sin1) self.leftFootPos.sin2.value = self.robot.forceSensorInLeftAnkle self.rightFootPos = Multiply_of_matrixHomo("rightFootPos") plug(self.robot.dynamicEncoders.signal('right-ankle'), self.rightFootPos.sin1) self.rightFootPos.sin2.value = self.robot.forceSensorInRightAnkle # Contacts velocities self.leftFootVelocity = Multiply_matrix_vector('leftFootVelocity') plug(self.robot.frames['leftFootForceSensor'].jacobian, self.leftFootVelocity.sin1) plug(self.robot.dynamicEncoders.velocity, self.leftFootVelocity.sin2) self.rightFootVelocity = Multiply_matrix_vector('rightFootVelocity') plug(self.robot.frames['rightFootForceSensor'].jacobian, self.rightFootVelocity.sin1) plug(self.robot.dynamicEncoders.velocity, self.rightFootVelocity.sin2) # Contacts positions and velocities plug(self.leftFootPos.sout, self.interface.position_lf) plug(self.rightFootPos.sout, self.interface.position_rf) plug(self.leftFootVelocity.sout, self.interface.velocity_lf) plug(self.rightFootVelocity.sout, self.interface.velocity_rf) plug(self.robot.dynamicEncoders.signal('right-wrist'), self.interface.position_lh) plug(self.robot.dynamicEncoders.signal('left-wrist'), self.interface.position_rh) # Compute contacts number plug(self.interface.supportContactsNbr, self.contactNbr) # Contacts model and config plug(self.interface.contactsModel, self.contactsModel) self.setWithConfigSignal(True) plug(self.interface.config, self.config) # Drift self.drift = DriftFromMocap(name + 'Drift') # Compute measurement vector plug(self.robot.device.accelerometer, self.interface.accelerometer) plug(self.robot.device.gyrometer, self.interface.gyrometer) plug(self.drift.driftVector, self.interface.drift) plug(self.interface.measurement, self.measurement) # Input reconstruction # IMU Vector # Creating an operational point for the IMU self.robot.dynamicEncoders.createJacobian(name + 'ChestJ_OpPoint', 'chest') self.imuOpPoint = OpPointModifier(name + 'IMU_oppoint') self.imuOpPoint.setTransformation( matrixToTuple( np.linalg.inv(np.matrix( self.robot.dynamicEncoders.chest.value)) * np.matrix(self.robot.frames['accelerometer'].position.value))) self.imuOpPoint.setEndEffector(False) plug(self.robot.dynamicEncoders.chest, self.imuOpPoint.positionIN) plug(self.robot.dynamicEncoders.signal(name + 'ChestJ_OpPoint'), self.imuOpPoint.jacobianIN) # IMU position self.PosAccelerometer = Multiply_of_matrixHomo(name + "PosAccelerometer") plug(self.robot.dynamicEncoders.chest, self.PosAccelerometer.sin1) self.PosAccelerometer.sin2.value = matrixToTuple( self.robot.accelerometerPosition) self.inputPos = MatrixHomoToPoseUTheta(name + 'InputPosition') plug(self.PosAccelerometer.sout, self.inputPos.sin) # IMU velocity self.inputVel = Multiply_matrix_vector(name + 'InputVelocity') plug(self.imuOpPoint.jacobian, self.inputVel.sin1) plug(self.robot.dynamicEncoders.velocity, self.inputVel.sin2) # Concatenate self.inputPosVel = Stack_of_vector(name + 'InputPosVel') plug(self.inputPos.sout, self.inputPosVel.sin1) plug(self.inputVel.sout, self.inputPosVel.sin2) self.inputPosVel.selec1(0, 6) self.inputPosVel.selec2(0, 6) # IMU Vector self.IMUVector = PositionStateReconstructor(name + 'EstimatorInput') plug(self.inputPosVel.sout, self.IMUVector.sin) self.IMUVector.inputFormat.value = '001111' self.IMUVector.outputFormat.value = '011111' self.IMUVector.setFiniteDifferencesInterval(2) # CoM and derivatives self.comIn = self.robot.dynamicEncoders.com self.comVector = PositionStateReconstructor(name + 'ComVector') plug(self.comIn, self.comVector.sin) self.comVector.inputFormat.value = '000001' self.comVector.outputFormat.value = '010101' self.comVector.setFiniteDifferencesInterval(20) # Compute derivative of Angular Momentum self.angMomDerivator = Derivator_of_Vector(name + 'angMomDerivator') plug(self.robot.dynamicEncoders.angularmomentum, self.angMomDerivator.sin) self.angMomDerivator.dt.value = self.robot.timeStep # Concatenate with interace estimator plug(self.comVector.sout, self.interface.comVector) plug(self.robot.dynamicEncoders.inertia, self.interface.inertia) plug(self.robot.dynamicEncoders.angularmomentum, self.interface.angMomentum) plug(self.angMomDerivator.sout, self.interface.dangMomentum) self.interface.dinertia.value = (0, 0, 0, 0, 0, 0) plug(self.robot.dynamicEncoders.waist, self.interface.positionWaist) plug(self.IMUVector.sout, self.interface.imuVector) plug(self.interface.input, self.input) self.robot.flextimator = self
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
plug(robot.device.accelerometer, sensorStack.sin1) plug(robot.device.gyrometer, sensorStack.sin2) sensorStack.selec1(0, 3) sensorStack.selec2(0, 3) plug(sensorStack.sout, meas) inputPos = MatrixHomoToPoseUTheta("inputPosition") plug(robot.frames["accelerometer"].position, inputPos.sin) inputVector = sotso.PositionStateReconstructor("estimatorInput") robot.dynamic.createJacobian("jchest", "chest") imu = OpPointModifier("IMU_oppoint") imu.setEndEffector(True) imu.setTransformation( matrixToTuple( matrix(robot.frames["accelerometer"].position.value) * np.linalg.inv(matrix(robot.dynamic.chest.value)) ) ) plug(inputPos.sout, inputVector.sin) inputVector.inputFormat.value = "000011" inputVector.outputFormat.value = "011111" plug(inputVector.sout, inputs) flex = est.signal("flexMatrixInverse") flexdot = est.signal("flexInverseVelocityVector")
def __init__(self, robot, name='flextimatorEncoders'): DGIMUModelBaseFlexEstimation.__init__(self,name) self.setSamplingPeriod(0.005) self.robot = robot # Covariances self.setProcessNoiseCovariance(matrixToTuple(np.diag((1e-8,)*12+(1e-4,)*3+(1e-4,)*3+(1e-4,)*3+(1e-4,)*3+(1.e-2,)*6+(1e-15,)*2+(1.e-8,)*3))) self.setMeasurementNoiseCovariance(matrixToTuple(np.diag((1e-3,)*3+(1e-6,)*3))) self.setUnmodeledForceVariance(1e-13) self.setForceVariance(1e-4) self.setAbsolutePosVariance(1e-4) # Contact model definition self.setContactModel(1) self.setKfe(matrixToTuple(np.diag((40000,40000,40000)))) self.setKfv(matrixToTuple(np.diag((600,600,600)))) self.setKte(matrixToTuple(np.diag((600,600,600)))) self.setKtv(matrixToTuple(np.diag((60,60,60)))) #Estimator interface self.interface=EstimatorInterface(name+"EstimatorInterface") self.interface.setLeftHandSensorTransformation((0.,0.,1.57)) self.interface.setRightHandSensorTransformation((0.,0.,1.57)) self.interface.setFDInertiaDot(True) # State and measurement definition self.interface.setWithUnmodeledMeasurements(False) self.interface.setWithModeledForces(True) self.interface.setWithAbsolutePose(False) self.setWithComBias(False) # Contacts forces plug (self.robot.device.forceLLEG,self.interface.force_lf) plug (self.robot.device.forceRLEG,self.interface.force_rf) plug (self.robot.device.forceLARM,self.interface.force_lh) plug (self.robot.device.forceRARM,self.interface.force_rh) # Selecting robotState self.robot.device.robotState.value=46*(0.,) self.robotState = Selec_of_vector('robotState') plug(self.robot.device.robotState,self.robotState.sin) self.robotState.selec(0,36) # Reconstruction of the position of the free flyer from encoders # Create dynamic with the free flyer at the origin of the control frame self.robot.dynamicOdo=self.createDynamic(self.robotState.sout,'_dynamicOdo') self.robot.dynamicOdo.inertia.recompute(1) self.robot.dynamicOdo.waist.recompute(1) # Reconstruction of the position of the contacts in dynamicOdo self.leftFootPosOdo=Multiply_of_matrixHomo(name+"leftFootPosOdo") plug(self.robot.dynamicOdo.signal('left-ankle'),self.leftFootPosOdo.sin1) self.leftFootPosOdo.sin2.value=self.robot.forceSensorInLeftAnkle self.rightFootPosOdo=Multiply_of_matrixHomo(name+"rightFootPosOdo") plug(self.robot.dynamicOdo.signal('right-ankle'),self.rightFootPosOdo.sin1) self.rightFootPosOdo.sin2.value=self.robot.forceSensorInRightAnkle # Odometry self.odometry=Odometry (name+'odometry') plug (self.robot.frames['leftFootForceSensor'].position,self.odometry.leftFootPositionRef) plug (self.robot.frames['rightFootForceSensor'].position,self.odometry.rightFootPositionRef) plug (self.rightFootPosOdo.sout,self.odometry.rightFootPositionIn) plug (self.leftFootPosOdo.sout,self.odometry.leftFootPositionIn) plug (self.robot.device.forceLLEG,self.odometry.force_lf) plug (self.robot.device.forceRLEG,self.odometry.force_rf) self.odometry.setLeftFootPosition(self.robot.frames['leftFootForceSensor'].position.value) self.odometry.setRightFootPosition(self.robot.frames['rightFootForceSensor'].position.value) plug(self.interface.stackOfSupportContacts,self.odometry.stackOfSupportContacts) # Create dynamicEncoders self.robotStateWoFF = Selec_of_vector('robotStateWoFF') plug(self.robot.device.robotState,self.robotStateWoFF.sin) self.robotStateWoFF.selec(6,36) self.stateEncoders = Stack_of_vector (name+'stateEncoders') plug(self.odometry.freeFlyer,self.stateEncoders.sin1) plug(self.robotStateWoFF.sout,self.stateEncoders.sin2) self.stateEncoders.selec1 (0, 6) self.stateEncoders.selec2 (0, 30) self.robot.dynamicEncoders=self.createDynamic(self.stateEncoders.sout,'_dynamicEncoders') # self.robot.dynamicEncoders=self.createDynamic(self.robotState.sout,'_dynamicEncoders') # plug(self.odometry.freeFlyer,self.robot.dynamicEncoders.ffposition) # self.robot.dynamicEncoders=self.createDynamic(self.robot.device.state,'_dynamicEncoders') # Reconstruction of the position of the contacts in dynamicEncoders self.leftFootPos=Multiply_of_matrixHomo("leftFootPos") plug(self.robot.dynamicEncoders.signal('left-ankle'),self.leftFootPos.sin1) self.leftFootPos.sin2.value=self.robot.forceSensorInLeftAnkle self.rightFootPos=Multiply_of_matrixHomo("rightFootPos") plug(self.robot.dynamicEncoders.signal('right-ankle'),self.rightFootPos.sin1) self.rightFootPos.sin2.value=self.robot.forceSensorInRightAnkle # Contacts velocities self.leftFootVelocity = Multiply_matrix_vector ('leftFootVelocity') plug(self.robot.frames['leftFootForceSensor'].jacobian,self.leftFootVelocity.sin1) plug(self.robot.dynamicEncoders.velocity,self.leftFootVelocity.sin2) self.rightFootVelocity = Multiply_matrix_vector ('rightFootVelocity') plug(self.robot.frames['rightFootForceSensor'].jacobian,self.rightFootVelocity.sin1) plug(self.robot.dynamicEncoders.velocity,self.rightFootVelocity.sin2) # Contacts positions and velocities plug (self.leftFootPos.sout,self.interface.position_lf) plug (self.rightFootPos.sout,self.interface.position_rf) plug (self.leftFootVelocity.sout,self.interface.velocity_lf) plug (self.rightFootVelocity.sout,self.interface.velocity_rf) plug (self.robot.dynamicEncoders.signal('right-wrist'),self.interface.position_lh) plug (self.robot.dynamicEncoders.signal('left-wrist'),self.interface.position_rh) # Compute contacts number plug (self.interface.supportContactsNbr,self.contactNbr) # Contacts model and config plug(self.interface.contactsModel,self.contactsModel) self.setWithConfigSignal(True) plug(self.interface.config,self.config) # Drift self.drift = DriftFromMocap(name+'Drift') # Compute measurement vector plug(self.robot.device.accelerometer,self.interface.accelerometer) plug(self.robot.device.gyrometer,self.interface.gyrometer) plug(self.drift.driftVector,self.interface.drift) plug(self.interface.measurement,self.measurement) # Input reconstruction # IMU Vector # Creating an operational point for the IMU self.robot.dynamicEncoders.createJacobian(name+'ChestJ_OpPoint','chest') self.imuOpPoint = OpPointModifier(name+'IMU_oppoint') self.imuOpPoint.setTransformation(matrixToTuple(np.linalg.inv(np.matrix(self.robot.dynamicEncoders.chest.value))*np.matrix(self.robot.frames['accelerometer'].position.value))) self.imuOpPoint.setEndEffector(False) plug (self.robot.dynamicEncoders.chest,self.imuOpPoint.positionIN) plug (self.robot.dynamicEncoders.signal(name+'ChestJ_OpPoint'),self.imuOpPoint.jacobianIN) # IMU position self.PosAccelerometer=Multiply_of_matrixHomo(name+"PosAccelerometer") plug(self.robot.dynamicEncoders.chest,self.PosAccelerometer.sin1) self.PosAccelerometer.sin2.value=matrixToTuple(self.robot.accelerometerPosition) self.inputPos = MatrixHomoToPoseUTheta(name+'InputPosition') plug(self.PosAccelerometer.sout,self.inputPos.sin) # IMU velocity self.inputVel = Multiply_matrix_vector(name+'InputVelocity') plug(self.imuOpPoint.jacobian,self.inputVel.sin1) plug(self.robot.dynamicEncoders.velocity,self.inputVel.sin2) # Concatenate self.inputPosVel = Stack_of_vector (name+'InputPosVel') plug(self.inputPos.sout,self.inputPosVel.sin1) plug(self.inputVel.sout,self.inputPosVel.sin2) self.inputPosVel.selec1 (0, 6) self.inputPosVel.selec2 (0, 6) # IMU Vector self.IMUVector = PositionStateReconstructor (name+'EstimatorInput') plug(self.inputPosVel.sout,self.IMUVector.sin) self.IMUVector.inputFormat.value = '001111' self.IMUVector.outputFormat.value = '011111' self.IMUVector.setFiniteDifferencesInterval(2) # CoM and derivatives self.comIn=self.robot.dynamicEncoders.com self.comVector = PositionStateReconstructor (name+'ComVector') plug(self.comIn,self.comVector.sin) self.comVector.inputFormat.value = '000001' self.comVector.outputFormat.value = '010101' self.comVector.setFiniteDifferencesInterval(20) # Compute derivative of Angular Momentum self.angMomDerivator = Derivator_of_Vector(name+'angMomDerivator') plug(self.robot.dynamicEncoders.angularmomentum,self.angMomDerivator.sin) self.angMomDerivator.dt.value = self.robot.timeStep # Concatenate with interace estimator plug(self.comVector.sout,self.interface.comVector) plug(self.robot.dynamicEncoders.inertia,self.interface.inertia) plug(self.robot.dynamicEncoders.angularmomentum,self.interface.angMomentum) plug(self.angMomDerivator.sout,self.interface.dangMomentum) self.interface.dinertia.value=(0,0,0,0,0,0) plug(self.robot.dynamicEncoders.waist,self.interface.positionWaist) plug(self.IMUVector.sout,self.interface.imuVector) plug(self.interface.input,self.input) self.robot.flextimator = self
class MetaTaskKine6dRel(MetaTask6d): opPointBase = '' def createOpPointBase(self, opPointBase, opPointRefBase='left-wrist'): self.opPointBase = opPointBase if self.opPointExist(opPointBase): return self.dyn.createOpPoint(opPointBase, opPointRefBase) def createOpPointModifBase(self): self.opPointModifBase = OpPointModifier('opmodifBase' + self.name) plug(self.dyn.signal(self.opPointBase), self.opPointModifBase.signal('positionIN')) plug(self.dyn.signal('J' + self.opPointBase), self.opPointModifBase.signal('jacobianIN')) self.opPointModifBase.activ = False def createFeatures(self): self.feature = FeaturePoint6dRelative('featureRel' + self.name) self.featureDes = FeaturePoint6dRelative('featureRel' + self.name + '_ref') self.feature.selec.value = '111111' self.feature.frame('current') def plugEverything(self): self.feature.setReference(self.featureDes.name) plug(self.dyn.signal(self.opPoint), self.feature.signal('position')) plug(self.dyn.signal('J' + self.opPoint), self.feature.signal('Jq')) plug(self.dyn.signal(self.opPointBase), self.feature.signal('positionRef')) plug(self.dyn.signal('J' + self.opPointBase), self.feature.signal('JqRef')) self.task.add(self.feature.name) plug(self.task.error, self.gain.error) plug(self.gain.gain, self.task.controlGain) def keep(self): self.feature.position.recompute(self.dyn.position.time) self.feature.keep() def __init__(self, name, dyn, opPoint, opPointBase, opPointRef='right-wrist', opPointRefBase='left-wrist'): self.name = name self.defineDynEntities(dyn) self.createOpPoint(opPoint, opPointRef) self.createOpPointBase(opPointBase, opPointRefBase) self.createOpPointModif() self.createOpPointModifBase() self.createFeatures() self.createTask() self.createGain() self.plugEverything() @property def opmodifBase(self): if not self.opPointModifBase.activ: return False else: return self.opPointModifBase.getTransformation() @opmodifBase.setter def opmodifBase(self, m): if isinstance(m, bool) and not m: plug(self.dyn.signal(self.opPointBase), self.feature.signal('positionRef')) plug(self.dyn.signal('J' + self.opPointBase), self.feature.signal('JqRef')) self.opPointModifBase.activ = False else: if not self.opPointModifBase.activ: plug(self.opPointModifBase.signal('position'), self.feature.positionRef) plug(self.opPointModifBase.signal('jacobian'), self.feature.JqRef) self.opPointModifBase.setTransformation(m) self.opPointModifBase.activ = True
contactNbr = est1.signal('contactNbr') flexVect=est1.signal('flexibility') flex=est1.signal('flexMatrixInverse') flexdot = est1.signal('flexInverseVelocityVector') # contacts definition contactNbr.value = 2 est1.setContactModel(2) # Position of the anchorage in the global frame Peg = (0,0,4.60) # Positions of the contacts on the robot (in the local frame) with respect to the chest Prl1 = np.matrix([[1,0,0,0-3.19997004e-02],[0,1,0,0.15-0],[0,0,1,1.28-1],[0,0,0,1]]) Prl2 = np.matrix([[1,0,0,0-3.19997004e-02],[0,1,0,-0.15-0],[0,0,1,1.28-1],[0,0,0,1]]) # contact 1 contact1OpPoint = OpPointModifier('contact1_oppoint') contact1OpPoint.setEndEffector(False) contact1OpPoint.setTransformation(matrixToTuple(Prl1)) plug (robot.dynamic.chest,contact1OpPoint.positionIN) contact1Pos = MatrixHomoToPose('contact1Pos') plug(contact1OpPoint.position, contact1Pos.sin) contact1 = Stack_of_vector ('contact1') contact1.sin1.value = Peg plug(contact1Pos.sout,contact1.sin2) #contact1.sin2.value = (0,-0.15,1.28) contact1.selec1 (0, 3) contact1.selec2 (0, 3) # contact 2 contact2OpPoint = OpPointModifier('contact2_oppoint')
class MetaTask6d(object): name = '' opPoint = '' dyn = 0 task = 0 feature = 0 featureDes = 0 def opPointExist(self, opPoint): sigsP = filter(lambda x: x.getName().split(':')[-1] == opPoint, self.dyn.signals()) sigsJ = filter(lambda x: x.getName().split(':')[-1] == 'J' + opPoint, self.dyn.signals()) return len(sigsP) == 1 & len(sigsJ) == 1 def defineDynEntities(self, dyn): self.dyn = dyn def createOpPoint(self, opPoint, opPointRef='right-wrist'): self.opPoint = opPoint if self.opPointExist(opPoint): return self.dyn.createOpPoint(opPoint, opPointRef) def createOpPointModif(self): self.opPointModif = OpPointModifier('opmodif' + self.name) plug(self.dyn.signal(self.opPoint), self.opPointModif.signal('positionIN')) plug(self.dyn.signal('J' + self.opPoint), self.opPointModif.signal('jacobianIN')) self.opPointModif.activ = False def createFeatures(self): self.feature = FeaturePoint6d('feature' + self.name) self.featureDes = FeaturePoint6d('feature' + self.name + '_ref') self.feature.selec.value = '111111' self.feature.frame('current') def createTask(self): self.task = Task('task' + self.name) def createGain(self): self.gain = GainAdaptive('gain' + self.name) self.gain.set(0.1, 0.1, 125e3) def plugEverything(self): self.feature.setReference(self.featureDes.name) plug(self.dyn.signal(self.opPoint), self.feature.signal('position')) plug(self.dyn.signal('J' + self.opPoint), self.feature.signal('Jq')) self.task.add(self.feature.name) plug(self.task.error, self.gain.error) plug(self.gain.gain, self.task.controlGain) def keep(self): self.feature.position.recompute(self.dyn.position.time) self.feature.keep() def __init__(self, name, dyn, opPoint, opPointRef='right-wrist'): self.name = name self.defineDynEntities(dyn) self.createOpPoint(opPoint, opPointRef) self.createOpPointModif() self.createFeatures() self.createTask() self.createGain() self.plugEverything() @property def ref(self): return self.featureDes.position.value @ref.setter def ref(self, m): self.featureDes.position.value = m @property def opmodif(self): if not self.opPointModif.activ: return False else: return self.opPointModif.getTransformation() @opmodif.setter def opmodif(self, m): if isinstance(m, bool) and not m: plug(self.dyn.signal(self.opPoint), self.feature.signal('position')) plug(self.dyn.signal('J' + self.opPoint), self.feature.signal('Jq')) self.opPointModif.activ = False else: if not self.opPointModif.activ: plug(self.opPointModif.signal('position'), self.feature.position) plug(self.opPointModif.signal('jacobian'), self.feature.Jq) self.opPointModif.setTransformation(m) self.opPointModif.activ = True
self.sot.setNumberDofs(self.robot.dimension) if robot.simu: plug(self.sot.signal('control'), robot.simu.signal('control')) plug(self.robot.simu.state, self.robot.dynamic.position) robot = Hrp2("robot") solver = Solver(robot) timeStep = 0.005 robot.dynamic.gaze.recompute(0) robot.dynamic.Jgaze.recompute(0) lwcam = OpPointModifier('lwcam') plug(robot.dynamic.gaze, lwcam.positionIN) plug(robot.dynamic.Jgaze, lwcam.jacobianIN) # HRP2-14 extrinsic camera parameters leftWideCamera = ((1., 0., 0., 0.035), (0., 1., 0., 0.072), (0., 0., 1., 0.075), (0., 0., 0., 1.)) lwcam.setTransformation(leftWideCamera) lwcam.position.recompute(0) lwcam.jacobian.recompute(0) print "GAZE"
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
from dynamic_graph.sot.dynamics.tools import * def matrixToTuple(M): tmp = M.tolist() res = [] for i in tmp: res.append(tuple(i)) return tuple(res) robot.dynamic.gaze.recompute(0) robot.dynamic.Jgaze.recompute(0) # Camera related frames. w_M_c = OpPointModifier('w_M_c') # Compute the transformation between the # gaze and the left bottom (wide) camera. # Extrinsic camera parameters. g_M_c1 = np.matrix( [[1., 0., 0., -0.072], [0., 1., 0., -0.075], [0., 0., 1., 0.035], [0., 0., 0., 1.]]) # Frames re-orientation: # Z = depth (increase from near to far) # X = increase from left to right # Y = increase from top to bottom
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"): DGIMUModelBaseFlexEstimation.__init__(self, name) self.setSamplingPeriod(0.005) self.robot = robot # Covariances self.setProcessNoiseCovariance( matrixToTuple( np.diag( (1e-8,) * 12 + (1e-4,) * 3 + (1e-4,) * 3 + (1e-4,) * 3 + (1e-4,) * 3 + (1.0e-2,) * 6 + (1e-15,) * 2 + (1.0e-8,) * 3 ) ) ) self.setMeasurementNoiseCovariance(matrixToTuple(np.diag((1e-3,) * 3 + (1e-6,) * 3))) self.setUnmodeledForceVariance(1e-13) self.setForceVariance(1e-4) self.setAbsolutePosVariance(1e-4) # Contact model definition self.setKfe(matrixToTuple(np.diag((40000, 40000, 40000)))) self.setKfv(matrixToTuple(np.diag((600, 600, 600)))) self.setKte(matrixToTuple(np.diag((600, 600, 600)))) self.setKtv(matrixToTuple(np.diag((60, 60, 60)))) # Estimator interface self.interface = EstimatorInterface(name + "EstimatorInterface") self.interface.setLeftHandSensorTransformation((0.0, 0.0, 1.57)) self.interface.setRightHandSensorTransformation((0.0, 0.0, 1.57)) # State and measurement definition self.interface.setWithUnmodeledMeasurements(False) self.interface.setWithModeledForces(True) self.interface.setWithAbsolutePose(False) self.setWithComBias(False) # Contacts velocities self.leftFootVelocity = Multiply_matrix_vector("leftFootVelocity") plug(self.robot.frames["leftFootForceSensor"].jacobian, self.leftFootVelocity.sin1) plug(self.robot.device.velocity, self.leftFootVelocity.sin2) self.rightFootVelocity = Multiply_matrix_vector("rightFootVelocity") plug(self.robot.frames["rightFootForceSensor"].jacobian, self.rightFootVelocity.sin1) plug(self.robot.device.velocity, self.rightFootVelocity.sin2) self.interface.setFDInertiaDot(True) # Contacts forces, positions and velocities # Feet plug(self.robot.device.forceLLEG, self.interface.force_lf) plug(self.robot.device.forceRLEG, self.interface.force_rf) plug(self.robot.frames["leftFootForceSensor"].position, self.interface.position_lf) plug(self.robot.frames["rightFootForceSensor"].position, self.interface.position_rf) plug(self.leftFootVelocity.sout, self.interface.velocity_lf) plug(self.rightFootVelocity.sout, self.interface.velocity_rf) # Hands plug(self.robot.device.forceLARM, self.interface.force_lh) plug(self.robot.device.forceRARM, self.interface.force_rh) plug(self.robot.dynamic.signal("right-wrist"), self.interface.position_lh) plug(self.robot.dynamic.signal("left-wrist"), self.interface.position_rh) # Strings self.Peg = (0, 0, 4.60) # Position of the anchorage in the global frame self.Prl1 = np.matrix( [[1, 0, 0, 0 - 3.19997004e-02], [0, 1, 0, 0.15 - 0], [0, 0, 1, 1.28 - 1], [0, 0, 0, 1]] ) # Positions of the contacts on the robot (in the local frame) with respect to the chest self.Prl2 = np.matrix([[1, 0, 0, 0 - 3.19997004e-02], [0, 1, 0, -0.15 - 0], [0, 0, 1, 1.28 - 1], [0, 0, 0, 1]]) (self.contact1OpPoint, self.contact1Pos, self.contact1) = self.createContact("contact1", self.Prl1, self.Peg) (self.contact2OpPoint, self.contact2Pos, self.contact2) = self.createContact("contact2", self.Prl2, self.Peg) plug(self.contact1.sout, self.interface.position_ls) plug(self.contact2.sout, self.interface.position_rs) # Contacts model and config plug(self.interface.contactsModel, self.contactsModel) self.setWithConfigSignal(True) plug(self.interface.config, self.config) # Mocap signal self.ros = RosExport("rosExportMocap") self.ros.add("matrixHomoStamped", "chest", "/evart/hrp2_head_sf/hrp2_head_sf") # Filtering self.mocapFilter = MocapDataFilter("MocapDataFilter") plug(self.ros.signal("chest"), self.mocapFilter.sin) self.mocapSignal = self.mocapFilter.sout # Drift self.drift = DriftFromMocap(name + "Drift") plug(self.mocapSignal, self.drift.limbGlobal) plug(self.robot.dynamic.chest, self.drift.limbLocal) self.drift.init() # Measurement reconstruction plug(self.robot.device.accelerometer, self.interface.accelerometer) plug(self.robot.device.gyrometer, self.interface.gyrometer) plug(self.drift.driftVector, self.interface.drift) plug(self.interface.measurement, self.measurement) # Input reconstruction # IMU Vector self.inputPos = MatrixHomoToPoseUTheta(name + "InputPosition") plug(robot.frames["accelerometer"].position, self.inputPos.sin) self.robot.dynamic.createJacobian(name + "ChestJ_OpPoint", "chest") self.imuOpPoint = OpPointModifier(name + "IMU_oppoint") self.imuOpPoint.setEndEffector(False) self.imuOpPoint.setTransformation( matrixToTuple( np.linalg.inv(np.matrix(self.robot.dynamic.chest.value)) * np.matrix(self.robot.frames["accelerometer"].position.value) ) ) plug(self.robot.dynamic.chest, self.imuOpPoint.positionIN) plug(self.robot.dynamic.signal(name + "ChestJ_OpPoint"), self.imuOpPoint.jacobianIN) self.inputVel = Multiply_matrix_vector(name + "InputVelocity") plug(self.imuOpPoint.jacobian, self.inputVel.sin1) plug(self.robot.device.velocity, self.inputVel.sin2) self.inputPosVel = Stack_of_vector(name + "InputPosVel") plug(self.inputPos.sout, self.inputPosVel.sin1) plug(self.inputVel.sout, self.inputPosVel.sin2) self.inputPosVel.selec1(0, 6) self.inputPosVel.selec2(0, 6) self.IMUVector = PositionStateReconstructor(name + "EstimatorInput") plug(self.inputPosVel.sout, self.IMUVector.sin) self.IMUVector.inputFormat.value = "001111" self.IMUVector.outputFormat.value = "011111" self.IMUVector.setFiniteDifferencesInterval(1) self.inputPosVel.sout.recompute(0) self.IMUVector.setLastVector(self.inputPosVel.sout.value + (0.0,) * 6) # CoM and derivatives self.com = self.robot.dynamic.com self.DCom = Multiply_matrix_vector(name + "DCom") plug(self.robot.dynamic.Jcom, self.DCom.sin1) plug(self.robot.device.velocity, self.DCom.sin2) self.comVectorIn = Stack_of_vector(name + "ComVectorIn") plug(self.com, self.comVectorIn.sin1) plug(self.DCom.sout, self.comVectorIn.sin2) self.comVectorIn.selec1(0, 3) self.comVectorIn.selec2(0, 3) self.comVector = PositionStateReconstructor(name + "ComVector") plug(self.comVectorIn.sout, self.comVector.sin) self.comVector.inputFormat.value = "000101" self.comVector.outputFormat.value = "010101" self.comVector.setFiniteDifferencesInterval(1) self.DCom.sout.recompute(0) self.comVector.setLastVector(self.com.value + (0.0,) * 15) # (0.,)*3+self.DCom.sout.value+(0.,)*9) # Compute derivative of Angular Momentum self.angMomDerivator = Derivator_of_Vector(name + "angMomDerivator") plug(self.robot.dynamic.angularmomentum, self.angMomDerivator.sin) self.angMomDerivator.dt.value = self.robot.timeStep # self.angMomDerivator = PositionStateReconstructor (name+'angMomDerivator') # plug(self.robot.dynamic.angularmomentum,self.angMomDerivator.sin) # self.angMomDerivator.inputFormat.value = '000001' # self.angMomDerivator.outputFormat.value = '000100' # self.angMomDerivator.setFiniteDifferencesInterval(2) # self.robot.dynamic.angularmomentum.recompute(0) # self.angMomDerivator.setLastVector(self.robot.dynamic.angularmomentum.value+(0.,)*15) # Concatenate with interface estimator plug(self.comVector.sout, self.interface.comVector) plug(self.robot.dynamic.inertia, self.interface.inertia) self.interface.dinertia.value = (0, 0, 0, 0, 0, 0) plug(self.robot.dynamic.angularmomentum, self.interface.angMomentum) plug(self.angMomDerivator.sout, self.interface.dangMomentum) plug(self.robot.dynamic.waist, self.interface.positionWaist) plug(self.IMUVector.sout, self.interface.imuVector) plug(self.interface.input, self.input) plug(self.interface.modeledContactsNbr, self.contactNbr) self.robot.flextimator = self def initAbsolutePoses(self): self.drift.init() def createContact(self, name, prl, peg): self.contactOpPoint = OpPointModifier(name + "_opPoint") self.contactOpPoint.setEndEffector(False) self.contactOpPoint.setTransformation(matrixToTuple(prl)) plug(self.robot.dynamic.chest, self.contactOpPoint.positionIN) self.contactPos = MatrixHomoToPose(name + "_pos") plug(self.contactOpPoint.position, self.contactPos.sin) self.contact = Stack_of_vector(name) self.contact.sin1.value = peg plug(self.contactPos.sout, self.contact.sin2) self.contact.selec1(0, 3) self.contact.selec2(0, 3) return (self.contactOpPoint, self.contactPos, self.contact)