NAO实验6

基础实验 手臂与头部

整体流程图:

其中,Go to position StandInit的Block由右侧的姿势库拖拽出,而Robot dance的Block类型为时间轴

Dance Block 具体行为层:

中级实验 完成机器人舞

在基础实验的基础上完成.

这两个实验最重要的是知道行为层可以将各个行为分开,并且在不影响其他行为下,改变某一个行为.

高级任务 绕圈走

其中,Walk Python的代码为:

import math
import time

class MyClass(GeneratedClass):
    def __init__(self):
        GeneratedClass.__init__(self)
        self.motionProxy = ALProxy("ALMotion")
        
    def simplifyAngle(self, theta):
        while(theta > math.pi):
            theta -= 2*math.pi
        while(theta < -math.pi):
            theta += 2*math.pi
        return theta
        
    def updateTheta(self, previousTheta, theta):
        nextTheta = self.motionProxy.getRobotPosition(False)[2]
        theta += self.simplifyAngle(nextTheta - previousTheta)
        return (theta, nextTheta)

    def onLoad(self):
        #put initialization code here
        pass

    def onUnload(self):
        #put clean-up code here
        pass

    def onInput_onStart(self):
        #self.onStopped() #activate the output of the box
        self.motionProxy.setWalkArmsEnabled(True, True)
        globalTheta = self.motionProxy.getRobotPosition(False)[2]
        theta = 0
        rotationSpeed = 0.25   #range from -1 to 1
        
        #Walk in a circle
        while(abs(theta) < 2*math.pi):
            #Update the rotation odometry
            (theta, globalTheta) = self.updateTheta(globalTheta, theta)
            #Walk in an arc
            self.motionProxy.setWalkTargetVelocity(0.5, 0.0, rotationSpeed, 1.0)
            #wait for some time
            time.sleep(0.1)
        
        self.motionProxy.setWalkTargetVelocity(0.0, 0.0, 0.0, 1.0)
        
        self.onStopped() #~ activate output of the box
        

    def onInput_onStop(self):
        self.onUnload() #it is recommended to reuse the clean-up as the box is stopped
        self.onStopped() #activate the output of the box