运动控制

控制行走

调用 python 模块

为了方便开发调用，我们将控制机器人行走的方法封装成模块。leju_lib_pkg 是我们提供的 python 接口库，该库存在于 ~/robot_ros_application/catkin_ws/src/leju_lib_pkg

walk 函数

def walk(self):
    if self.__check_id() != True:
        return False
    status = self.__get_status()
    if status == None:
        return -1
    if status.data == 'walking':
        return True
    if self.ready() == True:
        result = self.__set_status(self.ctl_id, 'walking')
        if result == None:
            return -2
        if result == self.__state_walking:
            return True
    return False

走路之前需要调用该方法设置机器人的状态为走路状态，否则机器人将不会执行走路

walking 函数

# path: ~/robot_ros_application/catkin_ws/src/leju_lib_pkg/src/motion/bodyhub_client.py
def walking(self, delta_x, delta_y, theta, timeout = None):
    try:
        rospy.wait_for_message('/requestGaitCommand', Bool, self.__timeout if timeout is None else timeout)
    except:
        rospy.logwarn("wait_for_service requestGaitCommand timeout!")
        result = 1
    else:
        result = 0
    self.__is_walking = 1
    self.__gait_cmd_pub.publish(data=[delta_x, delta_y, theta])
    return result

这是最底层的行走封装，有基于该函数封装的更高层函数
walking 函数直接向 /requestGaitCommand 发送单步信息，包括 delta_x（前后），delta_y（左右）以及 theta（旋转）
以右手定则划分，向前、向左和逆时针为正方向，反之为负方向
如果 delta_x、delta_y 或 theta 参数不合理（超过了机器人所能到达的极限），会出现逆解失败（ ik failed!!! ）的警告，同时机器人不会执行这步调用。因此不建议开发者直接对该函数进行调用，可以用以下几个函数，都对机器人的最大步幅做了限制

walking_the_distance 函数

# path: ~/robot_ros_application/catkin_ws/src/leju_lib_pkg/src/motion/bodyhub_client.py
def walking_the_distance(self, delta_x, delta_y, theta):
    direction = [0, 0, 0]
    direction[0] = 1 if delta_x >= 0 else -1
    direction[1] = 1 if delta_y >= 0 else -1
    direction[2] = 1 if theta >= 0 else -1

    xNumberOfStep = abs(delta_x)//self.__step_len_max[0]
    yNumberOfStep = abs(delta_y)//self.__step_len_max[1]
    aNumberOfStep = abs(theta)//self.__step_len_max[2]
    delta_x = delta_x - ((self.__step_len_max[0] * xNumberOfStep)*direction[0])
    delta_y = delta_y - ((self.__step_len_max[1] * yNumberOfStep)*direction[1])
    theta = theta - ((self.__step_len_max[2] * aNumberOfStep)*direction[2])
    numberOfStep = max(xNumberOfStep, yNumberOfStep, aNumberOfStep)

    for i in range(0, int(numberOfStep+1)):
        if xNumberOfStep >= 1:
            x = self.__step_len_max[0]*direction[0]
            xNumberOfStep = xNumberOfStep - 1
        elif xNumberOfStep == 0:
            x = delta_x
            xNumberOfStep = xNumberOfStep - 1
        else:
            x = 0.0

        if yNumberOfStep >= 1:
            y = self.__step_len_max[1]*direction[1]
            yNumberOfStep = yNumberOfStep - 1
        elif yNumberOfStep == 0:
            y = delta_y
            yNumberOfStep = yNumberOfStep - 1
        else:
            y = 0.0

        if aNumberOfStep >= 1:
            a = self.__step_len_max[2]*direction[2]
            aNumberOfStep = aNumberOfStep - 1
        elif aNumberOfStep == 0:
            a = theta
            aNumberOfStep = aNumberOfStep - 1
        else:
            a = 0.0
        self.walking(x, y, a)

该函数允许用户传入要走的距离及旋转的角度，函数中会将距离按照机器人行走的最大限幅划分步数。参数传递与 walking 相同，只不过在这个函数，你可以直接传目标点与机器人的距离了
在属性 self.__step_len_max 中限定了机器人的限幅，避免了出现逆解失败的情况

walking_n_steps 函数

# path: ~/robot_ros_application/catkin_ws/src/leju_lib_pkg/src/motion/bodyhub_client.py
def walking_n_steps(self, cmd, n):
    for i in range(0, n):
        if self.walking(cmd[0], cmd[1], cmd[2]):
            return 1
    return 0

该函数允许用户传入单个步幅及步数，但同时需要在传入时限制步幅大小
参数 cmd 是一个列表，定义如下
- @cmd: [delta_x, delta_y, theta]
- 与 walking 函数的参数相同，相当于是可以传入步数的 walking 函数
参数 n 即为步数，该值应该大于 0

