Python Task.add方法代码示例

# 需要导入模块: from dynamic_graph.sot.core import Task [as 别名]
# 或者: from dynamic_graph.sot.core.Task import add [as 别名]
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

# 需要导入模块: from dynamic_graph.sot.core import Task [as 别名]
# 或者: from dynamic_graph.sot.core.Task import add [as 别名]
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

# 需要导入模块: from dynamic_graph.sot.core import Task [as 别名]
# 或者: from dynamic_graph.sot.core.Task import add [as 别名]
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)

# 需要导入模块: from dynamic_graph.sot.core import Task [as 别名]
# 或者: from dynamic_graph.sot.core.Task import add [as 别名]
 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)

# 需要导入模块: from dynamic_graph.sot.core import Task [as 别名]
# 或者: from dynamic_graph.sot.core.Task import add [as 别名]
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

# 需要导入模块: from dynamic_graph.sot.core import Task [as 别名]
# 或者: from dynamic_graph.sot.core.Task import add [as 别名]
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)

# 需要导入模块: from dynamic_graph.sot.core import Task [as 别名]
# 或者: from dynamic_graph.sot.core.Task import add [as 别名]
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)

# 需要导入模块: from dynamic_graph.sot.core import Task [as 别名]
# 或者: from dynamic_graph.sot.core.Task import add [as 别名]
    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)

# 需要导入模块: from dynamic_graph.sot.core import Task [as 别名]
# 或者: from dynamic_graph.sot.core.Task import add [as 别名]
 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

# 需要导入模块: from dynamic_graph.sot.core import Task [as 别名]
# 或者: from dynamic_graph.sot.core.Task import add [as 别名]
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.

# 需要导入模块: from dynamic_graph.sot.core import Task [as 别名]
# 或者: from dynamic_graph.sot.core.Task import add [as 别名]
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)

# 需要导入模块: from dynamic_graph.sot.core import Task [as 别名]
# 或者: from dynamic_graph.sot.core.Task import add [as 别名]
class LegsFollowerGraph(object):

    legsFollower = None

    postureTask = None
    postureFeature = None
    postureFeatureDes = None
    postureError = None

    legsTask = None
    legsFeature = None
    legsFeatureDes = None
    legsError = None

    waistTask = None
    waistFeature = None
    waistFeatureDes = None
    waistError = None

    trace = None

    def __init__(self, robot, solver, trace = None, postureTaskDofs = None):
        print("Constructor of LegsFollower Graph")
        self.robot = robot
        self.solver = solver
	self.legsFollower = LegsFollower('legs-follower')
        self.statelength = len(robot.device.state.value)

 	# Initialize the posture task.
	print("Posture Task")
        self.postureTaskDofs = postureTaskDofs
        if not self.postureTaskDofs:
            self.postureTaskDofs = []
            for i in xrange(len(robot.halfSitting) - 6):
                # Disable legs dofs.
                if i < 12: #FIXME: not generic enough
                    self.postureTaskDofs.append((i + 6, False))
                else:
                    self.postureTaskDofs.append((i + 6, True))
        
        # This part is taken from feet_follower_graph
        self.postureTask = Task(self.robot.name + '_posture')
        
        self.postureFeature = FeaturePosture(self.robot.name + '_postureFeature')
        plug(self.robot.device.state, self.postureFeature.state)

        posture = list(self.robot.halfSitting)
        self.postureFeature.setPosture(tuple(posture))
        for (dof, isEnabled) in self.postureTaskDofs:
            self.postureFeature.selectDof(dof, isEnabled)
        self.postureTask.add(self.postureFeature.name)
        self.postureTask.controlGain.value = 2.

	# Initialize the waist follower task.
	print("Waist Task")
        self.robot.features['waist'].selec.value = '111111'
        plug(self.legsFollower.waist, self.robot.features['waist'].reference)
        self.robot.tasks['waist'].controlGain.value = 1.

	# Initialize the legs follower task.
	print("Legs Task")
        self.legsTask = Task(self.robot.name + '_legs')
        self.legsFeature = FeatureGeneric(self.robot.name + '_legsFeature')
        legsFeatureDesName = self.robot.name + '_legsFeatureDes'
        self.legsFeatureDes = FeatureGeneric(legsFeatureDesName)
        self.legsError = LegsError('LegsError')
        plug(self.robot.device.state, self.legsError.state)

        # self.legsFeatureDes.errorIN.value = self.legsFollower.ldof.value        
        plug(self.legsFollower.ldof,self.legsFeatureDes.errorIN)
        self.legsFeature.jacobianIN.value = self.legsJacobian()

        self.legsFeature.setReference(legsFeatureDesName)
        plug(self.legsError.error, self.legsFeature.errorIN)            

        self.legsTask.add(self.legsFeature.name)
        self.legsTask.controlGain.value = 5.

	#CoM task
        print("Com Task")
        print (0., 0., self.robot.dynamic.com.value[2])
	self.robot.comTask.controlGain.value = 50.
        self.robot.featureComDes.errorIN.value =  (0., 0., self.robot.dynamic.com.value[2])
        self.robot.featureCom.selec.value = '111'
	plug(self.legsFollower.com, self.robot.featureComDes.errorIN)

        # Plug the legs follower zmp output signals.
        plug(self.legsFollower.zmp, self.robot.device.zmp)


	solver.sot.remove(self.robot.comTask.name)

	print("Push in solver.")
        solver.sot.push(self.legsTask.name)
        solver.sot.push(self.postureTask.name)
	solver.sot.push(self.robot.tasks['waist'].name)
        solver.sot.push(self.robot.comTask.name)
        
        solver.sot.remove(self.robot.tasks['left-ankle'].name)
	solver.sot.remove(self.robot.tasks['right-ankle'].name)
