目录

1、关节运动与身体平衡

1.1 moveTo

1.2move

1.3moveToward

---

1、关节运动与身体平衡

步频：步数/秒

当关节移动时，机器人的重心会改变，严重时机器人会摔倒。为了保持身体平衡，机器人不仅需要同时改变多个关节的角度，还需要同时调整关节的运动速度。例如，改变髋关节的角度，也就是说，当弯曲时，身体向前倾斜，重心向前移动，机器人很容易摔倒。如果同时改变踝关节的角度，使踝关节向后倾斜，弯曲角度在一定范围内，重心可以保持不变。

NAO行走控制采用线性倒立行走控制ALMotion传感器的实际关节位置信号采集在周期中。与位移（即位置）和身体倾斜角度进行比较后，使用控制算法计算控制量，驱动电机实现关节的实时控制。行走控制的目标是尽快达到平衡位置，当达到预期位置时，没有大的振荡和过大的角度和速度，NAO能克服随机扰动，保持稳定位置。

使用每个运动位置Pose2D类定义(属于ALMath如下图所示，在描述左脚位置时，以右脚为参考点，pX和pY左脚在x和y方向与参考点之间的距离，pTheta绕z轴旋转角，即左右角。

无论采用哪种行走控制，都需要使用步态规划，指定步长、步频、最大高度等参数。步态参数可以取系统默认值或通过setFootSteps()或setFootStepsWithSpeed()方法规定(为避免参数冲突或肢体碰撞，内部规划师将修改指定参数)。步态参数如表所示。

名称	含义	缺省	最小	最大	可修改
MaxStepX	沿x最大方向向前平移(米)	0.040	0.001	0.080	是
MinStepX	沿x最大方向向后平移(米)	-0.040			否
MaxStepY	沿y最大平移方向的绝对值(米)	0.140	0.101	0.160	是
MaxStepTheta	沿z最大的绝对值(弧度)	0.349	0.001	0.524	是
MaxStepFrequency	最大步频	1.0	0.0	1.0	是
MinStepPeriod	最小步周期	0.42			否
MaxStepPeriod	最大步周期	0.6			否
StepHeight	Z轴方向抬脚最大高度(米)	0.020	0.005	0.040	是
TorsoWx	躯干与x轴间最大角度(弧度)	0.000	-0.122	0.122	是
TorsoWy	躯干与y轴间最大角度(弧度)	0.000	-0.122	0.122	是
FootSeparation	y方向两脚之间距离(米)	0.1			否
MinFootSeparation	y方向两脚之间最小距离(米)	0.088			否

1.1 moveTo

moveTo方法使机器人在平面上移动到指定位置，阻塞调用方法。moveTo方法包括以下4种形式： (1)moveTo(x,y,theta)，移动到指定位置。其中x为x轴方向距离(米)，y为y轴方向距离(米)，theta为以弧度表示的绕z轴旋转的角度(取值范围为[-3.14159,3.14159])。

# moveTo方法（移动终点：前0.2米，左0.2米，逆时针/左转90度）
import  math
class  MyClass(GeneratedClass):
    def  __init__(self):
        GeneratedClass.__init__(self)
        self.posture=ALProxy("ALRobotPosture")
        self.motion=ALProxy("ALMotion")
    def  onLoad(self):
        pass
    def  onUnload(self):
        pass
    def  onInput_onStart(self):
        self.motion.wakeUp()
        self.posture.goToPosture("StandInit", 1.0)
        x= 0.2
        y= 0.2
        theta= math.pi/2              #绕z轴转正90度，即左转90度
        self.motion.moveTo(x, y, theta)
        pass
    def  onInput_onStop(self):
        self.onUnload() 
        self.onStopped() 

移动到：前0.2米，左0.2米，逆时针左转90°

(2)moveTo(x,y,theta,moveConfig)，按给定的步态参数移动到指定位置。其中x为x轴方向距离(米)，y为y轴方向距离(米)，theta为以弧度表示的绕z轴旋转的角度(取值范围为[-3.14159,3.14159]). moveConfig为自定义步态参数列表。 moveTo方法只接受以键-值对形式表示的步态参数列表，参数对左脚和右脚都有效。 [["MaxStepFrequency",1.0], ["MaxStepX",0.06]]

        x= 0.2
        y= 0.2
        theta= math.pi/2
        tstart=time.time() 
        self.motion.moveTo(x, y, theta,[["MaxStepFrequency",1.0],["MaxStepX",0.06]])
        tend=time.time()
        self.logger.info(tend-tstart)

(3)moveTo(controlPoints)，沿控制点移动到指定位置，其中controlPoints为控制点列表，每个列表项是一个位置，列表格式为：[[x1,y1,theta1], ..., [xN,yN,thetaN]]。 (4)moveTo(controlPoints,moveConfig)，按给定的步态参数，沿控制点移动到指定位置。其中controlPoints为控制点列表，moveConfig为自定义步态参数列表。

1.2 move

move方法使机器人按指定速度行走，非阻塞调用方法。move方法包括以下2种形式： (1)move(x,y,theta)，按指定速度行走。其中x为x方向速度(米/秒)，负数表示向后运动；y为y方向速度(米/秒)，正数表示向左；theta为绕z轴旋转角速度(弧度/秒)，负数表示顺时针旋转

# move方法（原地转圈）
import  math
import  time
class  MyClass(GeneratedClass):
    def  __init__(self):
        GeneratedClass.__init__(self)
        self.posture=ALProxy("ALRobotPosture")
        self.motion=ALProxy("ALMotion")
    def  onLoad(self):
        pass
    def  onUnload(self):
        pass
    def  onInput_onStart(self):
        self.motion.wakeUp()
        self.posture.goToPosture("StandInit", 1.0)
        x= 0.02
        y= 0.02
        theta= math.pi/16
        self.motion.move(x, y, theta)
        time.sleep(33.2)                #32秒+起始阶段0.6秒+终止阶段0.6秒
        self.motion.stopMove()
        pass
    def  onInput_onStop(self):
        self.onUnload() 
        self.onStopped()

move方法为非阻塞调用方法，需要使用time.sleep()语句延时。延时时间除了转动360度所需时间外，还应该包括机器人行走过程的初始化阶段和终止阶段所需时间。延时结束后，使用stopMove()方法停止运动。 常见的几种运动方式参数设置： 前进：x>0,y=0,theta=0； 后退：x<0,y=0,theta=0； 左移：x=0,y>0,theta=0； 右移：x=0,y<0,theta=0。

(2)move(x,y,theta,moveConfig)，按给定的步态参数、指定速度行走。其中x为x方向速度(米/秒)，负数表示向后运动；y为y方向速度(米/秒)，正数表示向左；theta为绕z轴旋转角速度(弧度/秒)，负数表示顺时针旋转；moveConfig为自定义步态参数列表，可以分别设置左脚和右脚的步态参数。

1.3 moveToward

moveToward方法使机器人按指定速度行走，非阻塞调用方法。moveToward方法包括以下2种形式：

(1)moveToward(x,y,theta)，按指定速度行走。其中x为x方向速度，取值范围为[-1,1]，其中1表示向前最大速度，-1表示向后最大速度；y为y方向速度，取值范围为[-1,1]，其中1表示向左最大速度，-1表示向右最大速度；theta为绕z轴旋转速度，取值范围为[-1,1]，其中1表示逆时针最大转速，-1表示顺时针最大转速。

(2)moveToward(x,y,theta,moveConfig)，按给定的步态参数、指定速度行走。其中x,y,theta含义与(1)相同；moveConfig为自定义步态参数列表，可以分别设置左脚和右脚的步态参数。

# moveToward方法
import  math
import  time
class  MyClass(GeneratedClass):
    def  __init__(self):
        GeneratedClass.__init__(self)
        self.posture=ALProxy("ALRobotPosture")
        self.motion=ALProxy("ALMotion")
    def  onLoad(self):
        pass
    def  onUnload(self):
        pass

    def  onInput_onStart(self):
        self.motion.wakeUp()
        self.posture.goToPosture("StandInit", 1.0)
        x= 1.0
        y= 0.0
        theta = 0.0
        frequency = 1.0
        self.motion.moveToward(x, y, theta, [["Frequency", frequency]])
        time.sleep(3)          #以最快速度向前走3秒
        x=0.5
        theta=0.6
        self.motion.moveToward(x, y, theta, [["Frequency", frequency]])
        time.sleep(3)         #左转前行3秒
        frequency = 0.5
        self.motion.moveToward(x, y, theta, [["Frequency", frequency]])
        time.sleep(3)        #降低步频，左转前行3秒
        self.motion.stopMove()  
        self.motion.rest()       #进入休息状态
        pass

方法名	说明	调用方式
waitUntilMoveIsFinished()	等待，直到行走任务完成。用于阻塞程序向下运行直到行走任务结束。	阻塞调用
getMoveConfig(config)	获取步态参数，config取"Max", "Min"或"Default"，返回值为步态参数列表。	阻塞调用
getRobotPosition(useSensors)	获取机器人位置。useSensors为True，返回MRE传感器测量值,返回值为[x,y,theta]。	阻塞调用
getRobotVelocity()	获取机器人速度。返回值为x方向速度(米/秒), y方向速度(米/秒)和绕z轴旋转速度(弧度/秒)。	阻塞调用
setMoveArmsEnabled(leftArmEnable, rightArmEnable)	设置运动过程中手臂是否可动。leftArmEnable和rightArmEnable取True时可动，取False时不可动。	阻塞调用
getMoveArmsEnabled(chainName)	获取运动过程中手臂是否可动，返回值为True或False。chainName取"LArm","RArm"或"Arms"。	阻塞调用