《NAO机器人程序设计》---第四章 运动控制

《NAO机器人程序设计》—第四章 运动控制
Nao机器人—Choregraphe

关节名

机器人边走边说话

moveInit():运动进程的初始化,检查机器人的当前状态,并选择一个正确的姿势，阻塞调用。
moveTo():移动到指定坐标位置,阻塞调用。
【moveInit()为阻塞调用方法,完成机器人行走前的准备动作。例如,如果机器人初始状态为休息状态,调用moveInit方法后,机器人进入准备行走状态(站立、屈膝)。moveInit方法的最长执行时间会超过1s,方法执行完成后,才会执行moveTo()方法。】
【moveTo()为阻塞调用方法,使用post对象调用moveTo()方法,将创建新的并行线程,在新线程中调用moveTo()方法,原线程继续调用后面的say()方法。】
程序执行结果:机器人一边向前走,一边说I’m walking ,在0.5m处停止。

ALRobotPosture模块

ALMotion模块–刚度

#刚度设置1
import time
class MyClass(GeneratedClass):
    def __init__(self):
        GeneratedClass.__init__(self)
        #代表ALMotion模块
        self.motion=ALProxy("ALMotion")
        
    def onLoad(self):
        #put initialization code here
        pass

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

    def onInput_onStart(self):
        #唤醒机器人，各关节关节刚度设置为1
        self.motion.wakeUp()
        jointName="Body"
        #获取body的刚度包括26个元素的列表
        stiffnesses=self.motion.getStiffnesses(jointName)
        self.logger.info(stiffnesses)
        self.motion.rest()

        stiffnesses=self.motion.getStiffnesses(jointName)
        self.logger.info(stiffnesses)
        names="Head"
        stiffness=0.5
        self.motion.setStiffnesses(names,stiffness)
        time.sleep(1.0)

        stiffnesseses=self.motion.getStiffnesses(jointName)
        self.logger.info(stiffnesses)
        pass

    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

结果

#####---刚度设置2
import time
class MyClass(GeneratedClass):
    def __init__(self):
        GeneratedClass.__init__(self)
        #代表ALMotion模块
        self.motion=ALProxy("ALMotion")
        self.posture=ALProxy("ALRobotPosture")
    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.posture.goToPosture("Crouch",1.0)
        jointName="Head"
        names=['HeadYaw']
        stiffnessLists=[0.25,0.5,1.0,0.0]
        timeLists=[1.0,2.0,3.0,4.0]
        self.motion.post.stiffnessInterpolation(names,stiffnessLists,timeLists)      
        for i in range(5):
            stiffnesses = self.motion.getStiffnesses(jointName)
            self.logger.info(stiffnesses)
            time.sleep(1.0)
        pass

    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

结果：

NAO使用刚度控制电机最大电流。电机的转矩(驱动力)与电流相关,设置关节的刚度相当于设置电机的转矩限制。

刚度为0.0,关节位置不受电机控制,关节是自由的。
刚度为1.0,关节使用最大转矩功率转到指定位置。
刚度为0.0～1.0,关节电机的转矩介于0与最大值之间(如果关节移动到目标位置所需要的转矩高于刚度的限制,关节不会到达目标位置)。

ALMotion模块–关节
关节控制方法：
（1）插值法，阻塞调用
（2）反应式法，非阻塞调用

#########---控制关节（头部左转30°）
import time
import almath
class MyClass(GeneratedClass):
    def __init__(self):
        GeneratedClass.__init__(self)
        self.motion = ALProxy("ALMotion")
    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.motion.setStiffnesses("Head",1.0)
        
        names="HeadYaw"
        angles=30.0*almath.TO_RAD
        fractionMaxSpeed=0.1
        self.motion.setAngles(names,angles,fractionMaxSpeed)
        time.sleep(3.0)
        
        self.motion,setStiffnesses("Head",0.0)
        pass
        

    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

almath是NAOqi系统提供的数学函数库,almath.TO__RAD为1度所对应的弧度数,在关节控制方法中,目标角度都以弧度为单位,本例及以后示例中,目标角度都用角度X1度对应弧度数形式表示。setAngles()方法为非阻塞调用方法,本例中转头动作后面的语句将头部刚度设置为0,因此调用setAngles()方法时需要使用延时。

小插曲:
almath有1.6.6, 1.6.7, 1.6.8, 1.6.12, 1.6.13, 1.6.14, 1.6.15, 1.7.0, 1.7.1, 1.7.2, 1.8.0, 1.8.1, 1.8.2, 1.8.3这几个版本
经实测 pip install almath==1.6.8 -i https://pypi.tuna.tsinghua.edu.cn/simple

#######-----头部插值运动
import time
import almath
class MyClass(GeneratedClass):
    def __init__(self):
        GeneratedClass.__init__(self)
        self.motion = ALProxy("ALMotion")
    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.motion.setStiffnesses("Head",1.0)
        
        names="HeadYaw"
        angleLists=50.0*almath.TO_RAD
        timeLists=1.0
        isAbsolute=True
        self.motion.angleInterpolation(names,angleLists,timeLists,isAbsolute)
        timeLists.sleep(1.0)
        
        names="HeadYaw"
        angleLists=[30.0*almath.TO_RAD,0.0]
        timeLists=[1.0,2.0]
        isAbsolute=True
        self.motion.angleInterpolation(names,angleLists,timeLists,isAbsolute)
        time.sleep(1.0)

        names=["HeadYaw","HeadPitch"]
        angleLists=[30.0*almath.TO_RAD,30.0*almath.TO_RAD]
        timeLists=[1.0,1.2]
        isAbsolute=True
        self.motion.angleInterpolation(names,angleLists,timeLists,isAbsolute)
        
        angleLists=[[50.0*almath.TO_RAD,0.0],[-30.0*almath.TO_RAD,30.0*almath.TO_RAD,0.0]]
        timeLists=[1.0,1.2]
        isAbsolute=True
        self.motion.angleInterpolation(names,angleLists,timeLists,isAbsolute)

        self.motion.setStiffnesses("Head",1.0)

        pass
        
        
    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

angleInterpolation()方法为阻塞调用方法,在完成当前语句后,才执行下一条语句。程序执行结果为:
1.头部左转50°,延时1s ;
2.右转20°(绝对30°),右转30°(绝对0°);
3.左转30°低头30°(两个关节同时运动);
4.左转20°仰头60°(绝对左转50°,仰头30°,两个关节同时运动),
右转50°低头60°(绝对右转0°,低头 30°,两个关节同时运动),
仰头30°(绝对0°)。