目录
一、关节运动与身体平衡
1.1 moveTo
1.2 move
1.3 moveToward
一、关节运动与身体平衡
步频：步数/秒
当关节运动时，机器人的重心会发生变化，严重时机器人会摔倒。为保持身体平衡，不仅需要同时改变多个关节的角度，通常还需要同时调节关节运动速度。例如，改变髋关节角度，即做弯腰动作时，身体前倾，重心前移，机器人很容易摔倒。如果同时改变踝关节角度，使踝关节后仰，弯腰角度在一定范围内时，可以保持重心不变。
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"。 | 阻塞调用 |
