跳转至

R1 软件介绍 ROS 1 Noetic

环境依赖

  1. 硬件依赖：R1计算单元
  2. 操作系统依赖：Ubuntu 20.04 LTS
  3. 中间件依赖：ROS 1 Noetic

获取SDK

查看 R1 软件版本更新日志，获取最新的SDK包及更新信息。

启动SDK

当前支持分开启动和一键启动两种方式启动软件接口。为了您的安全，强烈建议使用分开启动的方式。

分开启动各接口

所有组件可以通过以下命令模板启动。

点击此处查看详细启动方式。

source {your_download_path}install/setup.bash
roslaunch <Package Name> <Launch File>
# 示例：
roslaunch HDAS r1.launch
接口信息 文件包名 路径 启动文件
Arms Driver Interface
Torso Driver Interface
Chassis Driver Interface
IMU Interface
BMS Interface
Remote Controller Interface		 HDAS install/HDAS/share/HDAS/launch/ r1.launch
Camera Interface signal_camera install/signal_camera_node/share/signal_camera_node/launch/ signal_camera.launch
LiDAR Interface livox_ros_driver2 /install/livox_ros_driver2/share/livox_ros_driver2/launch_ROS2/ msg_MID360.launch

一键启动所有接口

注意：执行以下命令将启动所有驱动和运控接口。

  sudo apt-get install tmux tmuxp
  cd {your_download_path}install/share/startup_config/script
  ./ota_script.sh boot

Demo演示

访问页面 R1 Demo演示指南，并按照说明操作R1。

软件接口

当前的Galaxea R1控制图如下所示，由5个主要部分组成：关节控制、手臂姿态控制、躯干速度控制、底盘控制和姿态估计。整个软件包被简称为“mobiman”,表示移动操作。 R1_control_interface_draw

驱动接口

当前的Galaxea R1驱动程序由多个组件组成，包括执行器接口、传感器接口和外部功能接口。

手臂驱动接口

该接口是用于机械臂控制和状态反馈的ROS软件包，定义了多个话题用于发布和订阅臂的状态、控制命令和相关错误代码。接口信息如下所示：

话题名称 I/O 描述 消息类型
/hdas/feedback_arm_left Output 左臂关节反馈 sensor_msgs::JointState
/hdas/feedback_arm_right Output 右臂关节反馈 sensor_msgs::JointState
/hdas/feedback_gripper_left Output 左夹爪行程 sensor_msgs::JointState
/hdas/feedback_gripper_right Output 右夹爪行程 sensor_msgs::JointState
/hdas/feedback_status_arm_left* Output 左臂状态反馈 hdas_msg::feedback_status
/hdas/feedback_status_arm_right* Output 右臂状态反馈 hdas_msg::feedback_status
/hdas/feedback_status_gripper_left Output 左夹爪状态反馈 hdas_msg::feedback_status
/hdas/feedback_status_gripper_right Output 右夹爪状态反馈 hdas_msg::feedback_status
/motion_control/control_arm_left Input 左臂电机控制 hdas_msg::motor_control
/motion_control/control_arm_right Input 右臂电机控制 hdas_msg::motor_control
/motion_control/control_gripper_left Input 左夹爪电机控制 hdas_msg::motor_control
/motion_control/control_gripper_right Input 右夹爪电机控制 hdas_msg::motor_control
/motion_control/position_control_gripper_left Input 左夹爪位置控制 std_msgs::Float32
/motion_control/position_control_gripper_right Input 右夹爪位置控制 std_msgs::Float32

手臂使能/失能/标定/控制模式切换可通过以下服务接口进行控制：

服务名称 I/O 描述 消息类型
/hdas/function_frame_left_arm Input 机械臂功能帧 hdas_msg/FunctionFrame
/hdas/function_frame_right_arm Input 机械臂功能帧 hdas_msg/FunctionFrame

针对以上话题的具体字段及其详细描述如下表所示：

