Exemplo n.º 1
0
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
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)


	print solver.sot.display()

        print("Tasks added in solver.\n")
	print("Command are : \n - f.plug()\n - f.plugViewer()\n - f.plugPlanner()\n"
              " - f.plugPlannerWithoutMocap()\n - f.start()\n - f.stop()\n - f.readMocap()\n")


    def legsJacobian(self):
        j = []
        for i in xrange(12):
            j.append(oneVector(6+i,self.statelength)) 
        return tuple(j)

    def waistJacobian(self):
        j = []
        for i in xrange(6):
            j.append(oneVector(i,self.statelength)) 
        return tuple(j)

    def postureJacobian(self):
        j = []
        for i in xrange(self.statelength):
            if i >= 6 + 2 * 6:
                j.append(oneVector(i))
            if i == 3 or i == 4:
                j.append(zeroVector())
        return tuple(j)

    def computeDesiredValue(self):
        e = self.robot.halfSitting
        #e = halfSitting
        e_ = [e[3], e[4]]
        offset = 6 + 2 * 6
        for i in xrange(len(e) - offset):
            e_.append(e[offset + i])
        return tuple(e_)
    
    def canStart(self):
        securityThreshold = 1e-3
        return (self.postureTask.error.value <=
                (securityThreshold,) * len(self.postureTask.error.value))

    def setupTrace(self):
	self.trace = TracerRealTime('trace')
	self.trace.setBufferSize(2**20)
	self.trace.open('/tmp/','legs_follower_','.dat')
	
	self.trace.add('legs-follower.com', 'com')
	self.trace.add('legs-follower.zmp', 'zmp')
	self.trace.add('legs-follower.ldof', 'ldof')
	self.trace.add('legs-follower.waist', 'waist')
	self.trace.add(self.robot.device.name + '.state', 'state')
	self.trace.add(self.legsTask.name + '.error', 'errorLegs')
        self.trace.add(self.robot.comTask.name + '.error', 'errorCom')

        #self.trace.add('legs-follower.outputStart','start')
        #self.trace.add('legs-follower.outputYaw','yaw')
        self.trace.add('corba.planner_steps','steps')
        self.trace.add('corba.planner_outputGoal','goal')
        self.trace.add('corba.waist','waistMocap')
	self.trace.add('corba.left-foot','footMocap')
        self.trace.add('corba.table','tableMocap')
        self.trace.add('corba.bar','barMocap')
        self.trace.add('corba.chair','chairMocap')
	self.trace.add('corba.helmet','helmetMocap')
	self.trace.add('corba.planner_outputObs','obstacles')

        self.trace.add(self.robot.dynamic.name + '.left-ankle',
                       self.robot.dynamic.name + '-left-ankle')
        self.trace.add(self.robot.dynamic.name + '.right-ankle',
                       self.robot.dynamic.name + '-right-ankle')


	# Recompute trace.triger at each iteration to enable tracing.
	self.robot.device.after.addSignal('legs-follower.zmp')
	self.robot.device.after.addSignal('legs-follower.outputStart')
	self.robot.device.after.addSignal('legs-follower.outputYaw')
	self.robot.device.after.addSignal('corba.planner_steps')
	self.robot.device.after.addSignal('corba.planner_outputGoal')
	self.robot.device.after.addSignal('corba.waist')
	self.robot.device.after.addSignal('corba.left-foot')
	self.robot.device.after.addSignal('corba.table')
	self.robot.device.after.addSignal('corba.bar')
	self.robot.device.after.addSignal('corba.chair')
	self.robot.device.after.addSignal('corba.helmet')
	self.robot.device.after.addSignal('corba.planner_outputObs')
        self.robot.device.after.addSignal(self.robot.dynamic.name + '.left-ankle')
	self.robot.device.after.addSignal(self.robot.dynamic.name + '.right-ankle')
	self.robot.device.after.addSignal('trace.triger')
	return

    def plugPlanner(self):
        print("Plug planner.")
	plug(corba.planner_radQ, self.legsFollower.inputRef)
	plug(self.legsFollower.outputStart, corba.planner_inputStart)
	plug(corba.waistTimestamp, corba.planner_timestamp)
	plug(corba.table, corba.planner_table)
	plug(corba.chair, corba.planner_chair)
	plug(corba.bar, corba.planner_bar)
	plug(corba.waist, corba.planner_waist)
	plug(corba.helmet, corba.planner_inputGoal)
        plug(corba.torus1, corba.planner_torus1)
	plug(corba.torus2, corba.planner_torus2)
        plug(corba.signal('left-foot'), corba.planner_foot)
        plug(corba.signal('left-footTimestamp'), corba.planner_footTimestamp)
	return

    def plugPlannerWithoutMocap(self):
        print("Plug planner without mocap.")
	plug(corba.planner_radQ, self.legsFollower.inputRef)
	plug(self.legsFollower.outputStart, corba.planner_inputStart)
	return

    def plugViewer(self):
        print("Plug viewer.")
	plug(self.legsFollower.ldof, corba.viewer_Ldof)
	plug(self.legsFollower.outputStart, corba.viewer_Start)
	plug(self.legsFollower.com, corba.viewer_Com)
	plug(self.legsFollower.outputYaw, corba.viewer_Yaw)
	plug(corba.planner_steps, corba.viewer_Steps)
	plug(corba.planner_outputGoal, corba.viewer_Goal)
	plug(corba.table, corba.viewer_Table)
	plug(corba.chair, corba.viewer_Chair)
	plug(corba.bar, corba.viewer_Bar)
        plug(corba.torus1, corba.viewer_Torus1)
	plug(corba.torus2, corba.viewer_Torus2)
	plug(corba.waist, corba.viewer_Waist)
	plug(corba.planner_outputLabyrinth, corba.viewer_Labyrinth)
	plug(corba.planner_outputObs, corba.viewer_Obs)
	return

    def plug(self):
	self.plugPlanner()
	self.plugViewer()
	return

    def readMocap(self):
	print "Table : " 
	print corba.table.value
	print "Waist : " 
	if len(corba.waist.value)<3:
	    corba.waist.value = (0,0,0)
	print corba.waist.value
	print "Helmet : " 
	print corba.helmet.value
	return;

    def start(self):
        if not self.canStart():
            print("Robot has not yet converged to the initial position,"
                  " please wait and try again.")
            return

        print("Start.")
	self.postureTask.controlGain.value = 180.
        #self.waistTask.controlGain.value = 90.
	self.legsTask.controlGain.value = 180.
	self.robot.comTask.controlGain.value = 180.
	self.robot.tasks['waist'].controlGain.value = 45.

	self.setupTrace()
	self.trace.start()
        self.legsFollower.start()
	return

    def stop(self):
	self.legsFollower.stop()
	self.trace.dump()
	return