#.........这里部分代码省略.........

# 需要导入模块: from dynamic_graph.sot.core import Task [as 别名]
# 或者: from dynamic_graph.sot.core.Task import add [as 别名]
class MotionVisualPoint(Motion):
    yaml_tag = u'visual-point'

    type = None
    gain = None
    objectName = None

    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))

    def __str__(self):
        msg = "visual point motion (frame: {0}, object: {1})"
        return msg.format(self.frameName, self.objectName)

    def setupTrace(self, trace):
        for s in ['xy', 'Z']:
            addTrace(self.robot, trace, self.vispPointProjection.name, s)

        for s in ['xy']:
            addTrace(self.robot, trace, self.fvpDes.name, s)

        for s in ['xy', 'Z', 'error']:
            addTrace(self.robot, trace, self.fvp.name, s)

        for s in ['error']:
            addTrace(self.robot, trace, self.task.name, s)

# 需要导入模块: from dynamic_graph.sot.core import Task [as 别名]
# 或者: from dynamic_graph.sot.core.Task import add [as 别名]
robot = youbot('youbot', device=RobotSimu('youbot'))
#robot = Pr2('youbot', device=RobotSimu('youbot'))
dimension = robot.dynamic.getDimension()
robot.device.resize (dimension)
from dynamic_graph.ros import Ros
ros = Ros(robot)
#waisttask-1

robot_pose = ((1.,0,0,0.4),
    (0,1.,0,0),
    (0,0,1.,0.0),
    (0,0,0,1.),)	
feature_waist = FeaturePosition ('position_waist', robot.dynamic.base_joint,robot.dynamic.Jbase_joint, robot_pose)
task_waist = Task ('waist_task')
task_waist.controlGain.value = 0.5
task_waist.add (feature_waist.name)

#waisttask
task_waist_metakine=MetaTaskKine6d('task_waist_metakine',robot.dynamic,'base_joint','base_joint')
goal_waist = ((1.,0,0,0.0),(0,1.,0,-0.0),(0,0,1.,0.0),(0,0,0,1.),)
task_waist_metakine.feature.frame('desired')
#task_waist_metakine.feature.selec.value = '011101'#RzRyRxTzTyTx
task_waist_metakine.gain.setConstant(0.5)
task_waist_metakine.featureDes.position.value = goal_waist
solver = SolverKine('sot_solver')
solver.setSize (robot.dynamic.getDimension())
robot.device.resize (robot.dynamic.getDimension())
'''
#taskinequality
task_waist=TaskInequality('taskcollision')
collision_feature = FeatureGeneric('collisionfeature')

# 需要导入模块: from dynamic_graph.sot.core import Task [as 别名]
# 或者: from dynamic_graph.sot.core.Task import add [as 别名]
task_waist_metakine.gain.setConstant(1)
task_waist_metakine.featureDes.position.value = goal_waist

##POSTURE TASK###
posture_feature = FeaturePosture('featurePosition')
plug(robot.device.state,posture_feature.state)
robotDim = len(robot.dynamic.velocity.value)
posture_feature.posture.value = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
postureTaskDofs = [True]*(36)
for dof,isEnabled in enumerate(postureTaskDofs):
    if dof > 6:
      posture_feature.selectDof(dof,isEnabled)

#robot.features['featurePosition'].selectDof(dof,isEnabled)
task_posture=Task('robot_task_position')
task_posture.add(posture_feature.name)
# featurePosition.selec.value = toFlags((6,24))
gainPosition = GainAdaptive('gainPosition')
gainPosition.set(0.1,0.1,125e3)
gainPosition.gain.value = 5
plug(task_posture.error,gainPosition.error)
plug(gainPosition.gain,task_posture.controlGain)
solver.damping.value =3e-2
solver.push (joint_limit_task.name)
time.sleep(1)
solver.push (task_waist_metakine.task.name)
time.sleep(1)
solver.push (task_posture.name)
time.sleep(1)
plug (solver.control,robot.device.control)