话题名称 字段 描述
/hdas/feedback_arm_left header 标准消息头
position [关节1位置, 关节2位置, 关节3位置, 关节4位置, 关节5位置, 关节6位置, 夹爪关节位置]
velocity [关节1速度, 关节2速度, 关节3速度, 关节4速度, 关节5速度, 关节6速度, 夹爪关节速度]
effort [关节1力矩, 关节2力矩, 关节3力矩, 关节4力矩, 关节5力矩, 关节6力矩, 夹爪关节力矩]
/hdas/feedback_arm_right header 标准消息头
position [关节1位置, 关节2位置, 关节3位置, 关节4位置, 关节5位置, 关节6位置, 夹爪关节位置]
velocity [关节1速度, 关节2速度, 关节3速度, 关节4速度, 关节5速度, 关节6速度, 夹爪关节速度]
effort [关节1力矩, 关节2力矩, 关节3力矩, 关节4力矩, 关节5力矩, 关节6力矩, 夹爪关节力矩]
/hdas/feedback_gripper_left header 标准消息头
position 夹爪行程 (0-100mm)
velocity 暂不使用
effort 暂不使用
/hdas/feedback_gripper_right header 标准消息头
position 夹爪行程 (0-100mm)
velocity 暂不使用
effort 暂不使用
/hdas/feedback_status_arm_left header 标准消息头
name_id 关节名称
errors 包含错误码和对应的错误描述
/hdas/feedback_status_arm_right header 标准消息头
name_id 关节名称
errors 包含错误码和对应的错误描述
/hdas/feedback_status_gripper_left header 标准消息头
name_id 关节名称
errors 包含错误码和对应的错误描述
/hdas/feedback_status_gripper_right header 标准消息头
name_id 关节名称
errors 包含错误码和对应的错误描述
/motion_control/control_arm_left
（伺服模式）		 header 标准消息头
name -
p_des [关节1位置, 关节2位置, 关节3位置, 关节4位置, 关节5位置, 关节6位置, 夹爪关节位置]
v_des [关节1速度最大限制, 关节2速度最大限制, 关节3速度最大限制, 关节4速度最大限制, 关节5速度最大限制, 关节6速度最大限制, 夹爪关节速度最大限制]
t_ff [关节1力矩最大限制, 关节2力矩最大限制, 关节3力矩最大限制, 关节4力矩最大限制, 关节5力矩最大限制, 关节6力矩最大限制, 夹爪关节力矩最大限制]
mode -
/motion_control/control_arm_left
（力位混合控制模式）		 header 标准消息头
name -
p_des [关节1位置, 关节2位置, 关节3位置, 关节4位置, 关节5位置, 关节6位置, 夹爪关节位置]
v_des [关节1速度, 关节2速度, 关节3速度, 关节4速度, 关节5速度, 关节6速度, 夹爪关节速度]
kp [关节1kp, 关节2kp, 关节3kp, 关节4kp, 关节5kp, 关节6kp, 夹爪关节kp]
kd [关节1kd, 关节2kd, 关节3kd, 关节4kd, 关节5kd, 关节6kd, 夹爪关节kd]
t_ff [关节1力矩, 关节2力矩, 关节3力矩, 关节4力矩, 关节5力矩, 关节6力矩, 夹爪关节力矩]
mode -
/motion_control/control_arm_right
（伺服模式）		 header 标准消息头
name -
p_des [关节1位置, 关节2位置, 关节3位置, 关节4位置, 关节5位置, 关节6位置, 夹爪关节位置]
v_des [关节1速度最大限制, 关节2速度最大限制, 关节3速度最大限制, 关节4速度最大限制, 关节5速度最大限制, 关节6速度最大限制, 夹爪关节速度最大限制]
t_ff [关节1力矩最大限制, 关节2力矩最大限制, 关节3力矩最大限制, 关节4力矩最大限制, 关节5力矩最大限制, 关节6力矩最大限制, 夹爪关节力矩最大限制]
mode -
/motion_control/control_arm_right
（力位混合控制模式）		 header 标准消息头
name -
p_des [关节1位置, 关节2位置, 关节3位置, 关节4位置, 关节5位置, 关节6位置, 夹爪关节位置]
v_des [关节1速度, 关节2速度, 关节3速度, 关节4速度, 关节5速度, 关节6速度, 夹爪关节速度]
kp [关节1kp, 关节2kp, 关节3kp, 关节4kp, 关节5kp, 关节6kp, 夹爪关节kp]
kd [关节1kd, 关节2kd, 关节3kd, 关节4kd, 关节5kd, 关节6kd, 夹爪关节kd]
t_ff [关节1力矩, 关节2力矩, 关节3力矩, 关节4力矩, 关节5力矩, 关节6力矩, 夹爪关节力矩]
mode -
/motion_control/control_gripper_left header 标准消息头
name -
p_des 夹爪电机位置
v_des 夹爪电机速度
kp 夹爪电机kp
kd 夹爪电机kd
t_ff 夹爪力矩
mode -
/motion_control/control_gripper_right header 标准消息头
name -
p_des 夹爪电机位置
v_des 夹爪电机速度
kp 夹爪电机kp
kd 夹爪电机kd
t_ff 夹爪力矩
mode -
/motion_control/position_control_gripper_left header 标准消息头
data 夹爪行程期待值 (0-100mm)
/motion_control/position_control_gripper_right header 标准消息头
data 夹爪行程期待值 (0-100mm)