Python 示例

#!/usr/bin python
# -*- coding: utf-8 -*-
import rospy
import rospkg
import sys

sys.path.append(rospkg.RosPack().get_path('leju_lib_pkg'))
import motion.bodyhub_client as bodycli

CTL_ID = 2

class Walking(bodycli.BodyhubClient):
    def __init__(self):
        super(Walking, self).__init__(CTL_ID)

    def do_some_walking(self):
        # 走 S 形曲线

        # 方法一    调用 walking 函数
        for i in range(0, 5):
            self.walking(0.0, 0.0, 12.0)
        for i in range(0, 16):
            self.walking(0.07, 0.0, -7.5)
        for i in range(0, 16):
            self.walking(0.07, 0.0, 7.5)
        for i in range(0, 5):
            self.walking(0.0, 0.0, -12.0)

        # 方法二    调用 walking_the_distance 函数
        # self.walking_the_distance(0.0, 0.0, 60.0)
        # self.walking_the_distance(1.12, 0.0, -120.0)
        # self.walking_the_distance(1.12, 0.0, 120.0)
        # self.walking_the_distance(0.0, 0.0, -60.0)

        # 方法三    调用 walk_n_step 函数
        # self.walk_n_step([0.0, 0.0, 12.0], 5)
        # self.walk_n_step([0.07, 0.0, -7.5], 16)
        # self.walk_n_step([0.07, 0.0, 7.5], 16)
        # self.walk_n_step([0.0, 0.0, -12.0], 5)

        self.wait_walking_done()

    def start(self):
        self.walk()
        self.do_some_walking()
        rospy.signal_shutdown('exit')

    def rosShutdownHook(self):
        self.reset()

if __name__ == "__main__":
    rospy.init_node('walking_node', anonymous=True)
    walking = Walking()
    rospy.on_shutdown(walking.rosShutdownHook)
    walking.start()

控制末端轨迹

调用 Python 模块

末端轨迹的控制是通过 ikmodule 逆解模块实现的，模块定义在 ~/robot_ros_application/catkin_ws/src/leju_lib_pkg/src/ik_lib/ikmodulesim/ikmodulesim.py 下面将介绍调用的接口函数：

toInitPoses 函数

函数定义如下：

def toInitPoses(self):
    if self.setstatus() and self.getposes():
        self.PosPara_wF.Torso_z = -self.leftLegZ
        self.PosPara_wF.Lfoot_y = self.leftLegY
        self.PosPara_wF.Rfoot_y = self.rightLegY
        self.PosPara_wF.RArm_x = self.rightArmX
        self.PosPara_wF.RArm_y = self.rightArmY
        self.PosPara_wF.RArm_z = self.rightArmZ
        self.PosPara_wF.LArm_x = self.leftArmX
        self.PosPara_wF.LArm_y = self.leftArmY
        self.PosPara_wF.LArm_z = self.leftArmZ
        return True
    return False

初始化 ikmodule，必须在调用 body_motion 前调用
初始化成功返回 True，否则返回 False

末端位置

在 ~/robot_ros_application/catkin_ws/src/leju_lib_pkg/src/ik_lib/ikmodulesim/CtrlType.py 的 CtrlType 类中定义了末端位置。末端位置在 body_motion 函数中用到，用于指定需要控制的末端位置

定义了如下末端点

定义	含义
Torso	躯干质心
Torso_R	躯干质心绕 x 轴方向的旋转，逆时针为正
Torso_P	躯干质心绕 y 轴方向的旋转，逆时针为正
Torso_Y	躯干质心绕 z 轴方向的旋转，逆时针为正
Torso_x	躯干质心沿 x 轴方向的位移，向前为正
Torso_y	躯干质心沿 y 轴方向的位移，向左为正
Torso_z	躯干质心沿 z 轴方向的位移，向上为正
Lfoot	左脚
Lfoot_R	左脚绕 x 轴方向的旋转，逆时针为正
Lfoot_P	左脚绕 y 轴方向的旋转，逆时针为正
Lfoot_Y	左脚绕 z 轴方向的旋转，逆时针为正
Lfoot_x	左脚沿 x 轴方向的位移，向前为正
Lfoot_y	左脚沿 y 轴方向的位移，向左为正
Lfoot_z	左脚沿 z 轴方向的位移，向上为正
Rfoot	右脚
Rfoot_...	与左脚类似
LArm_x	左手沿 x 轴方向的位移，向前为正
LArm_y	左手沿 y 轴方向的位移，向左为正
LArm_z	左手沿 z 轴方向的位移，向上为正
RArm_...	与左手类似

body_motion 函数

函数定义如下：

