跳到主要内容
版本:V2.0.4.x

8.19 强化学习控制接口

请确保本体驱动和强化学习运控都已经启动。手柄向上拨动 E 键,可通过下述 ROS service 切换状态,通过 topic 下发速度指令,遥控器按键和摇杆无效(只有C 有效),E 置中恢复遥控器功能。

8.19.1运控模式切换

  • 接口名称: /hric/motion/set_motion_mode

  • 类型: ROS service

  • 描述: 请求切换运动模式

  • 发布模块: 运动控制

  • client 模块: 机器人管理

  • 消息类型: hric_msgs/SetMotionMode.srv

    Service msg参数名参数类型是 否必填备注
    请求字段walk_mode_requestuint8请求切换的运动模式1: stop, 2: zero, 3: stand, 4: walk, 5: run
    is_need_swing_armboolwalk 是否需要摆臂,true:需要,false:不需要(由于操作版本 , locomotion 不控手臂 , 因此设置 true 或 false 都可以)
    回复字段sucessbool表示服务调用是否成功
    error_codeuint32错误码 NONE=0 UNKNOWN=400
  • 发布示例:

    cd ros2ws/
    source install/setup.bash

    # 僵停:
    ros2 service call /hric/motion/set_motion_mode hric_msgs/srv/SetMotionMode "{walk_mode_request: 1, is_need_swing_arm: false}"

    # 回零:
    ros2 service call /hric/motion/set_motion_mode hric_msgs/srv/SetMotionMode "{walk_mode_request: 2, is_need_swing_arm: false}"

    # 站立:
    ros2 service call /hric/motion/set_motion_mode hric_msgs/srv/SetMotionMode "{walk_mode_request: 3, is_need_swing_arm: false}"

    # 原地踏步(回零状态放到地面后,直接调用本接口的话可以从回零状态进入原地踏步状态,但是比较危险,不要这样操作,先进入站立状态,再进入原地踏步):
    ros2 service call /hric/motion/set_motion_mode hric_msgs/srv/SetMotionMode "{walk_mode_request: 4, is_need_swing_arm: false}"

    # 原地跑步:
    ros2 service call /hric/motion/set_motion_mode hric_msgs/srv/SetMotionMode "{walk_mode_request: 5, is_need_swing_arm: false}"

8.19.2 动作编号调用接口

  • 接口名称: /hric/motion/set_motion_number

  • 类型: ROS service

  • 描述: 请求调用动作

  • 发布模块: 运动控制

  • 订阅模块: 机器人管理

  • 消息类型: hric_msgs/SetMotionNumber.srv

    Service msg参数名参数类型是否必填备注
    请求字段is_motionbool仅站立+F 上拨状态下有效,置为 true立即触发动作,false 不会打断动作,true 状态下会一直循环当前动作
    motion_numberint320~4,其他值会置为 0(招手)
    回复字段sucessbool表示服务调用是否成功
  • 发布示例:

    cd ros2ws
    source install/setup.bash

    # 做动作(5组动作,上一条是执行动作,下一条是取消执行,如果不执行下一条命令,则会一直重复执行该动作):
    ros2 service call /hric/motion/set_motion_number hric_msgs/srv/SetMotionNumber "{is_motion: true, motion_number: 0}"
    ros2 service call /hric/motion/set_motion_number hric_msgs/srv/SetMotionNumber "{is_motion: false, motion_number: 0}"

    ros2 service call /hric/motion/set_motion_number hric_msgs/srv/SetMotionNumber "{is_motion: true, motion_number: 1}"
    ros2 service call /hric/motion/set_motion_number hric_msgs/srv/SetMotionNumber "{is_motion: false, motion_number: 1}"

    ros2 service call /hric/motion/set_motion_number hric_msgs/srv/SetMotionNumber "{is_motion: true, motion_number: 2}"
    ros2 service call /hric/motion/set_motion_number hric_msgs/srv/SetMotionNumber "{is_motion: false, motion_number: 2}"

    ros2 service call /hric/motion/set_motion_number hric_msgs/srv/SetMotionNumber "{is_motion: true, motion_number: 3}"
    ros2 service call /hric/motion/set_motion_number hric_msgs/srv/SetMotionNumber "{is_motion: false, motion_number: 3}"

    ros2 service call /hric/motion/set_motion_number hric_msgs/srv/SetMotionNumber "{is_motion: true, motion_number: 4}"
    ros2 service call /hric/motion/set_motion_number hric_msgs/srv/SetMotionNumber "{is_motion: false, motion_number: 4}"

