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"。 |
阻塞调用 |