手臂服务接口：

服务名称 字段 描述
/hdas/function_frame_left_arm command 1: 使能
2: 失能
3: 整臂标定
4: 清除错误
5: MIT（力位混合控制模式）
6: PID（伺服控制模式）
/hdas/function_frame_right_arm command 1: 使能
2: 失能
3: 整臂标定
4: 清除错误
5: MIT（力位混合控制模式）
6: PID（伺服控制模式）

注意：机械臂开机默认为MIT（力位混合控制模式）模式。以左臂为例，切换为PID（伺服控制模式）需通过/hdas/function_frame_left_arm接口。按顺序依次调用“2-6-1”命令，即先将机械臂“失能”，再切换机械臂控制模式为“PID（伺服控制模式）”，最后开启机械臂“使能”。如设备重新启动，机械臂会自动恢复至MIT（力位混合控制模式）。

机械臂关节电机控制接口说明

/motion_control/control_arm_left
/motion_control/control_arm_right

  1. MIT-力位混合控制模式架构： R1_arm_driver_mit_mode 电机的输出扭矩公式用于计算电流环给定跟踪的力矩值 Tref，公式如下：
    \(K_p(p_d - p_e) + K_d(v_d - v_e)+t_{ff} = T_{ref}\)，其中：

    • Kp，Kd 为位置增益和速度增益的比例项；pd，vd 为期望的位置和速度；t_ff 为前馈力矩；pe 为编码器反馈的位置，ve 为微分得到的电机转速。
    • 输入项包括：Kp，Kd，pd，vd，t_ff。
    • 反馈项 pe 和 ve 无需手动输入。

    使用须知:

    • 前馈力矩：该值为必须项。由于位置项和速度项无法弥补过大的力矩误差，所以前馈力矩至少应补偿机械臂自身重力的影响。

    • 增益设置推荐：以下是A1电机的推荐 kp 和 kd 值。调整时请谨慎操作。

      A1_kp = [ 140.0, 200.0, 120.0, 20.0, 20.0, 20.0 ]

      A1_kd = [ 10.0, 50.0, 5.0, 1.0, 1.0, 0.4 ]

  2. PID-伺服控制模式架构： R1_arm_pid_cn 伺服控制模式为三环PID（位置环、速度环、电流环）控制，该模式引入积分环节，稳态误差较小。为保持接口一致性，在控制数据格式上，与MIT接口相同。在/motion_control/control_arm_left接口中：

    • p_des：表示设定角度（rad）。
    • v_des：表示速度限制（rad/s）。
    • k_p，k_d：无意义。
    • t_ff：表示对应力限保护。保护值范围为[0,1]，即电机允许的最大力矩百分比。

    使用须知:

    • 在PID-伺服模式下，设置所有电机控制信号值 t_ff = 0.2，其余值为0，机械臂将会进入带阻尼的自由拖动状态（当速度为0 rad/s时，电机会抑制转动）。
    • 当v_des ≠ 0且t_ff ≠ 0时，即可开启对电机的位置控制。推荐首次调试数值为：v_des = 0.5，t_ff=0.2。
    • 正常使用时，推荐设定t_ff ＜ 0.8，可防止电机触发力矩保护。

躯干驱动接口

该接口是用于躯干控制和提供状态反馈的ROS软件包，它定义了多个话题，用于发布和订阅躯干电机的状态和控制命令。接口信息如下所示：

