def initPostureTask(robot): # --- TASK POSTURE -------------------------------------------------- # set a default position for the joints. robot.features['featurePosition'] = FeaturePosture('featurePosition') plug(robot.device.state, robot.features['featurePosition'].state) robot.features['featurePosition'].posture.value = robot.halfSitting # Remove the dofs of the feet. postureTaskDofs = [True] * (len(robot.dynamic.position.value) - 6) jla = robot.dynamic.signal('J' + robot.OperationalPointsMap['left-ankle']).value postureTaskDofs = removeDofUsed(jla, postureTaskDofs) jra = robot.dynamic.signal('J' + robot.OperationalPointsMap['right-ankle']).value postureTaskDofs = removeDofUsed(jra, postureTaskDofs) for dof, isEnabled in enumerate(postureTaskDofs): robot.features['featurePosition'].selectDof(dof + 6, isEnabled) robot.tasks['robot_task_position'] = Task('robot_task_position') robot.tasks['robot_task_position'].add('featurePosition') gainPosition = GainAdaptive('gainPosition') gainPosition.set(0.1, 0.1, 125e3) gainPosition.gain.value = 5 plug(robot.tasks['robot_task_position'].error, gainPosition.error) plug(gainPosition.gain, robot.tasks['robot_task_position'].controlGain)
class MetaTaskKine6d(MetaTask6d): 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()
class MetaTask6d(object): name = '' opPoint = '' dyn = 0 task = 0 feature = 0 featureDes = 0 def opPointExist(self, opPoint): sigsP = [x for x in self.dyn.signals() if x.getName().split(':')[-1] == opPoint] sigsJ = [x for x in self.dyn.signals() if x.getName().split(':')[-1] == 'J' + opPoint] 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 = Flags('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
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)