class Motion (object):
    reachTime = 3.
    stillTime = .5
    def __init__ (self, robot, solver):
        self.samplingPeriod = .005
        self.robot = robot
        self.solver = solver
        self.ros = Ros (robot)
        self.ros.rosExport.add ('matrixHomoStamped', 'ballInCamera',
                                '/wide/blobs/rose/transform')
        self.ros.rosExport.add ('matrixHomoStamped', 'ballInWaist',
                                '/wide/blobs/rose/transform_base_link')
        self.rightGripperId = 28
        # Right hand task
        self.featureRightHand = \
            FeaturePosition ('featureRightHand',
                             robot.frames ['rightHand'].position,
                             robot.frames ['rightHand'].jacobian)
        self.featureRightHand.selec.value = '000111'

        # interpolation
        self.prodSE3 = Multiply_of_matrixHomo ("prod SE3")
        self.interpolation = CylindricalCubicInterpolationSE3 ('interpolation')
        self.interpolation.setSamplingPeriod (self.samplingPeriod)
        plug (self.robot.waist.position, self.prodSE3.sin1)
        plug (self.interpolation.reference,
              self.featureRightHand.signal('reference'))
        plug (self.ros.rosExport.ballInWaist, self.prodSE3.sin2)
        plug (self.prodSE3.sout, self.interpolation.goal)
        plug (self.featureRightHand.position, self.interpolation.init)
        plug (self.robot.waist.position, self.interpolation.localFrame)

        self.taskRightHand = TaskPD ('taskRightHand')
        self.taskRightHand.add (self.featureRightHand.name)
        #plug (interpolation.errorDot, taskRightHand.errorDot)
        self.taskRightHand.controlGain.value = 180.0
        self.solver.push (self.taskRightHand)

        # Waist task
        self.robot.waist.selec.value = '011000'
        self.robot.waist.value = I4
        self.solver.push (self.robot.tasks ['waist'])

        # Ball tracking
        refBallInCamera = FeatureVisualPoint ('featureBallInCameraRef')
        refBallInCamera.xy.value = (0., 0.)
        self.pinholeProjection = VispPointProjection('pinholeProjection')
        plug (self.ros.rosExport.ballInCamera, self.pinholeProjection.cMo)
        plug (self.ros.rosTime.time, self.pinholeProjection.time)
        plug (self.ros.rosExport.ballInCameraTimestamp,
              self.pinholeProjection.cMoTimestamp)
        self.ballInCamera = FeatureVisualPoint ('featureBallInCamera')
        plug (self.pinholeProjection.xy, self.ballInCamera.xy)
        self.ballInCamera.Z.value = 1.
        self.ballInCamera.setReference (refBallInCamera.name)
        self.ballInCamera.selec.value = '11'
        plug (self.robot.frames ['cameraTopRight'].jacobian,
              self.ballInCamera.Jq)

        self.taskBallTracking = TaskPD ('taskBallTracking')
        self.taskBallTracking.add (self.ballInCamera.name)
        self.taskBallTracking.controlGain.value = 1.0

        # Posture task
        self.posture = list (self.robot.halfSitting [::])
        self.featurePosture = FeaturePosture ('featurePosture')
        plug (self.robot.device.state, self.featurePosture.state)
        self.featurePosture.setPosture (tuple (self.posture))
        for dof in range (6, self.robot.dimension):
            self.featurePosture.selectDof (dof, False)
        self.postureTask = TaskPD ('postureTask')
        self.postureTask.add (self.featurePosture.name)
        self.postureTask.controlGain.value = 1.
        self.solver.push (self.postureTask)

    def trackBall (self):
        self.solver.push (self.taskBallTracking)

    def openHand (self):
       # open hand
        self.featurePosture.selectDof (self.rightGripperId, True)
        self.posture [self.rightGripperId] = 0.7
        self.featurePosture.setPosture (tuple (self.posture))

    def reach (self):
        # move hand
        self.handInitPos = self.robot.frames ['rightHand'].position.value
        self.interpolation.start (self.reachTime)

    def stopTracking (self):
        self.solver.remove (self.taskBallTracking)

    def closeHand (self):
        self.posture [self.rightGripperId] = 0.3
        self.featurePosture.setPosture (tuple (self.posture))
        self.postureTask.controlGain.value = 4.

    def moveBack (self):
        # move hand back
        self.postureTask.controlGain.value = 0.
        for dof in range (6, len (self.posture)):
            self.featurePosture.selectDof (dof, True)
        self.interpolation.goal.value = self.handInitPos
        self.postureTask.controlGain.value = 3.
        self.interpolation.start (2.)

    def stopMotion (self):
        self.solver.remove (self.taskBallTracking)
        plug (self.ros.rosExport.ballInWaist, self.interpolation.goal)

    def catchBall (self):
        #self.trackBall ()
        self.openHand ()
        self.reach ()
        time.sleep (self.reachTime)
        self.closeHand ()
        time.sleep (1.)
        self.moveBack ()
        self.stopMotion ()

    def play(self, totalTime):
        nbSteps = int(totalTime/timeStep) + 1
        path = []

        for i in xrange(nbSteps):
            print (i)
            if i == 100:
                self.start ()
            t = timeStep*i
            self.robot.device.increment(timeStep)
            config = self.robot.device.state.value
            path.append(config)
            if hasRobotViewer:
                clt.updateElementConfig(rvName, toViewerConfig(config))
                ballPos = []
                for i in range (3):
                    ballPos.append(self.ros.rosExport.ballInWaist.value[i][3])
                ballPos.extend (3*[0])
                clt.updateElementConfig("rose_ball", ballPos)