话题名称 I/O 描述 消息类型
/hdas/feedback_torso Output 躯干关节反馈 sensor_msgs/JointState
/hdas/feedback_status_torso Output 躯干状态反馈 hdas_msg::feedback_status
/motion_control/control_torso Input 躯干电机控制 hdas_msg::motor_control

针对以上话题的具体字段及其详细描述如下表所示：

话题名称 字段 描述
/hdas/feedback_torso header 标准消息头
position [关节1位置, 关节2位置, 关节3位置, 关节4位置]
velocity [关节1速度, 关节2速度, 关节3速度, 关节4速度]
effort 暂不使用
/hdas/feedback_status_torso header 标准消息头
name_id 关节名称
errors 包含错误码和对应的错误描述
/motion_control/control_torso header 标准消息头
name -
p_des [关节1位置, 关节2位置, 关节3位置, 关节4位置]
v_des [关节1速度, 关节2速度, 关节3速度, 关节4速度]
kp [关节1kp, 关节2kp, 关节3kp, 关节4kp]
kd [关节1kd, 关节2kd, 关节3kd, 关节4kd]
t_ff [关节1力矩, 关节2力矩, 关节3力矩, 关节4力矩]
mode -

底盘驱动接口

该接口是用于底盘状态反馈的ROS软件包，定义了多个话题以报告底盘电机的状态。以下是每个话题及其相关消息类型的详细描述：

话题名称 I/O 描述 消息类型
/hdas/feedback_chassis Output 底盘轮毂和转向电机反馈 sensor_msgs::JointState
/hdas/feedback_status_chassis Output 底盘状态反馈 hdas_msg::feedback_status
/motion_control/control_chassis Input 底盘轮毂和转向电机控制 hdas_msg::motor_control

针对以上话题的具体字段及其详细描述如下表所示：