8.19.3 矢量速度控制接口

  • 接口名称 : /hric/robot/cmd_vel

  • 类型 : topic

  • 描述 : 发布速度指令 ,控制机器人运动。

  • 发布模块 : 遥控器

  • 订阅模块 : 机器人管理

  • 消息类型 : geometry_msgs/TwistStamped.msg

    geometry_msgs/msg/TwistStamped 结构如下:
    std_msgs/Header header
    geometry_msgs/Twist twist

    其中 geometry_msgs/Twist 结构又如下:
    geometry_msgs/Vector3 linear
    geometry_msgs/Vector3 angular

    linear 表示线速度 (单位 m/s),x 表示前后移动,y 表示左右平移,z 表示上下。

    angular 表示角速度 (单位 rad/s),z 表示绕 z 轴转动(左转、右转)。
  • 消息频率 : 20hz

  • 发布示例:

    cd ros2ws
    source install/setup.bash

    # x方向速度为0.15,也就是速度0.15前进
    ros2 topic pub /hric/robot/cmd_vel geometry_msgs/msg/TwistStamped "{header: {frame_id: 'base_link' }, twist: {linear: {x: 0.15, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}}"

    # x方向速度为0.20,也就是速度0.20前进
    ros2 topic pub /hric/robot/cmd_vel geometry_msgs/msg/TwistStamped "{header: {frame_id: 'base_link' }, twist: {linear: {x: 0.20, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}}"

    # 所有线速度角速度均为0.0,也就是原地踏步
    ros2 topic pub /hric/robot/cmd_vel geometry_msgs/msg/TwistStamped "{header: {frame_id: 'base_link' }, twist: {linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}}"

    # 以0.1的角速度左转
    ros2 topic pub /hric/robot/cmd_vel geometry_msgs/msg/TwistStamped "{header: {frame_id: 'base_link' }, twist: {linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.1}}}"

    # 以0.1的角速度右转
    ros2 topic pub /hric/robot/cmd_vel geometry_msgs/msg/TwistStamped "{header: {frame_id: 'base_link' }, twist: {linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: -0.1}}}"

    # 最后为了让机器人再进入原地踏步状态需要再调用一次接口,线速度角速度均传0.0

8.19.4 机器人下肢运动状态发布

  • 接口名称: /hric/motion/status

  • 类型: topic

  • 描述: 发布运动控制模块状态和速度

  • 发布模块: 运动控制

  • 发布频率: 20hz

  • 订阅模块: 机器人管理、定位、导航

  • 消息类型: hric_msgs/MotionStatus.msg

    参数名参数类型是否必填备注
    headerHeader时间戳和坐标系
    velocitygeometry_msgs/Twist.msg机器人腰部坐标系速度
    walk_modeuint8行走状态 0:stop, 1:zero, 3:stand, 5:walk, 7:run
    is_console_controlbool是否为遥控器操控,true: 导航操控,false: 遥控器操纵
    is_swing_armbool是否为 walk 摆臂状态。
    error_codeuint32错误码 NONE=0 UNKNOWN=400
  • 消息格式示例

    {
    "header": {
    "seq": 1,
    "stamp": {
    "secs": 1622548800,
    "nsecs": 0
    },
    "frame_id": ""
    },
    "velocity": {
    "linear": {
    "x": 1.0,
    "y": 0.0,
    "z": 0.0
    },
    "angular": {
    "x": 0.0,
    "y": 0.0,
    "z": 0.0
    }
    },
    "walk_mode": 1,
    "error_code": 0,
    }