def body_motion(self, body_pose, value, count=100):
    """
    简易调用
    Args:
        - body_pose: CtrlType, or [CtrlType]
        - value: list, or two dim list. RPY, xyz.
        - count: int. times of division

    """
    if not (isinstance(body_pose, list) or isinstance(body_pose, tuple)):
        body_pose = [body_pose]
        value = [value]

    d = {}

    for t, diff in zip(body_pose, value):
        if isinstance(t.key(), str):
            d[t.key()] = diff
        else:
            for key, dif in zip(t.key(), diff):
                d[key] = dif

    for index in range(count):
        for k, v in d.items():
            setattr(self.PosPara_wF, k, getattr(self.PosPara_wF, k) + v * 1.0 / count)

        poseArrayMsg = PoseArray()
        poseArrayMsg.poses.append(self.getleftlegPosMsg())
        poseArrayMsg.poses.append(self.getrightlegPosMsg())
        poseArrayMsg.poses.append(self.getleftarmPosMsg())
        poseArrayMsg.poses.append(self.getrightarmPosMsg())
        poseArrayMsg.controlId = 6
        self.targetPosesPub.publish(poseArrayMsg)

    rospy.loginfo("waitPostureDone...")
    self.waitPostureDone()

机器⼈末端动作执⾏，给定⽬标位姿 body_pose，value 通过逆解得到舵机角度，控制机器⼈运动
该函数需要传递三个参数
- body_pose
  - 需要控制的末端位置，可以只传一个，如果要传多个需要放在列表内传入，如：
    body_motion(末端位置, ...) 或 body_motion([末端位置1, 末端位置2], ...)
- value
  - 表⽰给相应末端的位姿值，一个末端位置应该对应一个位姿值，如：
    body_motion(末端位置, 位姿值, ...) 或 body_motion([末端位置1, 末端位置2], [位姿值1, 位姿值2], ...)
  - 位姿值的定义
    - 如果是 Torso、Lfoot 或 Rfoot ，那么位姿值是一个列表，包含，定义如下：
      [x, y, z, R, P, Y]
    - 如果是指定了轴的旋转或或位移，如：Torso_y，只需要传躯干沿 y 轴方向的位移即可
  - 位姿值的单位
    - 位移的单位为米(m)，旋转的单位为度(°)
- count
  - 插值次数

reset 函数

函数定义如下：

def reset(self):
    rospy.wait_for_service("MediumSize/IKmodule/SetStatus", 30)
    try:
        resp = self.setStatusClient.call(6, "reset")
        rospy.loginfo("Set status with response: %d" % resp.stateRes)
        return True
    except rospy.ServiceException as e:
        rospy.logwarn("Reset status service call failed: %s" % e)

用于动作执行完成之后的复位

Python 示例

#!/usr/local/bin/python
# -*- coding: utf-8 -*-
import rospy
import math

from ik_lib.ikmodulesim import IkModuleSim
from ik_lib.ikmodulesim.CtrlType import CtrlType as C
from mediumsize_msgs.srv import *

class IkAction(IkModuleSim):
    def __init__(self):
        super(IkAction, self).__init__()                # 继承 IkModuleSim

    def do_some_action(self):
        if self.toInitPoses() == True:                  # 初始化 Ik
            self.body_motion(C.Torso, [0, 0, -0.04, 0, 0, 0])               # 躯干质心沿 z 轴向下 4cm ，即机器人会下蹲 4cm
            self.body_motion([C.Torso_x, C.Torso_P], [0.02, math.pi/18])    # 躯干质心沿 x 轴向前 2cm ，绕 y 轴向前旋转 Π / 18 °
            self.body_motion(C.Torso_z, 0.02)                               # 躯干质心沿 z 轴向上 2cm
            self.body_motion([C.LArm_x, C.RArm_z], [0.04, 0.04])            # 左手沿 x 轴向前 4cm ，右手沿 z 轴向上 4cm
        else:
            rospy.logerr("ik_lib to initpose failed")

    def rosShutdownHook(self):
        self.reset()

if __name__ == "__main__":
    rospy.init_node("IkAction")
    IKA = IkAction()
    rospy.on_shutdown(IKA.rosShutdownHook)
    IKA.do_some_action()

运行该脚本前，应该现在运行的终端加载工作空间的环境变量，再运行 Python 脚本
```
source ~/robot_ros_application/catkin_ws/devel/setup.bash
```

运动控制

控制行走​

调用 python 模块​

walk 函数​

walking 函数​

walking_the_distance 函数​

walking_n_steps 函数​

Python 示例​

控制末端轨迹​

调用 Python 模块​

toInitPoses 函数​

末端位置​

body_motion 函数​

reset 函数​

Python 示例​

控制行走

调用 python 模块

walk 函数

walking 函数

walking_the_distance 函数

walking_n_steps 函数

Python 示例

控制末端轨迹

调用 Python 模块

toInitPoses 函数

末端位置

body_motion 函数

reset 函数

Python 示例