话题名称 字段 描述
/hdas/feedback_chassis header 标准消息头
position [左前轮角度, 右前轮角度, 后轮角度]
velocity [左前轮线速度, 右前轮线速度, 后轮线速度]
[0.0, 0.0, 0.0]
effort [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
/hdas/feedback_status_chassis header 标准消息头
name_id 关节名称
errors 包含错误码和对应的错误描述
/motion_control/control_chassis header 标准消息头
name -
p_des [左前轮角度, 右前轮角度, 后轮角度]
v_des [左前轮线速度, 右前轮线速度, 后轮线速度]
kp -
kd -
t_ff -
mode -

相机接口

话题名称 I/O 描述 消息类型
/hdas/camera_chassis_front_left/rgb/compressed Output 底盘左前相机RGB压缩图 sensor_msgs::CompressedImage
/hdas/camera_chassis_front_right/rgb/compressed Output 底盘右前相机RGB压缩图 sensor_msgs::CompressedImage
/hdas/camera_chassis_left/rgb/compressed Output 底盘左相机RGB压缩图 sensor_msgs::CompressedImage
/hdas/camera_chassis_right/rgb/compressed Output 底盘右相机RGB压缩图 sensor_msgs::CompressedImage
/hdas/camera_chassis_rear/rgb/compressed Output 底盘后相机RGB压缩图 sensor_msgs::CompressedImage
/hdas/camera_wrist_left/color/image_raw/compressed Output 右腕相机RGB压缩图（如存在） sensor_msgs::CompressedImage
/hdas/camera_wrist_right/color/image_raw/compressed Output 左腕相机RGB压缩图（如存在） sensor_msgs::CompressedImage
/hdas/camera_head/left_raw/image_raw_color/compressed Output 头部相机RGB压缩图（如存在） sensor_msgs::CompressedImage
/hdas/camera_wrist_left/aligned_depth_to_color/image_raw Output 左腕相机深度图（如存在） sensor_msgs::Image
/hdas/camera_wrist_right/aligned_depth_to_color/image_raw Output 右腕相机深度图（如存在） sensor_msgs::Image
/hdas/camera_head/depth/depth_registered Output 头部相机深度图（如存在） sensor_msgs::Image

针对以上话题的具体字段及其详细描述如下表所示：

话题名称 字段 描述
/hdas/camera_chassis_front_left/rgb/compressed header 标准消息头
format JPEG
data 图像数据
/hdas/camera_chassis_front_right/rgb/compressed header 标准消息头
format JPEG
data 图像数据
/hdas/camera_chassis_left/rgb/compressed header 标准消息头
format JPEG
data 图像数据
/hdas/camera_chassis_right/rgb/compressed header 标准消息头
format JPEG
data 图像数据
/hdas/camera_chassis_rear/rgb/compressed header 标准消息头
format JPEG
data 图像数据
/hdas/camera_wrist_left/color/image_raw/compressed header 标准消息头
format JPEG
data 图像数据
/hdas/camera_wrist_right/color/image_raw/compressed header 标准消息头
format JPEG
data 图像数据
/hdas/camera_head/left_raw/image_raw_color/compressed header 标准消息头
format JPEG
data 图像数据
/hdas/camera_wrist_left/aligned_depth_to_color/image_raw header 标准消息头
height 取决于设置
width 取决于设置
encoding 16UC1
is_bigendian 0
step 根据配置决定
data 图像数据
/hdas/camera_wrist_right/aligned_depth_to_color/image_raw header 标准消息头
height 取决于设置
width 取决于设置
encoding 16UC1
is_bigendian 0
step 根据配置决定
data 图像数据
/hdas/camera_head/depth/depth_registered header 标准消息头
height 取决于设置
width 取决于设置
encoding 32FC1
is_bigendian 0
step 取决于设置
data 图像数据

激光雷达接口

话题名称 I/O 描述 消息类型
/hdas/lidar_chassis_left Output 雷达点云图 sensor_msgs::PointCloud2

针对以上话题的具体字段及其详细描述如下表所示：

话题名称 字段 描述
/hdas/lidar_chassis_left header 标准消息头
fields 雷达数据

IMU 接口

话题名称 I/O 描述 消息类型
/hdas/imu_chassis Output 底盘IMU反馈 sensor_msgs::Imu
/hdas/imu_torso Output 躯干IMU反馈 sensor_msgs::Imu

针对以上话题的具体字段及其详细描述如下表所示：

话题名称 字段 描述
/hdas/imu_chassis header 标准消息头
orientation.x 四元数x
orientation.y 四元数y
orientation.z 四元数z
orientation.w 四元数w
angular_velocity.x 陀螺仪角速度x
angular_velocity.y 陀螺仪角速度y
angular_velocity.z 陀螺仪角速度z
linear_acceleration.x 线加速度x
linear_acceleration.y 线加速度y
linear_acceleration.z 线加速度z
/hdas/imu_torso header 标准消息头
orientation.x 四元数x
orientation.y 四元数y
orientation.z 四元数z
orientation.w 四元数w
angular_velocity.x 陀螺仪角速度x
angular_velocity.y 陀螺仪角速度y
angular_velocity.z 陀螺仪角速度z
linear_acceleration.x 线加速度x
linear_acceleration.y 线加速度y
linear_acceleration.z 线加速度z

BMS 接口

话题名称 I/O 描述 消息类型
/hdas/bms Output 电池BMS消息 hdas_msg::bms

针对以上话题的具体字段及其详细描述如下表所示：

话题名称 字段 描述
/hdas/bms header 标准消息头
voltage 电压 (V)
current 电流 (I)
capital 电量剩余 (%)

遥控器接口

话题名称 I/O 描述 消息类型
/controller Output 遥控器信号 hdas_msg::controller_signal_stamped

针对以上话题的具体字段及其详细描述如下表所示：

话题名称 字段 描述
/controller header 标准消息头
data.left_x_axis 左摇杆x方向
data.left_y_axis 左摇杆y方向
data.right_x_axis 右摇杆x方向
data.right_y_axis 右摇杆y方向
mode 2: 遥控器控制 (底盘)
5: 上位机控制 (底盘)

运动控制接口

  • 关节控制：该节点控制R1躯干和手臂的每个关节。

  • 手臂姿态控制：该节点控制手臂运动至目标末端执行器（ee）的坐标系。

  • 躯干速度控制：该节点控制躯干运动至目标浮动基座坐标系。

  • 底盘控制：该节点使用矢量控制来控制R1底盘，允许同时发送x、y和w三个方向的速度命令。

  • 姿态估计：该节点接收来自HDAS的关节角反馈，并计算出对应三个坐标系的反馈。

关节控制

R1关节控制节点负责控制R1躯干和手臂的每个关节，总共有16个关节。

可通过以下命令启动：

source {your_download_path}install/setup.bash
roslaunch mobiman r1_jointTrackerdemo.launch
# For R1 Lite 
# roslaunch mobiman pi_jointTrackerdemo.launch

该文件将启动 r1_jointTracker_demo_node，该节点是负责控制每个关节的主要节点。

接口信息如下：

Topic Name I/O Description Message Type
/motion_target/target_joint_state_arm_left Input 左臂各关节的目标位置 sensor_msgs::JointState
/motion_target/target_joint_state_arm_right Input 右臂各关节的目标位置 sensor_msgs::JointState
/motion_target/target_joint_state_torso Input 躯干各关节目标位置 sensor_msgs::JointState
/hdas/feedback_arm_left Input 左臂关节反馈 sensor_msgs::JointState
/hdas/feedback_arm_right Input 右臂关节反馈 sensor_msgs::JointState
/hdas/feedback_torso Input 躯干关节反馈 sensor_msgs::JointState
/motion_control/control_arm_left Output 左臂电机控制 hdas_msg::motor_control
/motion_control/control_arm_right Output 右臂电机控制 hdas_msg::motor_control
/motion_control/control_torso Output 躯干电机控制 hdas_msg::motor_control

针对以上话题的具体字段及其详细描述如下表所示：

话题名称 字段 描述
/motion_target/target_joint_state_arm_left
/motion_target/target_joint_state_arm_right		 position 这是一个包含六个元素的向量，代表每个关节的六个目标位置。
velocity 这是一个包含六个元素的向量，代表每个关节在运动过程中的最大速度。最大速度如下：{3, 3, 3, 5, 5, 5}。 加速度和加加速度限制设置为速度限制的1.5倍。
/motion_target/target_joint_state_torso position 这是一个包含四个元素的向量，代表每个关节的四个目标位置。
velocity 这是一个包含四个元素的向量，代表每个关节在运动过程中的速度。最大速度如下：{1.5, 1.5, 1.5, 1.5}。 加速度和加加速度限制设置为速度限制的1.5倍。
/hdas/feedback_arm_left - 请参考手臂驱动接口
/hdas/feedback_arm_right - 请参考手臂驱动接口
/hdas/feedback_torso - 请参考躯干驱动接口
/motion_control/control_arm_left - 请参考手臂驱动接口
/motion_control/control_arm_right - 请参考手臂驱动接口
/motion_control/control_torso - 请参考躯干驱动接口

高跟随模式的关节控制

roslaunch mobiman r1_jointTrackerdemo_fast.launch
注意：高跟随模式需要上游提前对关节加速度进行约束和规划。如果直接调用高跟随模式接口来跟随较远的关节角度，可能会触发电机保护机制。以下是推荐的关节加速度和速度：

关节 速度限制 加速度限制
双臂关节 [3,3,3,5,5,5] rad/s [5,5,5,5,5,5] rad/s²
躯干关节 [1,1,1,1] rad/s [1.5, 1.5, 1.5, 1.5] rad/s²

禁用躯干控制功能

roslaunch mobiman r1_jointTrackerdemo_disable_torso.launch;

roslaunch mobiman r1_jointTrackerdemo_fast_disable_torso.launch
注意：此功能应与躯干速度控制接口一起使用。躯干速度控制接口将直接调用电机控制接口，因此应禁用躯干关节控制。

手臂姿态控制

R1手臂姿态控制是一个用于控制手臂移动到目标末端执行器（ee）坐标帧的ROS软件包。它主要包括两个launch文件,分别对应左臂的姿态控制和右臂的姿态控制, 可以通过以下命令启动

source {your_download_path}/install/setup.bash
roslaunch mobiman r1_left_arm_relaxed_ik_mit.launch
roslaunch mobiman r1_right_arm_relaxed_ik_mit.launch

请注意：

  • 当双臂姿态控制器启动后，还是需要将关节控制节点启动，原因是姿态控制是根据目标ee姿态不断解算出目标关节角下发给/motion_target/target_joint_state_arm_left和/motion_target/target_joint_state_arm_right。 
    source {your_download_path}/install/setup.bash
roslaunch mobiman r1_jointTrackerdemo.launch

上述topic 启动后，需要一直发送，示例如下： 

# 右臂：
rostopic pub /motion_target/target_pose_arm_right geometry_msgs/PoseStamped "header:
  seq: 0
  stamp:
    secs: 0
    nsecs: 0
  frame_id: ''
pose:
  position:
    x: 0.277
    y: -0.675
    z: -0.303
  orientation:
    x: 0.0
    y: 0.0
    z: -0.047
    w: 0.999"  -r 10


# 左臂：
rostopic pub /motion_target/target_pose_arm_left geometry_msgs/PoseStamped "header:
  seq: 0
  stamp:
    secs: 0
    nsecs: 0
  frame_id: ''
pose:
  position:
    x: 0.373
    y: 0.637
    z: 0.303
  orientation:
    x: 0.0
    y: 0.0
    z: -0.028
    w: 1.0"  -r 10
上述示例末端位姿如图：

R1_arm_pose_control_eg_cn

  • 当双臂姿态控制器启动后，左右双臂将自动调整至左图所示的状态。请确保将R1置于双臂自然垂下的位置，以避免因运动角度过大导致初始化失败。
  • 当前末端姿态控制的相对位姿是URDF中gripper_link相对于torso_link4的姿态转换。以左臂为例，这是左臂left_gripper_link坐标系相对于torso_link4坐标系的相对关系，包含了x、y、z的偏移量以及orientation对应的旋转偏移，如右图所示：

R1_arm_pose_control

接口信息如下所示：

话题名称 I/O 描述 消息类型
/motion_target/pose_target_arm_left Input 左臂目标姿态 geometry_msgs::PoseStamped
/motion_target/pose_target_arm_right Input 右臂目标姿态 geometry_msgs::PoseStamped
/hdas/feedback_arm_left Input 左臂关节反馈 sensor_msgs::JointState
/hdas/feedback_arm_right Input 右臂关节反馈 sensor_msgs::JointState
/motion_target/target_joint_state_arm_left Output 左臂关节目标 sensor_msgs::JointState
/motion_target/target_joint_state_arm_right Output 右臂关节目标 sensor_msgs::JointState

针对以上话题的具体字段及其详细描述如下表所示：

话题名称 字段 描述
/motion_target/pose_target_arm_left
/motion_target/pose_target_arm_right		 header 标准消息头
pose.position.x X轴偏移
pose.position.y Y轴偏移
pose.position.z Z轴偏移
pose.orientation.x 旋转四元数
pose.orientation.y 旋转四元数
pose.orientation.z 旋转四元数
pose.orientation.w 旋转四元数
/hdas/feedback_arm_left
/hdas/feedback_arm_right		 - 请参考手臂驱动接口
/motion_target/target_joint_state_arm_left
/motion_target/target_joint_state_arm_right		 - 请参考手臂驱动接口

躯干速度控制

R1躯干速度控制是一个用于控制躯干移动到目标浮动基座坐标帧的ROS软件包。它可以通过以下命令启动：

source {your_download_path}install/setup.bash
roslaunch mobiman torso_speed_control_hard.launch
# 注意，此命令不能与 joint_tracker 同时执行。
# 如果需要同时使用 joint_tracker，请改用 joint_tracker_disable_torso。
# 详细信息可在关节控制小节查询。

R1_torso_speed_control

该控制节点如图所显示，表示torsolink3相对base_link的的速度控制，其方向与base_link的frame相同。其中，

  • v_x是torso frame相对于base_link的x方向的速度（最大速度是0.2m/s），正值表示基于base_link向前，负值表示向后；
  • v_z是torso frame相对于base_link的z方向的速度（最大速度是0.2m/s），方向正值表示基于base_link向上，负值表示向下。

该接口如下所示。

话题名称 I/O 描述 消息类型
/motion_target/target_joint_state_torso Input 目标躯干各关节位置 sensor_msgs::JointState
/motion_target/target_speed_torso Input 浮动基座框架的目标速度 Geometry_msgs::Twist::ConstPtr
/hdas/feedback_torso Input 躯干关节反馈 sensor_msgs::JointState
/motion_control/control_torso Output 躯干关节控制 hdas_msg::motor_control

针对以上话题的具体字段及其详细描述如下表所示：

话题名称 字段 描述
/motion_target/target_speed_torso target_speed.v_x = msg->linear.x; 躯干笛卡尔空间x方向线速度，[-0.2,0.2]m/s；
-0.1≤x≤0.2
target_speed.v_z = msg->linear.z; 躯干笛卡尔空间z方向线速度，[-0.2,0.2]m/s；
0.1≤z≤0.7
/hdas/feedback_torso - 请参考躯干驱动接口

底盘控制

底盘控制是一个矢量控制节点，允许同时发送三个方向的速度命令：x、y 和w。

可通过以下命令启动：

roslaunch mobiman r1_chassis_control.launch

该文件将启动两个节点：chassis_control_node 和 r1_control_manager。chassis_control_node负责R1底盘的速度控制。其接口如下所示：

话题名称 I/O 描述 消息类型
/motion_target/target_speed_chassis Input 底盘的目标速度，包括vx, vy and omega. geometry_msgs::Twist
/motion_target/chassis_acc_limit Input 底盘的加速度限制，最大值分别为2.5, 1.0, 1.0 geometry_msgs::Twist
/motion_target/brake_mode Input 发出底盘是否进入制动模式的指令。如果处于制动模式，当速度为0时，底盘将通过将车轮转动一定角度来锁定自身。 std_msgs::Bool
/hdas/feedback_chassis Input R1底盘控制订阅此话题并控制底盘的目标速度。 sensor_msgs::JointState
/motion_control/control_chassis Output R1底盘控制发布此话题以控制电机。 hdas_msg::motor_control

针对以上话题的具体字段及其详细描述如下表所示：

消息名称 字段 描述
/motion_target/target_speed_chassis header 标准消息头
linear 线速度
.x 线速度x, 范围 (-1.5, 1.5) m/s
.y 线速度y, 范围 (-1.5, 1.5) m/s
angular 角速度
.z 角速度, 范围 (-3 - 3) rad/s
/motion_target/chassis_acc_limit header 标准消息头
linear 线速度
.x 加速度限制x, 范围 (-2.5, 2.5) m/s²
.y 加速度限制y, 范围 (-1.0, 1.0) m/s²
angular 角速度
.z 角速度限制, 范围 (-3, 3) rad/s²
/motion_target/brake_mode data 布尔值
进入刹车模式：True
退出刹车模式：False
/hdas/feedback_chassis - 请参考底盘驱动接口
/motion_control/control_chassis - 请参考底盘驱动接口

姿态估计

在底盘控制的launch文件中，还会启动一个名为eepose_pub_node的节点。该节点接收来自HDAS的关节角反馈，并计算出对应于三个坐标系的反馈。eepose_pub_node定义了三个坐标帧：基座连接框架（Base Link Frame，左图）、浮动基座框架（Floating Base Frame，中图）和末端执行器姿态框架（End-Effector Pose Frame，右图）。

R1_joint_control

接口信息如下:

话题名称 I/O 描述 消息类型
/motion_control/pose_ee_arm_left Output 从浮动基座变换到左端执行器姿态 geometry_msgs::PoseStamped
/motion_control/pose_ee_arm_right Output 从浮动基座变换到右端执行器姿态 geometry_msgs::PoseStamped
/motion_control/pose_floating_base Output 从基座链接变换到浮动基座 geometry_msgs::PoseStamped
/hdas/feedback_arm_right Input 左臂电机反馈 sensor_msgs::JointState
/hdas/feedback_arm_left Input 右臂电机反馈 sensor_msgs::JointState
/hdas/feedback_torso Input 躯干电机反馈 sensor_msgs::JointState

针对以上话题的具体字段及其详细描述如下表所示：

话题名称 字段 描述
/motion_control/pose_ee_arm_right
/motion_control/pose_ee_arm_left
/motion_control/pose_floating_base		 position 平移信息
pose.position.x X轴偏移
pose.position.y Y轴偏移
pose.position.z Z轴偏移
orientation 旋转信息
pose.orientation.x 旋转四元数
pose.orientation.y 旋转四元数
pose.orientation.z 旋转四元数
pose.orientation.w 旋转四元数