Python core.Task類代碼示例

def setupJointLimitTask():
    featureJl = FeatureJointLimits('featureJl')
    featureJl.actuate()
    dyn = robot.dynamic
    plug(dyn.signal('position'), featureJl.signal('joint'))
    plug(dyn.signal('upperJl'), featureJl.signal('upperJl'))
    plug(dyn.signal('lowerJl'), featureJl.signal('lowerJl'))
    taskJl = Task('taskJl')
    taskJl.add('featureJl')
    taskJl.signal('controlGain').value = .1

class MotionJoint(Motion):
    yaml_tag = u'joint'

    gain = None
    name = None
    reference = None

    # FIXME: not generic enough and joints are missing.
    nameToId = {
        'left-claw': 35,
        'right-claw': 28
        }

    def __init__(self, motion, yamlData, defaultDirectories):
        checkDict('interval', yamlData)

        Motion.__init__(self, motion, yamlData)

        self.gain = yamlData.get('gain', 1.)
        self.name = yamlData['name']
        self.reference = yamlData['reference']

        self.task = Task('joint'+str(id(yamlData)))
        self.feature = FeaturePosture('jointFeaturePosture'+str(id(yamlData)))
        plug(motion.robot.device.state, self.feature.state)

        jointId = self.nameToId[self.name]

        posture = np.array(motion.robot.halfSitting)
        posture[jointId] = self.reference

        self.feature.setPosture(tuple(posture.tolist()))

        for i in xrange(len(posture) - 6):
            if i + 6 == jointId:
                self.feature.selectDof(i + 6, True)
            else:
                self.feature.selectDof(i + 6, False)

        self.task.add(self.feature.name)
        self.task.controlGain.value = self.gain

        # Push the task into supervisor.
        motion.supervisor.addTask(self.task.name,
                                  self.interval[0], self.interval[1],
                                  self.priority,
                                  (jointId,))

    def __str__(self):
        return "joint motion ({0})".format(self.name)

    def setupTrace(self, trace):
        pass

def createPostureTask (robot, taskName, ingain = 1.):
    robot.dynamic.position.recompute(0)
    feature    = FeatureGeneric('feature'+taskName)
    featureDes = FeatureGeneric('featureDes'+taskName)
    featureDes.errorIN.value = robot.halfSitting
    plug(robot.dynamic.position,feature.errorIN)
    feature.setReference(featureDes.name)
    robotDim = len(robot.dynamic.position.value)
    feature.jacobianIN.value = matrixToTuple( identity(robotDim) )
    task = Task (taskName)    
    task.add (feature.name)
    gain = GainAdaptive('gain'+taskName)    
    plug(gain.gain,task.controlGain)
    plug(task.error,gain.error)
    gain.setConstant(ingain)
    return (feature, featureDes, task, gain)

    def __init__(self, motion, yamlData, defaultDirectories):
        checkDict('interval', yamlData)

        Motion.__init__(self, motion, yamlData)

        self.gain = yamlData.get('gain', 1.)
        self.name = yamlData['name']
        self.reference = yamlData['reference']

        self.task = Task('joint'+str(id(yamlData)))
        self.feature = FeaturePosture('jointFeaturePosture'+str(id(yamlData)))
        plug(motion.robot.device.state, self.feature.state)

        jointId = self.nameToId[self.name]

        posture = np.array(motion.robot.halfSitting)
        posture[jointId] = self.reference

        self.feature.setPosture(tuple(posture.tolist()))

        for i in xrange(len(posture) - 6):
            if i + 6 == jointId:
                self.feature.selectDof(i + 6, True)
            else:
                self.feature.selectDof(i + 6, False)

        self.task.add(self.feature.name)
        self.task.controlGain.value = self.gain

        # Push the task into supervisor.
        motion.supervisor.addTask(self.task.name,
                                  self.interval[0], self.interval[1],
                                  self.priority,
                                  (jointId,))

