def createJointsTask(diag = None, gain = 1, selec = toFlags(range(0,robot.dimension)), jointsPos = tuple(numpy.zeros(robot.dimension))): taskJoints = MetaTaskJointWeights('jointsTask',selec,robot,diag,gain,0) jointsVel = numpy.zeros(robot.dimension) taskJoints.task.setVelocity(tuple(jointsVel)) taskJoints.task.setPositionDes(tuple(jointsPos)) plug(robot.dynamic.position,taskJoints.task.position) return taskJoints
def createWeightsTask(diag=None, gain=0.001, sampleInterval=0, selec=toFlags(range(0, robot.dimension))): taskWeights = MetaTaskJointWeights("jointWeights", selec, robot, diag, gain, sampleInterval) plug(robot.device.velocity, taskWeights.task.velocity) jointsPos = numpy.zeros(robot.dimension) taskWeights.task.setPosition(tuple(jointsPos)) taskWeights.task.setPositionDes(tuple(jointsPos)) taskWeights.task.velocity.recompute(0) return taskWeights
def createWeightsTask(diag = None, gain = 0.001, sampleInterval = 0, selec = toFlags(range(0,robot.dimension))): taskWeights = MetaTaskJointWeights('jointWeights',selec,robot,diag,gain,sampleInterval) plug(robot.device.velocity,taskWeights.task.velocity) jointsPos = numpy.zeros(robot.dimension) taskWeights.task.setPosition(tuple(jointsPos)) taskWeights.task.setPositionDes(tuple(jointsPos)) taskWeights.task.velocity.recompute(0) return taskWeights
def gotoNd(task,position,selec=None,gain=None,resetJacobian=True): M=generic6dReference(position) if selec!=None: if isinstance(selec,str): task.feature.selec.value = selec else: task.feature.selec.value = toFlags(selec) task.featureDes.position.value = matrixToTuple(M) setGain(task.gain,gain) if 'resetJacobianDerivative' in task.task.__class__.__dict__.keys() and resetJacobian: task.task.resetJacobianDerivative()
def change6dPositionReference(task,feature,gain,position,selec=None,ingain=None,resetJacobian=True): M=generic6dReference(position) if selec!=None: if isinstance(selec,str): feature.selec.value = selec else: feature.selec.value = toFlags(selec) feature.reference.value = matrixToTuple(M) if gain!=None: setGain(gain,ingain) if 'resetJacobianDerivative' in task.__class__.__dict__.keys() and resetJacobian: task.resetJacobianDerivative()
def Pr2JointLimitsTask(robot,dt): robot.dynamic.upperJl.recompute(0) robot.dynamic.lowerJl.recompute(0) task = TaskJointLimits('taskJL') plug(robot.dynamic.position,task.position) task.controlGain.value = 10 task.referenceInf.value = robot.dynamic.lowerJl.value task.referenceSup.value = robot.dynamic.upperJl.value task.dt.value = dt task.selec.value = toFlags(range(18,25)+range(26,27)+range(28,31)+range(32,40)+range(41,42)+range(43,46)+range(47,50)) return task
def zeroPosition(): diag = None gain = 0.01 jointsPos = numpy.zeros(robot.dimension) selec = toFlags(range(6, robot.dimension)) taskZeroPosition = MetaTaskJointWeights("zeroPos", selec, robot, diag, gain, 0) jointsVel = numpy.zeros(robot.dimension) taskZeroPosition.task.setVelocity(tuple(jointsVel)) taskZeroPosition.task.setPositionDes(tuple(jointsPos)) plug(robot.dynamic.position, taskZeroPosition.task.position) return taskZeroPosition
def zeroPosition(): diag = None gain = 0.01 jointsPos = numpy.zeros(robot.dimension) selec = toFlags(range(6,robot.dimension)) taskZeroPosition = MetaTaskJointWeights('zeroPos',selec,robot,diag,gain,0) jointsVel = numpy.zeros(robot.dimension) taskZeroPosition.task.setVelocity(tuple(jointsVel)) taskZeroPosition.task.setPositionDes(tuple(jointsPos)) plug(robot.dynamic.position,taskZeroPosition.task.position) return taskZeroPosition
def setTaskLim(taskJL,robot): """ Sets the parameters for the 'joint-limits' """ robot.dynamic.upperJl.recompute(0) robot.dynamic.lowerJl.recompute(0) plug(robot.dynamic.position,taskJL.position) taskJL.controlGain.value = 10 taskJL.referenceInf.value = robot.dynamic.lowerJl.value taskJL.referenceSup.value = robot.dynamic.upperJl.value taskJL.dt.value = robot.timeStep taskJL.selec.value = toFlags(range(6,22)+range(22,28)+range(29,35))
def setTaskLim(taskJL, robot): """ Sets the parameters for the 'joint-limits' """ robot.dynamic.upperJl.recompute(0) robot.dynamic.lowerJl.recompute(0) plug(robot.dynamic.position, taskJL.position) taskJL.controlGain.value = 10 taskJL.referenceInf.value = robot.dynamic.lowerJl.value taskJL.referenceSup.value = robot.dynamic.upperJl.value taskJL.dt.value = robot.timeStep taskJL.selec.value = toFlags(range(6, 22) + range(22, 28) + range(29, 35))
def gotoNd(task, position, selec, gain=None, resetJacobian=True): M = eye(4) if isinstance(position, matrix): position = vectorToTuple(position) if (len(position) == 3): M[0:3, 3] = position else: #print "Position 6D with rotation ... todo" # done? M = array(rpy2tr(*position[3:7])) M[0:3, 3] = position[0:3] if isinstance(selec, str): task.feature.selec.value = selec else: task.feature.selec.value = toFlags(selec) task.featureDes.position.value = matrixToTuple(M) if resetJacobian: task.task.resetJacobianDerivative() setGain(task.gain, gain)
def gotoNd(task,position,selec,gain=None,resetJacobian=True): M=eye(4) if isinstance(position,matrix): position = vectorToTuple(position) if( len(position)==3 ): M[0:3,3] = position else: #print "Position 6D with rotation ... todo" # done? M = array( rpy2tr(*position[3:7]) ) M[0:3,3] = position[0:3] if isinstance(selec,str): task.feature.selec.value = selec else: task.feature.selec.value = toFlags(selec) task.featureDes.position.value = matrixToTuple(M) if resetJacobian: task.task.resetJacobianDerivative() setGain(task.gain,gain)
def Pr2JointLimitsTask(robot, dt): robot.dynamic.upperJl.recompute(0) robot.dynamic.lowerJl.recompute(0) task = TaskJointLimits('taskJL') plug(robot.dynamic.position, task.position) task.controlGain.value = 10 task.referenceInf.value = robot.dynamic.lowerJl.value task.referenceSup.value = robot.dynamic.upperJl.value task.dt.value = dt task.selec.value = toFlags( range(18, 25) + range(26, 27) + range(28, 31) + range(32, 40) + range(41, 42) + range(43, 46) + range(47, 50)) return task
def safePosition(gain=0.01): diag = None safePos = numpy.zeros(robot.dimension) safePos[9] = 0.2 safePos[16] = 0.2 jointsPos = tuple(safePos) selec = toFlags(range(6,robot.dimension)) taskSafePosition = MetaTaskJointWeights('safePos',selec,robot,diag,gain,0) jointsVel = numpy.zeros(robot.dimension) taskSafePosition.task.setVelocity(tuple(jointsVel)) taskSafePosition.task.setPositionDes(tuple(jointsPos)) plug(robot.dynamic.position,taskSafePosition.task.position) return taskSafePosition
def safePosition(gain=0.01): diag = None safePos = numpy.zeros(robot.dimension) safePos[9] = 0.2 safePos[16] = 0.2 jointsPos = tuple(safePos) selec = toFlags(range(6, robot.dimension)) taskSafePosition = MetaTaskJointWeights("safePos", selec, robot, diag, gain, 0) jointsVel = numpy.zeros(robot.dimension) taskSafePosition.task.setVelocity(tuple(jointsVel)) taskSafePosition.task.setPositionDes(tuple(jointsPos)) plug(robot.dynamic.position, taskSafePosition.task.position) return taskSafePosition
def gotoq(self, gain=None, **kwargs): act = list() if MetaTaskDynPosture.nbDof == None: MetaTaskDynPosture.nbDof = len(self.feature.errorIN.value) qdes = zeros((MetaTaskDynPosture.nbDof, 1)) for n, v in kwargs.items(): r = self.postureRange[n] act += r if isinstance(v, matrix): qdes[r, 0] = vectorToTuple(v) if isinstance(v, ndarray): qdes[r, 0] = vectorToTuple(v) else: qdes[r, 0] = v self.ref = vectorToTuple(qdes) self.feature.selec.value = toFlags(act) setGain(self.gain, gain)
def gotoq(self,gain=None,**kwargs): act=list() if MetaTaskDynPosture.nbDof==None: MetaTaskDynPosture.nbDof = len(self.feature.errorIN.value) qdes = zeros((MetaTaskDynPosture.nbDof,1)) for n,v in kwargs.items(): r = self.postureRange[n] act += r if isinstance(v,matrix): qdes[r,0] = vectorToTuple(v) if isinstance(v,ndarray): qdes[r,0] = vectorToTuple(v) else: qdes[r,0] = v self.ref = vectorToTuple(qdes) self.feature.selec.value = toFlags(act) setGain(self.gain,gain)
def __init__(self, dyn, config=None, name="config"): self.dyn = dyn self.name = name self.config = config self.feature = FeatureGeneric('feature' + name) self.featureDes = FeatureGeneric('featureDes' + name) self.gain = GainAdaptive('gain' + name) plug(dyn.position, self.feature.errorIN) robotDim = dyn.getDimension() self.feature.jacobianIN.value = matrixToTuple(identity(robotDim)) self.feature.setReference(self.featureDes.name) if config is not None: self.feature.selec.value = toFlags(self.config)
def setTaskLim(taskJL,robot): """ Sets the parameters for the 'joint-limits' """ robot.dynamic.upperJl.recompute(0) robot.dynamic.lowerJl.recompute(0) plug(robot.dynamic.position,taskJL.position) taskJL.controlGain.value = 10 refInf = array(robot.dynamic.lowerJl.value) # Avoid knee singularity refInf[9] = 0.7 refInf[15] = 0.7 taskJL.referenceInf.value = vectorToTuple(refInf) taskJL.referenceSup.value = robot.dynamic.upperJl.value taskJL.dt.value = robot.timeStep taskJL.selec.value = toFlags(range(6,22)+range(22,28)+range(29,35))
def change6dPositionReference(task, feature, gain, position, selec=None, ingain=None, resetJacobian=True): M = generic6dReference(position) if selec != None: if isinstance(selec, str): feature.selec.value = selec else: feature.selec.value = toFlags(selec) feature.reference.value = matrixToTuple(M) if gain != None: setGain(gain, ingain) if 'resetJacobianDerivative' in task.__class__.__dict__.keys( ) and resetJacobian: task.resetJacobianDerivative()
def gotoNdComp(task,position,selec=None,gain=None,resetJacobian=True,comp=[[0,1,0],[0,0,1],[1,0,0]]): ''' gotoNdComp takes care of the different frame orientations used in jrl-dynamics. Be careful about the comp matrix, it is specific for reem and reemc (and could be different among different joint frames) ''' M = generic6dReference(position.copy()) R = M[0:3,0:3] R = R*comp M[0:3,0:3] = R if selec!=None: if isinstance(selec,str): task.feature.selec.value = selec else: task.feature.selec.value = toFlags(selec) task.featureDes.position.value = matrixToTuple(M) setGain(task.gain,gain) if 'resetJacobianDerivative' in task.task.__class__.__dict__.keys() and resetJacobian: task.task.resetJacobianDerivative()
def createJointLimitsTask(gain=1, dt=None): robot.dynamic.upperJl.recompute(0) robot.dynamic.lowerJl.recompute(0) taskJL = TaskJointLimits("jointLimits") plug(robot.dynamic.position, taskJL.position) """ The internal gain for the joint limits task is defined as 1/gain, i.e. is used to limit the maximum reachable velocity in a single time interval (dt). A high value can be used to limit oscillations around the goal point but it slows down the motion. """ taskJL.controlGain.value = gain taskJL.referenceInf.value = robot.dynamic.lowerJl.value taskJL.referenceSup.value = robot.dynamic.upperJl.value taskJL.selec.value = toFlags(range(6, robot.dimension)) if dt: taskJL.dt.value = dt else: plug(robot.device.dt, taskJL.dt) return taskJL
def gotoq(self,gain=None,qdes=None,**kwargs): act=list() if qdes!=None: if isinstance(qdes,tuple): qdes=matrix(qdes).T else: if MetaTaskPosture.nbDof==None: MetaTaskPosture.nbDof = len(self.feature.errorIN.value) qdes = zeros((MetaTaskPosture.nbDof,1)) for limbName,jointValues in kwargs.items(): limbRange = self.postureRange[limbName] act += limbRange if jointValues!=[]: if isinstance(jointValues,matrix): qdes[limbRange,0] = vectorToTuple(jointValues) else: qdes[limbRange,0] = jointValues self.ref = vectorToTuple(qdes) if(len(act)>0): self.feature.selec.value = toFlags(act) setGain(self.gain,gain)
def createJointLimitsTask(gain=1, dt=None): robot.dynamic.upperJl.recompute(0) robot.dynamic.lowerJl.recompute(0) taskJL = TaskJointLimits('jointLimits') plug(robot.dynamic.position, taskJL.position) """ The internal gain for the joint limits task is defined as 1/gain, i.e. is used to limit the maximum reachable velocity in a single time interval (dt). A high value can be used to limit oscillations around the goal point but it slows down the motion. """ taskJL.controlGain.value = gain taskJL.referenceInf.value = robot.dynamic.lowerJl.value taskJL.referenceSup.value = robot.dynamic.upperJl.value taskJL.selec.value = toFlags(range(6, robot.dimension)) if (dt): taskJL.dt.value = dt else: plug(robot.device.dt, taskJL.dt) return taskJL
def gotoq(self, gain=None, qdes=None, **kwargs): act = list() if qdes != None: if isinstance(qdes, tuple): qdes = matrix(qdes).T else: if MetaTaskPosture.nbDof == None: MetaTaskPosture.nbDof = len(self.feature.errorIN.value) qdes = zeros((MetaTaskPosture.nbDof, 1)) for limbName, jointValues in kwargs.items(): limbRange = self.postureRange[limbName] act += limbRange if jointValues != []: if isinstance(jointValues, matrix): qdes[limbRange, 0] = vectorToTuple(jointValues) else: qdes[limbRange, 0] = jointValues self.ref = vectorToTuple(qdes) if (len(act) > 0): self.feature.selec.value = toFlags(act) setGain(self.gain, gain)
taskFoV.task.referenceSup.value = (Xmax,Ymax) # Xmax, Ymax taskFoV.task.dt.value=dt taskFoV.task.controlGain.value=0.01 taskFoV.featureDes.xy.value = (0,0) # --- Task Joint Limits ----------------------------------------- robot.dynamic.upperJl.recompute(0) robot.dynamic.lowerJl.recompute(0) taskJL = TaskJointLimits('taskJL') plug(robot.dynamic.position,taskJL.position) taskJL.controlGain.value = 10 taskJL.referenceInf.value = robot.dynamic.lowerJl.value taskJL.referenceSup.value = robot.dynamic.upperJl.value taskJL.dt.value = dt taskJL.selec.value = toFlags(range(6,22)+range(22,28)+range(29,35)) # --- CONTACTS # define contactLF and contactRF for name,joint in [ ['LF','left-ankle'], ['RF','right-ankle' ] ]: contact = MetaTaskKine6d('contact'+name,robot.dynamic,name,joint) contact.feature.frame('desired') contact.gain.setConstant(10) locals()['contact'+name] = contact # --- TRACER ----------------------------------------------------------------- tr = Tracer('tr') tr.open('/tmp/','','.dat') tr.start() robot.device.after.addSignal('tr.triger')
#tr.add(dyn.name+'.ffposition','ff') tr.add(taskRF.featureDes.name+'.position','refr') tr.add(taskLF.featureDes.name+'.position','refl') tr.add('dyn.rf','r') tr.add('dyn.lf','l') tr.add('featureComDes.errorIN','comref') tr.add('dyn.com','com') tr.add(taskWaist.gain.name+'.gain','gainwaist') history = History(dyn,1) # --- RUN ----------------------------------------------------------------- featurePosture.selec.value = toFlags( range(6,36) ) sot.clear() for task in [taskWaist, taskRF, taskLF]: task.feature.position.recompute(0) task.feature.keep() task.feature.selec.value = '111111' sot.push(task.task.name) taskWaist.ref = matrixToTuple(eye(4)) taskWaist.feature.selec.value = '111011' taskWaist.gain.setByPoint(18,0.1,0.005,0.8) attime(200 ,(lambda : sot.rm(taskWaist.task.name),'Rm waist') ,(lambda : pg.initState(),'Init PG')
def gotoq(qref, selec=None): featurePostureDes.errorIN.value = vectorToTuple(qref.transpose()) if selec != None: featurePosture.selec.value = toFlags(selec)
[go, stop, next, n] = loopShortcuts(runner) # Tasks taskzero = MetaTaskKine6d('rh', robot.dynamic, 'rh', 'right-wrist') taskzero.feature.frame('desired') robot.dynamic.upperJl.recompute(0) robot.dynamic.lowerJl.recompute(0) taskJL = TaskJointLimits('taskJL') plug(robot.dynamic.position, taskJL.position) taskJL.controlGain.value = 10 taskJL.referenceInf.value = robot.dynamic.lowerJl.value taskJL.referenceSup.value = robot.dynamic.upperJl.value taskJL.dt.value = dt taskJL.selec.value = toFlags( range(18, 25) + range(26, 27) + range(28, 31) + range(32, 40) + range(41, 42) + range(43, 46) + range(47, 50)) taskContact = MetaTaskKine6d('contact', robot.dynamic, 'contact', 'left-ankle') taskContact.feature.frame('desired') taskContact.feature.selec.value = '011100' taskContact.gain.setConstant(10) taskFixed = MetaTaskKine6d('contactFixed', robot.dynamic, 'contact', 'left-ankle') taskFixed.feature.frame('desired') taskFixed.gain.setConstant(10) # Problem from dynamic_graph.sot.core.meta_tasks_kine import gotoNd targetRH = (0.65, -0.2, 0.9)
def gotoq( qref,selec=None ): featurePostureDes.errorIN.value = vectorToTuple( qref.transpose() ) if selec!=None: featurePosture.selec.value = toFlags( selec )
#tr.add(dyn.name+'.ffposition','ff') tr.add(taskRF.featureDes.name + '.position', 'refr') tr.add(taskLF.featureDes.name + '.position', 'refl') tr.add('dyn.rf', 'r') tr.add('dyn.lf', 'l') tr.add('featureComDes.errorIN', 'comref') tr.add('dyn.com', 'com') tr.add(taskWaist.gain.name + '.gain', 'gainwaist') history = History(dyn, 1) # --- RUN ----------------------------------------------------------------- featurePosture.selec.value = toFlags(range(6, 36)) sot.clear() for task in [taskWaist, taskRF, taskLF]: task.feature.position.recompute(0) task.feature.keep() task.feature.selec.value = '111111' sot.push(task.task.name) taskWaist.ref = matrixToTuple(eye(4)) taskWaist.feature.selec.value = '111011' taskWaist.gain.setByPoint(18, 0.1, 0.005, 0.8) attime(200, (lambda: sot.rm(taskWaist.task.name), 'Rm waist'), (lambda: pg.initState(), 'Init PG'), (lambda: pg.parseCmd(":stepseq " + seqpart), 'Push step list'),
featurePostureDes.errorIN.value = dyn.position.value taskPosture = Task('taskPosture') taskPosture.add('featurePosture') gPosture = GainAdaptive('gPosture') plug(taskPosture.error,gPosture.error) plug(gPosture.gain,taskPosture.controlGain) gPosture.set(1,1,1) postureSelec = range(0,3) # right leg postureSelec += range(6,9) # left leg postureSelec += range(12,16) # chest+head #postureSelec += range(16,19) # right arm #postureSelec += range(23,26) # left arm featurePosture.selec.value = toFlags(postureSelec) # --- TASK JL ------------------------------------------------------ taskJL = TaskJointLimits('taskJL') plug(dyn.position,taskJL.position) plug(dyn.lowerJl,taskJL.referenceInf) plug(dyn.upperJl,taskJL.referenceSup) taskJL.dt.value = dt taskJL.selec.value = toFlags(range(6,robotDim)) # --- SOT KINE OpSpaceH ------------------------------------------- # SOT controller. sot = SolverKine('sot') sot.setSize(robotDim) sot.damping.value = 1e-6
runner=inc() [go,stop,next,n]=loopShortcuts(runner) # Tasks taskzero = MetaTaskKine6d('rh',robot.dynamic,'rh','right-wrist') taskzero.feature.frame('desired') robot.dynamic.upperJl.recompute(0) robot.dynamic.lowerJl.recompute(0) taskJL = TaskJointLimits('taskJL') plug(robot.dynamic.position,taskJL.position) taskJL.controlGain.value = 10 taskJL.referenceInf.value = robot.dynamic.lowerJl.value taskJL.referenceSup.value = robot.dynamic.upperJl.value taskJL.dt.value = dt taskJL.selec.value = toFlags(range(18,25)+range(26,27)+range(28,31)+range(32,40)+range(41,42)+range(43,46)+range(47,50)) taskContact = MetaTaskKine6d('contact',robot.dynamic,'contact','left-ankle') taskContact.feature.frame('desired') taskContact.feature.selec.value = '011100' taskContact.gain.setConstant(10) taskFixed = MetaTaskKine6d('contactFixed',robot.dynamic,'contact','left-ankle') taskFixed.feature.frame('desired') taskFixed.gain.setConstant(10) # Problem from dynamic_graph.sot.core.meta_tasks_kine import gotoNd targetRH = (0.65,-0.2,0.9) targetLH = (0.1, 0.0,1.2) #targetLH = targetRH
taskFoV.task.referenceInf.value = (-Xmax, -Ymax) # Xmin, Ymin taskFoV.task.referenceSup.value = (Xmax, Ymax) # Xmax, Ymax taskFoV.task.dt.value = dt taskFoV.task.controlGain.value = 0.01 taskFoV.featureDes.xy.value = (0, 0) # --- Task Joint Limits ----------------------------------------- dyn.upperJl.recompute(0) dyn.lowerJl.recompute(0) taskJL = TaskJointLimits('taskJL') plug(dyn.position, taskJL.position) taskJL.controlGain.value = 10 taskJL.referenceInf.value = dyn.lowerJl.value taskJL.referenceSup.value = dyn.upperJl.value taskJL.dt.value = dt taskJL.selec.value = toFlags(range(6, 22) + range(22, 28) + range(29, 35)) # --- CONTACTS # define contactLF and contactRF for name, joint in [['LF', 'left-ankle'], ['RF', 'right-ankle']]: contact = MetaTaskKine6d('contact' + name, dyn, name, joint) contact.feature.frame('desired') contact.gain.setConstant(10) locals()['contact' + name] = contact # --- TRACER ----------------------------------------------------------------- from dynamic_graph.tracer import * tr = Tracer('tr') tr.open('/tmp/', '', '.dat') tr.start() robot.after.addSignal('tr.triger')