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