Initial commit: extract M20 sim2real into ROS1 invicta project
Source: DeepRoboticsLab/sdk_deploy (BSD-3-Clause), M20 sim2real part only. Packages: - invicta_msgs: JointCommandArray, MotionCommand, MotionState; standard messages (sensor_msgs/JointState, sensor_msgs/Imu) elsewhere; shared motion value defs. - invicta_deploy: state machine (idle/standup/rl/joint-damping/lie-down), ONNX policy with per-gait models, auto RL entry after standup with readiness checks, safety loop, gait->model table; no protocol/network code. - invicta_remote: vendor basic_server protocol (UDP + JSON, proto.md V1.2.0), header/MsgId, 0xE0xx error codes, motion/usage-mode/gait translation, telemetry 1002/3,4,5,6 (2/10/2/2 Hz); publishes /motion_command, subscribes /motion_state. - invicta_hardware: joint/IMU bridge skeleton (SocketCAN I/O is TODO), publishes sensor_msgs/JointState + sensor_msgs/Imu, consumes /joint_commands. Verified on ROS Noetic (Ubuntu 20.04) in docker: clean catkin_make, 200 Hz joint state / 50 Hz RL commands / 20 Hz state report, protocol conformance (MotionParam, gait preconditions, error codes), telemetry push, and end-to-end switching of two policy models with a real remote controller.
This commit is contained in:
@@ -0,0 +1,26 @@
|
||||
cmake_minimum_required(VERSION 3.0.2)
|
||||
project(invicta_msgs)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
message_generation
|
||||
std_msgs
|
||||
)
|
||||
|
||||
add_message_files(
|
||||
FILES
|
||||
JointCommandArray.msg
|
||||
MotionCommand.msg
|
||||
MotionState.msg
|
||||
)
|
||||
|
||||
generate_messages(
|
||||
DEPENDENCIES
|
||||
std_msgs
|
||||
)
|
||||
|
||||
# 除了生成的消息，本包还导出 control_word.h 这个纯头文件，
|
||||
# 让 invicta_deploy 与 invicta_hardware 共用同一套控制字常量。
|
||||
catkin_package(
|
||||
INCLUDE_DIRS include
|
||||
CATKIN_DEPENDS message_runtime std_msgs
|
||||
)
|
||||
@@ -0,0 +1,55 @@
|
||||
# invicta_msgs
|
||||
|
||||
消息与共享值域。原则：**能复用标准消息就复用，只自定义标准消息表达不了的内容。**
|
||||
|
||||
## 消息清单
|
||||
|
||||
| 话题（默认） | 消息类型 | 来源 | 说明 |
|
||||
|---|---|---|---|
|
||||
| `/joint_states` | `sensor_msgs/JointState` | **标准** | 关节状态：`name` / `position[rad]` / `velocity[rad/s]` / `effort[Nm]` |
|
||||
| `/imu/data` | `sensor_msgs/Imu` | **标准** | IMU：`orientation`（四元数，rad）、`angular_velocity[rad/s]`、`linear_acceleration[m/s²]` |
|
||||
| `/joint_commands` | `invicta_msgs/JointCommandArray` | **自定义** | 关节指令：目标角/角速度/前馈力矩/`kp`/`kd`/`control_word` |
|
||||
| `/motion_command` | `invicta_msgs/MotionCommand` | **自定义** | 协议节点 → RL 节点：运动状态/步态/使用模式/轴指令 |
|
||||
| `/motion_state` | `invicta_msgs/MotionState` | **自定义** | RL 节点 → 协议节点：实际运动状态/步态等 |
|
||||
|
||||
自定义消息只有 3 条，原因：
|
||||
|
||||
- `JointCommandArray`：标准消息无法表达 `kp`/`kd`/前馈力矩/控制字（`sensor_msgs/JointState` 只有 position/velocity/effort，`trajectory_msgs/JointTrajectory` 也没有增益）。
|
||||
- `MotionCommand` / `MotionState`：是节点间的指令/状态接口，字段与厂家协议一一对应，不适合塞进 `std_msgs`。
|
||||
|
||||
## 共享值域：`include/invicta_msgs/motion_defs.h`
|
||||
|
||||
`MotionParam`、`GaitParam`、`UsageMode` 两个节点都要用，统一放在这里（纯头文件，随包导出）：
|
||||
|
||||
| MotionParam | 含义 | GaitParam | 含义 | UsageMode | 含义 |
|
||||
|---|---|---|---|---|---|
|
||||
| 0 | 空闲 | `0x1001` | 基础(平地) | 0 | 常规 |
|
||||
| 1 | 站立 | `0x1003` | 楼梯 | 1 | 导航 |
|
||||
| 2 | 关节阻尼/软急停 | `0x3002` | 平地(敏捷) | 2 | 辅助 |
|
||||
| 3 | 开机阻尼 | `0x3003` | 楼梯(敏捷) | | |
|
||||
| 4 | 趴下 | | | | |
|
||||
| 17 | RL 控制 | | | | |
|
||||
|
||||
`control_word.h` 同样是导出的纯头文件，定义 `JointCommandArray::control_word` 的取值（`1` 关闭使能、`2` 使能、`3` 正常控制、`4` 清错、`5` 请求状态）。
|
||||
|
||||
## 对照上游 M20（ROS2 drdds）
|
||||
|
||||
| M20 | invicta | 变化 |
|
||||
|---|---|---|
|
||||
| `/JOINTS_DATA` `drdds/JointsData` | `/joint_states` `sensor_msgs/JointState` | 换标准消息；丢掉 `motion_temp`/`driver_temp`/`status_word`（见下） |
|
||||
| `/IMU_DATA` `drdds/ImuData` | `/imu/data` `sensor_msgs/Imu` | 换标准消息；`rpy` 由**度**改为四元数 + 弧度 |
|
||||
| `/JOINTS_CMD` `drdds/JointsDataCmd` | `/joint_commands` `JointCommandArray` | 控制字重新定义（平台无关） |
|
||||
| `/BATTERY_DATA` `drdds/BatteryData` | 无 | RL 层不需要；若做协议状态上报，由 `invicta_hardware` 用标准消息提供 |
|
||||
| `drdds/GamepadData` | 无 | 不需要：手柄走 UDP，在 `invicta_remote` 里 |
|
||||
| — | `/motion_command` / `/motion_state` | 新增：协议节点与 RL 节点之间的接口 |
|
||||
|
||||
## 被删掉的字段与影响
|
||||
|
||||
`sensor_msgs/JointState` 放不下关节温度与驱动器状态字，相对 M20 少了两条保护：
|
||||
|
||||
| M20 字段 | 用途 | 现在的行为 |
|
||||
|---|---|---|
|
||||
| `motion_temp` | 电机过温 >100 ℃ 告警 | 温度接口返回常量，检查恒不触发 |
|
||||
| `status_word` | 驱动器报错判断（`!=1` 即报错） | 状态字接口返回全 1，检查恒不触发 |
|
||||
|
||||
要恢复就加一条**独立低频诊断话题**（例如 1 Hz 的 `JointDiagnostics`），不要塞回 200 Hz 控制路径。协议里的设备状态上报（`1002/5`）需要的温度/电池数据，也应走 `invicta_hardware` → 协议节点这条线。
|
||||
@@ -0,0 +1,23 @@
|
||||
/**
|
||||
* @file control_word.h
|
||||
* @brief JointCommandArray::control_word 的取值
|
||||
*
|
||||
* M20 原厂驱动器用 1/2/4/17/23 这套私有控制字（见上游 dds_types.h 的 CommandIndex），
|
||||
* 那是驱动器协议细节，不属于 RL 层的语义。这里重新定义为平台无关的最小集合，
|
||||
* 由 invicta_hardware 负责映射到具体电机协议；不认识的控制字应当被桥接节点忽略。
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
namespace invicta_msgs {
|
||||
|
||||
enum JointControlWord : uint16_t {
|
||||
kDisable = 1, ///< 关闭使能，电机自由（松腿）
|
||||
kEnable = 2, ///< 使能并保持当前关节角（增益取 kp/kd）
|
||||
kMotorControl = 3, ///< 正常控制：tau = kp*(q_des - q) + kd*(dq_des - dq) + tau_ff
|
||||
kErrorReset = 4, ///< 清除驱动错误
|
||||
kGetStatus = 5, ///< 请求驱动器状态（诊断用，当前 RL 层不主动发）
|
||||
};
|
||||
|
||||
} // namespace invicta_msgs
|
||||
@@ -0,0 +1,70 @@
|
||||
/**
|
||||
* @file motion_defs.h
|
||||
* @brief 运动相关的值域定义，供 invicta_remote（协议节点）与 invicta_deploy（RL 节点）共用
|
||||
*
|
||||
* 取值依据厂家协议 proto.md（山猫 M20 basic_server V1.2.0）§2.2-2.4 与 §3.1。
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
namespace invicta_msgs {
|
||||
|
||||
/// 运动状态（协议 MotionParam / BasicStatus.MotionState）
|
||||
enum MotionParam : uint8_t {
|
||||
kMotionIdle = 0, ///< 空闲
|
||||
kMotionStanding = 1, ///< 站立
|
||||
kMotionJointDamping = 2, ///< 关节阻尼 / 软急停
|
||||
kMotionBootDamping = 3, ///< 开机阻尼
|
||||
kMotionLieDown = 4, ///< 趴下
|
||||
kMotionRlControl = 17, ///< RL 控制
|
||||
};
|
||||
|
||||
/// 步态（协议 GaitParam）。**每个步态对应一个 RL 模型**
|
||||
enum GaitParam : uint32_t {
|
||||
kGaitBasic = 0x1001, ///< 基础（标准运动模式，平地）
|
||||
kGaitStairs = 0x1003, ///< 楼梯（标准运动模式）
|
||||
kGaitFlatAgile = 0x3002, ///< 平地（敏捷运动模式）
|
||||
kGaitStairsAgile = 0x3003, ///< 楼梯（敏捷运动模式）
|
||||
};
|
||||
|
||||
/// 使用模式（协议 Mode）
|
||||
enum UsageMode : uint8_t {
|
||||
kUsageNormal = 0, ///< 常规：支持轴指令 2/21
|
||||
kUsageNavigate = 1, ///< 导航：支持速度指令 2/25 与导航任务
|
||||
kUsageAux = 2, ///< 辅助
|
||||
};
|
||||
|
||||
inline const char* MotionParamName(uint8_t v) {
|
||||
switch (v) {
|
||||
case kMotionIdle: return "空闲";
|
||||
case kMotionStanding: return "站立";
|
||||
case kMotionJointDamping: return "关节阻尼";
|
||||
case kMotionBootDamping: return "开机阻尼";
|
||||
case kMotionLieDown: return "趴下";
|
||||
case kMotionRlControl: return "RL控制";
|
||||
default: return "未知";
|
||||
}
|
||||
}
|
||||
|
||||
inline const char* GaitName(uint32_t v) {
|
||||
switch (v) {
|
||||
case kGaitBasic: return "基础(平地)";
|
||||
case kGaitStairs: return "楼梯";
|
||||
case kGaitFlatAgile: return "平地(敏捷)";
|
||||
case kGaitStairsAgile: return "楼梯(敏捷)";
|
||||
default: return "未知";
|
||||
}
|
||||
}
|
||||
|
||||
/// 协议里定义过的步态值（判断「是不是合法枚举」，不等于「我们支不支持」）
|
||||
inline bool IsKnownGait(uint32_t v) {
|
||||
return v == kGaitBasic || v == kGaitStairs || v == kGaitFlatAgile || v == kGaitStairsAgile;
|
||||
}
|
||||
|
||||
inline bool IsKnownMotionParam(int v) {
|
||||
return v == kMotionIdle || v == kMotionStanding || v == kMotionJointDamping || v == kMotionBootDamping ||
|
||||
v == kMotionLieDown || v == kMotionRlControl;
|
||||
}
|
||||
|
||||
} // namespace invicta_msgs
|
||||
@@ -0,0 +1,18 @@
|
||||
# 关节指令（数组式，与 sensor_msgs/JointState 同形，便于对照）。
|
||||
#
|
||||
# 为什么自定义：标准 ROS1 消息里没有能表达 kp / kd / 前馈力矩 / 控制字的类型；
|
||||
# sensor_msgs/JointState 只覆盖 position/velocity/effort，trajectory_msgs/JointTrajectory
|
||||
# 也没有增益。所以本工程只保留这一条自定义消息，其余全部复用 sensor_msgs。
|
||||
#
|
||||
# 数组长度 = 关节数（当前固定 16，与 policy.onnx 绑定），索引顺序即 name 的顺序，
|
||||
# 见 invicta_deploy/README.md「关节顺序」一节。
|
||||
|
||||
std_msgs/Header header
|
||||
|
||||
string[] name # 关节名，与 /joint_states 的 name 一致；顺序即数组索引
|
||||
float64[] position # 目标关节角 [rad]
|
||||
float64[] velocity # 目标关节角速度 [rad/s]
|
||||
float64[] effort # 前馈力矩 [Nm]
|
||||
float64[] kp # 位置增益
|
||||
float64[] kd # 速度增益
|
||||
uint16 control_word # 控制字，全体关节同值；取值见 invicta_msgs/control_word.h
|
||||
@@ -0,0 +1,16 @@
|
||||
# 遥控/上层指令。由 invicta_remote（协议节点）发布，invicta_deploy 订阅。
|
||||
#
|
||||
# 字段语义对齐厂家协议 proto.md（山猫 M20 basic_server V1.2.0）：
|
||||
# Type=2 Cmd=22 → target_state（MotionParam）
|
||||
# Type=2 Cmd=21 → x / y / yaw（轴指令，比例）
|
||||
# Type=2 Cmd=23 → gait（GaitParam）
|
||||
# Type=1101 Cmd=5 → usage_mode
|
||||
|
||||
std_msgs/Header header
|
||||
|
||||
uint8 target_state # 运动状态：0 空闲 · 1 站立 · 2 关节阻尼/软急停 · 3 开机阻尼 · 4 趴下 · 17 RL控制
|
||||
uint32 gait # 步态：0 = 不改变；0x1001 基础 · 0x1003 楼梯 · 0x3002 平地敏捷 · 0x3003 楼梯敏捷
|
||||
uint8 usage_mode # 使用模式：0 常规 · 1 导航 · 2 辅助
|
||||
float32 x # 轴指令：前后速度比例 [-1, 1]，正=前进
|
||||
float32 y # 轴指令：左右速度比例 [-1, 1]，正=左移
|
||||
float32 yaw # 轴指令：偏航角速度比例 [-1, 1]，正=逆时针
|
||||
@@ -0,0 +1,15 @@
|
||||
# 机器人实际状态。由 invicta_deploy 发布（RL 节点最清楚当前处于什么状态），
|
||||
# invicta_remote 订阅它来做协议侧的合法性判断与状态上报（1002/6 基础状态）。
|
||||
#
|
||||
# 与 MotionCommand 的区别：这里是「实际上报」，motion_state 一定是当前真实状态；
|
||||
# MotionCommand 是「请求」，可能被拒绝。
|
||||
|
||||
std_msgs/Header header
|
||||
|
||||
uint8 motion_state # 同 MotionCommand::target_state 值域：0 空闲 · 1 站立 · 2 关节阻尼 · 3 开机阻尼 · 4 趴下 · 17 RL控制
|
||||
uint32 gait # 当前实际步态（GaitParam）
|
||||
uint8 usage_mode # 使用模式：0 常规 · 1 导航 · 2 辅助
|
||||
uint8 charge # 0 空闲 · 1 前往充电桩 · 2 充电中 · 3 退出充电桩 · 4 异常 · 5 在桩未充电
|
||||
uint8 hes # 硬急停：0 未触发 · 1 已触发
|
||||
uint8 sleep # 0 未休眠 · 1 已休眠 · 2 进入休眠中
|
||||
string version # 机型标识
|
||||
@@ -0,0 +1,19 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>invicta_msgs</name>
|
||||
<version>0.1.0</version>
|
||||
<description>
|
||||
消息定义。只包含一条自定义消息 JointCommandArray（标准消息无法表达 kp/kd/前馈力矩/控制字），
|
||||
关节状态与 IMU 直接复用 sensor_msgs/JointState 与 sensor_msgs/Imu，见 README.md。
|
||||
</description>
|
||||
<maintainer email="todo@example.com">TODO</maintainer>
|
||||
<license>BSD-3-Clause</license>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
|
||||
<build_depend>message_generation</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
|
||||
<exec_depend>message_runtime</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
</package>
|
||||
Reference in New Issue
Block a user