def setupPostureTask():
    global postureTask, postureError, postureFeature
    initialGain = 0.1

    postureTask = Task(robot.name + '_posture')
    postureFeature = FeatureGeneric(robot.name + '_postureFeature')
    postureError = PostureError('PostureError')

    posture = list(robot.halfSitting)
    posture[6 + 17] -= 1
    posture[6 + 24] += 1
    #    postureError.setPosture(tuple(posture))
    plug(robot.device.state, postureError.state)
    plug(postureError.error, postureFeature.errorIN)
    postureFeature.jacobianIN.value = computeJacobian()
    postureTask.add(postureFeature.name)
    postureTask.controlGain.value = initialGain

def createOperationalPointFeatureAndTask(robot, operationalPointName, featureName, taskName, ingain=.2):
    operationalPointMapped = operationalPointName
    jacobianName = 'J{0}'.format(operationalPointMapped)
    robot.dynamic.signal(operationalPointMapped).recompute(0)
    robot.dynamic.signal(jacobianName).recompute(0)
    feature = \
        FeaturePosition(featureName,
                        robot.dynamic.signal(operationalPointMapped),
                        robot.dynamic.signal(jacobianName),
                        robot.dynamic.signal(operationalPointMapped).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 createOperationalPointFeatureAndTask(self,
                                          operationalPointName,
                                          featureName,
                                          taskName,
                                          gain = .2):
     jacobianName = 'J{0}'.format(operationalPointName)
     self.dynamic.signal(operationalPointName).recompute(0)
     self.dynamic.signal(jacobianName).recompute(0)
     feature = \
         FeaturePosition(featureName,
                         self.dynamic.signal(operationalPointName),
                         self.dynamic.signal(jacobianName),
                         self.dynamic.signal(operationalPointName).value)
     task = Task(taskName)
     task.add(featureName)
     task.controlGain.value = gain
     return (feature, task)

 def createBalanceTask (self, taskName, gain = 1.):
     task = Task (taskName)
     task.add (self.featureCom.name)
     task.add (self.leftAnkle.name)
     task.add (self.rightAnkle.name)
     task.controlGain.value = gain
     return task

def createCenterOfMassFeatureAndTask(robot, featureName, featureDesName, taskName, selec='111', ingain=1.):
    robot.dynamic.com.recompute(0)
    robot.dynamic.Jcom.recompute(0)

    featureCom = FeatureGeneric(featureName)
    plug(robot.dynamic.com, featureCom.errorIN)
    plug(robot.dynamic.Jcom, featureCom.jacobianIN)
    featureCom.selec.value = selec
    featureComDes = FeatureGeneric(featureDesName)
    featureComDes.errorIN.value = robot.dynamic.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 createCenterOfMassFeatureAndTask(self,
                                         featureName, featureDesName,
                                         taskName,
                                         selec = '011',
                                         gain = 1.):
        self.dynamic.com.recompute(0)
        self.dynamic.Jcom.recompute(0)

        featureCom = FeatureGeneric(featureName)
        plug(self.dynamic.com, featureCom.errorIN)
        plug(self.dynamic.Jcom, featureCom.jacobianIN)
        featureCom.selec.value = selec
        featureComDes = FeatureGeneric(featureDesName)
        featureComDes.errorIN.value = self.dynamic.com.value
        featureCom.setReference(featureComDes.name)
        comTask = Task(taskName)
        comTask.add(featureName)
        comTask.controlGain.value = gain
        return (featureCom, featureComDes, comTask)

def createBalanceTask (robot, application, taskName, ingain = 1.):
    task = Task (taskName)
    task.add (application.featureCom.name)
    task.add (application.leftAnkle.name)
    task.add (application.rightAnkle.name)
    gain = GainAdaptive('gain'+taskName)
    gain.setConstant(ingain)
    plug(gain.gain, task.controlGain)
    plug(task.error, gain.error)      
    return (task, gain)

    def __init__(self, motion, yamlData, defaultDirectories):
        checkDict('interval', yamlData)

        Motion.__init__(self, motion, yamlData)

        self.objectName = yamlData['object-name']
        self.frameName = yamlData['frame-name']

        self.gain = yamlData.get('gain', 1.)

        # Cannot change the stack dynamically for now.
        if self.interval[0] != 0 and self.interval[1] != motion.duration:
            raise NotImplementedError

        # Desired feature
        self.fvpDes = FeatureVisualPoint('fvpDes'+str(id(yamlData)))
        self.fvpDes.xy.value = (0., 0.)

        # Feature
        self.vispPointProjection = VispPointProjection('vpp'+str(id(yamlData)))
        self.vispPointProjection.cMo.value = (
            (1., 0., 0., 0.),
            (0., 1., 0., 0.),
            (0., 0., 1., 1.),
            (0., 0., 0., 1.),)
        self.vispPointProjection.cMoTimestamp.value = (0., 0.)

        self.fvp = FeatureVisualPoint('fvp'+str(id(yamlData)))
        plug(self.vispPointProjection.xy, self.fvp.xy)
        plug(self.vispPointProjection.Z, self.fvp.Z)

        self.fvp.Z.value = 1.
        self.fvp.sdes.value = self.fvpDes
        self.fvp.selec.value = '11'
        plug(motion.robot.frames[self.frameName].jacobian, self.fvp.Jq)

        self.fvp.error.recompute(self.fvp.error.time + 1)
        self.fvp.jacobian.recompute(self.fvp.jacobian.time + 1)

        # Task
        self.task = Task('task_fvp_'+str(id(yamlData)))
        self.task.add(self.fvp.name)
        self.task.controlGain.value = self.gain

        self.task.error.recompute(self.task.error.time + 1)
        self.task.jacobian.recompute(self.task.jacobian.time + 1)

        # Push the task into supervisor.
        motion.supervisor.addTask(self.task.name,
                                  self.interval[0], self.interval[1],
                                  self.priority,
                                  #FIXME: HRP-2 specific
                                  (6 + 14, 6 + 15))

class Nao(AbstractHumanoidRobot):
    """
    This class instanciates a Nao robot as a humanoid robot
    """

    halfSitting = tuple([0.,0.,.31,0.,0.,0.] + halfSitting)

    def __init__(self, name, filename):
        AbstractHumanoidRobot.__init__(self, name)

        self.OperationalPoints.append('waist')
        p = Parser(self.name + '_dynamic', filename)
        self.dynamic = p.parse()
        self.dimension = self.dynamic.getDimension()
        if self.dimension != len(self.halfSitting):
            raise RuntimeError("invalid half-sitting pose")
        self.initializeRobot()
        self.compoundDrive = CompoundDrive(self.name + '_compoundDrive',
                                           self.device)
        self.compoundDriveTask = Task(self.name + '_compoundDriveTask')
        self.compoundDriveTask.add(self.name + '_compoundDrive')
        self.compoundDriveTask.signal('controlGain').value = 1.

    def __init__(self, name, filename):
        AbstractHumanoidRobot.__init__(self, name)

        self.OperationalPoints.append('waist')
        p = Parser(self.name + '_dynamic', filename)
        self.dynamic = p.parse()
        self.dimension = self.dynamic.getDimension()
        if self.dimension != len(self.halfSitting):
            raise RuntimeError("invalid half-sitting pose")
        self.initializeRobot()
        self.compoundDrive = CompoundDrive(self.name + '_compoundDrive',
                                           self.device)
        self.compoundDriveTask = Task(self.name + '_compoundDriveTask')
        self.compoundDriveTask.add(self.name + '_compoundDrive')
        self.compoundDriveTask.signal('controlGain').value = 1.

solver.sot.push(robot.name + '_task_right-wrist')


# Feature visual point
fvp = FeatureVisualPoint('fvp')
fvp.xy.value = (P(S(0,0), Xw(0))[0], P(S(0,0), Xw(0))[1])
fvp.Z.value = 1. + np.inner(np.linalg.inv(S(0,0)), Xw(0))[2]
plug(w_M_c.jacobian, fvp.Jq)

fvp_sdes = FeatureVisualPoint('fvp_sdes')
fvp_sdes.xy.value = (0., 0.)

fvp.sdes.value = fvp_sdes


task = Task('fvp_task')
task.add('fvp')
task.controlGain.value = 10.
solver.sot.push('fvp_task')


t = 0
for i in xrange(0,2000):
    robot.device.increment(timeStep)

    w_M_c.position.recompute(t)
    w_M_c.jacobian.recompute(t)

    fvp.xy.value = (P(S(0,0), Xw(t))[0], P(S(0,0), Xw(t))[1])

    if clt: