Compare commits
2 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
 zeitvex
|
302b6acc64 | ||
|
zeitvex
|
3bf6eb0c4c |
@@ -29,6 +29,7 @@ log/
|
||||
!05_software/real/sim2real/vendored/odin1_imu/build/
|
||||
!05_software/real/sim2real/vendored/odin1_imu/build/libodin1_imu_bridge.so
|
||||
!05_software/real/sim2real/vendored/odin1_imu/lib/*.a
|
||||
!05_software/real/sim2real_v2/vendored/odin1_imu/lib/*.a
|
||||
|
||||
# Training outputs
|
||||
logs/
|
||||
|
||||
@@ -14,6 +14,25 @@
|
||||
| `v0.7.0` | MuJoCo 工具 | 姿态优化、IK 扫描、动力学、MPC 和 GUI 调试工具 |
|
||||
| `v0.8.0` | 后期 Sim2Sim | ONNX 回放、IK/路线检查工具和比赛最终 Rough 策略 |
|
||||
| `v0.8.1` | 导航打点工具 | 地图/航点编辑、路线迭代和抽样 PCD 补充包 |
|
||||
| `v0.9.0` | Python Sim2Real v2 | 反馈新鲜度、Odin odom 诊断、Web 调试和安全监控增强 |
|
||||
| `v0.10.0` | ROS 2/C++ 初版 | 50 Hz C++ 推理、200 Hz CAN 热路径和 ROS 2 系统集成 |
|
||||
|
||||
> 原先临时归档为 `v0.9.0` 的最终 ROS 2/C++ 比赛部署已保存在 `backup/final-ros2-v0.9.0` 分支和 `backup-v0.9.0-ros2-final` 标签中，重排完成后将正式归入 `v1.0.0`。
|
||||
|
||||
## `v0.9.0` 的 Python Sim2Real v2
|
||||
|
||||
- 归档 `real/sim2real_v2` 真机部署版本，保持 `53D -> 16D` 策略观测和动作契约。
|
||||
- 增加电机反馈新鲜度判断、Odin odom 诊断、命令限加速度平滑和 Web 运行时诊断。
|
||||
- 保留 Python 策略运行时、ONNX/PT 模型、MJCF、Odin 接口、Web 工具和安全保护链路。
|
||||
- 排除运行日志、测试日志、临时 XML 和开发交接草稿；后续 ROS 2/C++ 版本另行归档。
|
||||
|
||||
## `v0.10.0` 的 ROS 2/C++ Sim2Real 初版
|
||||
|
||||
- 归档 `real/sim2real_ros2`，将 Python 部署契约迁移到 ROS 2 Humble 与 C++ 运行时。
|
||||
- 保留 53D 观测、16D 动作、50 Hz 策略循环和 200 Hz SocketCAN 电机热路径。
|
||||
- 增加消息接口、硬件桥、策略运行时、命令仲裁、Nav2 配置、Docker 和 Windows Web 调试工具。
|
||||
- 原始快照中的 `src/odin_ros_driver` 为空目录，因此本版本仍需外部 Odin 驱动，不能宣称传感器依赖已自包含。
|
||||
- 保留原始候选 ONNX 文件以记录初版部署试验；排除计划、任务和 walkthrough 草稿。
|
||||
|
||||
## `v0.4.0` 的模型变化
|
||||
|
||||
|
||||
@@ -8,7 +8,9 @@
|
||||
│ └─ rc_mjlab/ # 训练、MJCF、MuJoCo、Sim2Sim 和本地 mjlab 依赖
|
||||
└─ real/
|
||||
├─ ik_real/ # IK 轨迹与早期真机控制
|
||||
└─ sim2real/ # 第一代 Python 策略真机部署
|
||||
├─ sim2real/ # 第一代 Python 策略真机部署
|
||||
├─ sim2real_v2/ # Python Sim2Real v2
|
||||
└─ sim2real_ros2/ # ROS 2/C++ Sim2Real 初版
|
||||
```
|
||||
|
||||
## 数据流
|
||||
@@ -23,12 +25,14 @@ MJCF + mjlab task
|
||||
|
|
||||
+----> Sim2Sim 策略验证
|
||||
|
|
||||
+----> Python Sim2Real ----> 电机 / IMU
|
||||
+----> Python Sim2Real / v2 ----> 电机 / IMU
|
||||
|
|
||||
+----> ROS 2/C++ Sim2Real -----> CAN / IMU / 导航
|
||||
|
||||
IK real --------------------------------> 电机
|
||||
```
|
||||
|
||||
`rc_mjlab` 是自包含工程。训练、MJCF、MuJoCo、Sim2Sim、导航工具和策略权重通过相对路径绑定，因此保留其内部布局，没有为了目录外观拆散。第一代完整闭环见 `v0.3.0`，第一份新版 MJCF 与训练框架见 `v0.4.0`，随机化增强版见 `v0.5.0`，比赛最终训练架构见 `v0.6.0`，后期 MuJoCo 工具集见 `v0.7.0`，后期 Sim2Sim 与比赛 Rough 策略见 `v0.8.0`，完整导航打点工具见 `v0.8.1`。
|
||||
`rc_mjlab` 是自包含工程。训练、MJCF、MuJoCo、Sim2Sim、导航工具和策略权重通过相对路径绑定，因此保留其内部布局，没有为了目录外观拆散。第一代完整闭环见 `v0.3.0`，第一份新版 MJCF 与训练框架见 `v0.4.0`，随机化增强版见 `v0.5.0`，比赛最终训练架构见 `v0.6.0`，后期 MuJoCo 工具集见 `v0.7.0`，后期 Sim2Sim 与比赛 Rough 策略见 `v0.8.0`，完整导航打点工具见 `v0.8.1`，Python Sim2Real v2 对应 `v0.9.0`，ROS 2/C++ Sim2Real 初版对应 `v0.10.0`。
|
||||
|
||||
详细说明见：
|
||||
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
# 第一代真机控制
|
||||
# 真机控制版本演进
|
||||
|
||||
本目录保存 16DOF 轮足机器人的早期真机控制实现。
|
||||
本目录保存 16DOF 轮足机器人从早期 Python 闭环到 ROS 2 部署的真机控制演进。
|
||||
|
||||
## `ik_real`
|
||||
|
||||
@@ -20,6 +20,18 @@
|
||||
|
||||
部署说明见 [`sim2real/README.md`](sim2real/README.md) 与 [`sim2real/DEPLOYMENT.md`](sim2real/DEPLOYMENT.md)。
|
||||
|
||||
## `sim2real_v2`
|
||||
|
||||
Python Sim2Real v2，保留 `53D -> 16D` 策略接口，并增加电机反馈新鲜度、Odin odom 诊断、命令平滑、Web 运行时诊断和安全监控工具。该版本对应重排主线的 `v0.9.0`。
|
||||
|
||||
部署说明见 [`sim2real_v2/README.md`](sim2real_v2/README.md) 与 [`sim2real_v2/DEPLOYMENT.md`](sim2real_v2/DEPLOYMENT.md)。
|
||||
|
||||
## `sim2real_ros2`
|
||||
|
||||
ROS 2/C++ Sim2Real 初版，将策略热路径迁移为 50 Hz C++ 推理和 200 Hz CAN 电机循环，并加入 ROS 2 消息、命令仲裁、Nav2 与统一启动结构。该版本对应重排主线的 `v0.10.0`。
|
||||
|
||||
原始快照没有随工程保存 Odin ROS 2 驱动源码，该依赖边界见 [`sim2real_ros2/README.md`](sim2real_ros2/README.md)。
|
||||
|
||||
## 实机记录
|
||||
|
||||
[](../../06_assets/videos/early_sim2real.mp4)
|
||||
|
||||
@@ -0,0 +1,6 @@
|
||||
build/
|
||||
install/
|
||||
log/
|
||||
.colcon/
|
||||
.vscode/
|
||||
compile_commands.json
|
||||
@@ -0,0 +1,178 @@
|
||||
# ROS2 C++ Sim2Real 运动控制栈 - 部署指南
|
||||
|
||||
本工作区提供了一个自包含、独立的 C++ ROS2 Humble 实现，用于在 Jetson Orin 目标机上部署轮腿四足机器人控制策略。
|
||||
|
||||
---
|
||||
|
||||
## 1. 前提条件与环境
|
||||
|
||||
### 硬件
|
||||
* **目标计算机**：运行 Ubuntu 22.04 LTS 的 Jetson Orin Nano / Orin NX / AGX Orin。
|
||||
* **IMU 传感器**：Odin 集成 IMU，发布至 `/odin1/imu`。
|
||||
* **CAN 总线适配器**：Peak CAN、USB-to-CAN 或板载 SocketCAN 接口，使用 CAN0 和 CAN1。
|
||||
|
||||
### 主机依赖
|
||||
* **操作系统**：Ubuntu 22.04 LTS (Jammy Jellyfish)。
|
||||
* **ROS 2 发行版**：ROS 2 Humble（Desktop-Base 或 ROS-Base）。
|
||||
* **C++ 编译器**：支持 C++17 的 GCC/G++ 9.0+。
|
||||
* **库与 ROS2 包**：
|
||||
* `libyaml-cpp-dev`
|
||||
* `libeigen3-dev`
|
||||
* `libusb-1.0-0-dev`（Odin USB 传感器通信）
|
||||
* `libpcl-dev` 和 `libopencv-dev`（3D 点云与相机处理）
|
||||
* `ros-humble-navigation2` 和 `ros-humble-nav2-bringup`（Nav2 规划器/控制器服务器）
|
||||
* `ros-humble-pointcloud-to-laserscan`（点云转激光扫描，供 AMCL 使用）
|
||||
* `ros-humble-cv-bridge` 和 `ros-humble-pcl-conversions`（Odin 传感器驱动图像与点云处理）
|
||||
* `can-utils`（SocketCAN 验证工具）
|
||||
|
||||
---
|
||||
|
||||
## 2. 本地编译与部署
|
||||
|
||||
按以下步骤在主机系统上编译运行整个栈：
|
||||
|
||||
### 步骤 1：安装系统依赖
|
||||
```bash
|
||||
sudo apt-get update
|
||||
sudo apt-get install -y build-essential cmake can-utils libyaml-cpp-dev libeigen3-dev \
|
||||
libusb-1.0-0-dev libpcl-dev libopencv-dev ros-humble-navigation2 \
|
||||
ros-humble-nav2-bringup ros-humble-pointcloud-to-laserscan \
|
||||
ros-humble-cv-bridge ros-humble-pcl-conversions
|
||||
```
|
||||
|
||||
### 步骤 2：下载 ONNXRuntime C++ SDK
|
||||
策略需要 ONNXRuntime 库来运行推理。必须下载并解压到已知目录：
|
||||
|
||||
```bash
|
||||
# 创建目录
|
||||
sudo mkdir -p /opt/onnxruntime
|
||||
cd /opt
|
||||
|
||||
# 针对 Jetson Orin (ARM64 / aarch64)：
|
||||
sudo wget https://github.com/microsoft/onnxruntime/releases/download/v1.16.3/onnxruntime-linux-aarch64-1.16.3.tgz
|
||||
sudo tar -zxvf onnxruntime-linux-aarch64-1.16.3.tgz --strip-components=1 -C /opt/onnxruntime
|
||||
|
||||
# 或标准桌面仿真 (x86_64 / amd64)：
|
||||
# sudo wget https://github.com/microsoft/onnxruntime/releases/download/v1.16.3/onnxruntime-linux-x64-1.16.3.tgz
|
||||
# sudo tar -zxvf onnxruntime-linux-x64-1.16.3.tgz --strip-components=1 -C /opt/onnxruntime
|
||||
```
|
||||
|
||||
导出 CMake 辅助变量：
|
||||
```bash
|
||||
export ONNXRUNTIME_DIR=/opt/onnxruntime
|
||||
```
|
||||
|
||||
### 步骤 3：构建工作区
|
||||
进入包含 `src/` 的本包根目录，运行 `colcon`：
|
||||
```bash
|
||||
colcon build --merge-install --cmake-args -DCMAKE_BUILD_TYPE=Release
|
||||
```
|
||||
|
||||
### 步骤 4：配置 SocketCAN 接口
|
||||
启动前，以 1 Mbps 波特率激活 CAN 接口：
|
||||
```bash
|
||||
sudo ip link set can0 up type can bitrate 1000000
|
||||
sudo ip link set can1 up type can bitrate 1000000
|
||||
```
|
||||
使用 `ifconfig` 或 `ip link` 验证接口已启动。
|
||||
|
||||
### 步骤 5：启动节点
|
||||
使启动脚本可执行并运行：
|
||||
```bash
|
||||
chmod +x start_sim2real.sh
|
||||
./start_sim2real.sh
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 3. Docker 部署（推荐）
|
||||
|
||||
强烈推荐使用 Docker 隔离依赖，避免 Jetson Orin 上的库版本冲突。
|
||||
|
||||
### 步骤 1：构建镜像
|
||||
确保在 `sim2real_ros2` 目录中（包含 `Dockerfile`）：
|
||||
```bash
|
||||
# 使用标准 docker build：
|
||||
docker build -t sim2real_ros2:latest .
|
||||
|
||||
# 或使用 Docker Compose：
|
||||
docker compose build
|
||||
```
|
||||
|
||||
### 步骤 2：运行容器
|
||||
对于真实硬件部署，容器**必须**共享主机网络栈（用于 ROS2 DDS 和 SocketCAN）并具备线程优先级能力以实现实时调度：
|
||||
|
||||
```bash
|
||||
# 选项 A：手动运行
|
||||
docker run -it \
|
||||
--network host \
|
||||
--privileged \
|
||||
--cap-add=sys_nice \
|
||||
--volume=/dev:/dev \
|
||||
--shm-size=2g \
|
||||
--name sim2real_ros2_run \
|
||||
sim2real_ros2:latest
|
||||
|
||||
# 选项 B：通过 Docker Compose 运行（最简单）
|
||||
docker compose up -d
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 4. 系统拓扑与话题
|
||||
|
||||
控制节点通过标准 ROS 2 DDS 消息与传感器驱动和导航栈交互：
|
||||
|
||||
* **IMU 输入**：订阅 `/odin1/imu`（`sensor_msgs/msg/Imu`）。硬件节点自动执行逆轴旋转（`x_raw = -y_ros`，`y_raw = x_ros`）以重建 RL 策略期望的原始坐标系。
|
||||
* **控制命令**：订阅 `/cmd_vel` 和 `/cmd_vel_stamped`（`geometry_msgs/msg/Twist` / `TwistStamped`），由导航栈或手动键盘节点发布。
|
||||
* **里程计输入**：订阅 `/odom`（`nav_msgs/msg/Odometry`），由 `odom_relay_node` 从 `/odin1/odometry` 中继并重映射帧名后提供。
|
||||
* **急停**：订阅 `/safety/estop`（`std_msgs/msg/Bool`）。发布 `true` 触发软件急停，机器人进入低刚度阻尼刹车。
|
||||
* **状态遥测**：发布 `runtime/state`（`sim2real_interfaces/msg/RuntimeState`），包含当前关节速度、温度、IMU 输出和诊断信息。
|
||||
* **策略目标**：发布 `runtime/target`（`sim2real_interfaces/msg/RuntimeTarget`），包含策略推理输出的目标关节位置。
|
||||
|
||||
### TF 树
|
||||
```
|
||||
odom ──→ base_link （由 odom_relay_node 广播）
|
||||
map ──→ odom （由 AMCL / Odin SLAM 发布，取决于运行模式）
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 5. 集成 ROS 2 导航与传感器驱动
|
||||
|
||||
### USB 设备权限（Odin 传感器）
|
||||
要运行物理 Odin 传感器驱动（`odin_ros_driver`），目标计算机必须具有传感器 USB 接口的读写权限。在主机系统上添加以下 udev 规则：
|
||||
|
||||
```bash
|
||||
# 1. 添加 udev 规则
|
||||
echo 'SUBSYSTEM=="usb", ATTR{idVendor}=="2207", ATTR{idProduct}=="0019", MODE="0666", GROUP="plugdev"' | sudo tee /etc/udev/rules.d/99-odin-usb.rules
|
||||
|
||||
# 2. 重新加载 udev 规则并重新插拔传感器
|
||||
sudo udevadm control --reload
|
||||
sudo udevadm trigger
|
||||
```
|
||||
|
||||
### 集成启动参数
|
||||
统一启动文件 `sim2real_system.launch.py` 支持模块化激活传感器驱动和 Nav2 导航栈：
|
||||
|
||||
* `launch_driver`（默认：`true`）：启动 `odin_ros_driver` 节点以获取 IMU 和点云遥测。
|
||||
* `launch_nav2`（默认：`true`）：启动 ROS2 Navigation2 规划器、控制器、costmap、AMCL 和 pointcloud_to_laserscan。
|
||||
|
||||
#### 1. 完整真实硬件闭环（默认）
|
||||
启动运动控制运行时、物理 CAN 桥接、Odin 传感器驱动和 Nav2 导航：
|
||||
```bash
|
||||
ros2 launch sim2real_bringup sim2real_system.launch.py dry_run:=false launch_driver:=true launch_nav2:=true
|
||||
```
|
||||
|
||||
#### 2. Dry-Run / 仿真航点测试
|
||||
在 dry-run 模式下运行策略运行时和 Nav2 导航（不访问 CAN 总线或物理 USB 传感器，适合测试导航话题路由）：
|
||||
```bash
|
||||
ros2 launch sim2real_bringup sim2real_system.launch.py dry_run:=true launch_driver:=false launch_nav2:=true
|
||||
```
|
||||
|
||||
#### 3. 仅运动控制（无导航）
|
||||
禁用传感器驱动和 Nav2，让运动策略等待 `/cmd_vel` 上的手动速度输入（如键盘遥操作）：
|
||||
```bash
|
||||
ros2 launch sim2real_bringup sim2real_system.launch.py launch_driver:=false launch_nav2:=false
|
||||
```
|
||||
|
||||
@@ -0,0 +1,78 @@
|
||||
# 使用 ROS2 官方 Humble 基础镜像
|
||||
FROM ros:humble-ros-base-jammy
|
||||
|
||||
ENV DEBIAN_FRONTEND=noninteractive
|
||||
|
||||
# 安装 C++ 编译依赖、SocketCAN 调试工具及 Eigen 等核心库
|
||||
RUN apt-get update && apt-get install -y --no-install-recommends \
|
||||
build-essential \
|
||||
cmake \
|
||||
git \
|
||||
can-utils \
|
||||
libyaml-cpp-dev \
|
||||
libeigen3-dev \
|
||||
libusb-1.0-0-dev \
|
||||
libpcl-dev \
|
||||
libopencv-dev \
|
||||
ros-humble-navigation2 \
|
||||
ros-humble-nav2-bringup \
|
||||
ros-humble-pointcloud-to-laserscan \
|
||||
ros-humble-cv-bridge \
|
||||
ros-humble-pcl-conversions \
|
||||
wget \
|
||||
tar \
|
||||
python3-pip \
|
||||
&& rm -rf /var/lib/apt/lists/*
|
||||
|
||||
# ============================================================================
|
||||
# ONNX Runtime — 架构自适应，aarch64 启用 CUDA GPU 加速
|
||||
# ============================================================================
|
||||
# - Orin Nano (aarch64): pip 安装 onnxruntime-gpu（含 CUDA EP）
|
||||
# - x86_64 开发机: 下载 CPU-only 预编译包（GPU 不可用）
|
||||
WORKDIR /opt
|
||||
RUN ARCH=$(uname -m) && \
|
||||
if [ "$ARCH" = "aarch64" ]; then \
|
||||
echo "[ONNX] Installing CUDA-enabled ONNX Runtime for Jetson Orin..." && \
|
||||
pip3 install --no-cache-dir onnxruntime-gpu && \
|
||||
SITE_PKGS=$(python3 -c "import site; print(site.getsitepackages()[0])") && \
|
||||
mkdir -p onnxruntime/include onnxruntime/lib && \
|
||||
cp -r "$SITE_PKGS/onnxruntime/include/"* onnxruntime/include/ && \
|
||||
cp "$SITE_PKGS/onnxruntime/capi/libonnxruntime.so"* onnxruntime/lib/ && \
|
||||
echo "[ONNX] CUDA ONNX Runtime installed."; \
|
||||
else \
|
||||
echo "[ONNX] Installing CPU-only ONNX Runtime for x86_64 dev..." && \
|
||||
wget -q https://github.com/microsoft/onnxruntime/releases/download/v1.16.3/onnxruntime-linux-x64-1.16.3.tgz && \
|
||||
tar -zxf onnxruntime-linux-x64-1.16.3.tgz && \
|
||||
mv onnxruntime-linux-x64-1.16.3 onnxruntime && \
|
||||
rm onnxruntime-linux-x64-1.16.3.tgz; \
|
||||
fi
|
||||
|
||||
ENV ONNXRUNTIME_DIR=/opt/onnxruntime
|
||||
|
||||
# 创建工作空间，将所有 C++ 源码包拷入
|
||||
WORKDIR /sim2real_ws/src
|
||||
COPY src/sim2real_bringup sim2real_bringup
|
||||
COPY src/sim2real_common sim2real_common
|
||||
COPY src/sim2real_hw sim2real_hw
|
||||
COPY src/sim2real_interfaces sim2real_interfaces
|
||||
COPY src/sim2real_runtime sim2real_runtime
|
||||
COPY src/odin_ros_driver odin_ros_driver
|
||||
COPY src/sim2real_nav2 sim2real_nav2
|
||||
|
||||
# 拷贝策略文件与运行脚本
|
||||
WORKDIR /sim2real_ws
|
||||
COPY policies policies
|
||||
COPY start_sim2real.sh start_sim2real.sh
|
||||
RUN chmod +x start_sim2real.sh
|
||||
|
||||
# 编译 ROS2 工作空间
|
||||
SHELL ["/bin/bash", "-c"]
|
||||
RUN source /opt/ros/humble/setup.bash && \
|
||||
colcon build --merge-install --cmake-args -DCMAKE_BUILD_TYPE=Release
|
||||
|
||||
# 拷贝 Docker 入口脚本并设置
|
||||
COPY docker_entrypoint.sh /docker_entrypoint.sh
|
||||
RUN chmod +x /docker_entrypoint.sh
|
||||
|
||||
ENTRYPOINT ["/docker_entrypoint.sh"]
|
||||
CMD ["./start_sim2real.sh"]
|
||||
@@ -0,0 +1,74 @@
|
||||
# ROS 2/C++ Sim2Real 初版
|
||||
|
||||
本目录归档 `real/sim2real_ros2`，对应重排主线的 `v0.10.0`。这是轮腿机器人 Sim2Real 部署栈从 Python 运行时迁移到 ROS 2 + C++ 的第一版系统工程。
|
||||
|
||||
本工程保留当前 `sim2real` 已验证的部署契约，同时将运行时热路径迁移到 C++：
|
||||
|
||||
- `53D` 策略观测契约不变
|
||||
- `16D` 动作契约不变
|
||||
- `50Hz` 策略循环与训练对齐
|
||||
- `200Hz` 电机循环为专用 C++ 热路径
|
||||
- ROS 2 作为导航、TF、诊断和启动管理的系统集成层
|
||||
|
||||
## 工作区布局
|
||||
|
||||
- `src/sim2real_interfaces`
|
||||
硬件桥接与策略运行时共享的 ROS 2 消息定义。
|
||||
- `src/sim2real_common`
|
||||
共享常量、部署契约辅助函数、Mahony 姿态滤波器、站立平衡控制器、安全监控。
|
||||
- `src/sim2real_hw`
|
||||
面向硬件的桥接节点：RobStride CAN 收发、IMU/Odin 数据采集、看门狗、状态发布。
|
||||
- `src/sim2real_runtime`
|
||||
策略运行时节点：`53D→16D` ONNX 推理、命令滤波/仲裁、目标发布。
|
||||
同时包含 `odom_relay_node`（里程计中继与 TF 广播）。
|
||||
- `src/sim2real_nav2`
|
||||
ROS 2 Navigation2 (Nav2) 配置包：参数、启动文件、AMCL、costmap、planner/controller。
|
||||
- `src/sim2real_bringup`
|
||||
统一启动文件与运行时参数配置。
|
||||
- `src/odin_ros_driver`
|
||||
Odin 传感器 ROS 2 驱动（含 IMU、点云、里程计发布）。
|
||||
- `docs`
|
||||
架构说明与迁移计划。
|
||||
|
||||
## 目标架构
|
||||
|
||||
```text
|
||||
Odin / IMU / Odom ---> sim2real_hw ---> sim2real_runtime ---> sim2real_hw
|
||||
| | |
|
||||
v v v
|
||||
RuntimeState RuntimeTarget 电机 CAN 指令
|
||||
| |
|
||||
+-------> 诊断 / 遥测
|
||||
|
||||
Nav2 / cmd_vel ------------------------------> sim2real_runtime
|
||||
(经 odom_relay_node 提供 odom→base_link TF)
|
||||
```
|
||||
|
||||
## 当前状态
|
||||
|
||||
已完成 Phase 0-5 的全部迁移：
|
||||
|
||||
1. ✅ 冻结部署契约（deployment_contract.hpp）
|
||||
2. ✅ ROS 2 包结构搭建
|
||||
3. ✅ 硬件热路径迁移至 C++（SocketCAN 驱动、200Hz 电机循环）
|
||||
4. ✅ ONNX 策略运行时迁移至 C++（50Hz 推理循环）
|
||||
5. ✅ 导航与诊断通过 ROS 2 接入（Nav2 + odom_relay + TF）
|
||||
|
||||
## 契约来源
|
||||
|
||||
迁移过程中以下文件被视为真值源：
|
||||
|
||||
- `sim2real/deployment_manifest.yaml`
|
||||
- `sim2real/interface/motor_mapping.py`
|
||||
- `sim2real/interface/real_io.py`
|
||||
- `sim2real/policy/policy_runner.py`
|
||||
- `sim2real/web/session.py`
|
||||
|
||||
## 注意事项
|
||||
|
||||
- 开发目标为 Linux + ROS 2 Humble，运行于 Jetson Orin / x86_64。
|
||||
- Windows 仅作为编辑环境使用。
|
||||
- 观测顺序、动作缩放、默认站姿、电机映射不得独立修改，
|
||||
除非训练与部署同步更新。
|
||||
- 原始快照中的 `src/odin_ros_driver` 是空目录，本版本仍需要另行提供兼容的 Odin ROS 2 驱动；其源码从后续版本开始随工程归档。
|
||||
- 自研 ROS 包保留原始 `Proprietary` 清单字段，公开发布前仍需统一许可证和维护者信息。
|
||||
@@ -0,0 +1,22 @@
|
||||
version: '3.8'
|
||||
|
||||
services:
|
||||
sim2real_ros2:
|
||||
build:
|
||||
context: .
|
||||
dockerfile: Dockerfile
|
||||
container_name: sim2real_ros2_node
|
||||
runtime: nvidia
|
||||
network_mode: host
|
||||
privileged: true
|
||||
stdin_open: true
|
||||
tty: true
|
||||
environment:
|
||||
- NVIDIA_VISIBLE_DEVICES=all
|
||||
- NVIDIA_DRIVER_CAPABILITIES=compute,utility
|
||||
cap_add:
|
||||
- SYS_NICE
|
||||
shm_size: '2gb'
|
||||
volumes:
|
||||
- /dev:/dev
|
||||
restart: unless-stopped
|
||||
@@ -0,0 +1,12 @@
|
||||
#!/bin/bash
|
||||
set -e
|
||||
|
||||
# Source ROS2 Humble environment
|
||||
source /opt/ros/humble/setup.bash
|
||||
|
||||
# Source workspace install setup if compiled
|
||||
if [ -f "/sim2real_ws/install/setup.bash" ]; then
|
||||
source /sim2real_ws/install/setup.bash
|
||||
fi
|
||||
|
||||
exec "$@"
|
||||
@@ -0,0 +1,102 @@
|
||||
# sim2real_ros2 架构说明
|
||||
|
||||
## 设计目标
|
||||
|
||||
- 保留已验证的 RL 部署契约不变
|
||||
- 将低延迟循环从 Python 迁移至 C++
|
||||
- 暴露标准 ROS 2 接口用于导航和系统集成
|
||||
- 保持安全边界独立于策略正确性
|
||||
|
||||
## 各包职责
|
||||
|
||||
### `sim2real_interfaces`（接口消息）
|
||||
|
||||
定义最小化的运行时消息：
|
||||
|
||||
- `RuntimeState`
|
||||
硬件桥接发布的归一化运行时状态快照
|
||||
- `RuntimeTarget`
|
||||
策略运行时发送至硬件桥接的最新策略目标
|
||||
|
||||
### `sim2real_common`（共享常量）
|
||||
|
||||
存储编译期常量和部署契约辅助：
|
||||
|
||||
- 观测维度和字段布局
|
||||
- 动作维度和轮子索引
|
||||
- 关节顺序和默认站姿
|
||||
- 动作缩放因子和默认循环频率
|
||||
- Mahony 姿态滤波器
|
||||
- 站立平衡控制器
|
||||
- 安全监控器（SafetyMonitor / RuntimeGuard）
|
||||
|
||||
### `sim2real_hw`（硬件桥接）
|
||||
|
||||
拥有硬件侧执行循环和安全边界：
|
||||
|
||||
- RobStride CAN 收发
|
||||
- IMU 与 Odin 状态采集
|
||||
- 电机丢帧检测与保活逻辑（holdover）
|
||||
- 看门狗与阻尼刹车
|
||||
- 发布 `RuntimeState`
|
||||
- 订阅 `RuntimeTarget`
|
||||
- 订阅 `/odom` 里程计数据
|
||||
|
||||
目标热路径：
|
||||
|
||||
- 以 `200Hz` 频率读取状态
|
||||
- 应用最新安全目标
|
||||
- 超时或安全违规时立即停机
|
||||
|
||||
### `sim2real_runtime`（策略运行时）
|
||||
|
||||
拥有策略侧执行：
|
||||
|
||||
- 订阅 `RuntimeState`
|
||||
- 按当前部署契约精确构建 `53D` 观测
|
||||
- 以 `50Hz` 运行 ONNXRuntime 推理
|
||||
- 对 raw_action 做 `[-10, 10]` 安全裁剪
|
||||
- 发布 `RuntimeTarget`
|
||||
- 仲裁命令来源：estop > safety_hold > startup > navigation > web
|
||||
|
||||
同时包含：
|
||||
- `odom_relay_node`：将 `/odin1/odometry` 中继为 `/odom`，帧名 `odin1_base_link` → `base_link`，并广播 TF
|
||||
|
||||
### `sim2real_nav2`（导航配置）
|
||||
|
||||
拥有：
|
||||
|
||||
- Nav2 参数文件（planner、controller、costmap、AMCL、behavior）
|
||||
- Nav2 启动文件（含 AMCL、costmap 生命周期节点、pointcloud_to_laserscan）
|
||||
|
||||
### `sim2real_bringup`（启动管理）
|
||||
|
||||
拥有：
|
||||
|
||||
- 参数文件
|
||||
- 启动组合
|
||||
- 运行时模式选择
|
||||
- 集成 odin_ros_driver、sim2real_nav2 的条件启动
|
||||
|
||||
## 迁移规则
|
||||
|
||||
1. 优化之前先冻结当前契约
|
||||
2. 先迁移传输和循环结构，再调整控制算法
|
||||
3. C++ 运行时未达到影子模式一致性前，保留 Python 运行时可用
|
||||
4. 按段测量延迟：
|
||||
- 观测延迟
|
||||
- 策略推理延迟
|
||||
- 目标传输延迟
|
||||
- 执行器响应延迟
|
||||
|
||||
## 首个里程碑
|
||||
|
||||
首个里程碑不是"机器人在 ROS 2 下行走"，而是：
|
||||
|
||||
1. `sim2real_hw` 发布稳定的 `RuntimeState`
|
||||
2. `sim2real_runtime` 从该状态构建正确的 `53D` 观测
|
||||
3. `sim2real_runtime` 以 `50Hz` 发布 `RuntimeTarget`
|
||||
4. `sim2real_hw` 消费最新目标并执行超时刹车
|
||||
5. `cmd_vel` 可通过 ROS 2 注入而不改变策略契约
|
||||
|
||||
> ✅ 以上里程碑已全部完成。
|
||||
@@ -0,0 +1,80 @@
|
||||
# 迁移计划
|
||||
|
||||
## Phase 1: 硬件核心迁移 ✅ 已完成
|
||||
|
||||
将当前高频热路径从 Python 迁出。
|
||||
|
||||
吸收的源文件：
|
||||
|
||||
- `sim2real/interface/motor_driver.py`
|
||||
- `sim2real/interface/motor_mapping.py`
|
||||
- `sim2real/interface/imu_client.py`
|
||||
- `sim2real/safety/runtime_guard.py`
|
||||
- `sim2real/web/session.py`
|
||||
|
||||
交付物：
|
||||
|
||||
- C++ SocketCAN 电机总线封装
|
||||
- C++ 状态缓存
|
||||
- target 超时保活（timeout hold）
|
||||
- 阻尼刹车 / 急停通路
|
||||
- 发布 `RuntimeState`
|
||||
|
||||
## Phase 2: 策略运行时迁移 ✅ 已完成
|
||||
|
||||
吸收的源文件：
|
||||
|
||||
- `sim2real/policy/policy_runner.py`
|
||||
- `sim2real/interface/real_io.py`
|
||||
- `sim2real/web/session.py`
|
||||
|
||||
交付物：
|
||||
|
||||
- 精确的 `53D` 观测构造器
|
||||
- ONNXRuntime C++ 推理封装
|
||||
- `50Hz` 策略定时器
|
||||
- 命令平滑与来源仲裁
|
||||
- raw_action `[-10, 10]` 安全裁剪
|
||||
- 发布 `RuntimeTarget`
|
||||
|
||||
## Phase 3: ROS 2 系统集成 ✅ 已完成
|
||||
|
||||
参考的源项目：
|
||||
|
||||
- `00_ reference/odin_ros_driver`
|
||||
- `00_ reference/EDULITE_A3/el_a3_ros`
|
||||
- `00_ reference/rl_sar`
|
||||
|
||||
交付物：
|
||||
|
||||
- `cmd_vel` / `cmd_vel_stamped` 输入（支持 Twist 和 TwistStamped）
|
||||
- `odom_relay_node`：里程计中继 + TF 广播（odom → base_link）
|
||||
- 诊断话题
|
||||
- rosbag/foxglove 可观测性
|
||||
|
||||
## Phase 4: 导航集成 ✅ 已完成
|
||||
|
||||
目标：
|
||||
|
||||
- 导航通过 ROS 2 发送身体速度指令
|
||||
- RL 运行时保持为 locomotion 控制器
|
||||
- 看门狗和安全边界始终在导航之下
|
||||
|
||||
规则：
|
||||
|
||||
- 导航绝不直接写电机指令
|
||||
- 策略契约在重新训练前保持不变
|
||||
- 任何新增历史项或里程计项必须版本化
|
||||
|
||||
## 当前状态
|
||||
|
||||
所有 4 个 Phase 已全部完成。以下为已实现的关键组件：
|
||||
|
||||
| 组件 | 节点 | 说明 |
|
||||
|------|------|------|
|
||||
| 硬件桥接 | `sim2real_hw_node` | 200Hz CAN 收发 + IMU + Mahony + 安全 |
|
||||
| 策略运行时 | `sim2real_runtime_node` | 50Hz ONNX 推理 + 53D 观测 + raw_action clip |
|
||||
| 里程计中继 | `odom_relay_node` | /odin1/odometry → /odom + odom→base_link TF |
|
||||
| 导航栈 | Nav2 全套节点 | AMCL + costmap + DWB + Navfn + BT + lifecycle |
|
||||
| 传感器驱动 | `odin_ros_driver` | IMU + 点云 + 里程计原始发布 |
|
||||
| 点云转换 | `pointcloud_to_laserscan` | /odin1/cloud_slam → /scan (供 AMCL 使用) |
|
||||
@@ -0,0 +1,495 @@
|
||||
# sim2real_ros2 遥控器调用说明
|
||||
|
||||
本文档说明如何在 `sim2real_ros2` 中调用已接入的 SBUS UART 遥控器节点，以及执行后系统会产生什么效果。
|
||||
|
||||
## 1. 当前接入关系
|
||||
|
||||
遥控器节点位于：
|
||||
|
||||
```text
|
||||
src/sim2real_runtime/src/remote_uart_node.py
|
||||
```
|
||||
|
||||
该节点读取 SBUS 串口数据，并发布标准 ROS 2 控制话题：
|
||||
|
||||
| 输入 | 输出 | 作用 |
|
||||
|---|---|---|
|
||||
| SBUS UART 遥控器 | `/cmd_vel` | 给策略运行时发送速度命令 |
|
||||
| SBUS CH7 高位 | `/safety/estop` | 触发软件急停 |
|
||||
|
||||
策略节点 `sim2real_runtime_node` 已经订阅 `/cmd_vel` 和 `/safety/estop`，所以遥控器不直接控制电机，而是通过 ROS 2 标准速度接口进入策略控制链路。
|
||||
|
||||
## 2. 通道映射
|
||||
|
||||
通道映射与前一阶段 Python Sim2Real 中的遥控器实现保持一致。
|
||||
|
||||
| 遥控器通道 | ROS 2 输出 | 含义 | 默认最大值 |
|
||||
|---|---|---|---:|
|
||||
| `CH2` | `cmd_vel.linear.x` | 前后速度 `vx` | `0.8 m/s` |
|
||||
| `CH4` | `cmd_vel.linear.y` | 左右速度 `vy` | `0.3 m/s` |
|
||||
| `CH1` | `cmd_vel.angular.z` | 转向角速度 `yaw` | `0.5 rad/s` |
|
||||
| `CH7 HIGH` | `/safety/estop = true` | 软件急停 | - |
|
||||
|
||||
默认方向反转配置：
|
||||
|
||||
| 参数 | 默认值 | 含义 |
|
||||
|---|---:|---|
|
||||
| `remote_invert_vx` | `true` | 反转前后方向 |
|
||||
| `remote_invert_vy` | `false` | 不反转横移方向 |
|
||||
| `remote_invert_yaw` | `true` | 反转转向方向 |
|
||||
|
||||
## 3. 参数位置
|
||||
|
||||
遥控器参数在：
|
||||
|
||||
```text
|
||||
src/sim2real_bringup/config/runtime.yaml
|
||||
```
|
||||
|
||||
当前默认参数：
|
||||
|
||||
```yaml
|
||||
remote_enabled: true
|
||||
remote_port: "/dev/ttyACM0"
|
||||
remote_baudrate: 100000
|
||||
remote_timeout: 0.02
|
||||
remote_axis_deadzone: 50
|
||||
remote_active_threshold: 50
|
||||
remote_axis_full_scale: 660.0
|
||||
remote_max_vx: 0.8
|
||||
remote_max_vy: 0.3
|
||||
remote_max_yaw_rate: 0.5
|
||||
remote_invert_vx: true
|
||||
remote_invert_vy: false
|
||||
remote_invert_yaw: true
|
||||
remote_publish_inactive_zero: true
|
||||
remote_estop_latch: true
|
||||
remote_poll_hz: 50.0
|
||||
```
|
||||
|
||||
如果遥控器串口不是 `/dev/ttyACM0`，需要修改：
|
||||
|
||||
```yaml
|
||||
remote_port: "/dev/ttyUSB0"
|
||||
```
|
||||
|
||||
或改成实际设备路径。
|
||||
|
||||
## 4. 启动前检查
|
||||
|
||||
### 4.1 确认串口存在
|
||||
|
||||
```bash
|
||||
ls /dev/ttyACM* /dev/ttyUSB*
|
||||
```
|
||||
|
||||
如果使用默认配置，应能看到：
|
||||
|
||||
```bash
|
||||
/dev/ttyACM0
|
||||
```
|
||||
|
||||
### 4.2 确认串口权限
|
||||
|
||||
如果节点提示串口权限不足，可以临时执行：
|
||||
|
||||
```bash
|
||||
sudo chmod 666 /dev/ttyACM0
|
||||
```
|
||||
|
||||
更推荐的长期方式是把当前用户加入 `dialout` 组：
|
||||
|
||||
```bash
|
||||
sudo usermod -aG dialout $USER
|
||||
```
|
||||
|
||||
然后重新登录。
|
||||
|
||||
### 4.3 确认 Python serial 依赖
|
||||
|
||||
节点依赖 `pyserial`。如果系统没有安装：
|
||||
|
||||
```bash
|
||||
sudo apt update
|
||||
sudo apt install -y python3-serial
|
||||
```
|
||||
|
||||
## 5. 构建
|
||||
|
||||
如果刚修改过代码或参数，建议重新构建相关包：
|
||||
|
||||
```bash
|
||||
cd /path/to/sim2real_ros2
|
||||
source /opt/ros/humble/setup.bash
|
||||
colcon build --packages-select sim2real_runtime sim2real_bringup --symlink-install --merge-install
|
||||
```
|
||||
|
||||
构建完成后 source 环境：
|
||||
|
||||
```bash
|
||||
source install/setup.bash
|
||||
```
|
||||
|
||||
确认可执行节点存在：
|
||||
|
||||
```bash
|
||||
ros2 pkg executables sim2real_runtime
|
||||
```
|
||||
|
||||
应包含：
|
||||
|
||||
```text
|
||||
sim2real_runtime remote_uart_node.py
|
||||
```
|
||||
|
||||
## 6. 推荐启动方式
|
||||
|
||||
### 6.1 启动完整系统，不启动 Nav2
|
||||
|
||||
这是你当前常用方式：
|
||||
|
||||
```bash
|
||||
cd /path/to/sim2real_ros2
|
||||
source /opt/ros/humble/setup.bash
|
||||
source install/setup.bash
|
||||
ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false
|
||||
```
|
||||
|
||||
默认情况下，`launch_remote:=true`，所以上面命令会同时启动遥控器节点。
|
||||
|
||||
等价完整写法：
|
||||
|
||||
```bash
|
||||
ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false launch_remote:=true
|
||||
```
|
||||
|
||||
### 6.2 不启动遥控器
|
||||
|
||||
如果只想用手动 `ros2 topic pub` 或其他上位机发 `/cmd_vel`，可以关闭遥控器节点：
|
||||
|
||||
```bash
|
||||
ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false launch_remote:=false
|
||||
```
|
||||
|
||||
## 7. 单独启动遥控器节点
|
||||
|
||||
如果系统已经在运行，只想单独测试遥控器节点：
|
||||
|
||||
```bash
|
||||
cd /path/to/sim2real_ros2
|
||||
source /opt/ros/humble/setup.bash
|
||||
source install/setup.bash
|
||||
ros2 run sim2real_runtime remote_uart_node.py --ros-args --params-file src/sim2real_bringup/config/runtime.yaml
|
||||
```
|
||||
|
||||
如果要临时指定串口：
|
||||
|
||||
```bash
|
||||
ros2 run sim2real_runtime remote_uart_node.py --ros-args \
|
||||
--params-file src/sim2real_bringup/config/runtime.yaml \
|
||||
-p remote_port:=/dev/ttyUSB0
|
||||
```
|
||||
|
||||
## 8. 执行后会产生什么效果
|
||||
|
||||
启动以下命令后：
|
||||
|
||||
```bash
|
||||
ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false
|
||||
```
|
||||
|
||||
系统会产生以下效果。
|
||||
|
||||
### 8.1 启动硬件桥接节点
|
||||
|
||||
节点：
|
||||
|
||||
```text
|
||||
/sim2real_hw_node
|
||||
```
|
||||
|
||||
效果：
|
||||
|
||||
1. 打开 `can0` 和 `can1`。
|
||||
2. 如果 `dry_run: false` 且 CAN 初始化成功，会使能 16 个 RobStride 电机。
|
||||
3. 设置电机 MIT 模式。
|
||||
4. 设置电机速度限制和力矩限制。
|
||||
5. 以 `200Hz` 运行硬件读写循环。
|
||||
6. 发布 `/runtime/state`。
|
||||
7. 订阅 `/runtime/target` 执行策略目标。
|
||||
|
||||
### 8.2 启动策略运行节点
|
||||
|
||||
节点：
|
||||
|
||||
```text
|
||||
/sim2real_runtime_node
|
||||
```
|
||||
|
||||
效果：
|
||||
|
||||
1. 加载 ONNX 策略模型。
|
||||
2. 订阅 `/runtime/state`。
|
||||
3. 订阅 `/cmd_vel`。
|
||||
4. 订阅 `/safety/estop`。
|
||||
5. 执行启动站立流程：
|
||||
- `boot_hold`
|
||||
- `startup_soft_hold`
|
||||
- `startup_hold`
|
||||
- `runtime_zero_hold`
|
||||
- `runtime_policy`
|
||||
6. 以 `50Hz` 发布 `/runtime/target`。
|
||||
|
||||
### 8.3 启动遥控器节点
|
||||
|
||||
节点：
|
||||
|
||||
```text
|
||||
/sim2real_remote_uart_node
|
||||
```
|
||||
|
||||
效果：
|
||||
|
||||
1. 打开默认串口 `/dev/ttyACM0`。
|
||||
2. 以 `50Hz` 轮询 SBUS 数据。
|
||||
3. 遥控器摇杆居中时持续发布零速度：
|
||||
|
||||
```text
|
||||
/cmd_vel:
|
||||
linear.x = 0.0
|
||||
linear.y = 0.0
|
||||
angular.z = 0.0
|
||||
```
|
||||
|
||||
4. 推动遥控器时发布非零速度，例如：
|
||||
|
||||
```text
|
||||
/cmd_vel:
|
||||
linear.x = vx
|
||||
linear.y = vy
|
||||
angular.z = yaw
|
||||
```
|
||||
|
||||
5. 当 CH7 打到高位时发布：
|
||||
|
||||
```text
|
||||
/safety/estop: true
|
||||
```
|
||||
|
||||
由于当前 `remote_estop_latch: true`，急停是锁存式行为：一旦 CH7 高位触发，节点会发布急停，并保持内部急停已触发状态。恢复运行通常需要重启系统或手动发布复位信号，并确认机器人安全。
|
||||
|
||||
### 8.4 机器人行为效果
|
||||
|
||||
正常启动后，机器人不会立即按策略行走，而是按阶段执行：
|
||||
|
||||
1. 电机使能。
|
||||
2. 读取当前关节位置。
|
||||
3. 软保持当前姿态。
|
||||
4. 平滑过渡到默认站立姿态。
|
||||
5. 稳定后进入 runtime。
|
||||
6. 遥控器无输入时保持站立平衡，即 `runtime_zero_hold`。
|
||||
7. 遥控器有输入时进入策略控制，即 `runtime_policy`。
|
||||
|
||||
也就是说：
|
||||
|
||||
| 遥控器状态 | 机器人效果 |
|
||||
|---|---|
|
||||
| 摇杆居中 | 站立保持，不主动行走 |
|
||||
| CH2 前后推动 | 前进/后退 |
|
||||
| CH4 左右推动 | 横向移动 |
|
||||
| CH1 左右推动 | 原地转向 |
|
||||
| CH7 高位 | 软件急停，进入安全刹车 |
|
||||
|
||||
## 9. 如何确认遥控器已经生效
|
||||
|
||||
### 9.1 查看节点是否存在
|
||||
|
||||
```bash
|
||||
ros2 node list
|
||||
```
|
||||
|
||||
应看到：
|
||||
|
||||
```text
|
||||
/sim2real_remote_uart_node
|
||||
/sim2real_runtime_node
|
||||
/sim2real_hw_node
|
||||
```
|
||||
|
||||
### 9.2 查看 `/cmd_vel`
|
||||
|
||||
```bash
|
||||
ros2 topic echo /cmd_vel
|
||||
```
|
||||
|
||||
摇动遥控器时应看到 `linear.x`、`linear.y` 或 `angular.z` 变化。
|
||||
|
||||
### 9.3 查看 `/safety/estop`
|
||||
|
||||
```bash
|
||||
ros2 topic echo /safety/estop
|
||||
```
|
||||
|
||||
CH7 高位时应看到：
|
||||
|
||||
```yaml
|
||||
data: true
|
||||
```
|
||||
|
||||
### 9.4 查看策略目标阶段
|
||||
|
||||
```bash
|
||||
ros2 topic echo /runtime/target --field target_source
|
||||
```
|
||||
|
||||
常见输出含义：
|
||||
|
||||
| `target_source` | 含义 |
|
||||
|---|---|
|
||||
| `boot_hold` | 刚启动，保持初始姿态 |
|
||||
| `startup_soft_hold` | 启动软保持 |
|
||||
| `startup_hold` | 正在站立或站立后保持 |
|
||||
| `runtime_zero_hold` | 已进入 runtime，遥控器无有效输入 |
|
||||
| `runtime_policy` | 遥控器有输入，策略已经介入 |
|
||||
| `safety_brake` | 安全刹车 |
|
||||
| `timeout_hold` | 目标超时，硬件保持默认姿态 |
|
||||
|
||||
### 9.5 查看完整目标状态
|
||||
|
||||
```bash
|
||||
ros2 topic echo --once /runtime/target
|
||||
```
|
||||
|
||||
重点关注字段：
|
||||
|
||||
```yaml
|
||||
target_source:
|
||||
zero_command:
|
||||
runtime_released:
|
||||
release_alpha:
|
||||
command:
|
||||
raw_command:
|
||||
```
|
||||
|
||||
如果遥控器摇杆有输入，通常会看到：
|
||||
|
||||
```yaml
|
||||
target_source: runtime_policy
|
||||
zero_command: false
|
||||
runtime_released: true
|
||||
release_alpha: 1.0
|
||||
```
|
||||
|
||||
## 10. 常见问题
|
||||
|
||||
### 10.1 启动后提示无法打开串口
|
||||
|
||||
可能原因：
|
||||
|
||||
1. 串口路径不对。
|
||||
2. 权限不足。
|
||||
3. 设备没有插好。
|
||||
4. 设备被其他程序占用。
|
||||
|
||||
检查：
|
||||
|
||||
```bash
|
||||
ls /dev/ttyACM* /dev/ttyUSB*
|
||||
```
|
||||
|
||||
修改 `runtime.yaml`：
|
||||
|
||||
```yaml
|
||||
remote_port: "/dev/ttyUSB0"
|
||||
```
|
||||
|
||||
### 10.2 `/cmd_vel` 没有变化
|
||||
|
||||
检查：
|
||||
|
||||
```bash
|
||||
ros2 node list
|
||||
ros2 topic echo /cmd_vel
|
||||
```
|
||||
|
||||
如果节点存在但无变化，可能是：
|
||||
|
||||
1. 遥控器没有输出 SBUS。
|
||||
2. 串口波特率不对。
|
||||
3. SBUS 接线错误。
|
||||
4. 遥控器通道未校准。
|
||||
5. 死区 `remote_axis_deadzone` 或 `remote_active_threshold` 太大。
|
||||
|
||||
### 10.3 摇杆方向反了
|
||||
|
||||
修改：
|
||||
|
||||
```yaml
|
||||
remote_invert_vx: true
|
||||
remote_invert_vy: false
|
||||
remote_invert_yaw: true
|
||||
```
|
||||
|
||||
例如前后方向反了，就切换：
|
||||
|
||||
```yaml
|
||||
remote_invert_vx: false
|
||||
```
|
||||
|
||||
### 10.4 急停后不恢复
|
||||
|
||||
当前配置：
|
||||
|
||||
```yaml
|
||||
remote_estop_latch: true
|
||||
```
|
||||
|
||||
这表示急停锁存。触发后建议：
|
||||
|
||||
1. 先确认机器人物理安全。
|
||||
2. 停止 launch。
|
||||
3. 将 CH7 打回安全位置。
|
||||
4. 重新启动系统。
|
||||
|
||||
如果需要非锁存模式，可以改为：
|
||||
|
||||
```yaml
|
||||
remote_estop_latch: false
|
||||
```
|
||||
|
||||
但实机调试时更建议使用锁存模式。
|
||||
|
||||
## 11. 快速验证命令清单
|
||||
|
||||
```bash
|
||||
cd /path/to/sim2real_ros2
|
||||
source /opt/ros/humble/setup.bash
|
||||
source install/setup.bash
|
||||
|
||||
ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false
|
||||
```
|
||||
|
||||
另开终端：
|
||||
|
||||
```bash
|
||||
cd /path/to/sim2real_ros2
|
||||
source /opt/ros/humble/setup.bash
|
||||
source install/setup.bash
|
||||
|
||||
ros2 node list
|
||||
ros2 topic echo /cmd_vel
|
||||
ros2 topic echo /runtime/target --field target_source
|
||||
```
|
||||
|
||||
如果只测遥控器，不启动电机系统：
|
||||
|
||||
```bash
|
||||
ros2 run sim2real_runtime remote_uart_node.py --ros-args --params-file src/sim2real_bringup/config/runtime.yaml
|
||||
```
|
||||
|
||||
另开终端：
|
||||
|
||||
```bash
|
||||
ros2 topic echo /cmd_vel
|
||||
ros2 topic echo /safety/estop
|
||||
```
|
||||
@@ -0,0 +1,246 @@
|
||||
# sim2real_ros2 Web UDP 调试说明
|
||||
|
||||
本文档说明本次新增的最小 Web 调试链路。
|
||||
|
||||
## 1. 架构
|
||||
|
||||
```text
|
||||
Windows 本地浏览器/HTTP 服务
|
||||
|
|
||||
| UDP JSON
|
||||
v
|
||||
Nano: sim2real_web_udp_bridge_node.py
|
||||
|
|
||||
| ROS 2 topics
|
||||
v
|
||||
sim2real_cmd_mux_node.py -> /cmd_vel -> sim2real_runtime_node
|
||||
```
|
||||
|
||||
Web 页面在 Windows 本地渲染，Nano 只运行轻量 UDP bridge 和 ROS2 节点。
|
||||
|
||||
## 2. 新增 ROS2 节点
|
||||
|
||||
### `remote_uart_node.py`
|
||||
|
||||
遥控器节点现在发布：
|
||||
|
||||
```text
|
||||
/cmd_vel_remote
|
||||
```
|
||||
|
||||
不再直接发布 `/cmd_vel`。
|
||||
|
||||
通道触发阈值改为：
|
||||
|
||||
```yaml
|
||||
remote_axis_deadzone: 40
|
||||
remote_active_threshold: 40
|
||||
```
|
||||
|
||||
只有通道归一化值绝对值大于 `40` 才认为是有效输入。
|
||||
|
||||
### `cmd_mux_node.py`
|
||||
|
||||
输入：
|
||||
|
||||
```text
|
||||
/cmd_vel_remote
|
||||
/cmd_vel_web
|
||||
/cmd_vel_nav
|
||||
/control/mode
|
||||
/remote/enabled
|
||||
/web/enabled
|
||||
/nav/enabled
|
||||
/safety/estop
|
||||
```
|
||||
|
||||
输出：
|
||||
|
||||
```text
|
||||
/cmd_vel
|
||||
/control/mode_state
|
||||
/control/mux_status
|
||||
```
|
||||
|
||||
控制模式：
|
||||
|
||||
```text
|
||||
DISABLED
|
||||
REMOTE
|
||||
WEB
|
||||
NAV
|
||||
```
|
||||
|
||||
急停 `/safety/estop=true` 会强制进入 `DISABLED`，并输出零速度。
|
||||
|
||||
### `web_udp_bridge_node.py`
|
||||
|
||||
Nano 端 UDP 监听：
|
||||
|
||||
```text
|
||||
0.0.0.0:15000
|
||||
```
|
||||
|
||||
发布：
|
||||
|
||||
```text
|
||||
/cmd_vel_web
|
||||
/safety/estop
|
||||
/control/mode
|
||||
/web/enabled
|
||||
/remote/enabled
|
||||
/nav/enabled
|
||||
```
|
||||
|
||||
订阅并回传状态：
|
||||
|
||||
```text
|
||||
/runtime/state
|
||||
/runtime/target
|
||||
/cmd_vel
|
||||
/safety/estop
|
||||
/control/mode_state
|
||||
/control/mux_status
|
||||
```
|
||||
|
||||
## 3. Nano 启动
|
||||
|
||||
```bash
|
||||
cd /path/to/sim2real_ros2
|
||||
source /opt/ros/humble/setup.bash
|
||||
source install/setup.bash
|
||||
ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false
|
||||
```
|
||||
|
||||
默认会启动：
|
||||
|
||||
```text
|
||||
sim2real_remote_uart_node
|
||||
sim2real_cmd_mux_node
|
||||
sim2real_web_udp_bridge_node
|
||||
```
|
||||
|
||||
如果不想启动 Web UDP bridge：
|
||||
|
||||
```bash
|
||||
ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false launch_web_bridge:=false
|
||||
```
|
||||
|
||||
## 4. Windows 本地 Web 启动
|
||||
|
||||
把目录复制到 Windows 或通过共享目录访问：
|
||||
|
||||
```text
|
||||
tools/win_web_debug
|
||||
```
|
||||
|
||||
在 Windows 上安装 Python 3 后运行：
|
||||
|
||||
```bash
|
||||
python server.py --nano-host <Nano_IP> --http-port 8088 --udp-port 15001
|
||||
```
|
||||
|
||||
浏览器打开：
|
||||
|
||||
```text
|
||||
http://127.0.0.1:8088
|
||||
```
|
||||
|
||||
## 5. UDP 命令格式
|
||||
|
||||
### 切换模式
|
||||
|
||||
```json
|
||||
{"type":"mode","mode":"REMOTE"}
|
||||
```
|
||||
|
||||
```json
|
||||
{"type":"mode","mode":"WEB"}
|
||||
```
|
||||
|
||||
```json
|
||||
{"type":"mode","mode":"DISABLED"}
|
||||
```
|
||||
|
||||
### Web 速度控制
|
||||
|
||||
```json
|
||||
{
|
||||
"type": "cmd_vel",
|
||||
"linear": {"x": 0.2, "y": 0.0, "z": 0.0},
|
||||
"angular": {"x": 0.0, "y": 0.0, "z": 0.1}
|
||||
}
|
||||
```
|
||||
|
||||
Nano 端会再次限幅：
|
||||
|
||||
```text
|
||||
vx <= ±0.8 m/s
|
||||
vy <= ±0.3 m/s
|
||||
yaw <= ±0.5 rad/s
|
||||
```
|
||||
|
||||
### 零速度
|
||||
|
||||
```json
|
||||
{"type":"zero"}
|
||||
```
|
||||
|
||||
### 软急停
|
||||
|
||||
```json
|
||||
{"type":"estop","data":true}
|
||||
```
|
||||
|
||||
## 6. 安全保护
|
||||
|
||||
当前最小版本已经包含：
|
||||
|
||||
1. 遥控器误触发阈值：`40`。
|
||||
2. 遥控器/Web/Nav 互斥控制模式。
|
||||
3. `cmd_mux` 二次限幅。
|
||||
4. `cmd_mux` 加速度限制。
|
||||
5. Web UDP 超时自动发布零速度。
|
||||
6. 急停优先级最高。
|
||||
7. Web 页面切换到 `WEB` 模式需要确认。
|
||||
8. Web 松开虚拟摇杆会自动发送零速度。
|
||||
|
||||
建议实机调试流程：
|
||||
|
||||
1. 先点击 `DISABLED`。
|
||||
2. 确认 `/cmd_vel` 为零。
|
||||
3. 如果使用遥控器，点击 `REMOTE`。
|
||||
4. 如果使用 Web，点击 `WEB` 并确认周围安全。
|
||||
5. 一旦异常，立即点击 `软急停`。
|
||||
|
||||
## 7. 验证命令
|
||||
|
||||
查看最终输出速度：
|
||||
|
||||
```bash
|
||||
ros2 topic echo /cmd_vel
|
||||
```
|
||||
|
||||
查看遥控器输入：
|
||||
|
||||
```bash
|
||||
ros2 topic echo /cmd_vel_remote
|
||||
```
|
||||
|
||||
查看 Web 输入：
|
||||
|
||||
```bash
|
||||
ros2 topic echo /cmd_vel_web
|
||||
```
|
||||
|
||||
查看当前仲裁模式：
|
||||
|
||||
```bash
|
||||
ros2 topic echo /control/mode_state
|
||||
```
|
||||
|
||||
查看策略状态：
|
||||
|
||||
```bash
|
||||
ros2 topic echo /runtime/target --field target_source
|
||||
```
|
||||
@@ -0,0 +1,20 @@
|
||||
Tcl_0: [-0.009160, -0.999960, 0.000320, 0.032150,
|
||||
0.002390, -0.000340, -1.000000, -0.011850,
|
||||
0.999960, -0.009160, 0.002390, 0.005360,
|
||||
0.000000, 0.000000, 0.000000, 1.000000]
|
||||
cam_0:
|
||||
image_width: 1600
|
||||
image_height: 1296
|
||||
k2: 0.000656
|
||||
k3: -0.028961
|
||||
k4: 0.045390
|
||||
k5: -0.064513
|
||||
k6: 0.038735
|
||||
k7: -0.009903
|
||||
p1: 0.000000
|
||||
p2: 0.000000
|
||||
A11: 736.894262
|
||||
A12: -0.161150
|
||||
A22: 736.611354
|
||||
u0: 806.125535
|
||||
v0: 639.650710
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,5 @@
|
||||
# Odin 驱动依赖占位
|
||||
|
||||
`real/sim2real_ros2` 原始快照中的 `src/odin_ros_driver` 为空目录，但启动文件、Dockerfile 和 `sim2real_bringup` 已经引用该包。
|
||||
|
||||
因此 `v0.10.0` 记录的是 ROS 2/C++ 迁移初版，不能仅凭本目录宣称 Odin 驱动可独立构建。兼容的 Odin ROS 2 驱动源码从后续版本开始随工程归档。
|
||||
@@ -0,0 +1,11 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(sim2real_bringup)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
|
||||
install(
|
||||
DIRECTORY launch config
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
ament_package()
|
||||
@@ -0,0 +1,103 @@
|
||||
# =============================================================================
|
||||
# 部署契约参考文件（仅供参考，C++ 代码不读取此文件）
|
||||
# =============================================================================
|
||||
#
|
||||
# ⚠️ 注意：所有部署参数（电机映射、动作缩放、默认姿态等）均硬编码在
|
||||
# sim2real_common/include/sim2real_common/deployment_contract.hpp 中。
|
||||
# 本 YAML 文件仅作为可读参考，修改此文件不会影响运行时行为！
|
||||
# 如需修改部署参数，请同步更新 .hpp 文件和本文件。
|
||||
#
|
||||
# =============================================================================
|
||||
|
||||
model:
|
||||
path: "policies/model_rough.onnx"
|
||||
source_pt: "policies/model_rough.pt"
|
||||
backend: "onnxruntime"
|
||||
obs_dim: 53
|
||||
action_dim: 16
|
||||
clip_obs: 100.0
|
||||
|
||||
observation:
|
||||
terms:
|
||||
- {name: base_ang_vel, dim: 3, scale: 0.25}
|
||||
- {name: projected_gravity, dim: 3}
|
||||
- {name: command, dim: 3}
|
||||
- {name: joint_pos_rel, dim: 12}
|
||||
- {name: joint_vel_rel, dim: 12, scale: 0.05}
|
||||
- {name: wheel_vel, dim: 4, scale: 0.05}
|
||||
- {name: last_actions, dim: 16}
|
||||
|
||||
action:
|
||||
joint_order:
|
||||
- fl_hip_abduction
|
||||
- fl_hip_pitch
|
||||
- fl_knee
|
||||
- fr_hip_abduction
|
||||
- fr_hip_pitch
|
||||
- fr_knee
|
||||
- rl_hip_abduction
|
||||
- rl_hip_pitch
|
||||
- rl_knee
|
||||
- rr_hip_abduction
|
||||
- rr_hip_pitch
|
||||
- rr_knee
|
||||
- fl_wheel
|
||||
- fr_wheel
|
||||
- rl_wheel
|
||||
- rr_wheel
|
||||
wheel_indices: [12, 13, 14, 15]
|
||||
scale: [0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 5.0, 5.0, 5.0, 5.0]
|
||||
default_dof_pos: [0.0, 0.9, -1.8, 0.0, 0.9, -1.8, 0.0, 0.9, -1.8, 0.0, 0.9, -1.8, 0.0, 0.0, 0.0, 0.0]
|
||||
|
||||
motor_mapping:
|
||||
can_id_map:
|
||||
fl_hip_abduction: [1, 1]
|
||||
fl_hip_pitch: [1, 2]
|
||||
fl_knee: [1, 3]
|
||||
fl_wheel: [1, 4]
|
||||
fr_hip_abduction: [1, 5]
|
||||
fr_hip_pitch: [1, 6]
|
||||
fr_knee: [1, 7]
|
||||
fr_wheel: [1, 8]
|
||||
rl_hip_abduction: [2, 1]
|
||||
rl_hip_pitch: [2, 2]
|
||||
rl_knee: [2, 3]
|
||||
rl_wheel: [2, 4]
|
||||
rr_hip_abduction: [2, 5]
|
||||
rr_hip_pitch: [2, 6]
|
||||
rr_knee: [2, 7]
|
||||
rr_wheel: [2, 8]
|
||||
direction_map:
|
||||
fl_hip_abduction: -1
|
||||
fl_hip_pitch: -1
|
||||
fl_knee: -1
|
||||
fl_wheel: -1
|
||||
fr_hip_abduction: -1
|
||||
fr_hip_pitch: 1
|
||||
fr_knee: 1
|
||||
fr_wheel: 1
|
||||
rl_hip_abduction: 1
|
||||
rl_hip_pitch: -1
|
||||
rl_knee: -1
|
||||
rl_wheel: -1
|
||||
rr_hip_abduction: 1
|
||||
rr_hip_pitch: 1
|
||||
rr_knee: 1
|
||||
rr_wheel: 1
|
||||
zero_offset_map:
|
||||
fl_hip_abduction: 0.003
|
||||
fl_hip_pitch: 0.030
|
||||
fl_knee: 0.028
|
||||
fl_wheel: 0.0
|
||||
fr_hip_abduction: 0.004
|
||||
fr_hip_pitch: 0.038
|
||||
fr_knee: 0.011
|
||||
fr_wheel: 0.0
|
||||
rl_hip_abduction: 0.019
|
||||
rl_hip_pitch: -0.034
|
||||
rl_knee: 0.025
|
||||
rl_wheel: 0.0
|
||||
rr_hip_abduction: -0.001
|
||||
rr_hip_pitch: 0.039
|
||||
rr_knee: 0.018
|
||||
rr_wheel: 0.0
|
||||
@@ -0,0 +1,80 @@
|
||||
/**:
|
||||
ros__parameters:
|
||||
policy_hz: 50.0
|
||||
motor_hz: 200.0
|
||||
status_hz: 10.0
|
||||
target_timeout_ms: 150.0
|
||||
model_path: policies/model_rough.onnx
|
||||
use_cuda: true # 启用 CUDA Execution Provider（Orin Nano GPU 加速）
|
||||
contract_file: deployment_contract.yaml
|
||||
dry_run: false
|
||||
can0_name: "can0"
|
||||
can1_name: "can1"
|
||||
imu_topic: "/odin1/imu"
|
||||
odom_topic: "/odom"
|
||||
|
||||
# Remote UART / SBUS parameters, aligned with the Python deployment
|
||||
remote_enabled: true
|
||||
remote_port: "/dev/ttyACM0"
|
||||
remote_baudrate: 100000
|
||||
remote_timeout: 0.02
|
||||
remote_axis_deadzone: 40
|
||||
remote_active_threshold: 40
|
||||
remote_axis_full_scale: 660.0
|
||||
remote_max_vx: 0.8
|
||||
remote_max_vy: 0.3
|
||||
remote_max_yaw_rate: 0.5
|
||||
remote_invert_vx: true
|
||||
remote_invert_vy: false
|
||||
remote_invert_yaw: true
|
||||
remote_publish_inactive_zero: true
|
||||
remote_estop_latch: true
|
||||
remote_poll_hz: 50.0
|
||||
|
||||
# Command mux parameters
|
||||
cmd_mux_default_mode: "REMOTE"
|
||||
cmd_mux_output_hz: 50.0
|
||||
cmd_mux_remote_timeout_ms: 250.0
|
||||
cmd_mux_web_timeout_ms: 300.0
|
||||
cmd_mux_nav_timeout_ms: 500.0
|
||||
cmd_mux_max_vx: 0.8
|
||||
cmd_mux_max_vy: 0.3
|
||||
cmd_mux_max_yaw_rate: 0.5
|
||||
cmd_mux_max_vx_acc: 1.0
|
||||
cmd_mux_max_vy_acc: 1.0
|
||||
cmd_mux_max_yaw_acc: 1.5
|
||||
|
||||
# Windows/Nano Web UDP bridge parameters
|
||||
web_bridge_enabled: true
|
||||
web_udp_listen_host: "0.0.0.0"
|
||||
web_udp_listen_port: 15000
|
||||
web_udp_remote_host: ""
|
||||
web_udp_remote_port: 15001
|
||||
web_udp_state_hz: 20.0
|
||||
web_udp_cmd_timeout_ms: 300.0
|
||||
web_udp_max_packet_bytes: 8192
|
||||
web_udp_max_vx: 0.8
|
||||
web_udp_max_vy: 0.3
|
||||
web_udp_max_yaw_rate: 0.5
|
||||
web_udp_estop_on_timeout: false
|
||||
|
||||
# Safety parameters
|
||||
safety_enabled: true
|
||||
max_target_offset: 0.6
|
||||
hard_target_offset: 2.0
|
||||
max_ang_vel: 10.0
|
||||
max_tilt_z: -0.3
|
||||
clip_to_brake: 0
|
||||
imu_age_warn_ms: 60.0
|
||||
imu_age_stop_ms: 200.0
|
||||
|
||||
# Policy alignment with the Python deployment
|
||||
command_release_s: 0.35
|
||||
release_command_hold_s: 0.12
|
||||
release_posture_max_err: 0.35
|
||||
release_target_blend_s: 0.30
|
||||
clip_obs: 100.0
|
||||
hold_zero_command_pose: true
|
||||
enable_zero_cmd_suppression: true
|
||||
require_active_command_to_release: true
|
||||
zero_cmd_use_yaw_rate: true
|
||||
@@ -0,0 +1,129 @@
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
|
||||
from launch.conditions import IfCondition
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.parameter_descriptions import ParameterFile
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
runtime_params = ParameterFile(
|
||||
PathJoinSubstitution([
|
||||
FindPackageShare("sim2real_bringup"),
|
||||
"config",
|
||||
"runtime.yaml",
|
||||
]),
|
||||
allow_substs=True,
|
||||
)
|
||||
|
||||
# Declare launch configurations
|
||||
launch_driver_arg = DeclareLaunchArgument(
|
||||
'launch_driver',
|
||||
default_value='true',
|
||||
description='Whether to launch the odin_ros_driver sensor node'
|
||||
)
|
||||
|
||||
launch_nav2_arg = DeclareLaunchArgument(
|
||||
'launch_nav2',
|
||||
default_value='true',
|
||||
description='Whether to launch the Nav2 navigation stack'
|
||||
)
|
||||
|
||||
launch_remote_arg = DeclareLaunchArgument(
|
||||
'launch_remote',
|
||||
default_value='true',
|
||||
description='Whether to launch the SBUS UART remote control node'
|
||||
)
|
||||
|
||||
launch_web_bridge_arg = DeclareLaunchArgument(
|
||||
'launch_web_bridge',
|
||||
default_value='true',
|
||||
description='Whether to launch the Windows/Nano UDP web debug bridge'
|
||||
)
|
||||
|
||||
# Include odin_ros_driver launch
|
||||
driver_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
PathJoinSubstitution([
|
||||
FindPackageShare('odin_ros_driver'),
|
||||
'launch',
|
||||
'odin1_ros2.launch.py'
|
||||
])
|
||||
),
|
||||
launch_arguments={'launch_rviz': 'false'}.items(),
|
||||
condition=IfCondition(LaunchConfiguration('launch_driver'))
|
||||
)
|
||||
|
||||
# Include sim2real_nav2 launch
|
||||
nav2_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
PathJoinSubstitution([
|
||||
FindPackageShare('sim2real_nav2'),
|
||||
'launch',
|
||||
'nav2.launch.py'
|
||||
])
|
||||
),
|
||||
condition=IfCondition(LaunchConfiguration('launch_nav2'))
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
launch_driver_arg,
|
||||
launch_nav2_arg,
|
||||
launch_remote_arg,
|
||||
launch_web_bridge_arg,
|
||||
Node(
|
||||
package="sim2real_hw",
|
||||
executable="sim2real_hw_node",
|
||||
name="sim2real_hw_node",
|
||||
output="screen",
|
||||
parameters=[runtime_params],
|
||||
),
|
||||
Node(
|
||||
package="sim2real_runtime",
|
||||
executable="sim2real_runtime_node",
|
||||
name="sim2real_runtime_node",
|
||||
output="screen",
|
||||
parameters=[runtime_params],
|
||||
),
|
||||
Node(
|
||||
package="sim2real_runtime",
|
||||
executable="cmd_mux_node.py",
|
||||
name="sim2real_cmd_mux_node",
|
||||
output="screen",
|
||||
parameters=[runtime_params],
|
||||
),
|
||||
Node(
|
||||
package="sim2real_runtime",
|
||||
executable="web_udp_bridge_node.py",
|
||||
name="sim2real_web_udp_bridge_node",
|
||||
output="screen",
|
||||
parameters=[runtime_params],
|
||||
condition=IfCondition(LaunchConfiguration('launch_web_bridge')),
|
||||
),
|
||||
Node(
|
||||
package="sim2real_runtime",
|
||||
executable="remote_uart_node.py",
|
||||
name="sim2real_remote_uart_node",
|
||||
output="screen",
|
||||
parameters=[runtime_params],
|
||||
condition=IfCondition(LaunchConfiguration('launch_remote')),
|
||||
),
|
||||
Node(
|
||||
package="sim2real_runtime",
|
||||
executable="odom_relay_node",
|
||||
name="odom_relay_node",
|
||||
output="screen",
|
||||
parameters=[{
|
||||
"odom_input_topic": "/odin1/odometry",
|
||||
"odom_output_topic": "/odom",
|
||||
"base_frame": "base_link",
|
||||
"publish_tf": True,
|
||||
}],
|
||||
condition=IfCondition(LaunchConfiguration('launch_driver')),
|
||||
),
|
||||
driver_launch,
|
||||
nav2_launch,
|
||||
])
|
||||
|
||||
@@ -0,0 +1,24 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>sim2real_bringup</name>
|
||||
<version>0.1.0</version>
|
||||
<description>Launch and configuration package for sim2real_ros2.</description>
|
||||
<maintainer email="todo@example.com">todo</maintainer>
|
||||
<license>Proprietary</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<exec_depend>launch</exec_depend>
|
||||
<exec_depend>launch_ros</exec_depend>
|
||||
<exec_depend>sim2real_common</exec_depend>
|
||||
<exec_depend>sim2real_hw</exec_depend>
|
||||
<exec_depend>sim2real_interfaces</exec_depend>
|
||||
<exec_depend>sim2real_runtime</exec_depend>
|
||||
<exec_depend>sim2real_nav2</exec_depend>
|
||||
<exec_depend>odin_ros_driver</exec_depend>
|
||||
<exec_depend>tf2_ros</exec_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,24 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(sim2real_common)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
|
||||
add_library(${PROJECT_NAME} INTERFACE)
|
||||
target_include_directories(${PROJECT_NAME} INTERFACE
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
$<INSTALL_INTERFACE:include>
|
||||
)
|
||||
target_compile_features(${PROJECT_NAME} INTERFACE cxx_std_17)
|
||||
|
||||
install(
|
||||
DIRECTORY include/
|
||||
DESTINATION include
|
||||
)
|
||||
|
||||
install(
|
||||
TARGETS ${PROJECT_NAME}
|
||||
EXPORT export_${PROJECT_NAME}
|
||||
)
|
||||
|
||||
ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
|
||||
ament_package()
|
||||
+94
@@ -0,0 +1,94 @@
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <cstddef>
|
||||
|
||||
namespace sim2real_common
|
||||
{
|
||||
|
||||
struct DeploymentContract
|
||||
{
|
||||
static constexpr std::size_t kObsDim = 53;
|
||||
static constexpr std::size_t kActionDim = 16;
|
||||
static constexpr std::size_t kLegJointCount = 12;
|
||||
static constexpr std::size_t kWheelCount = 4;
|
||||
static constexpr double kPolicyHz = 50.0;
|
||||
static constexpr double kMotorHz = 200.0;
|
||||
static constexpr double kStatusHz = 10.0;
|
||||
|
||||
static constexpr std::array<int, 4> kWheelIndices = {12, 13, 14, 15};
|
||||
static constexpr float kLegKp = 50.0f;
|
||||
static constexpr float kLegKd = 1.5f;
|
||||
static constexpr float kLegHoldKp = 80.0f;
|
||||
static constexpr float kLegHoldKd = 4.0f;
|
||||
static constexpr float kWheelKd = 1.0f;
|
||||
|
||||
static constexpr std::array<int, 16> kCanBusMap = {
|
||||
1, 1, 1, // fl legs
|
||||
1, 1, 1, // fr legs
|
||||
2, 2, 2, // rl legs
|
||||
2, 2, 2, // rr legs
|
||||
1, 1, 2, 2 // wheels: fl, fr, rl, rr
|
||||
};
|
||||
|
||||
static constexpr std::array<int, 16> kCanIdMap = {
|
||||
1, 2, 3, // fl legs
|
||||
5, 6, 7, // fr legs
|
||||
1, 2, 3, // rl legs
|
||||
5, 6, 7, // rr legs
|
||||
4, 8, 4, 8 // wheels: fl, fr, rl, rr
|
||||
};
|
||||
|
||||
static constexpr std::array<float, 16> kDirectionMap = {
|
||||
-1.0f, -1.0f, -1.0f, // fl
|
||||
-1.0f, 1.0f, 1.0f, // fr
|
||||
1.0f, -1.0f, -1.0f, // rl
|
||||
1.0f, 1.0f, 1.0f, // rr
|
||||
-1.0f, 1.0f, -1.0f, 1.0f // wheels
|
||||
};
|
||||
|
||||
static constexpr std::array<float, 16> kZeroOffsetMap = {
|
||||
0.003f, 0.030f, 0.028f, // fl
|
||||
0.004f, 0.038f, 0.011f, // fr
|
||||
0.019f, -0.034f, 0.025f, // rl
|
||||
-0.001f, 0.039f, 0.018f, // rr
|
||||
0.000f, 0.000f, 0.000f, 0.000f // wheels
|
||||
};
|
||||
|
||||
static constexpr std::array<float, 16> kActionScale = {
|
||||
0.125f, 0.25f, 0.25f,
|
||||
0.125f, 0.25f, 0.25f,
|
||||
0.125f, 0.25f, 0.25f,
|
||||
0.125f, 0.25f, 0.25f,
|
||||
5.0f, 5.0f, 5.0f, 5.0f
|
||||
};
|
||||
|
||||
static constexpr std::array<float, 16> kDefaultDofPos = {
|
||||
0.0f, 0.9f, -1.8f,
|
||||
0.0f, 0.9f, -1.8f,
|
||||
0.0f, 0.9f, -1.8f,
|
||||
0.0f, 0.9f, -1.8f,
|
||||
0.0f, 0.0f, 0.0f, 0.0f
|
||||
};
|
||||
};
|
||||
|
||||
static constexpr std::array<const char *, 16> kJointLabels = {
|
||||
"fl_hip_abduction",
|
||||
"fl_hip_pitch",
|
||||
"fl_knee",
|
||||
"fr_hip_abduction",
|
||||
"fr_hip_pitch",
|
||||
"fr_knee",
|
||||
"rl_hip_abduction",
|
||||
"rl_hip_pitch",
|
||||
"rl_knee",
|
||||
"rr_hip_abduction",
|
||||
"rr_hip_pitch",
|
||||
"rr_knee",
|
||||
"fl_wheel",
|
||||
"fr_wheel",
|
||||
"rl_wheel",
|
||||
"rr_wheel"
|
||||
};
|
||||
|
||||
} // namespace sim2real_common
|
||||
+55
@@ -0,0 +1,55 @@
|
||||
#pragma once
|
||||
|
||||
#include <vector>
|
||||
#include <cmath>
|
||||
#include <algorithm>
|
||||
|
||||
#ifndef M_PI
|
||||
#define M_PI 3.14159265358979323846
|
||||
#endif
|
||||
|
||||
namespace sim2real_common
|
||||
{
|
||||
|
||||
class LowPassFilter
|
||||
{
|
||||
public:
|
||||
LowPassFilter(double cutoff_freq, double dt, std::size_t dim)
|
||||
: dim_(dim), initialized_(false)
|
||||
{
|
||||
alpha_ = static_cast<float>(1.0 - std::exp(-2.0 * M_PI * cutoff_freq * dt));
|
||||
y_prev_.resize(dim, 0.0f);
|
||||
}
|
||||
|
||||
void filter(const float* x, float* y)
|
||||
{
|
||||
if (!initialized_) {
|
||||
for (std::size_t i = 0; i < dim_; ++i) {
|
||||
y_prev_[i] = x[i];
|
||||
}
|
||||
initialized_ = true;
|
||||
}
|
||||
for (std::size_t i = 0; i < dim_; ++i) {
|
||||
y[i] = alpha_ * x[i] + (1.0f - alpha_) * y_prev_[i];
|
||||
y_prev_[i] = y[i];
|
||||
}
|
||||
}
|
||||
|
||||
void filter(const std::vector<float>& x, std::vector<float>& y)
|
||||
{
|
||||
filter(x.data(), y.data());
|
||||
}
|
||||
|
||||
void reset()
|
||||
{
|
||||
initialized_ = false;
|
||||
}
|
||||
|
||||
private:
|
||||
float alpha_;
|
||||
std::size_t dim_;
|
||||
bool initialized_;
|
||||
std::vector<float> y_prev_;
|
||||
};
|
||||
|
||||
} // namespace sim2real_common
|
||||
+152
@@ -0,0 +1,152 @@
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <cmath>
|
||||
#include <algorithm>
|
||||
|
||||
namespace sim2real_common
|
||||
{
|
||||
|
||||
// Helper to calculate gravity orientation from quaternion [w, x, y, z]
|
||||
inline std::array<float, 3> get_gravity_orientation(const std::array<float, 4>& quat_wxyz)
|
||||
{
|
||||
float qw = quat_wxyz[0];
|
||||
float qx = quat_wxyz[1];
|
||||
float qy = quat_wxyz[2];
|
||||
float qz = quat_wxyz[3];
|
||||
|
||||
float gx = 2.0f * (-qz * qx + qw * qy);
|
||||
float gy = -2.0f * (qz * qy + qw * qx);
|
||||
float gz = 1.0f - 2.0f * (qw * qw + qz * qz);
|
||||
return {gx, gy, gz};
|
||||
}
|
||||
|
||||
// Helper to create quaternion from acceleration vector
|
||||
inline std::array<float, 4> quat_from_accel(const std::array<float, 3>& accel)
|
||||
{
|
||||
float norm_a = std::sqrt(accel[0]*accel[0] + accel[1]*accel[1] + accel[2]*accel[2]);
|
||||
if (norm_a < 1e-9f) {
|
||||
return {1.0f, 0.0f, 0.0f, 0.0f};
|
||||
}
|
||||
|
||||
float ax = accel[0] / norm_a;
|
||||
float ay = accel[1] / norm_a;
|
||||
float az = accel[2] / norm_a;
|
||||
|
||||
// Ref gravity vector is [0.0, 0.0, 1.0]
|
||||
float cross_x = -ay;
|
||||
float cross_y = ax;
|
||||
float cross_z = 0.0f;
|
||||
float dot = az;
|
||||
|
||||
if (dot < -0.999999f) {
|
||||
return {0.0f, 1.0f, 0.0f, 0.0f};
|
||||
}
|
||||
|
||||
float s = std::sqrt((1.0f + dot) * 2.0f);
|
||||
std::array<float, 4> q = {
|
||||
s * 0.5f,
|
||||
cross_x / s,
|
||||
cross_y / s,
|
||||
cross_z / s
|
||||
};
|
||||
|
||||
float norm_q = std::sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
|
||||
if (norm_q < 1e-9f) {
|
||||
return {1.0f, 0.0f, 0.0f, 0.0f};
|
||||
}
|
||||
q[0] /= norm_q;
|
||||
q[1] /= norm_q;
|
||||
q[2] /= norm_q;
|
||||
q[3] /= norm_q;
|
||||
|
||||
return q;
|
||||
}
|
||||
|
||||
class MahonyFilter
|
||||
{
|
||||
public:
|
||||
MahonyFilter(float kp = 2.0f, float ki = 0.0f)
|
||||
: kp_(kp), ki_(ki)
|
||||
{
|
||||
q_ = {1.0f, 0.0f, 0.0f, 0.0f};
|
||||
e_int_ = {0.0f, 0.0f, 0.0f};
|
||||
}
|
||||
|
||||
void reset_with_accel(const std::array<float, 3>& accel)
|
||||
{
|
||||
q_ = quat_from_accel(accel);
|
||||
e_int_ = {0.0f, 0.0f, 0.0f};
|
||||
}
|
||||
|
||||
std::array<float, 4> update(const std::array<float, 3>& accel, const std::array<float, 3>& gyro, float dt)
|
||||
{
|
||||
float norm_a = std::sqrt(accel[0]*accel[0] + accel[1]*accel[1] + accel[2]*accel[2]);
|
||||
std::array<float, 3> gyro_corr = gyro;
|
||||
|
||||
if (norm_a > 1e-6f) {
|
||||
float ax = accel[0] / norm_a;
|
||||
float ay = accel[1] / norm_a;
|
||||
float az = accel[2] / norm_a;
|
||||
|
||||
float qw = q_[0];
|
||||
float qx = q_[1];
|
||||
float qy = q_[2];
|
||||
float qz = q_[3];
|
||||
|
||||
float vx = 2.0f * (qx * qz - qw * qy);
|
||||
float vy = 2.0f * (qw * qx + qy * qz);
|
||||
float vz = qw * qw - qx * qx - qy * qy + qz * qz;
|
||||
|
||||
// Error = cross(a, v)
|
||||
float ex = ay * vz - az * vy;
|
||||
float ey = az * vx - ax * vz;
|
||||
float ez = ax * vy - ay * vx;
|
||||
|
||||
if (ki_ > 0.0f) {
|
||||
e_int_[0] += ex * dt;
|
||||
e_int_[1] += ey * dt;
|
||||
e_int_[2] += ez * dt;
|
||||
} else {
|
||||
e_int_ = {0.0f, 0.0f, 0.0f};
|
||||
}
|
||||
|
||||
gyro_corr[0] += kp_ * ex + ki_ * e_int_[0];
|
||||
gyro_corr[1] += kp_ * ey + ki_ * e_int_[1];
|
||||
gyro_corr[2] += kp_ * ez + ki_ * e_int_[2];
|
||||
}
|
||||
|
||||
float qw = q_[0];
|
||||
float qx = q_[1];
|
||||
float qy = q_[2];
|
||||
float qz = q_[3];
|
||||
|
||||
float q_dot_w = 0.5f * (-qx * gyro_corr[0] - qy * gyro_corr[1] - qz * gyro_corr[2]);
|
||||
float q_dot_x = 0.5f * ( qw * gyro_corr[0] + qy * gyro_corr[2] - qz * gyro_corr[1]);
|
||||
float q_dot_y = 0.5f * ( qw * gyro_corr[1] - qx * gyro_corr[2] + qz * gyro_corr[0]);
|
||||
float q_dot_z = 0.5f * ( qw * gyro_corr[2] + qx * gyro_corr[1] - qy * gyro_corr[0]);
|
||||
|
||||
q_[0] += q_dot_w * dt;
|
||||
q_[1] += q_dot_x * dt;
|
||||
q_[2] += q_dot_y * dt;
|
||||
q_[3] += q_dot_z * dt;
|
||||
|
||||
float norm_q = std::sqrt(q_[0]*q_[0] + q_[1]*q_[1] + q_[2]*q_[2] + q_[3]*q_[3]) + 1e-9f;
|
||||
q_[0] /= norm_q;
|
||||
q_[1] /= norm_q;
|
||||
q_[2] /= norm_q;
|
||||
q_[3] /= norm_q;
|
||||
|
||||
return q_;
|
||||
}
|
||||
|
||||
const std::array<float, 4>& get_q() const { return q_; }
|
||||
|
||||
private:
|
||||
float kp_;
|
||||
float ki_;
|
||||
std::array<float, 4> q_;
|
||||
std::array<float, 3> e_int_;
|
||||
};
|
||||
|
||||
} // namespace sim2real_common
|
||||
+114
@@ -0,0 +1,114 @@
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <cmath>
|
||||
|
||||
namespace sim2real_common
|
||||
{
|
||||
|
||||
enum class GuardLevel : int {
|
||||
OK = 0,
|
||||
WARN = 1,
|
||||
STOP = 2
|
||||
};
|
||||
|
||||
struct GuardDecision {
|
||||
GuardLevel level{GuardLevel::OK};
|
||||
std::string reason;
|
||||
};
|
||||
|
||||
class RuntimeGuard {
|
||||
public:
|
||||
RuntimeGuard(
|
||||
float max_ang_vel = 12.0f,
|
||||
float max_tilt_z = -0.30f,
|
||||
float imu_age_warn_ms = 60.0f,
|
||||
float imu_age_stop_ms = 200.0f)
|
||||
: max_ang_vel_(max_ang_vel),
|
||||
max_tilt_z_(max_tilt_z),
|
||||
imu_age_warn_ms_(imu_age_warn_ms),
|
||||
imu_age_stop_ms_(imu_age_stop_ms)
|
||||
{}
|
||||
|
||||
GuardDecision check(
|
||||
const std::array<float, 3>& imu_gyro,
|
||||
const std::array<float, 3>& projected_gravity,
|
||||
float imu_age_ms,
|
||||
bool estop_triggered,
|
||||
const std::vector<float>& extra_vals = {})
|
||||
{
|
||||
GuardDecision decision;
|
||||
|
||||
// 1) user E-stop
|
||||
if (estop_triggered) {
|
||||
decision.level = GuardLevel::STOP;
|
||||
decision.reason = "user E-stop";
|
||||
return decision;
|
||||
}
|
||||
|
||||
// 2) NaN/Inf check
|
||||
for (float v : imu_gyro) {
|
||||
if (std::isnan(v) || std::isinf(v)) {
|
||||
decision.level = GuardLevel::STOP;
|
||||
decision.reason = "NaN/Inf detected in imu_gyro";
|
||||
return decision;
|
||||
}
|
||||
}
|
||||
for (float v : projected_gravity) {
|
||||
if (std::isnan(v) || std::isinf(v)) {
|
||||
decision.level = GuardLevel::STOP;
|
||||
decision.reason = "NaN/Inf detected in projected_gravity";
|
||||
return decision;
|
||||
}
|
||||
}
|
||||
for (float v : extra_vals) {
|
||||
if (std::isnan(v) || std::isinf(v)) {
|
||||
decision.level = GuardLevel::STOP;
|
||||
decision.reason = "NaN/Inf detected in checked values";
|
||||
return decision;
|
||||
}
|
||||
}
|
||||
|
||||
// 3) IMU stale
|
||||
if (imu_age_ms > imu_age_stop_ms_) {
|
||||
decision.level = GuardLevel::STOP;
|
||||
decision.reason = "IMU stale " + std::to_string(imu_age_ms) + "ms";
|
||||
return decision;
|
||||
}
|
||||
bool warned_imu = (imu_age_ms > imu_age_warn_ms_);
|
||||
|
||||
// 4) Tilt check
|
||||
if (projected_gravity[2] > max_tilt_z_) {
|
||||
decision.level = GuardLevel::STOP;
|
||||
decision.reason = "tilt: g_z=" + std::to_string(projected_gravity[2]);
|
||||
return decision;
|
||||
}
|
||||
|
||||
// 5) Angular velocity check
|
||||
float ang_norm = std::sqrt(imu_gyro[0] * imu_gyro[0] + imu_gyro[1] * imu_gyro[1] + imu_gyro[2] * imu_gyro[2]);
|
||||
if (ang_norm > max_ang_vel_) {
|
||||
decision.level = GuardLevel::STOP;
|
||||
decision.reason = "ang_vel overflow: |w|=" + std::to_string(ang_norm);
|
||||
return decision;
|
||||
}
|
||||
|
||||
if (warned_imu) {
|
||||
decision.level = GuardLevel::WARN;
|
||||
decision.reason = "IMU age " + std::to_string(imu_age_ms) + "ms";
|
||||
return decision;
|
||||
}
|
||||
|
||||
decision.level = GuardLevel::OK;
|
||||
return decision;
|
||||
}
|
||||
|
||||
private:
|
||||
float max_ang_vel_;
|
||||
float max_tilt_z_;
|
||||
float imu_age_warn_ms_;
|
||||
float imu_age_stop_ms_;
|
||||
};
|
||||
|
||||
} // namespace sim2real_common
|
||||
+119
@@ -0,0 +1,119 @@
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <string>
|
||||
#include <cmath>
|
||||
#include <algorithm>
|
||||
|
||||
namespace sim2real_common
|
||||
{
|
||||
|
||||
enum class SafetyLevel : int {
|
||||
NORMAL = 0,
|
||||
CLIP = 1,
|
||||
BRAKE = 2,
|
||||
ESTOP = 3
|
||||
};
|
||||
|
||||
struct SafetyDecision {
|
||||
SafetyLevel level{SafetyLevel::NORMAL};
|
||||
std::string message;
|
||||
std::array<float, 16> clipped_target{};
|
||||
};
|
||||
|
||||
class SafetyMonitor {
|
||||
public:
|
||||
SafetyMonitor(
|
||||
float max_target_offset = 0.6f,
|
||||
float max_ang_vel = 10.0f,
|
||||
float max_tilt_z = -0.3f,
|
||||
int clip_to_brake = 0,
|
||||
float hard_target_offset = 1.2f)
|
||||
: max_target_offset_(max_target_offset),
|
||||
max_ang_vel_(max_ang_vel),
|
||||
max_tilt_z_(max_tilt_z),
|
||||
clip_to_brake_(clip_to_brake),
|
||||
hard_target_offset_(hard_target_offset),
|
||||
consecutive_clips_(0)
|
||||
{}
|
||||
|
||||
SafetyDecision check(
|
||||
const std::array<float, 16>& target_pose,
|
||||
const std::array<float, 16>& default_pose,
|
||||
const std::array<float, 3>& imu_gyro,
|
||||
const std::array<float, 3>& projected_gravity,
|
||||
bool estop_triggered)
|
||||
{
|
||||
SafetyDecision decision;
|
||||
decision.clipped_target = target_pose;
|
||||
|
||||
if (estop_triggered) {
|
||||
decision.level = SafetyLevel::ESTOP;
|
||||
decision.message = "user E-stop";
|
||||
return decision;
|
||||
}
|
||||
|
||||
// Tilt check (g_z should be ~ -1.0, if it is > max_tilt_z e.g. -0.3, it is tilted)
|
||||
if (projected_gravity[2] > max_tilt_z_) {
|
||||
decision.level = SafetyLevel::BRAKE;
|
||||
decision.message = "tilt detected: g_z=" + std::to_string(projected_gravity[2]);
|
||||
return decision;
|
||||
}
|
||||
|
||||
// Angular velocity norm check
|
||||
float ang_vel_norm = std::sqrt(imu_gyro[0] * imu_gyro[0] + imu_gyro[1] * imu_gyro[1] + imu_gyro[2] * imu_gyro[2]);
|
||||
if (ang_vel_norm > max_ang_vel_) {
|
||||
decision.level = SafetyLevel::BRAKE;
|
||||
decision.message = "angular velocity overflow: |w|=" + std::to_string(ang_vel_norm);
|
||||
return decision;
|
||||
}
|
||||
|
||||
// Offset check
|
||||
bool needs_clip = false;
|
||||
float max_offset = 0.0f;
|
||||
for (std::size_t i = 0; i < 12; ++i) { // check leg joint offsets from default pose
|
||||
float offset = target_pose[i] - default_pose[i];
|
||||
max_offset = std::max(max_offset, std::abs(offset));
|
||||
if (std::abs(offset) > max_target_offset_) {
|
||||
needs_clip = true;
|
||||
float clipped_val = std::clamp(offset, -max_target_offset_, max_target_offset_);
|
||||
decision.clipped_target[i] = default_pose[i] + clipped_val;
|
||||
}
|
||||
}
|
||||
|
||||
if (needs_clip) {
|
||||
consecutive_clips_++;
|
||||
if (hard_target_offset_ > 0.0f && max_offset > hard_target_offset_) {
|
||||
decision.level = SafetyLevel::BRAKE;
|
||||
decision.message = "target leg offset exceeds hard limit: " + std::to_string(max_offset);
|
||||
return decision;
|
||||
}
|
||||
if (clip_to_brake_ > 0 && consecutive_clips_ >= clip_to_brake_) {
|
||||
decision.level = SafetyLevel::BRAKE;
|
||||
decision.message = "clipped " + std::to_string(consecutive_clips_) + " frames in a row";
|
||||
return decision;
|
||||
}
|
||||
decision.level = SafetyLevel::CLIP;
|
||||
decision.message = "target leg offset out of range";
|
||||
return decision;
|
||||
}
|
||||
|
||||
consecutive_clips_ = 0;
|
||||
decision.level = SafetyLevel::NORMAL;
|
||||
return decision;
|
||||
}
|
||||
|
||||
void reset() {
|
||||
consecutive_clips_ = 0;
|
||||
}
|
||||
|
||||
private:
|
||||
float max_target_offset_;
|
||||
float max_ang_vel_;
|
||||
float max_tilt_z_;
|
||||
int clip_to_brake_;
|
||||
float hard_target_offset_;
|
||||
int consecutive_clips_;
|
||||
};
|
||||
|
||||
} // namespace sim2real_common
|
||||
+127
@@ -0,0 +1,127 @@
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <vector>
|
||||
#include <cmath>
|
||||
#include <algorithm>
|
||||
|
||||
#ifndef M_PI
|
||||
#define M_PI 3.14159265358979323846
|
||||
#endif
|
||||
|
||||
namespace sim2real_common
|
||||
{
|
||||
|
||||
class StandBalanceController
|
||||
{
|
||||
public:
|
||||
StandBalanceController(double control_dt = 0.02)
|
||||
: control_dt_(control_dt)
|
||||
{
|
||||
profile_h_ = {0.157f, 0.248f, 0.311f, 0.366f, 0.411f, 0.448f};
|
||||
profile_hip_ = {1.5f, 1.2f, 1.0f, 0.8f, 0.6f, 0.4f};
|
||||
profile_knee_ = {-2.5f, -2.1f, -1.8f, -1.5f, -1.2f, -0.9f};
|
||||
reset();
|
||||
}
|
||||
|
||||
void reset()
|
||||
{
|
||||
stable_time_ = 0.0f;
|
||||
}
|
||||
|
||||
std::array<float, 16> computeTarget(
|
||||
const std::array<float, 3>& projected_gravity,
|
||||
const std::array<float, 3>& imu_gyro,
|
||||
const std::array<float, 3>& cmd)
|
||||
{
|
||||
float hip_base = 0.9f;
|
||||
float knee_base = -1.8f;
|
||||
estimateBaseLegPose(hip_base, knee_base);
|
||||
|
||||
float roll = 0.0f;
|
||||
float pitch = 0.0f;
|
||||
estimateRollPitch(projected_gravity, roll, pitch);
|
||||
|
||||
float roll_rate = imu_gyro[0];
|
||||
float pitch_rate = imu_gyro[1];
|
||||
|
||||
float roll_corr = -kp_roll_ * roll - kd_roll_rate_ * roll_rate;
|
||||
|
||||
float lateral_lean = lateral_lean_gain_ * cmd[1];
|
||||
|
||||
std::array<float, 16> target{};
|
||||
for (int leg_idx = 0; leg_idx < 4; ++leg_idx) {
|
||||
float side = (leg_idx == 0 || leg_idx == 2) ? 1.0f : -1.0f;
|
||||
|
||||
target[leg_idx * 3 + 0] = std::clamp(side * roll_corr + lateral_lean, -hip_abduction_clip_, hip_abduction_clip_);
|
||||
target[leg_idx * 3 + 1] = std::clamp(hip_base, hip_pitch_clip_[0], hip_pitch_clip_[1]);
|
||||
target[leg_idx * 3 + 2] = std::clamp(knee_base, knee_clip_[0], knee_clip_[1]);
|
||||
}
|
||||
// wheels 0
|
||||
target[12] = target[13] = target[14] = target[15] = 0.0f;
|
||||
|
||||
bool stable = (std::abs(roll * 180.0f / static_cast<float>(M_PI)) <= stable_roll_deg_) &&
|
||||
(std::abs(pitch * 180.0f / static_cast<float>(M_PI)) <= stable_pitch_deg_) &&
|
||||
(std::max(std::abs(roll_rate * 180.0f / static_cast<float>(M_PI)), std::abs(pitch_rate * 180.0f / static_cast<float>(M_PI))) <= stable_gyro_deg_s_);
|
||||
|
||||
stable_time_ = stable ? (stable_time_ + static_cast<float>(control_dt_)) : 0.0f;
|
||||
|
||||
return target;
|
||||
}
|
||||
|
||||
bool isStable() const
|
||||
{
|
||||
return stable_time_ >= enter_hold_s_;
|
||||
}
|
||||
|
||||
private:
|
||||
void estimateRollPitch(const std::array<float, 3>& projected_gravity, float& roll, float& pitch)
|
||||
{
|
||||
float gx = projected_gravity[0];
|
||||
float gy = projected_gravity[1];
|
||||
float gz = projected_gravity[2];
|
||||
roll = std::atan2(-gy, std::max(1e-6f, -gz));
|
||||
pitch = std::atan2(gx, std::sqrt(std::max(1e-6f, gy * gy + gz * gz)));
|
||||
}
|
||||
|
||||
void estimateBaseLegPose(float& hip, float& knee)
|
||||
{
|
||||
float h_clamp = std::clamp(height_, profile_h_.front(), profile_h_.back());
|
||||
hip = interpolate(h_clamp, profile_h_, profile_hip_);
|
||||
knee = interpolate(h_clamp, profile_h_, profile_knee_);
|
||||
}
|
||||
|
||||
float interpolate(float x, const std::vector<float>& xp, const std::vector<float>& fp)
|
||||
{
|
||||
if (x <= xp.front()) return fp.front();
|
||||
if (x >= xp.back()) return fp.back();
|
||||
for (std::size_t i = 0; i < xp.size() - 1; ++i) {
|
||||
if (x >= xp[i] && x <= xp[i+1]) {
|
||||
float f = (x - xp[i]) / (xp[i+1] - xp[i]);
|
||||
return fp[i] + f * (fp[i+1] - fp[i]);
|
||||
}
|
||||
}
|
||||
return fp.back();
|
||||
}
|
||||
|
||||
double control_dt_;
|
||||
float height_{0.33f};
|
||||
float kp_roll_{0.85f};
|
||||
float kd_roll_rate_{0.03f};
|
||||
float lateral_lean_gain_{0.0f};
|
||||
float hip_abduction_clip_{0.45f};
|
||||
std::array<float, 2> hip_pitch_clip_{-1.0f, 2.5f};
|
||||
std::array<float, 2> knee_clip_{-2.6f, -0.3f};
|
||||
float stable_roll_deg_{6.0f};
|
||||
float stable_pitch_deg_{8.0f};
|
||||
float stable_gyro_deg_s_{45.0f};
|
||||
float enter_hold_s_{1.0f};
|
||||
|
||||
std::vector<float> profile_h_;
|
||||
std::vector<float> profile_hip_;
|
||||
std::vector<float> profile_knee_;
|
||||
|
||||
float stable_time_{0.0f};
|
||||
};
|
||||
|
||||
} // namespace sim2real_common
|
||||
@@ -0,0 +1,10 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>sim2real_common</name>
|
||||
<version>0.1.0</version>
|
||||
<description>Shared constants and deployment contract helpers for sim2real_ros2.</description>
|
||||
<maintainer email="todo@example.com">todo</maintainer>
|
||||
<license>Proprietary</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
</package>
|
||||
@@ -0,0 +1,37 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(sim2real_hw)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(sensor_msgs REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
find_package(nav_msgs REQUIRED)
|
||||
find_package(sim2real_common REQUIRED)
|
||||
find_package(sim2real_interfaces REQUIRED)
|
||||
|
||||
add_executable(sim2real_hw_node
|
||||
src/hardware_bridge_node.cpp
|
||||
)
|
||||
|
||||
target_include_directories(sim2real_hw_node PRIVATE include)
|
||||
target_compile_features(sim2real_hw_node PRIVATE cxx_std_17)
|
||||
ament_target_dependencies(sim2real_hw_node
|
||||
rclcpp
|
||||
sensor_msgs
|
||||
std_msgs
|
||||
nav_msgs
|
||||
sim2real_common
|
||||
sim2real_interfaces
|
||||
)
|
||||
|
||||
install(
|
||||
DIRECTORY include/
|
||||
DESTINATION include
|
||||
)
|
||||
|
||||
install(
|
||||
TARGETS sim2real_hw_node
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
ament_package()
|
||||
+156
@@ -0,0 +1,156 @@
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <mutex>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "sensor_msgs/msg/imu.hpp"
|
||||
#include "nav_msgs/msg/odometry.hpp"
|
||||
#include "std_msgs/msg/bool.hpp"
|
||||
#include "sim2real_interfaces/msg/runtime_state.hpp"
|
||||
#include "sim2real_interfaces/msg/runtime_target.hpp"
|
||||
#include "sim2real_common/low_pass_filter.hpp"
|
||||
#include "sim2real_common/mahony_filter.hpp"
|
||||
#include "sim2real_common/safety_monitor.hpp"
|
||||
#include "sim2real_common/runtime_guard.hpp"
|
||||
|
||||
namespace sim2real_hw
|
||||
{
|
||||
|
||||
struct MotorConfig
|
||||
{
|
||||
int bus; // 1 or 2
|
||||
int id; // motor CAN id
|
||||
float direction;
|
||||
float offset;
|
||||
};
|
||||
|
||||
struct MotorStateInternal
|
||||
{
|
||||
float position{0.0f};
|
||||
float velocity{0.0f};
|
||||
float torque{0.0f};
|
||||
float temperature{0.0f};
|
||||
std::uint32_t update_count{0};
|
||||
std::uint32_t stale_count{0};
|
||||
// Hold-over state
|
||||
float last_valid_pos{0.0f};
|
||||
float last_valid_vel{0.0f};
|
||||
float last_valid_torque{0.0f};
|
||||
std::uint32_t prev_update_count{0};
|
||||
bool has_valid_data{false};
|
||||
};
|
||||
|
||||
class HardwareBridgeNode : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
HardwareBridgeNode();
|
||||
~HardwareBridgeNode();
|
||||
|
||||
private:
|
||||
void onTarget(const sim2real_interfaces::msg::RuntimeTarget::SharedPtr msg);
|
||||
void onReadLoop();
|
||||
void onWriteLoop();
|
||||
void onImu(const sensor_msgs::msg::Imu::SharedPtr msg);
|
||||
void onOdom(const nav_msgs::msg::Odometry::SharedPtr msg);
|
||||
|
||||
bool initCan(const std::string& ifname, int& fd);
|
||||
bool sendCanFrame(int fd, std::uint32_t can_id, const std::uint8_t* data, std::uint8_t dlc);
|
||||
bool readCanFrame(int fd, void* frame, int timeout_us);
|
||||
|
||||
bool enableMotor(int fd, int motor_id);
|
||||
bool disableMotor(int fd, int motor_id);
|
||||
bool setModeRaw(int fd, int motor_id, std::int8_t mode);
|
||||
bool writeLimit(int fd, int motor_id, std::uint16_t param_id, float limit);
|
||||
bool writeOperationFrame(int fd, int motor_id, double pos, double vel, double kp, double kd, double torque);
|
||||
|
||||
rclcpp::Publisher<sim2real_interfaces::msg::RuntimeState>::SharedPtr state_pub_;
|
||||
rclcpp::Subscription<sim2real_interfaces::msg::RuntimeTarget>::SharedPtr target_sub_;
|
||||
rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;
|
||||
rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr estop_sub_;
|
||||
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
|
||||
|
||||
rclcpp::TimerBase::SharedPtr read_timer_;
|
||||
rclcpp::TimerBase::SharedPtr write_timer_;
|
||||
|
||||
std::mutex target_mutex_;
|
||||
std::array<float, 16> latest_target_{};
|
||||
std::array<float, 16> latest_raw_action_{};
|
||||
std::string latest_target_source_{"boot_hold"};
|
||||
rclcpp::Time latest_target_stamp_{0, 0, RCL_ROS_TIME};
|
||||
std::uint32_t target_sequence_{0};
|
||||
std::uint32_t state_sequence_{0};
|
||||
double target_timeout_ms_{150.0};
|
||||
|
||||
// SocketCAN file descriptors
|
||||
int can0_fd_{-1};
|
||||
int can1_fd_{-1};
|
||||
std::string can0_name_{"can0"};
|
||||
std::string can1_name_{"can1"};
|
||||
|
||||
// CAN error recovery
|
||||
static constexpr int kCanErrorThreshold = 50; // consecutive errors before reinit
|
||||
int can0_error_count_{0};
|
||||
int can1_error_count_{0};
|
||||
bool reinitCan(const std::string& ifname, int& fd, int& error_count);
|
||||
|
||||
// Hold-over constants
|
||||
static constexpr std::uint32_t kHoldoverThreshold = 2;
|
||||
|
||||
// Motor configurations and states
|
||||
std::array<MotorConfig, 16> motors_;
|
||||
std::array<MotorStateInternal, 16> motor_states_;
|
||||
|
||||
// IMU state
|
||||
std::mutex imu_mutex_;
|
||||
std::array<float, 3> imu_gyro_{};
|
||||
std::array<float, 3> imu_accel_{};
|
||||
std::array<float, 3> projected_gravity_{0.0f, 0.0f, -1.0f};
|
||||
bool imu_fresh_{false};
|
||||
rclcpp::Time last_imu_stamp_{0, 0, RCL_ROS_TIME};
|
||||
std::chrono::steady_clock::time_point last_imu_recv_time_{};
|
||||
bool has_received_imu_{false};
|
||||
std::array<float, 3> imu_gravity_sum_{0.0f, 0.0f, 0.0f};
|
||||
std::uint32_t imu_gravity_sample_count_{0};
|
||||
static constexpr std::uint32_t kImuGravityAlignSamples = 50;
|
||||
|
||||
// Odom state
|
||||
std::mutex odom_mutex_;
|
||||
rclcpp::Time last_odom_stamp_{0, 0, RCL_ROS_TIME};
|
||||
std::array<float, 3> odom_pos_{};
|
||||
std::array<float, 4> odom_quat_wxyz_{1.0f, 0.0f, 0.0f, 0.0f};
|
||||
std::array<float, 3> odom_linear_vel_{};
|
||||
std::array<float, 3> odom_angular_vel_{};
|
||||
bool odom_fresh_{false};
|
||||
|
||||
// Filters and Estimators
|
||||
std::unique_ptr<sim2real_common::LowPassFilter> lpf_legs_;
|
||||
std::unique_ptr<sim2real_common::LowPassFilter> lpf_wheels_;
|
||||
std::unique_ptr<sim2real_common::MahonyFilter> mahony_filter_;
|
||||
std::unique_ptr<sim2real_common::SafetyMonitor> safety_monitor_;
|
||||
std::unique_ptr<sim2real_common::RuntimeGuard> runtime_guard_;
|
||||
bool mahony_initialized_{false};
|
||||
rclcpp::Time last_read_time_{0, 0, RCL_ROS_TIME};
|
||||
rclcpp::Time startup_soft_hold_start_time_{0, 0, RCL_ROS_TIME};
|
||||
|
||||
// Telemetry
|
||||
std::uint32_t fresh_count_{0};
|
||||
std::uint32_t holdover_count_{0};
|
||||
std::uint32_t stale_max_{0};
|
||||
std::uint32_t holdover_events_total_{0};
|
||||
|
||||
bool dry_run_{false};
|
||||
std::atomic<bool> estop_triggered_{false};
|
||||
std::atomic<bool> safety_enabled_{true};
|
||||
std::atomic<bool> safety_triggered_{false};
|
||||
std::string safety_reason_{""};
|
||||
|
||||
void onEstop(const std_msgs::msg::Bool::SharedPtr msg);
|
||||
};
|
||||
|
||||
} // namespace sim2real_hw
|
||||
@@ -0,0 +1,17 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>sim2real_hw</name>
|
||||
<version>0.1.0</version>
|
||||
<description>Hardware bridge and safety boundary for sim2real_ros2.</description>
|
||||
<maintainer email="todo@example.com">todo</maintainer>
|
||||
<license>Proprietary</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>nav_msgs</depend>
|
||||
<depend>sim2real_common</depend>
|
||||
<depend>sim2real_interfaces</depend>
|
||||
</package>
|
||||
@@ -0,0 +1,852 @@
|
||||
#include "sim2real_hw/hardware_bridge_node.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <cstring>
|
||||
#include <string>
|
||||
|
||||
#include <sys/ioctl.h>
|
||||
#include <sys/socket.h>
|
||||
#include <linux/can.h>
|
||||
#include <linux/can/raw.h>
|
||||
#include <net/if.h>
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
#include <errno.h>
|
||||
|
||||
#include "sim2real_common/deployment_contract.hpp"
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
namespace sim2real_hw
|
||||
{
|
||||
|
||||
// Protocol constants
|
||||
const std::uint32_t COMM_ENABLE = 3;
|
||||
const std::uint32_t COMM_DISABLE = 4;
|
||||
const std::uint32_t COMM_WRITE_PARAMETER = 18;
|
||||
const std::uint32_t COMM_OPERATION_CONTROL = 1;
|
||||
const std::uint32_t COMM_SET_ZERO_POSITION = 6;
|
||||
const std::uint16_t PARAM_MODE = 0x7005;
|
||||
const std::uint16_t PARAM_VELOCITY_LIMIT = 0x7017;
|
||||
const std::uint16_t PARAM_TORQUE_LIMIT = 0x700B;
|
||||
const std::uint8_t HOST_ID = 0xFD;
|
||||
|
||||
inline void pack_u16_be(std::uint8_t* buf, std::uint16_t val)
|
||||
{
|
||||
buf[0] = (val >> 8) & 0xFF;
|
||||
buf[1] = val & 0xFF;
|
||||
}
|
||||
|
||||
inline float nearest_periodic(float val, float ref)
|
||||
{
|
||||
float diff = val - ref;
|
||||
float wrapped = diff - 2.0f * static_cast<float>(M_PI) * std::floor((diff + static_cast<float>(M_PI)) / (2.0f * static_cast<float>(M_PI)));
|
||||
return ref + wrapped;
|
||||
}
|
||||
|
||||
HardwareBridgeNode::HardwareBridgeNode()
|
||||
: Node("sim2real_hw_node")
|
||||
{
|
||||
// 1. Declare and get parameters
|
||||
target_timeout_ms_ = declare_parameter<double>("target_timeout_ms", 150.0);
|
||||
can0_name_ = declare_parameter<std::string>("can0_name", "can0");
|
||||
can1_name_ = declare_parameter<std::string>("can1_name", "can1");
|
||||
dry_run_ = declare_parameter<bool>("dry_run", true); // Default to dry-run for safety
|
||||
|
||||
// Safety parameters
|
||||
safety_enabled_ = declare_parameter<bool>("safety_enabled", true);
|
||||
double max_target_offset = declare_parameter<double>("max_target_offset", 0.6);
|
||||
double hard_target_offset = declare_parameter<double>("hard_target_offset", 1.2);
|
||||
double max_ang_vel = declare_parameter<double>("max_ang_vel", 10.0);
|
||||
double max_tilt_z = declare_parameter<double>("max_tilt_z", -0.3);
|
||||
int clip_to_brake = declare_parameter<int>("clip_to_brake", 0);
|
||||
double imu_age_warn_ms = declare_parameter<double>("imu_age_warn_ms", 60.0);
|
||||
double imu_age_stop_ms = declare_parameter<double>("imu_age_stop_ms", 200.0);
|
||||
|
||||
RCLCPP_INFO(get_logger(), "Initializing hardware bridge node (Dry run: %s)", dry_run_ ? "true" : "false");
|
||||
if (safety_enabled_) {
|
||||
RCLCPP_INFO(get_logger(), "Safety monitoring is ENABLED (tilt threshold: %f, ang_vel threshold: %f)", max_tilt_z, max_ang_vel);
|
||||
} else {
|
||||
RCLCPP_WARN(get_logger(), "Safety monitoring is DISABLED!");
|
||||
}
|
||||
|
||||
// 2. Set up logical motors mapping matching contract
|
||||
// Mapping index in array matches joint ordering in kJointLabels
|
||||
for (std::size_t i = 0; i < 16; ++i) {
|
||||
motors_[i].direction = sim2real_common::DeploymentContract::kDirectionMap[i];
|
||||
motors_[i].offset = sim2real_common::DeploymentContract::kZeroOffsetMap[i];
|
||||
motors_[i].bus = sim2real_common::DeploymentContract::kCanBusMap[i];
|
||||
motors_[i].id = sim2real_common::DeploymentContract::kCanIdMap[i];
|
||||
}
|
||||
|
||||
// 3. Initialize filters & safety monitors
|
||||
lpf_legs_ = std::make_unique<sim2real_common::LowPassFilter>(5.0, 0.005, 12);
|
||||
lpf_wheels_ = std::make_unique<sim2real_common::LowPassFilter>(15.0, 0.005, 4);
|
||||
mahony_filter_ = std::make_unique<sim2real_common::MahonyFilter>(2.0f, 0.0f);
|
||||
|
||||
safety_monitor_ = std::make_unique<sim2real_common::SafetyMonitor>(
|
||||
static_cast<float>(max_target_offset),
|
||||
static_cast<float>(max_ang_vel),
|
||||
static_cast<float>(max_tilt_z),
|
||||
clip_to_brake,
|
||||
static_cast<float>(hard_target_offset)
|
||||
);
|
||||
|
||||
runtime_guard_ = std::make_unique<sim2real_common::RuntimeGuard>(
|
||||
static_cast<float>(max_ang_vel + 2.0), // slightly higher limit for runtime guard stop
|
||||
static_cast<float>(max_tilt_z),
|
||||
static_cast<float>(imu_age_warn_ms),
|
||||
static_cast<float>(imu_age_stop_ms)
|
||||
);
|
||||
|
||||
// 4. Initialize CAN sockets if not in dry-run
|
||||
if (!dry_run_) {
|
||||
if (!initCan(can0_name_, can0_fd_) || !initCan(can1_name_, can1_fd_)) {
|
||||
RCLCPP_ERROR(get_logger(), "CAN initialization failed! Falling back to dry-run.");
|
||||
dry_run_ = true;
|
||||
}
|
||||
}
|
||||
|
||||
// 5. Initialize motor target states
|
||||
latest_target_ = sim2real_common::DeploymentContract::kDefaultDofPos;
|
||||
latest_raw_action_.fill(0.0f);
|
||||
|
||||
// 6. Set up ROS publishers & subscriptions
|
||||
state_pub_ = create_publisher<sim2real_interfaces::msg::RuntimeState>("runtime/state", 10);
|
||||
target_sub_ = create_subscription<sim2real_interfaces::msg::RuntimeTarget>(
|
||||
"runtime/target", 10,
|
||||
std::bind(&HardwareBridgeNode::onTarget, this, std::placeholders::_1));
|
||||
std::string imu_topic = declare_parameter<std::string>("imu_topic", "/odin1/imu");
|
||||
imu_sub_ = create_subscription<sensor_msgs::msg::Imu>(
|
||||
imu_topic, 10,
|
||||
std::bind(&HardwareBridgeNode::onImu, this, std::placeholders::_1));
|
||||
estop_sub_ = create_subscription<std_msgs::msg::Bool>(
|
||||
"/safety/estop", 10,
|
||||
std::bind(&HardwareBridgeNode::onEstop, this, std::placeholders::_1));
|
||||
|
||||
// Odom subscription
|
||||
std::string odom_topic = declare_parameter<std::string>("odom_topic", "/odom");
|
||||
odom_sub_ = create_subscription<nav_msgs::msg::Odometry>(
|
||||
odom_topic, 10,
|
||||
std::bind(&HardwareBridgeNode::onOdom, this, std::placeholders::_1));
|
||||
|
||||
// 7. Enable motors on total startup
|
||||
if (!dry_run_) {
|
||||
RCLCPP_INFO(get_logger(), "Enabling RobStride motors...");
|
||||
for (std::size_t i = 0; i < 16; ++i) {
|
||||
int fd = (motors_[i].bus == 1) ? can0_fd_ : can1_fd_;
|
||||
enableMotor(fd, motors_[i].id);
|
||||
setModeRaw(fd, motors_[i].id, 0); // MIT Mode
|
||||
writeLimit(fd, motors_[i].id, PARAM_VELOCITY_LIMIT, 20.0f);
|
||||
writeLimit(fd, motors_[i].id, PARAM_TORQUE_LIMIT, 17.0f);
|
||||
}
|
||||
}
|
||||
|
||||
// 8. Timers at 200Hz (5ms)
|
||||
read_timer_ = create_wall_timer(5ms, std::bind(&HardwareBridgeNode::onReadLoop, this));
|
||||
write_timer_ = create_wall_timer(5ms, std::bind(&HardwareBridgeNode::onWriteLoop, this));
|
||||
}
|
||||
|
||||
HardwareBridgeNode::~HardwareBridgeNode()
|
||||
{
|
||||
if (!dry_run_) {
|
||||
RCLCPP_INFO(get_logger(), "Disabling RobStride motors on shutdown...");
|
||||
for (std::size_t i = 0; i < 16; ++i) {
|
||||
int fd = (motors_[i].bus == 1) ? can0_fd_ : can1_fd_;
|
||||
disableMotor(fd, motors_[i].id);
|
||||
}
|
||||
if (can0_fd_ >= 0) {
|
||||
if (::close(can0_fd_) < 0) {
|
||||
RCLCPP_WARN(get_logger(), "Failed to close can0 socket: %s", strerror(errno));
|
||||
}
|
||||
}
|
||||
if (can1_fd_ >= 0) {
|
||||
if (::close(can1_fd_) < 0) {
|
||||
RCLCPP_WARN(get_logger(), "Failed to close can1 socket: %s", strerror(errno));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void HardwareBridgeNode::onTarget(const sim2real_interfaces::msg::RuntimeTarget::SharedPtr msg)
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(target_mutex_);
|
||||
latest_target_ = msg->target;
|
||||
latest_raw_action_ = msg->raw_action;
|
||||
latest_target_source_ = msg->target_source;
|
||||
latest_target_stamp_ = rclcpp::Time(msg->stamp);
|
||||
target_sequence_ = msg->sequence;
|
||||
}
|
||||
|
||||
void HardwareBridgeNode::onImu(const sensor_msgs::msg::Imu::SharedPtr msg)
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(imu_mutex_);
|
||||
|
||||
const float gyro_x = static_cast<float>(msg->angular_velocity.x);
|
||||
const float gyro_y = static_cast<float>(msg->angular_velocity.y);
|
||||
const float gyro_z = static_cast<float>(msg->angular_velocity.z);
|
||||
|
||||
const float accel_x = static_cast<float>(msg->linear_acceleration.x);
|
||||
const float accel_y = static_cast<float>(msg->linear_acceleration.y);
|
||||
const float accel_z = static_cast<float>(msg->linear_acceleration.z);
|
||||
|
||||
imu_gyro_ = {gyro_x, gyro_y, gyro_z};
|
||||
imu_accel_ = {accel_x, accel_y, accel_z};
|
||||
if (!mahony_initialized_ && imu_gravity_sample_count_ < kImuGravityAlignSamples) {
|
||||
imu_gravity_sum_[0] += accel_x;
|
||||
imu_gravity_sum_[1] += accel_y;
|
||||
imu_gravity_sum_[2] += accel_z;
|
||||
imu_gravity_sample_count_++;
|
||||
}
|
||||
// Track both ROS header time and local receive time. The local steady clock
|
||||
// is used for stale detection so scheduler jitter or device timestamp quirks
|
||||
// don't falsely trip the runtime guard.
|
||||
last_imu_stamp_ = rclcpp::Time(msg->header.stamp);
|
||||
last_imu_recv_time_ = std::chrono::steady_clock::now();
|
||||
has_received_imu_ = true;
|
||||
imu_fresh_ = true;
|
||||
}
|
||||
|
||||
void HardwareBridgeNode::onEstop(const std_msgs::msg::Bool::SharedPtr msg)
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(target_mutex_);
|
||||
estop_triggered_ = msg->data;
|
||||
if (estop_triggered_) {
|
||||
RCLCPP_WARN(get_logger(), "!!! Physical E-stop received over /safety/estop !!!");
|
||||
} else {
|
||||
RCLCPP_INFO(get_logger(), "Physical E-stop reset.");
|
||||
}
|
||||
}
|
||||
|
||||
void HardwareBridgeNode::onOdom(const nav_msgs::msg::Odometry::SharedPtr msg)
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(odom_mutex_);
|
||||
last_odom_stamp_ = rclcpp::Time(msg->header.stamp);
|
||||
|
||||
odom_pos_[0] = static_cast<float>(msg->pose.pose.position.x);
|
||||
odom_pos_[1] = static_cast<float>(msg->pose.pose.position.y);
|
||||
odom_pos_[2] = static_cast<float>(msg->pose.pose.position.z);
|
||||
|
||||
odom_quat_wxyz_[0] = static_cast<float>(msg->pose.pose.orientation.w);
|
||||
odom_quat_wxyz_[1] = static_cast<float>(msg->pose.pose.orientation.x);
|
||||
odom_quat_wxyz_[2] = static_cast<float>(msg->pose.pose.orientation.y);
|
||||
odom_quat_wxyz_[3] = static_cast<float>(msg->pose.pose.orientation.z);
|
||||
|
||||
odom_linear_vel_[0] = static_cast<float>(msg->twist.twist.linear.x);
|
||||
odom_linear_vel_[1] = static_cast<float>(msg->twist.twist.linear.y);
|
||||
odom_linear_vel_[2] = static_cast<float>(msg->twist.twist.linear.z);
|
||||
|
||||
odom_angular_vel_[0] = static_cast<float>(msg->twist.twist.angular.x);
|
||||
odom_angular_vel_[1] = static_cast<float>(msg->twist.twist.angular.y);
|
||||
odom_angular_vel_[2] = static_cast<float>(msg->twist.twist.angular.z);
|
||||
|
||||
odom_fresh_ = true;
|
||||
}
|
||||
|
||||
void HardwareBridgeNode::onReadLoop()
|
||||
{
|
||||
// 1. Process CAN messages (only if CAN is open)
|
||||
if (!dry_run_) {
|
||||
for (std::size_t i = 0; i < 16; ++i) {
|
||||
motor_states_[i].stale_count++;
|
||||
}
|
||||
|
||||
struct can_frame frame;
|
||||
// Process can0 (bus 1)
|
||||
while (readCanFrame(can0_fd_, &frame, 50)) {
|
||||
if (!(frame.can_id & CAN_EFF_FLAG)) continue;
|
||||
std::uint32_t comm_type = (frame.can_id >> 24) & 0x1F;
|
||||
if (comm_type == 2) { // Status Frame
|
||||
std::uint32_t extra_data = (frame.can_id >> 8) & 0xFFFF;
|
||||
int motor_id = extra_data & 0xFF;
|
||||
|
||||
for (std::size_t i = 0; i < 16; ++i) {
|
||||
if (motors_[i].bus == 1 && motors_[i].id == motor_id) {
|
||||
std::uint16_t p_u16 = (frame.data[0] << 8) | frame.data[1];
|
||||
std::uint16_t v_u16 = (frame.data[2] << 8) | frame.data[3];
|
||||
std::uint16_t t_u16 = (frame.data[4] << 8) | frame.data[5];
|
||||
std::uint16_t temp_u16 = (frame.data[6] << 8) | frame.data[7];
|
||||
|
||||
double pos_raw = (static_cast<double>(p_u16) / 32767.0 - 1.0) * (4.0 * M_PI);
|
||||
double vel_raw = (static_cast<double>(v_u16) / 32767.0 - 1.0) * 44.0;
|
||||
double torque_raw = (static_cast<double>(t_u16) / 32767.0 - 1.0) * 17.0;
|
||||
|
||||
// Apply motor mapping: real_to_sim
|
||||
// real = sign * sim + offset -> sim = (real - offset) / sign
|
||||
float pos_sim = (static_cast<float>(pos_raw) - motors_[i].offset) / motors_[i].direction;
|
||||
float vel_sim = static_cast<float>(vel_raw) / motors_[i].direction;
|
||||
float torque_sim = static_cast<float>(torque_raw) / motors_[i].direction;
|
||||
|
||||
if (i < 12) {
|
||||
pos_sim = nearest_periodic(pos_sim, sim2real_common::DeploymentContract::kDefaultDofPos[i]);
|
||||
}
|
||||
|
||||
motor_states_[i].position = pos_sim;
|
||||
motor_states_[i].velocity = vel_sim;
|
||||
motor_states_[i].torque = torque_sim;
|
||||
motor_states_[i].temperature = static_cast<float>(temp_u16) * 0.1f;
|
||||
motor_states_[i].update_count++;
|
||||
motor_states_[i].stale_count = 0;
|
||||
// Update hold-over valid data
|
||||
motor_states_[i].last_valid_pos = pos_sim;
|
||||
motor_states_[i].last_valid_vel = vel_sim;
|
||||
motor_states_[i].last_valid_torque = torque_sim;
|
||||
motor_states_[i].has_valid_data = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Process can1 (bus 2)
|
||||
while (readCanFrame(can1_fd_, &frame, 50)) {
|
||||
if (!(frame.can_id & CAN_EFF_FLAG)) continue;
|
||||
std::uint32_t comm_type = (frame.can_id >> 24) & 0x1F;
|
||||
if (comm_type == 2) {
|
||||
std::uint32_t extra_data = (frame.can_id >> 8) & 0xFFFF;
|
||||
int motor_id = extra_data & 0xFF;
|
||||
|
||||
for (std::size_t i = 0; i < 16; ++i) {
|
||||
if (motors_[i].bus == 2 && motors_[i].id == motor_id) {
|
||||
std::uint16_t p_u16 = (frame.data[0] << 8) | frame.data[1];
|
||||
std::uint16_t v_u16 = (frame.data[2] << 8) | frame.data[3];
|
||||
std::uint16_t t_u16 = (frame.data[4] << 8) | frame.data[5];
|
||||
std::uint16_t temp_u16 = (frame.data[6] << 8) | frame.data[7];
|
||||
|
||||
double pos_raw = (static_cast<double>(p_u16) / 32767.0 - 1.0) * (4.0 * M_PI);
|
||||
double vel_raw = (static_cast<double>(v_u16) / 32767.0 - 1.0) * 44.0;
|
||||
double torque_raw = (static_cast<double>(t_u16) / 32767.0 - 1.0) * 17.0;
|
||||
|
||||
float pos_sim = (static_cast<float>(pos_raw) - motors_[i].offset) / motors_[i].direction;
|
||||
float vel_sim = static_cast<float>(vel_raw) / motors_[i].direction;
|
||||
float torque_sim = static_cast<float>(torque_raw) / motors_[i].direction;
|
||||
|
||||
if (i < 12) {
|
||||
pos_sim = nearest_periodic(pos_sim, sim2real_common::DeploymentContract::kDefaultDofPos[i]);
|
||||
}
|
||||
|
||||
motor_states_[i].position = pos_sim;
|
||||
motor_states_[i].velocity = vel_sim;
|
||||
motor_states_[i].torque = torque_sim;
|
||||
motor_states_[i].temperature = static_cast<float>(temp_u16) * 0.1f;
|
||||
motor_states_[i].update_count++;
|
||||
motor_states_[i].stale_count = 0;
|
||||
// Update hold-over valid data
|
||||
motor_states_[i].last_valid_pos = pos_sim;
|
||||
motor_states_[i].last_valid_vel = vel_sim;
|
||||
motor_states_[i].last_valid_torque = torque_sim;
|
||||
motor_states_[i].has_valid_data = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Hold-over: apply last valid data for stale motors
|
||||
for (std::size_t i = 0; i < 16; ++i) {
|
||||
if (motor_states_[i].stale_count >= kHoldoverThreshold && motor_states_[i].has_valid_data) {
|
||||
motor_states_[i].position = motor_states_[i].last_valid_pos;
|
||||
motor_states_[i].velocity = motor_states_[i].last_valid_vel;
|
||||
motor_states_[i].torque = motor_states_[i].last_valid_torque;
|
||||
holdover_events_total_++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 2. Fetch IMU data & update MahonyFilter
|
||||
auto now_time = now();
|
||||
double dt = 0.005;
|
||||
if (last_read_time_.nanoseconds() > 0) {
|
||||
dt = (now_time - last_read_time_).seconds();
|
||||
if (dt <= 0.0 || dt > 0.5) {
|
||||
dt = 0.005;
|
||||
}
|
||||
}
|
||||
last_read_time_ = now_time;
|
||||
|
||||
std::array<float, 3> gyro{};
|
||||
std::array<float, 3> accel{0.0f, 0.0f, 9.81f};
|
||||
bool imu_fresh = false;
|
||||
double imu_age_ms = 0.0;
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(imu_mutex_);
|
||||
gyro = imu_gyro_;
|
||||
accel = imu_accel_;
|
||||
imu_fresh = imu_fresh_;
|
||||
imu_fresh_ = false;
|
||||
|
||||
if (has_received_imu_) {
|
||||
const auto age = std::chrono::steady_clock::now() - last_imu_recv_time_;
|
||||
imu_age_ms = std::chrono::duration<double, std::milli>(age).count();
|
||||
} else if (last_imu_stamp_.nanoseconds() > 0) {
|
||||
const auto age_ns = (now_time - last_imu_stamp_).nanoseconds();
|
||||
imu_age_ms = age_ns > 0 ? static_cast<double>(age_ns) / 1.0e6 : 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
std::array<float, 4> quat{1.0f, 0.0f, 0.0f, 0.0f};
|
||||
if (imu_fresh || mahony_initialized_) {
|
||||
if (!mahony_initialized_) {
|
||||
std::array<float, 3> gravity_init = accel;
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(imu_mutex_);
|
||||
if (imu_gravity_sample_count_ >= kImuGravityAlignSamples) {
|
||||
gravity_init = {
|
||||
imu_gravity_sum_[0] / static_cast<float>(imu_gravity_sample_count_),
|
||||
imu_gravity_sum_[1] / static_cast<float>(imu_gravity_sample_count_),
|
||||
imu_gravity_sum_[2] / static_cast<float>(imu_gravity_sample_count_)
|
||||
};
|
||||
}
|
||||
}
|
||||
mahony_filter_->reset_with_accel(gravity_init);
|
||||
mahony_initialized_ = true;
|
||||
}
|
||||
quat = mahony_filter_->update(accel, gyro, static_cast<float>(dt));
|
||||
}
|
||||
std::array<float, 3> projected_gravity = sim2real_common::get_gravity_orientation(quat);
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(imu_mutex_);
|
||||
projected_gravity_ = projected_gravity;
|
||||
}
|
||||
|
||||
// Run RuntimeGuard check
|
||||
if (safety_enabled_ && !safety_triggered_) {
|
||||
std::vector<float> extra_vals;
|
||||
extra_vals.reserve(32);
|
||||
for (std::size_t i = 0; i < 16; ++i) {
|
||||
extra_vals.push_back(motor_states_[i].position);
|
||||
extra_vals.push_back(motor_states_[i].velocity);
|
||||
}
|
||||
bool estop_active = false;
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(target_mutex_);
|
||||
estop_active = estop_triggered_;
|
||||
}
|
||||
|
||||
auto guard_decision = runtime_guard_->check(gyro, projected_gravity, imu_age_ms, estop_active, extra_vals);
|
||||
if (guard_decision.level == sim2real_common::GuardLevel::STOP) {
|
||||
safety_triggered_ = true;
|
||||
safety_reason_ = "Runtime Guard Stop: " + guard_decision.reason;
|
||||
RCLCPP_ERROR(get_logger(), "SAFETY TRIGGERED: %s", safety_reason_.c_str());
|
||||
} else if (guard_decision.level == sim2real_common::GuardLevel::WARN) {
|
||||
RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "Safety Guard Warning: %s", guard_decision.reason.c_str());
|
||||
}
|
||||
}
|
||||
|
||||
// 3. Populate RuntimeState message
|
||||
sim2real_interfaces::msg::RuntimeState msg;
|
||||
msg.stamp = now_time;
|
||||
msg.sequence = state_sequence_++;
|
||||
msg.source = dry_run_ ? "stub_hw" : "socket_can_hw";
|
||||
|
||||
fresh_count_ = 0;
|
||||
holdover_count_ = 0;
|
||||
stale_max_ = 0;
|
||||
|
||||
for (std::size_t i = 0; i < 16; ++i) {
|
||||
// Stale frames detection & holdover count
|
||||
if (!dry_run_) {
|
||||
if (motor_states_[i].stale_count > 0) {
|
||||
holdover_count_++;
|
||||
stale_max_ = std::max(stale_max_, motor_states_[i].stale_count);
|
||||
} else {
|
||||
fresh_count_++;
|
||||
}
|
||||
}
|
||||
|
||||
if (dry_run_) {
|
||||
// Mock motor positions tracking target
|
||||
msg.joint_pos[i] = latest_target_[i];
|
||||
msg.joint_vel[i] = 0.0f;
|
||||
msg.joint_torque[i] = 0.0f;
|
||||
msg.update_counts[i] = target_sequence_;
|
||||
} else {
|
||||
msg.joint_pos[i] = motor_states_[i].position;
|
||||
msg.joint_vel[i] = motor_states_[i].velocity;
|
||||
msg.joint_torque[i] = motor_states_[i].torque;
|
||||
msg.update_counts[i] = motor_states_[i].update_count;
|
||||
}
|
||||
}
|
||||
|
||||
msg.imu_gyro = gyro;
|
||||
msg.imu_accel = accel;
|
||||
msg.quat_wxyz = quat;
|
||||
msg.projected_gravity = projected_gravity;
|
||||
msg.imu_age_ms = imu_age_ms;
|
||||
msg.imu_fresh = imu_fresh || (imu_age_ms < 60.0); // Allow brief staleness
|
||||
|
||||
msg.odom_age_ms = 0.0f;
|
||||
msg.odom_fresh = false;
|
||||
msg.odom_pos = {0.0f, 0.0f, 0.0f};
|
||||
msg.odom_quat_wxyz = {1.0f, 0.0f, 0.0f, 0.0f};
|
||||
msg.odom_linear_vel = {0.0f, 0.0f, 0.0f};
|
||||
msg.odom_angular_vel = {0.0f, 0.0f, 0.0f};
|
||||
msg.odom_local_pos = {0.0f, 0.0f, 0.0f};
|
||||
msg.odom_local_yaw = 0.0f;
|
||||
|
||||
// Populate odom fields from subscriber data
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(odom_mutex_);
|
||||
if (odom_fresh_) {
|
||||
double odom_age = (now_time - last_odom_stamp_).seconds() * 1000.0;
|
||||
msg.odom_age_ms = static_cast<float>(odom_age);
|
||||
msg.odom_fresh = (odom_age < 200.0); // 200ms threshold
|
||||
msg.odom_pos = odom_pos_;
|
||||
msg.odom_quat_wxyz = odom_quat_wxyz_;
|
||||
msg.odom_linear_vel = odom_linear_vel_;
|
||||
msg.odom_angular_vel = odom_angular_vel_;
|
||||
msg.odom_local_pos = odom_pos_;
|
||||
// Compute yaw from quaternion
|
||||
float qw = odom_quat_wxyz_[0], qx = odom_quat_wxyz_[1];
|
||||
float qy = odom_quat_wxyz_[2], qz = odom_quat_wxyz_[3];
|
||||
float siny_c = 2.0f * (qw * qz + qx * qy);
|
||||
float cosy_c = 1.0f - 2.0f * (qy * qy + qz * qz);
|
||||
msg.odom_local_yaw = std::atan2(siny_c, cosy_c);
|
||||
}
|
||||
}
|
||||
|
||||
msg.fresh_count = dry_run_ ? 16 : fresh_count_;
|
||||
msg.holdover_count = dry_run_ ? 0 : holdover_count_;
|
||||
msg.stale_max = dry_run_ ? 0 : stale_max_;
|
||||
|
||||
state_pub_->publish(msg);
|
||||
}
|
||||
|
||||
void HardwareBridgeNode::onWriteLoop()
|
||||
{
|
||||
const auto now_time = now();
|
||||
std::array<float, 16> target{};
|
||||
std::string target_source;
|
||||
double age_ms = 0.0;
|
||||
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(target_mutex_);
|
||||
target = latest_target_;
|
||||
target_source = latest_target_source_;
|
||||
|
||||
if (latest_target_stamp_.nanoseconds() > 0) {
|
||||
const auto age_ns = (now_time - latest_target_stamp_).nanoseconds();
|
||||
age_ms = age_ns > 0 ? static_cast<double>(age_ns) / 1.0e6 : 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
// Timeout guard: default stand pose if target is stale
|
||||
if (latest_target_stamp_.nanoseconds() == 0 || age_ms > target_timeout_ms_) {
|
||||
target = sim2real_common::DeploymentContract::kDefaultDofPos;
|
||||
target_source = "timeout_hold";
|
||||
}
|
||||
|
||||
// Run SafetyMonitor check on incoming target commands
|
||||
std::array<float, 3> gyro{};
|
||||
std::array<float, 3> proj_grav{};
|
||||
bool estop_active = false;
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(imu_mutex_);
|
||||
gyro = imu_gyro_;
|
||||
proj_grav = projected_gravity_;
|
||||
}
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(target_mutex_);
|
||||
estop_active = estop_triggered_;
|
||||
}
|
||||
|
||||
if (safety_enabled_ && !safety_triggered_) {
|
||||
auto safety_decision = safety_monitor_->check(target, sim2real_common::DeploymentContract::kDefaultDofPos, gyro, proj_grav, estop_active);
|
||||
if (safety_decision.level == sim2real_common::SafetyLevel::ESTOP || safety_decision.level == sim2real_common::SafetyLevel::BRAKE) {
|
||||
safety_triggered_ = true;
|
||||
safety_reason_ = "Safety Monitor Stop: " + safety_decision.message;
|
||||
RCLCPP_ERROR(get_logger(), "SAFETY TRIGGERED: %s", safety_reason_.c_str());
|
||||
} else if (safety_decision.level == sim2real_common::SafetyLevel::CLIP) {
|
||||
target = safety_decision.clipped_target;
|
||||
target_source = "safety_clip";
|
||||
RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "Safety Monitor: Joint target clipped.");
|
||||
}
|
||||
}
|
||||
|
||||
// Override to safety_brake if safety is triggered locally or by estop
|
||||
if (safety_triggered_) {
|
||||
target_source = "safety_brake";
|
||||
}
|
||||
|
||||
// 1. Joint LPF command filtering (200Hz, dt=0.005s)
|
||||
std::array<float, 12> legs_in{};
|
||||
std::array<float, 12> legs_out{};
|
||||
std::array<float, 4> wheels_in{};
|
||||
std::array<float, 4> wheels_out{};
|
||||
|
||||
std::copy(target.begin(), target.begin() + 12, legs_in.begin());
|
||||
std::copy(target.begin() + 12, target.end(), wheels_in.begin());
|
||||
|
||||
lpf_legs_->filter(legs_in.data(), legs_out.data());
|
||||
lpf_wheels_->filter(wheels_in.data(), wheels_out.data());
|
||||
|
||||
std::array<float, 16> filtered_target{};
|
||||
std::copy(legs_out.begin(), legs_out.end(), filtered_target.begin());
|
||||
std::copy(wheels_out.begin(), wheels_out.end(), filtered_target.begin() + 12);
|
||||
|
||||
// 2. Control execution (MIT mode write over CAN)
|
||||
if (!dry_run_) {
|
||||
for (std::size_t i = 0; i < 16; ++i) {
|
||||
// Coordinate transform: sim_to_real
|
||||
// real = sign * sim + offset
|
||||
float sim_val = filtered_target[i];
|
||||
float real_val = motors_[i].direction * sim_val + motors_[i].offset;
|
||||
|
||||
int fd = (motors_[i].bus == 1) ? can0_fd_ : can1_fd_;
|
||||
|
||||
if (i < 12) {
|
||||
// Leg joints: MIT position control
|
||||
// Kp & Kd depend on whether we are holding pose, running policy, or in safety damping mode
|
||||
double kp_val = sim2real_common::DeploymentContract::kLegKp;
|
||||
double kd_val = sim2real_common::DeploymentContract::kLegKd;
|
||||
|
||||
if (target_source == "safety_brake" || target_source == "safety_estop") {
|
||||
kp_val = 0.0;
|
||||
kd_val = 2.5; // Leg damping Kd
|
||||
real_val = 0.0; // Set to zero position (sign/offset will be ignored anyway under kp=0)
|
||||
} else if (target_source == "startup_soft_hold") {
|
||||
if (startup_soft_hold_start_time_.nanoseconds() == 0) {
|
||||
startup_soft_hold_start_time_ = now_time;
|
||||
}
|
||||
double elapsed = (now_time - startup_soft_hold_start_time_).seconds();
|
||||
double kp_scale = 0.125 + (1.0 - 0.125) * std::min(1.0, elapsed / 1.0); // 1.0s ramp
|
||||
kp_val = sim2real_common::DeploymentContract::kLegHoldKp * kp_scale;
|
||||
kd_val = sim2real_common::DeploymentContract::kLegHoldKd;
|
||||
} else {
|
||||
startup_soft_hold_start_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME);
|
||||
if (target_source == "timeout_hold" || target_source == "boot_hold" || target_source == "runtime_zero_hold" || target_source == "startup_hold") {
|
||||
kp_val = sim2real_common::DeploymentContract::kLegHoldKp;
|
||||
kd_val = sim2real_common::DeploymentContract::kLegHoldKd;
|
||||
}
|
||||
}
|
||||
writeOperationFrame(fd, motors_[i].id, real_val, 0.0, kp_val, kd_val, 0.0);
|
||||
} else {
|
||||
// Wheel joints: MIT velocity control (Kp = 0, Kd = kWheelKd, velocity = target, position = 0)
|
||||
double vel_real = motors_[i].direction * sim_val; // Wheels actions are in velocity, apply sign
|
||||
double kd_val = sim2real_common::DeploymentContract::kWheelKd;
|
||||
if (target_source == "safety_brake" || target_source == "safety_estop") {
|
||||
vel_real = 0.0;
|
||||
kd_val = 2.0; // Wheel damping Kd
|
||||
}
|
||||
writeOperationFrame(fd, motors_[i].id, 0.0, vel_real, 0.0, kd_val, 0.0);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool HardwareBridgeNode::enableMotor(int fd, int motor_id)
|
||||
{
|
||||
std::uint32_t ext_id = (COMM_ENABLE << 24) | (HOST_ID << 8) | motor_id;
|
||||
return sendCanFrame(fd, ext_id, nullptr, 0);
|
||||
}
|
||||
|
||||
bool HardwareBridgeNode::disableMotor(int fd, int motor_id)
|
||||
{
|
||||
std::uint32_t ext_id = (COMM_DISABLE << 24) | (HOST_ID << 8) | motor_id;
|
||||
std::uint8_t data[8] = {0};
|
||||
return sendCanFrame(fd, ext_id, data, 8);
|
||||
}
|
||||
|
||||
bool HardwareBridgeNode::setModeRaw(int fd, int motor_id, std::int8_t mode)
|
||||
{
|
||||
std::uint32_t ext_id = (COMM_WRITE_PARAMETER << 24) | (HOST_ID << 8) | motor_id;
|
||||
std::uint8_t data[8] = {0};
|
||||
data[0] = PARAM_MODE & 0xFF;
|
||||
data[1] = (PARAM_MODE >> 8) & 0xFF;
|
||||
data[4] = static_cast<std::uint8_t>(mode);
|
||||
return sendCanFrame(fd, ext_id, data, 8);
|
||||
}
|
||||
|
||||
bool HardwareBridgeNode::writeLimit(int fd, int motor_id, std::uint16_t param_id, float limit)
|
||||
{
|
||||
std::uint32_t ext_id = (COMM_WRITE_PARAMETER << 24) | (HOST_ID << 8) | motor_id;
|
||||
std::uint8_t data[8] = {0};
|
||||
data[0] = param_id & 0xFF;
|
||||
data[1] = (param_id >> 8) & 0xFF;
|
||||
std::memcpy(&data[4], &limit, sizeof(float));
|
||||
return sendCanFrame(fd, ext_id, data, 8);
|
||||
}
|
||||
|
||||
bool HardwareBridgeNode::writeOperationFrame(int fd, int motor_id, double pos, double vel, double kp_val, double kd_val, double torque)
|
||||
{
|
||||
const double P_LIMIT = 4.0 * M_PI;
|
||||
const double V_LIMIT = 44.0;
|
||||
const double T_LIMIT = 17.0;
|
||||
const double KP_LIMIT = 500.0;
|
||||
const double KD_LIMIT = 5.0;
|
||||
|
||||
double pos_clamped = std::max(-P_LIMIT, std::min(P_LIMIT, pos));
|
||||
double vel_clamped = std::max(-V_LIMIT, std::min(V_LIMIT, vel));
|
||||
double kp_clamped = std::max(0.0, std::min(KP_LIMIT, kp_val));
|
||||
double kd_clamped = std::max(0.0, std::min(KD_LIMIT, kd_val));
|
||||
double torque_clamped = std::max(-T_LIMIT, std::min(T_LIMIT, torque));
|
||||
|
||||
std::uint16_t pos_u16 = static_cast<std::uint16_t>(((pos_clamped / P_LIMIT) + 1.0) * 32767.0);
|
||||
std::uint16_t vel_u16 = static_cast<std::uint16_t>(((vel_clamped / V_LIMIT) + 1.0) * 32767.0);
|
||||
std::uint16_t kp_u16 = static_cast<std::uint16_t>((kp_clamped / KP_LIMIT) * 65535.0);
|
||||
std::uint16_t kd_u16 = static_cast<std::uint16_t>((kd_clamped / KD_LIMIT) * 65535.0);
|
||||
std::uint16_t torque_u16 = static_cast<std::uint16_t>(((torque_clamped / T_LIMIT) + 1.0) * 32767.0);
|
||||
|
||||
std::uint8_t data[8];
|
||||
pack_u16_be(&data[0], pos_u16);
|
||||
pack_u16_be(&data[2], vel_u16);
|
||||
pack_u16_be(&data[4], kp_u16);
|
||||
pack_u16_be(&data[6], kd_u16);
|
||||
|
||||
std::uint32_t ext_id = (COMM_OPERATION_CONTROL << 24) | (torque_u16 << 8) | motor_id;
|
||||
return sendCanFrame(fd, ext_id, data, 8);
|
||||
}
|
||||
|
||||
bool HardwareBridgeNode::initCan(const std::string& ifname, int& fd)
|
||||
{
|
||||
struct sockaddr_can addr;
|
||||
struct ifreq ifr;
|
||||
|
||||
if ((fd = ::socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
|
||||
RCLCPP_ERROR(get_logger(), "Failed to create SocketCAN socket for %s", ifname.c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
// Set non-blocking mode
|
||||
int flags = ::fcntl(fd, F_GETFL, 0);
|
||||
if (flags < 0 || ::fcntl(fd, F_SETFL, flags | O_NONBLOCK) < 0) {
|
||||
RCLCPP_ERROR(get_logger(), "Failed to set socket to non-blocking for %s", ifname.c_str());
|
||||
::close(fd);
|
||||
fd = -1;
|
||||
return false;
|
||||
}
|
||||
|
||||
std::strncpy(ifr.ifr_name, ifname.c_str(), IFNAMSIZ - 1);
|
||||
if (::ioctl(fd, SIOCGIFINDEX, &ifr) < 0) {
|
||||
RCLCPP_ERROR(get_logger(), "Failed to ioctl SIOCGIFINDEX for %s", ifname.c_str());
|
||||
::close(fd);
|
||||
fd = -1;
|
||||
return false;
|
||||
}
|
||||
|
||||
addr.can_family = AF_CAN;
|
||||
addr.can_ifindex = ifr.ifr_ifindex;
|
||||
|
||||
if (::bind(fd, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
|
||||
RCLCPP_ERROR(get_logger(), "Failed to bind SocketCAN socket for %s", ifname.c_str());
|
||||
::close(fd);
|
||||
fd = -1;
|
||||
return false;
|
||||
}
|
||||
|
||||
RCLCPP_INFO(get_logger(), "Successfully bound to SocketCAN interface %s", ifname.c_str());
|
||||
return true;
|
||||
}
|
||||
|
||||
bool HardwareBridgeNode::sendCanFrame(int fd, std::uint32_t can_id, const std::uint8_t* data, std::uint8_t dlc)
|
||||
{
|
||||
if (fd < 0) return false;
|
||||
struct can_frame frame;
|
||||
frame.can_id = can_id | CAN_EFF_FLAG; // Extended frame format (29-bit CAN ID)
|
||||
frame.can_dlc = dlc;
|
||||
if (data) {
|
||||
std::memcpy(frame.data, data, dlc);
|
||||
} else {
|
||||
std::memset(frame.data, 0, 8);
|
||||
}
|
||||
|
||||
ssize_t bytes_written = ::write(fd, &frame, sizeof(struct can_frame));
|
||||
if (bytes_written != sizeof(struct can_frame)) {
|
||||
int err = errno;
|
||||
// Track errors per bus for recovery logic
|
||||
if (fd == can0_fd_) {
|
||||
can0_error_count_++;
|
||||
if (can0_error_count_ >= kCanErrorThreshold) {
|
||||
RCLCPP_ERROR(get_logger(), "CAN0 write: %d consecutive errors (errno=%d: %s). Attempting reinit.",
|
||||
can0_error_count_, err, strerror(err));
|
||||
if (!reinitCan(can0_name_, can0_fd_, can0_error_count_)) {
|
||||
RCLCPP_FATAL(get_logger(), "CAN0 reinit failed! Triggering safety brake.");
|
||||
safety_triggered_ = true;
|
||||
safety_reason_ = "CAN0 bus failure - reinit failed";
|
||||
}
|
||||
}
|
||||
} else if (fd == can1_fd_) {
|
||||
can1_error_count_++;
|
||||
if (can1_error_count_ >= kCanErrorThreshold) {
|
||||
RCLCPP_ERROR(get_logger(), "CAN1 write: %d consecutive errors (errno=%d: %s). Attempting reinit.",
|
||||
can1_error_count_, err, strerror(err));
|
||||
if (!reinitCan(can1_name_, can1_fd_, can1_error_count_)) {
|
||||
RCLCPP_FATAL(get_logger(), "CAN1 reinit failed! Triggering safety brake.");
|
||||
safety_triggered_ = true;
|
||||
safety_reason_ = "CAN1 bus failure - reinit failed";
|
||||
}
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
// Reset error count on success
|
||||
if (fd == can0_fd_) can0_error_count_ = 0;
|
||||
else if (fd == can1_fd_) can1_error_count_ = 0;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool HardwareBridgeNode::readCanFrame(int fd, void* frame_ptr, int timeout_us)
|
||||
{
|
||||
if (fd < 0) return false;
|
||||
auto* frame = static_cast<struct can_frame*>(frame_ptr);
|
||||
|
||||
if (timeout_us > 0) {
|
||||
struct timeval tv;
|
||||
tv.tv_sec = 0;
|
||||
tv.tv_usec = timeout_us;
|
||||
fd_set rdfs;
|
||||
FD_ZERO(&rdfs);
|
||||
FD_SET(fd, &rdfs);
|
||||
|
||||
int ret = ::select(fd + 1, &rdfs, nullptr, nullptr, &tv);
|
||||
if (ret < 0) {
|
||||
int err = errno;
|
||||
if (err != EINTR) {
|
||||
RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000,
|
||||
"CAN select error (fd=%d): %s", fd, strerror(err));
|
||||
}
|
||||
return false;
|
||||
}
|
||||
if (ret == 0) {
|
||||
return false; // timeout, normal
|
||||
}
|
||||
}
|
||||
|
||||
ssize_t bytes_read = ::read(fd, frame, sizeof(struct can_frame));
|
||||
if (bytes_read < 0) {
|
||||
int err = errno;
|
||||
if (err != EAGAIN && err != EWOULDBLOCK) {
|
||||
RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000,
|
||||
"CAN read error (fd=%d): %s", fd, strerror(err));
|
||||
}
|
||||
return false;
|
||||
}
|
||||
return (bytes_read == sizeof(struct can_frame));
|
||||
}
|
||||
|
||||
bool HardwareBridgeNode::reinitCan(const std::string& ifname, int& fd, int& error_count)
|
||||
{
|
||||
RCLCPP_WARN(get_logger(), "Attempting to reinitialize CAN interface: %s", ifname.c_str());
|
||||
if (fd >= 0) {
|
||||
::close(fd);
|
||||
fd = -1;
|
||||
}
|
||||
bool success = initCan(ifname, fd);
|
||||
if (success) {
|
||||
error_count = 0;
|
||||
RCLCPP_INFO(get_logger(), "CAN interface %s reinitialized successfully.", ifname.c_str());
|
||||
}
|
||||
return success;
|
||||
}
|
||||
|
||||
} // namespace sim2real_hw
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::spin(std::make_shared<sim2real_hw::HardwareBridgeNode>());
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,16 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(sim2real_interfaces)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(builtin_interfaces REQUIRED)
|
||||
find_package(rosidl_default_generators REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
|
||||
rosidl_generate_interfaces(${PROJECT_NAME}
|
||||
"msg/RuntimeState.msg"
|
||||
"msg/RuntimeTarget.msg"
|
||||
DEPENDENCIES builtin_interfaces std_msgs
|
||||
)
|
||||
|
||||
ament_export_dependencies(rosidl_default_runtime)
|
||||
ament_package()
|
||||
@@ -0,0 +1,30 @@
|
||||
builtin_interfaces/Time stamp
|
||||
uint32 sequence
|
||||
|
||||
float32[16] joint_pos
|
||||
float32[16] joint_vel
|
||||
float32[16] joint_torque
|
||||
|
||||
float32[3] imu_gyro
|
||||
float32[3] imu_accel
|
||||
float32[4] quat_wxyz
|
||||
float32[3] projected_gravity
|
||||
|
||||
float32 imu_age_ms
|
||||
bool imu_fresh
|
||||
|
||||
float32 odom_age_ms
|
||||
bool odom_fresh
|
||||
float32[3] odom_pos
|
||||
float32[4] odom_quat_wxyz
|
||||
float32[3] odom_linear_vel
|
||||
float32[3] odom_angular_vel
|
||||
float32[3] odom_local_pos
|
||||
float32 odom_local_yaw
|
||||
|
||||
uint32 fresh_count
|
||||
uint32 holdover_count
|
||||
uint32 stale_max
|
||||
uint32[16] update_counts
|
||||
|
||||
string source
|
||||
@@ -0,0 +1,15 @@
|
||||
builtin_interfaces/Time stamp
|
||||
uint32 sequence
|
||||
|
||||
float32[16] target
|
||||
float32[16] raw_action
|
||||
float32[16] scaled_action
|
||||
float32[3] command
|
||||
float32[3] raw_command
|
||||
|
||||
bool zero_command
|
||||
bool runtime_released
|
||||
float32 release_alpha
|
||||
float32 target_age_ms
|
||||
|
||||
string target_source
|
||||
@@ -0,0 +1,18 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>sim2real_interfaces</name>
|
||||
<version>0.1.0</version>
|
||||
<description>ROS 2 interfaces for the sim2real wheel-leg runtime.</description>
|
||||
<maintainer email="todo@example.com">todo</maintainer>
|
||||
<license>Proprietary</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<buildtool_depend>rosidl_default_generators</buildtool_depend>
|
||||
|
||||
<depend>builtin_interfaces</depend>
|
||||
<depend>std_msgs</depend>
|
||||
|
||||
<exec_depend>rosidl_default_runtime</exec_depend>
|
||||
|
||||
<member_of_group>rosidl_interface_packages</member_of_group>
|
||||
</package>
|
||||
@@ -0,0 +1,12 @@
|
||||
cmake_minimum_required(VERSION 3.5)
|
||||
project(sim2real_nav2)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
|
||||
install(DIRECTORY
|
||||
config
|
||||
launch
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
ament_package()
|
||||
@@ -0,0 +1,232 @@
|
||||
amcl:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
alpha1: 0.2
|
||||
alpha2: 0.2
|
||||
alpha3: 0.2
|
||||
alpha4: 0.2
|
||||
alpha5: 0.2
|
||||
base_frame_id: "base_link"
|
||||
beam_skip_distance: 0.5
|
||||
beam_skip_error_threshold: 0.9
|
||||
beam_skip_threshold: 0.3
|
||||
do_beamskip: false
|
||||
global_frame_id: "map"
|
||||
odom_frame_id: "odom"
|
||||
laser_likelihood_max_dist: 2.0
|
||||
laser_max_range: -1.0
|
||||
laser_min_range: -1.0
|
||||
laser_model_type: "likelihood_field"
|
||||
max_beams: 60
|
||||
max_particles: 2000
|
||||
min_particles: 500
|
||||
recovery_alpha_fast: 0.0
|
||||
recovery_alpha_slow: 0.0
|
||||
resample_interval: 1
|
||||
robot_model_type: "nav2_amcl::DifferentialMotionModel"
|
||||
save_pose_rate: 0.5
|
||||
sigma_hit: 0.2
|
||||
transform_tolerance: 1.0
|
||||
update_min_d: 0.25
|
||||
update_min_a: 0.2
|
||||
z_hit: 0.5
|
||||
z_max: 0.05
|
||||
z_rand: 0.5
|
||||
z_short: 0.05
|
||||
scan_topic: "scan"
|
||||
|
||||
bt_navigator:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
odom_frame: odom
|
||||
default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
|
||||
plugin_lib_names:
|
||||
- nav2_back_up_action_bt_node
|
||||
- nav2_spin_action_bt_node
|
||||
- nav2_wait_action_bt_node
|
||||
- nav2_clear_costmap_service_bt_node
|
||||
- nav2_is_stuck_condition_bt_node
|
||||
- nav2_goal_reached_condition_bt_node
|
||||
- nav2_goal_updated_condition_bt_node
|
||||
- nav2_initial_pose_received_condition_bt_node
|
||||
- nav2_recompute_path_to_pose_action_bt_node
|
||||
- nav2_compute_path_to_pose_action_bt_node
|
||||
- nav2_follow_path_action_bt_node
|
||||
- nav2_rate_controller_bt_node
|
||||
- nav2_distance_controller_bt_node
|
||||
- nav2_speed_controller_bt_node
|
||||
- nav2_truncate_path_action_bt_node
|
||||
- nav2_goal_updater_node
|
||||
- nav2_recovery_node
|
||||
- nav2_pipeline_sequence_node
|
||||
- nav2_round_robin_node
|
||||
- nav2_transform_available_condition_bt_node
|
||||
- nav2_time_expired_condition_bt_node
|
||||
- nav2_distance_traveled_condition_bt_node
|
||||
|
||||
controller_server:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
controller_frequency: 10.0
|
||||
min_x_velocity_threshold: 0.001
|
||||
min_y_velocity_threshold: 0.001
|
||||
min_theta_velocity_threshold: 0.001
|
||||
failure_tolerance: 0.3
|
||||
progress_checker_plugin: "progress_checker"
|
||||
goal_checker_plugins: ["general_goal_checker"]
|
||||
controller_plugins: ["FollowPath"]
|
||||
|
||||
progress_checker:
|
||||
plugin: "nav2_controller::SimpleProgressChecker"
|
||||
required_movement_radius: 0.5
|
||||
movement_time_allowance: 10.0
|
||||
|
||||
general_goal_checker:
|
||||
stateful: true
|
||||
plugin: "nav2_controller::SimpleGoalChecker"
|
||||
xy_goal_tolerance: 0.25
|
||||
yaw_goal_tolerance: 0.25
|
||||
|
||||
FollowPath:
|
||||
plugin: "dwb_core::DWBLocalPlanner"
|
||||
prune_plan: true
|
||||
prune_distance: 1.0
|
||||
debug_trajectory_details: false
|
||||
trajectory_generator_name: "dwb_plugins::StandardTrajectoryGenerator"
|
||||
velocity_iterator_name: "dwb_plugins::LimitedVelocityIterator"
|
||||
critics: ["ObstacleFootprint", "PathAlign", "GoalAlign", "PathDist", "GoalDist"]
|
||||
|
||||
# DWB Velocity parameters matching Units
|
||||
min_vel_x: 0.0
|
||||
max_vel_x: 0.6
|
||||
min_vel_y: 0.0
|
||||
max_vel_y: 0.0
|
||||
max_vel_theta: 2.0
|
||||
min_speed_xy: 0.0
|
||||
max_speed_xy: 0.6
|
||||
min_speed_theta: 0.0
|
||||
|
||||
# DWB Acceleration parameters matching Units
|
||||
acc_lim_x: 15.0
|
||||
acc_lim_y: 15.0
|
||||
acc_lim_theta: 12.0
|
||||
decel_lim_x: -15.0
|
||||
decel_lim_y: -15.0
|
||||
decel_lim_theta: -12.0
|
||||
|
||||
# Critics tuning
|
||||
ObstacleFootprint.scale: 0.2
|
||||
PathAlign.scale: 32.0
|
||||
PathAlign.forward_point_distance: 0.1
|
||||
GoalAlign.scale: 24.0
|
||||
GoalAlign.forward_point_distance: 0.1
|
||||
PathDist.scale: 32.0
|
||||
GoalDist.scale: 24.0
|
||||
|
||||
planner_server:
|
||||
ros__parameters:
|
||||
expected_planner_frequency: 1.0
|
||||
use_sim_time: false
|
||||
planner_plugins: ["GridTransition"]
|
||||
GridTransition:
|
||||
plugin: "nav2_navfn_planner/NavfnPlanner"
|
||||
tolerance: 0.5
|
||||
use_astar: false
|
||||
allow_unknown: true
|
||||
|
||||
behavior_server:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
recovery_plugins: ["spin", "backup", "wait"]
|
||||
spin:
|
||||
plugin: "nav2_behaviors::Spin"
|
||||
backup:
|
||||
plugin: "nav2_behaviors::Backup"
|
||||
wait:
|
||||
plugin: "nav2_behaviors::Wait"
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
transform_tolerance: 0.1
|
||||
simulate_ahead_time: 2.0
|
||||
max_rotational_vel: 1.0
|
||||
min_rotational_vel: 0.4
|
||||
rotational_acc_lim: 3.2
|
||||
|
||||
global_costmap:
|
||||
global_costmap:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
robot_radius: 0.25
|
||||
obstacle_range: 2.5
|
||||
raytrace_range: 3.0
|
||||
publish_frequency: 1.0
|
||||
update_frequency: 1.0
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
rolling_window: true
|
||||
width: 30
|
||||
height: 30
|
||||
resolution: 0.05
|
||||
track_unknown_space: true
|
||||
plugins: ["obstacle_layer", "inflation_layer"]
|
||||
|
||||
obstacle_layer:
|
||||
plugin: "nav2_costmap_2d::ObstacleLayer"
|
||||
enabled: true
|
||||
observation_sources: pointcloud
|
||||
pointcloud:
|
||||
topic: /odin1/cloud_slam
|
||||
sensor_frame: base_link
|
||||
data_type: "PointCloud2"
|
||||
clearing: true
|
||||
marking: true
|
||||
max_obstacle_height: 2.0
|
||||
min_obstacle_height: 0.05
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.1
|
||||
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
enabled: true
|
||||
inflation_radius: 0.6
|
||||
cost_scaling_factor: 4.0
|
||||
|
||||
local_costmap:
|
||||
local_costmap:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
robot_radius: 0.25
|
||||
obstacle_range: 2.5
|
||||
raytrace_range: 3.0
|
||||
publish_frequency: 5.0
|
||||
update_frequency: 5.0
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
rolling_window: true
|
||||
width: 4
|
||||
height: 4
|
||||
resolution: 0.05
|
||||
plugins: ["obstacle_layer", "inflation_layer"]
|
||||
|
||||
obstacle_layer:
|
||||
plugin: "nav2_costmap_2d::ObstacleLayer"
|
||||
enabled: true
|
||||
observation_sources: pointcloud
|
||||
pointcloud:
|
||||
topic: /odin1/cloud_slam
|
||||
sensor_frame: base_link
|
||||
data_type: "PointCloud2"
|
||||
clearing: true
|
||||
marking: true
|
||||
max_obstacle_height: 2.0
|
||||
min_obstacle_height: 0.05
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.1
|
||||
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
enabled: true
|
||||
inflation_radius: 0.4
|
||||
cost_scaling_factor: 4.0
|
||||
@@ -0,0 +1,146 @@
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
def generate_launch_description():
|
||||
# Get package directories
|
||||
my_share_dir = get_package_share_directory('sim2real_nav2')
|
||||
|
||||
# Declare launch configuration variables
|
||||
params_file_arg = DeclareLaunchArgument(
|
||||
'params_file',
|
||||
default_value=os.path.join(my_share_dir, 'config', 'nav2_params.yaml'),
|
||||
description='Full path to the ROS2 parameters file to use for all launched nodes'
|
||||
)
|
||||
|
||||
params_file = LaunchConfiguration('params_file')
|
||||
|
||||
# Define Nav2 lifecycle nodes to run
|
||||
lifecycle_nodes = ['controller_server', 'planner_server', 'behavior_server', 'bt_navigator',
|
||||
'global_costmap', 'local_costmap', 'amcl']
|
||||
|
||||
# Controller server node
|
||||
controller_server_node = Node(
|
||||
package='nav2_controller',
|
||||
executable='controller_server',
|
||||
name='controller_server',
|
||||
output='screen',
|
||||
parameters=[params_file]
|
||||
)
|
||||
|
||||
# Planner server node
|
||||
planner_server_node = Node(
|
||||
package='nav2_planner',
|
||||
executable='planner_server',
|
||||
name='planner_server',
|
||||
output='screen',
|
||||
parameters=[params_file]
|
||||
)
|
||||
|
||||
# Behavior server node (called recovery_server in Galactic, behavior_server in Humble)
|
||||
behavior_server_node = Node(
|
||||
package='nav2_behaviors',
|
||||
executable='behavior_server',
|
||||
name='behavior_server',
|
||||
output='screen',
|
||||
parameters=[params_file]
|
||||
)
|
||||
|
||||
# BT Navigator node
|
||||
bt_navigator_node = Node(
|
||||
package='nav2_bt_navigator',
|
||||
executable='bt_navigator',
|
||||
name='bt_navigator',
|
||||
output='screen',
|
||||
parameters=[params_file]
|
||||
)
|
||||
|
||||
# Global costmap node
|
||||
global_costmap_node = Node(
|
||||
package='nav2_costmap_2d',
|
||||
executable='nav2_costmap_2d',
|
||||
name='global_costmap',
|
||||
output='screen',
|
||||
parameters=[params_file]
|
||||
)
|
||||
|
||||
# Local costmap node
|
||||
local_costmap_node = Node(
|
||||
package='nav2_costmap_2d',
|
||||
executable='nav2_costmap_2d',
|
||||
name='local_costmap',
|
||||
output='screen',
|
||||
parameters=[params_file]
|
||||
)
|
||||
|
||||
# AMCL node (Adaptive Monte Carlo Localization), now receives /scan from pointcloud_to_laserscan
|
||||
amcl_node = Node(
|
||||
package='nav2_amcl',
|
||||
executable='amcl',
|
||||
name='amcl',
|
||||
output='screen',
|
||||
parameters=[params_file]
|
||||
)
|
||||
|
||||
# PointCloud2 to LaserScan converter (AMCL needs LaserScan, LiDAR publishes PointCloud2)
|
||||
pointcloud_to_laserscan_node = Node(
|
||||
package='pointcloud_to_laserscan',
|
||||
executable='pointcloud_to_laserscan_node',
|
||||
name='pointcloud_to_laserscan',
|
||||
output='screen',
|
||||
remappings=[
|
||||
('cloud_in', '/odin1/cloud_slam'),
|
||||
('scan', '/scan')
|
||||
],
|
||||
parameters=[{
|
||||
'target_frame': 'base_link',
|
||||
'transform_tolerance': 0.01,
|
||||
'min_height': 0.05,
|
||||
'max_height': 2.0,
|
||||
'angle_min': -3.14159,
|
||||
'angle_max': 3.14159,
|
||||
'angle_increment': 0.0087, # ~0.5 degrees
|
||||
'scan_time': 0.1,
|
||||
'range_min': 0.1,
|
||||
'range_max': 10.0,
|
||||
'use_inf': True,
|
||||
'inf_epsilon': 1.0,
|
||||
'concurrency_level': 1
|
||||
}]
|
||||
)
|
||||
|
||||
# Lifecycle manager node to transition Nav2 nodes to ACTIVE state
|
||||
lifecycle_manager_node = Node(
|
||||
package='nav2_lifecycle_manager',
|
||||
executable='lifecycle_manager',
|
||||
name='lifecycle_manager_navigation',
|
||||
output='screen',
|
||||
parameters=[{
|
||||
'use_sim_time': False,
|
||||
'autostart': True,
|
||||
'node_names': lifecycle_nodes
|
||||
}]
|
||||
)
|
||||
|
||||
# Create launch description
|
||||
ld = LaunchDescription()
|
||||
|
||||
# Set stdout line buffering
|
||||
ld.add_action(SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'))
|
||||
|
||||
# Add actions
|
||||
ld.add_action(params_file_arg)
|
||||
ld.add_action(controller_server_node)
|
||||
ld.add_action(planner_server_node)
|
||||
ld.add_action(behavior_server_node)
|
||||
ld.add_action(bt_navigator_node)
|
||||
ld.add_action(global_costmap_node)
|
||||
ld.add_action(local_costmap_node)
|
||||
ld.add_action(amcl_node)
|
||||
ld.add_action(pointcloud_to_laserscan_node)
|
||||
ld.add_action(lifecycle_manager_node)
|
||||
|
||||
return ld
|
||||
@@ -0,0 +1,18 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>sim2real_nav2</name>
|
||||
<version>0.0.1</version>
|
||||
<description>ROS2 Nav2 configuration package for legged-wheeled quadruped</description>
|
||||
<maintainer email="31560@todo.todo">USER</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<exec_depend>nav2_bringup</exec_depend>
|
||||
<exec_depend>navigation2</exec_depend>
|
||||
<exec_depend>pointcloud_to_laserscan</exec_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,99 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(sim2real_runtime)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
find_package(nav_msgs REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
find_package(tf2_ros REQUIRED)
|
||||
find_package(sim2real_common REQUIRED)
|
||||
find_package(sim2real_interfaces REQUIRED)
|
||||
|
||||
# Search for ONNX Runtime headers and library
|
||||
find_path(ONNXRUNTIME_INCLUDE_DIR onnxruntime_cxx_api.h
|
||||
PATHS
|
||||
/usr/include
|
||||
/usr/include/onnxruntime
|
||||
/usr/local/include
|
||||
/usr/local/include/onnxruntime
|
||||
/opt/onnxruntime/include
|
||||
)
|
||||
|
||||
find_library(ONNXRUNTIME_LIBRARY NAMES onnxruntime
|
||||
PATHS
|
||||
/usr/lib
|
||||
/usr/lib/x86_64-linux-gnu
|
||||
/usr/lib/aarch64-linux-gnu
|
||||
/usr/local/lib
|
||||
/opt/onnxruntime/lib
|
||||
)
|
||||
|
||||
get_filename_component(ONNXRUNTIME_LIBRARY_DIR ${ONNXRUNTIME_LIBRARY} DIRECTORY)
|
||||
|
||||
if(NOT ONNXRUNTIME_INCLUDE_DIR OR NOT ONNXRUNTIME_LIBRARY)
|
||||
message(FATAL_ERROR "ONNX Runtime not found! Please install it or specify include/library paths.")
|
||||
endif()
|
||||
|
||||
add_executable(sim2real_runtime_node
|
||||
src/policy_runtime_node.cpp
|
||||
)
|
||||
|
||||
add_executable(odom_relay_node
|
||||
src/odom_relay_node.cpp
|
||||
)
|
||||
|
||||
target_include_directories(sim2real_runtime_node PRIVATE
|
||||
include
|
||||
${ONNXRUNTIME_INCLUDE_DIR}
|
||||
)
|
||||
|
||||
target_include_directories(odom_relay_node PRIVATE include)
|
||||
|
||||
target_link_libraries(sim2real_runtime_node
|
||||
${ONNXRUNTIME_LIBRARY}
|
||||
)
|
||||
|
||||
set_target_properties(sim2real_runtime_node PROPERTIES
|
||||
BUILD_RPATH "${ONNXRUNTIME_LIBRARY_DIR}"
|
||||
INSTALL_RPATH "${ONNXRUNTIME_LIBRARY_DIR}"
|
||||
)
|
||||
|
||||
target_compile_features(sim2real_runtime_node PRIVATE cxx_std_17)
|
||||
|
||||
ament_target_dependencies(sim2real_runtime_node
|
||||
geometry_msgs
|
||||
nav_msgs
|
||||
rclcpp
|
||||
std_msgs
|
||||
tf2_ros
|
||||
sim2real_common
|
||||
sim2real_interfaces
|
||||
)
|
||||
|
||||
ament_target_dependencies(odom_relay_node
|
||||
geometry_msgs
|
||||
nav_msgs
|
||||
rclcpp
|
||||
tf2_ros
|
||||
)
|
||||
|
||||
install(
|
||||
DIRECTORY include/
|
||||
DESTINATION include
|
||||
)
|
||||
|
||||
install(
|
||||
TARGETS sim2real_runtime_node odom_relay_node
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
install(
|
||||
PROGRAMS
|
||||
src/remote_uart_node.py
|
||||
src/cmd_mux_node.py
|
||||
src/web_udp_bridge_node.py
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
ament_package()
|
||||
+32
@@ -0,0 +1,32 @@
|
||||
#pragma once
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "nav_msgs/msg/odometry.hpp"
|
||||
#include "geometry_msgs/msg/transform_stamped.hpp"
|
||||
#include "tf2_ros/transform_broadcaster.h"
|
||||
|
||||
namespace sim2real_runtime
|
||||
{
|
||||
|
||||
/// Subscribes to odin_ros_driver's odometry (e.g. /odin1/odometry),
|
||||
/// remaps child_frame_id to "base_link", republishes on /odom,
|
||||
/// and broadcasts the odom → base_link TF.
|
||||
class OdomRelayNode : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
OdomRelayNode();
|
||||
|
||||
private:
|
||||
void onOdom(const nav_msgs::msg::Odometry::SharedPtr msg);
|
||||
|
||||
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;
|
||||
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
|
||||
|
||||
std::string odom_input_topic_;
|
||||
std::string odom_output_topic_;
|
||||
std::string base_frame_;
|
||||
bool publish_tf_;
|
||||
};
|
||||
|
||||
} // namespace sim2real_runtime
|
||||
+124
@@ -0,0 +1,124 @@
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <mutex>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
|
||||
#include "geometry_msgs/msg/twist.hpp"
|
||||
#include "geometry_msgs/msg/twist_stamped.hpp"
|
||||
#include "std_msgs/msg/bool.hpp"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "sim2real_interfaces/msg/runtime_state.hpp"
|
||||
#include "sim2real_interfaces/msg/runtime_target.hpp"
|
||||
#include "sim2real_common/stand_balance_controller.hpp"
|
||||
#include "sim2real_common/safety_monitor.hpp"
|
||||
#include "sim2real_common/runtime_guard.hpp"
|
||||
|
||||
// ONNXRuntime C++ API
|
||||
#include <onnxruntime_cxx_api.h>
|
||||
|
||||
namespace sim2real_runtime
|
||||
{
|
||||
|
||||
class PolicyRuntimeNode : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
PolicyRuntimeNode();
|
||||
|
||||
private:
|
||||
void onState(const sim2real_interfaces::msg::RuntimeState::SharedPtr msg);
|
||||
void onCmdVel(const geometry_msgs::msg::Twist::SharedPtr msg);
|
||||
void onCmdVelStamped(const geometry_msgs::msg::TwistStamped::SharedPtr msg);
|
||||
void applyCmdVel(float vx, float vy, float vyaw);
|
||||
void onPolicyLoop();
|
||||
|
||||
std::array<float, 53> buildObservation(
|
||||
const sim2real_interfaces::msg::RuntimeState & state,
|
||||
const std::array<float, 3> & cmd,
|
||||
const std::array<float, 16> & last_actions) const;
|
||||
|
||||
std::array<float, 16> runPolicy(const std::array<float, 53> & obs);
|
||||
bool isZeroCommand(const std::array<float, 3> & cmd, const std::array<float, 3> & imu_gyro) const;
|
||||
bool isCommandActive(const std::array<float, 3> & cmd) const;
|
||||
|
||||
rclcpp::Publisher<sim2real_interfaces::msg::RuntimeTarget>::SharedPtr target_pub_;
|
||||
rclcpp::Subscription<sim2real_interfaces::msg::RuntimeState>::SharedPtr state_sub_;
|
||||
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_sub_;
|
||||
rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr cmd_stamped_sub_;
|
||||
rclcpp::TimerBase::SharedPtr policy_timer_;
|
||||
|
||||
std::mutex mutex_;
|
||||
sim2real_interfaces::msg::RuntimeState latest_state_;
|
||||
bool has_state_{false};
|
||||
std::chrono::steady_clock::time_point last_state_recv_time_{};
|
||||
std::array<float, 3> cmd_{{0.0f, 0.0f, 0.0f}};
|
||||
std::array<float, 3> raw_cmd_{{0.0f, 0.0f, 0.0f}};
|
||||
std::array<float, 16> last_actions_{};
|
||||
std::uint32_t sequence_{0};
|
||||
|
||||
// Startup State Machine
|
||||
enum class StartupState {
|
||||
BOOT_HOLD,
|
||||
STARTUP_SOFT_HOLD,
|
||||
STARTUP_TRANSITION,
|
||||
STARTUP_HOLD_AFTER,
|
||||
RUNTIME
|
||||
};
|
||||
StartupState startup_state_{StartupState::BOOT_HOLD};
|
||||
std::array<float, 16> start_pose_{};
|
||||
std::array<float, 16> startup_delta_{};
|
||||
rclcpp::Time state_start_time_{0, 0, RCL_ROS_TIME};
|
||||
double transition_time_{4.0};
|
||||
double hold_time_{1.0};
|
||||
std::unique_ptr<sim2real_common::StandBalanceController> stand_balance_;
|
||||
|
||||
// ONNX Runtime members
|
||||
std::string model_path_{"policies/model_rough.onnx"};
|
||||
bool use_cuda_{false}; // enable CUDA Execution Provider on Orin Nano
|
||||
std::unique_ptr<Ort::Env> env_;
|
||||
std::unique_ptr<Ort::Session> session_;
|
||||
std::unique_ptr<Ort::MemoryInfo> memory_info_;
|
||||
|
||||
std::vector<std::string> input_names_str_;
|
||||
std::vector<std::string> output_names_str_;
|
||||
std::vector<const char*> input_names_char_;
|
||||
std::vector<const char*> output_names_char_;
|
||||
|
||||
std::vector<std::int64_t> input_shape_;
|
||||
std::vector<std::int64_t> output_shape_;
|
||||
|
||||
// Command filter and release states
|
||||
std::array<float, 3> filtered_cmd_{{0.0f, 0.0f, 0.0f}};
|
||||
float release_alpha_{0.0f};
|
||||
float command_release_s_{0.35f};
|
||||
float release_command_hold_s_{0.12f};
|
||||
float release_posture_max_err_{0.35f};
|
||||
float release_target_blend_s_{0.30f};
|
||||
float clip_obs_{100.0f};
|
||||
bool hold_zero_command_pose_{true};
|
||||
bool enable_zero_cmd_suppression_{true};
|
||||
bool require_active_command_to_release_{true};
|
||||
bool zero_cmd_use_yaw_rate_{false};
|
||||
bool runtime_released_{false};
|
||||
float release_active_time_{0.0f};
|
||||
float zero_cmd_lin_thresh_{0.05f};
|
||||
float zero_cmd_yaw_thresh_{0.05f};
|
||||
float zero_yaw_rate_thresh_{0.10f};
|
||||
|
||||
// E-stop and Safety variables
|
||||
rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr estop_sub_;
|
||||
std::atomic<bool> estop_triggered_{false};
|
||||
std::atomic<bool> safety_enabled_{true};
|
||||
std::atomic<bool> safety_triggered_{false};
|
||||
std::string safety_reason_{""};
|
||||
std::unique_ptr<sim2real_common::SafetyMonitor> safety_monitor_;
|
||||
std::unique_ptr<sim2real_common::RuntimeGuard> runtime_guard_;
|
||||
|
||||
void onEstop(const std_msgs::msg::Bool::SharedPtr msg);
|
||||
};
|
||||
|
||||
} // namespace sim2real_runtime
|
||||
@@ -0,0 +1,24 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>sim2real_runtime</name>
|
||||
<version>0.1.0</version>
|
||||
<description>Policy runtime node for sim2real_ros2.</description>
|
||||
<maintainer email="todo@example.com">todo</maintainer>
|
||||
<license>Proprietary</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>nav_msgs</depend>
|
||||
<depend>rclcpp</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>tf2_ros</depend>
|
||||
<depend>sim2real_common</depend>
|
||||
<depend>sim2real_interfaces</depend>
|
||||
<exec_depend>python3-serial</exec_depend>
|
||||
<exec_depend>rclpy</exec_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,190 @@
|
||||
#!/usr/bin/env python3
|
||||
from __future__ import annotations
|
||||
|
||||
from enum import Enum
|
||||
from typing import Optional
|
||||
|
||||
import rclpy
|
||||
from geometry_msgs.msg import Twist
|
||||
from rclpy.executors import ExternalShutdownException
|
||||
from rclpy.node import Node
|
||||
from std_msgs.msg import Bool, String
|
||||
|
||||
|
||||
class ControlMode(str, Enum):
|
||||
DISABLED = "DISABLED"
|
||||
REMOTE = "REMOTE"
|
||||
WEB = "WEB"
|
||||
NAV = "NAV"
|
||||
|
||||
|
||||
class CmdMuxNode(Node):
|
||||
def __init__(self) -> None:
|
||||
super().__init__("sim2real_cmd_mux_node", allow_undeclared_parameters=True)
|
||||
|
||||
self.default_mode = str(self.declare_parameter("cmd_mux_default_mode", "REMOTE").value).upper()
|
||||
self.output_hz = float(self.declare_parameter("cmd_mux_output_hz", 50.0).value)
|
||||
self.remote_timeout_ms = float(self.declare_parameter("cmd_mux_remote_timeout_ms", 250.0).value)
|
||||
self.web_timeout_ms = float(self.declare_parameter("cmd_mux_web_timeout_ms", 300.0).value)
|
||||
self.nav_timeout_ms = float(self.declare_parameter("cmd_mux_nav_timeout_ms", 500.0).value)
|
||||
self.max_vx = float(self.declare_parameter("cmd_mux_max_vx", 0.8).value)
|
||||
self.max_vy = float(self.declare_parameter("cmd_mux_max_vy", 0.3).value)
|
||||
self.max_yaw = float(self.declare_parameter("cmd_mux_max_yaw_rate", 0.5).value)
|
||||
self.max_vx_acc = float(self.declare_parameter("cmd_mux_max_vx_acc", 1.0).value)
|
||||
self.max_vy_acc = float(self.declare_parameter("cmd_mux_max_vy_acc", 1.0).value)
|
||||
self.max_yaw_acc = float(self.declare_parameter("cmd_mux_max_yaw_acc", 1.5).value)
|
||||
|
||||
self.mode = self.parse_mode(self.default_mode)
|
||||
self.estop = False
|
||||
self.remote_enabled = self.mode == ControlMode.REMOTE
|
||||
self.web_enabled = self.mode == ControlMode.WEB
|
||||
self.nav_enabled = self.mode == ControlMode.NAV
|
||||
|
||||
self.latest_remote = Twist()
|
||||
self.latest_web = Twist()
|
||||
self.latest_nav = Twist()
|
||||
self.remote_stamp: Optional[rclpy.time.Time] = None
|
||||
self.web_stamp: Optional[rclpy.time.Time] = None
|
||||
self.nav_stamp: Optional[rclpy.time.Time] = None
|
||||
self.last_output = Twist()
|
||||
self.last_pub_time = self.get_clock().now()
|
||||
|
||||
self.cmd_pub = self.create_publisher(Twist, "cmd_vel", 10)
|
||||
self.mode_pub = self.create_publisher(String, "control/mode_state", 10)
|
||||
self.status_pub = self.create_publisher(String, "control/mux_status", 10)
|
||||
|
||||
self.create_subscription(Twist, "cmd_vel_remote", self.on_remote, 10)
|
||||
self.create_subscription(Twist, "cmd_vel_web", self.on_web, 10)
|
||||
self.create_subscription(Twist, "cmd_vel_nav", self.on_nav, 10)
|
||||
self.create_subscription(String, "control/mode", self.on_mode, 10)
|
||||
self.create_subscription(Bool, "remote/enabled", self.on_remote_enabled, 10)
|
||||
self.create_subscription(Bool, "web/enabled", self.on_web_enabled, 10)
|
||||
self.create_subscription(Bool, "nav/enabled", self.on_nav_enabled, 10)
|
||||
self.create_subscription(Bool, "/safety/estop", self.on_estop, 10)
|
||||
|
||||
period = 1.0 / self.output_hz if self.output_hz > 0.0 else 0.02
|
||||
self.timer = self.create_timer(period, self.on_timer)
|
||||
self.get_logger().info(f"Command mux started in mode {self.mode.value}")
|
||||
|
||||
def parse_mode(self, value: str) -> ControlMode:
|
||||
try:
|
||||
return ControlMode(value.upper())
|
||||
except ValueError:
|
||||
self.get_logger().warn(f"Unknown control mode '{value}', using DISABLED")
|
||||
return ControlMode.DISABLED
|
||||
|
||||
def on_remote(self, msg: Twist) -> None:
|
||||
self.latest_remote = msg
|
||||
self.remote_stamp = self.get_clock().now()
|
||||
|
||||
def on_web(self, msg: Twist) -> None:
|
||||
self.latest_web = msg
|
||||
self.web_stamp = self.get_clock().now()
|
||||
|
||||
def on_nav(self, msg: Twist) -> None:
|
||||
self.latest_nav = msg
|
||||
self.nav_stamp = self.get_clock().now()
|
||||
|
||||
def on_mode(self, msg: String) -> None:
|
||||
new_mode = self.parse_mode(msg.data)
|
||||
if new_mode != self.mode:
|
||||
self.mode = new_mode
|
||||
self.remote_enabled = self.mode == ControlMode.REMOTE
|
||||
self.web_enabled = self.mode == ControlMode.WEB
|
||||
self.nav_enabled = self.mode == ControlMode.NAV
|
||||
self.get_logger().info(f"Control mode changed to {self.mode.value}")
|
||||
|
||||
def on_remote_enabled(self, msg: Bool) -> None:
|
||||
self.remote_enabled = bool(msg.data)
|
||||
if self.remote_enabled:
|
||||
self.mode = ControlMode.REMOTE
|
||||
|
||||
def on_web_enabled(self, msg: Bool) -> None:
|
||||
self.web_enabled = bool(msg.data)
|
||||
if self.web_enabled:
|
||||
self.mode = ControlMode.WEB
|
||||
|
||||
def on_nav_enabled(self, msg: Bool) -> None:
|
||||
self.nav_enabled = bool(msg.data)
|
||||
if self.nav_enabled:
|
||||
self.mode = ControlMode.NAV
|
||||
|
||||
def on_estop(self, msg: Bool) -> None:
|
||||
self.estop = bool(msg.data)
|
||||
if self.estop:
|
||||
self.mode = ControlMode.DISABLED
|
||||
|
||||
def on_timer(self) -> None:
|
||||
now = self.get_clock().now()
|
||||
target = Twist()
|
||||
source = "zero"
|
||||
|
||||
if not self.estop:
|
||||
if self.mode == ControlMode.REMOTE and self.remote_enabled and self.is_fresh(self.remote_stamp, self.remote_timeout_ms, now):
|
||||
target = self.latest_remote
|
||||
source = "remote"
|
||||
elif self.mode == ControlMode.WEB and self.web_enabled and self.is_fresh(self.web_stamp, self.web_timeout_ms, now):
|
||||
target = self.latest_web
|
||||
source = "web"
|
||||
elif self.mode == ControlMode.NAV and self.nav_enabled and self.is_fresh(self.nav_stamp, self.nav_timeout_ms, now):
|
||||
target = self.latest_nav
|
||||
source = "nav"
|
||||
|
||||
target = self.limit_twist(target)
|
||||
target = self.accel_limit(target, now)
|
||||
self.cmd_pub.publish(target)
|
||||
self.mode_pub.publish(String(data=self.mode.value))
|
||||
self.status_pub.publish(String(data=f"mode={self.mode.value},source={source},estop={self.estop}"))
|
||||
|
||||
def is_fresh(self, stamp: Optional[rclpy.time.Time], timeout_ms: float, now: rclpy.time.Time) -> bool:
|
||||
if stamp is None:
|
||||
return False
|
||||
age_ms = (now - stamp).nanoseconds / 1.0e6
|
||||
return age_ms <= timeout_ms
|
||||
|
||||
def limit_twist(self, msg: Twist) -> Twist:
|
||||
out = Twist()
|
||||
out.linear.x = self.clamp(msg.linear.x, -self.max_vx, self.max_vx)
|
||||
out.linear.y = self.clamp(msg.linear.y, -self.max_vy, self.max_vy)
|
||||
out.angular.z = self.clamp(msg.angular.z, -self.max_yaw, self.max_yaw)
|
||||
return out
|
||||
|
||||
def accel_limit(self, target: Twist, now: rclpy.time.Time) -> Twist:
|
||||
dt = max((now - self.last_pub_time).nanoseconds / 1.0e9, 1.0e-3)
|
||||
out = Twist()
|
||||
out.linear.x = self.step(self.last_output.linear.x, target.linear.x, self.max_vx_acc * dt)
|
||||
out.linear.y = self.step(self.last_output.linear.y, target.linear.y, self.max_vy_acc * dt)
|
||||
out.angular.z = self.step(self.last_output.angular.z, target.angular.z, self.max_yaw_acc * dt)
|
||||
self.last_output = out
|
||||
self.last_pub_time = now
|
||||
return out
|
||||
|
||||
@staticmethod
|
||||
def clamp(value: float, low: float, high: float) -> float:
|
||||
return max(low, min(high, float(value)))
|
||||
|
||||
@staticmethod
|
||||
def step(current: float, target: float, max_delta: float) -> float:
|
||||
delta = target - current
|
||||
if delta > max_delta:
|
||||
return current + max_delta
|
||||
if delta < -max_delta:
|
||||
return current - max_delta
|
||||
return target
|
||||
|
||||
|
||||
def main(args: Optional[list[str]] = None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = CmdMuxNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except (KeyboardInterrupt, ExternalShutdownException):
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,60 @@
|
||||
#include "sim2real_runtime/odom_relay_node.hpp"
|
||||
|
||||
namespace sim2real_runtime
|
||||
{
|
||||
|
||||
OdomRelayNode::OdomRelayNode()
|
||||
: Node("odom_relay_node")
|
||||
{
|
||||
odom_input_topic_ = declare_parameter<std::string>("odom_input_topic", "/odin1/odometry");
|
||||
odom_output_topic_ = declare_parameter<std::string>("odom_output_topic", "/odom");
|
||||
base_frame_ = declare_parameter<std::string>("base_frame", "base_link");
|
||||
publish_tf_ = declare_parameter<bool>("publish_tf", true);
|
||||
|
||||
odom_sub_ = create_subscription<nav_msgs::msg::Odometry>(
|
||||
odom_input_topic_, 10,
|
||||
std::bind(&OdomRelayNode::onOdom, this, std::placeholders::_1));
|
||||
|
||||
odom_pub_ = create_publisher<nav_msgs::msg::Odometry>(odom_output_topic_, 10);
|
||||
|
||||
if (publish_tf_) {
|
||||
tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
|
||||
}
|
||||
|
||||
RCLCPP_INFO(get_logger(),
|
||||
"Odom relay: %s -> %s (base_frame=%s, publish_tf=%s)",
|
||||
odom_input_topic_.c_str(), odom_output_topic_.c_str(),
|
||||
base_frame_.c_str(), publish_tf_ ? "true" : "false");
|
||||
}
|
||||
|
||||
void OdomRelayNode::onOdom(const nav_msgs::msg::Odometry::SharedPtr msg)
|
||||
{
|
||||
// Remap child_frame_id and republish
|
||||
auto out_msg = *msg;
|
||||
out_msg.header.frame_id = "odom";
|
||||
out_msg.child_frame_id = base_frame_;
|
||||
odom_pub_->publish(out_msg);
|
||||
|
||||
// Broadcast TF: odom → base_link
|
||||
if (publish_tf_ && tf_broadcaster_) {
|
||||
geometry_msgs::msg::TransformStamped tf;
|
||||
tf.header.stamp = msg->header.stamp;
|
||||
tf.header.frame_id = "odom";
|
||||
tf.child_frame_id = base_frame_;
|
||||
tf.transform.translation.x = msg->pose.pose.position.x;
|
||||
tf.transform.translation.y = msg->pose.pose.position.y;
|
||||
tf.transform.translation.z = msg->pose.pose.position.z;
|
||||
tf.transform.rotation = msg->pose.pose.orientation;
|
||||
tf_broadcaster_->sendTransform(tf);
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace sim2real_runtime
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::spin(std::make_shared<sim2real_runtime::OdomRelayNode>());
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,583 @@
|
||||
#include "sim2real_runtime/policy_runtime_node.hpp"
|
||||
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <algorithm>
|
||||
|
||||
#include "sim2real_common/deployment_contract.hpp"
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
namespace sim2real_runtime
|
||||
{
|
||||
|
||||
// Named constants for timing and command filtering
|
||||
constexpr float kCmdAccelLimitXY = 0.02f; // m/s per step (at 50Hz)
|
||||
constexpr float kCmdAccelLimitYaw = 0.03f; // rad/s per step (at 50Hz)
|
||||
constexpr float kPolicyDt = 0.02f; // policy loop period (50Hz)
|
||||
|
||||
PolicyRuntimeNode::PolicyRuntimeNode()
|
||||
: Node("sim2real_runtime_node")
|
||||
{
|
||||
// 1. Declare and get parameters
|
||||
model_path_ = declare_parameter<std::string>("model_path", "policies/model_rough.onnx");
|
||||
use_cuda_ = declare_parameter<bool>("use_cuda", false); // enable CUDA EP on Orin Nano
|
||||
|
||||
// Safety parameters
|
||||
safety_enabled_ = declare_parameter<bool>("safety_enabled", true);
|
||||
double max_target_offset = declare_parameter<double>("max_target_offset", 0.6);
|
||||
double hard_target_offset = declare_parameter<double>("hard_target_offset", 1.2);
|
||||
double max_ang_vel = declare_parameter<double>("max_ang_vel", 10.0);
|
||||
double max_tilt_z = declare_parameter<double>("max_tilt_z", -0.3);
|
||||
int clip_to_brake = declare_parameter<int>("clip_to_brake", 0);
|
||||
double imu_age_warn_ms = declare_parameter<double>("imu_age_warn_ms", 60.0);
|
||||
double imu_age_stop_ms = declare_parameter<double>("imu_age_stop_ms", 200.0);
|
||||
|
||||
command_release_s_ = static_cast<float>(declare_parameter<double>("command_release_s", 0.35));
|
||||
release_command_hold_s_ = static_cast<float>(declare_parameter<double>("release_command_hold_s", 0.12));
|
||||
release_posture_max_err_ = static_cast<float>(declare_parameter<double>("release_posture_max_err", 0.35));
|
||||
release_target_blend_s_ = static_cast<float>(declare_parameter<double>("release_target_blend_s", 0.30));
|
||||
clip_obs_ = static_cast<float>(declare_parameter<double>("clip_obs", 100.0));
|
||||
hold_zero_command_pose_ = declare_parameter<bool>("hold_zero_command_pose", true);
|
||||
enable_zero_cmd_suppression_ = declare_parameter<bool>("enable_zero_cmd_suppression", true);
|
||||
require_active_command_to_release_ = declare_parameter<bool>("require_active_command_to_release", true);
|
||||
zero_cmd_use_yaw_rate_ = declare_parameter<bool>("zero_cmd_use_yaw_rate", true);
|
||||
runtime_released_ = !require_active_command_to_release_;
|
||||
|
||||
RCLCPP_INFO(get_logger(), "Loading ONNX policy model from: %s", model_path_.c_str());
|
||||
|
||||
// Initialize StandBalanceController
|
||||
stand_balance_ = std::make_unique<sim2real_common::StandBalanceController>(0.02);
|
||||
|
||||
// Initialize SafetyMonitor and RuntimeGuard
|
||||
safety_monitor_ = std::make_unique<sim2real_common::SafetyMonitor>(
|
||||
static_cast<float>(max_target_offset),
|
||||
static_cast<float>(max_ang_vel),
|
||||
static_cast<float>(max_tilt_z),
|
||||
clip_to_brake,
|
||||
static_cast<float>(hard_target_offset)
|
||||
);
|
||||
|
||||
runtime_guard_ = std::make_unique<sim2real_common::RuntimeGuard>(
|
||||
static_cast<float>(max_ang_vel + 2.0),
|
||||
static_cast<float>(max_tilt_z),
|
||||
static_cast<float>(imu_age_warn_ms),
|
||||
static_cast<float>(imu_age_stop_ms)
|
||||
);
|
||||
|
||||
// 2. Initialize Ort C++ environment
|
||||
try {
|
||||
env_ = std::make_unique<Ort::Env>(ORT_LOGGING_LEVEL_WARNING, "sim2real_onnx_env");
|
||||
|
||||
Ort::SessionOptions session_options;
|
||||
// single-thread ORIN optimization to prevent thread scheduling jitter
|
||||
session_options.SetIntraOpNumThreads(1);
|
||||
session_options.SetInterOpNumThreads(1);
|
||||
session_options.SetGraphOptimizationLevel(GraphOptimizationLevel::ORT_ENABLE_ALL);
|
||||
|
||||
// CUDA Execution Provider (Orin Nano GPU acceleration)
|
||||
if (use_cuda_) {
|
||||
try {
|
||||
OrtCUDAProviderOptions cuda_opts{};
|
||||
cuda_opts.device_id = 0;
|
||||
// enable_cuda_graph: false for single-inference RL policy (avoids overhead)
|
||||
session_options.AppendExecutionProvider_CUDA(cuda_opts);
|
||||
RCLCPP_INFO(get_logger(), "CUDA Execution Provider enabled (device 0)");
|
||||
} catch (const std::exception& e) {
|
||||
RCLCPP_WARN(get_logger(),
|
||||
"CUDA EP init failed (ONNX Runtime built without CUDA?): %s. Falling back to CPU.",
|
||||
e.what());
|
||||
use_cuda_ = false;
|
||||
}
|
||||
}
|
||||
|
||||
session_ = std::make_unique<Ort::Session>(*env_, model_path_.c_str(), session_options);
|
||||
memory_info_ = std::make_unique<Ort::MemoryInfo>(Ort::MemoryInfo::CreateCpu(OrtDeviceAllocator, OrtMemTypeCPU));
|
||||
|
||||
// Get input/output nodes names and shapes
|
||||
Ort::AllocatorWithDefaultOptions allocator;
|
||||
|
||||
std::size_t num_inputs = session_->GetInputCount();
|
||||
for (std::size_t i = 0; i < num_inputs; ++i) {
|
||||
auto name = session_->GetInputNameAllocated(i, allocator);
|
||||
input_names_str_.push_back(std::string(name.get()));
|
||||
}
|
||||
for (const auto& name : input_names_str_) {
|
||||
input_names_char_.push_back(name.c_str());
|
||||
}
|
||||
|
||||
std::size_t num_outputs = session_->GetOutputCount();
|
||||
for (std::size_t i = 0; i < num_outputs; ++i) {
|
||||
auto name = session_->GetOutputNameAllocated(i, allocator);
|
||||
output_names_str_.push_back(std::string(name.get()));
|
||||
}
|
||||
for (const auto& name : output_names_str_) {
|
||||
output_names_char_.push_back(name.c_str());
|
||||
}
|
||||
|
||||
auto input_type_info = session_->GetInputTypeInfo(0);
|
||||
auto input_tensor_info = input_type_info.GetTensorTypeAndShapeInfo();
|
||||
input_shape_ = input_tensor_info.GetShape();
|
||||
if (input_shape_[0] < 0) {
|
||||
input_shape_[0] = 1;
|
||||
}
|
||||
|
||||
auto output_type_info = session_->GetOutputTypeInfo(0);
|
||||
auto output_tensor_info = output_type_info.GetTensorTypeAndShapeInfo();
|
||||
output_shape_ = output_tensor_info.GetShape();
|
||||
if (output_shape_[0] < 0) {
|
||||
output_shape_[0] = 1;
|
||||
}
|
||||
|
||||
// Validate output shape matches expected action dimension
|
||||
if (output_shape_.size() < 2 || output_shape_[1] != static_cast<std::int64_t>(sim2real_common::DeploymentContract::kActionDim)) {
|
||||
RCLCPP_FATAL(get_logger(),
|
||||
"ONNX model output dimension mismatch! Expected %ld, got %ld. Wrong model?",
|
||||
static_cast<std::int64_t>(sim2real_common::DeploymentContract::kActionDim),
|
||||
output_shape_.size() >= 2 ? output_shape_[1] : -1);
|
||||
throw std::runtime_error("ONNX model output shape mismatch");
|
||||
}
|
||||
|
||||
RCLCPP_INFO(get_logger(), "Successfully loaded ONNX policy model. Input shape: [%ld, %ld], Output shape: [%ld, %ld]",
|
||||
input_shape_[0], input_shape_[1], output_shape_[0], output_shape_[1]);
|
||||
} catch (const std::exception& e) {
|
||||
RCLCPP_FATAL(get_logger(), "Failed to load ONNX model: %s", e.what());
|
||||
throw;
|
||||
}
|
||||
|
||||
// 3. Create publishers and subscriptions
|
||||
target_pub_ = create_publisher<sim2real_interfaces::msg::RuntimeTarget>("runtime/target", 10);
|
||||
state_sub_ = create_subscription<sim2real_interfaces::msg::RuntimeState>(
|
||||
"runtime/state", 10,
|
||||
std::bind(&PolicyRuntimeNode::onState, this, std::placeholders::_1));
|
||||
cmd_sub_ = create_subscription<geometry_msgs::msg::Twist>(
|
||||
"cmd_vel", 10,
|
||||
std::bind(&PolicyRuntimeNode::onCmdVel, this, std::placeholders::_1));
|
||||
cmd_stamped_sub_ = create_subscription<geometry_msgs::msg::TwistStamped>(
|
||||
"cmd_vel_stamped", 10,
|
||||
std::bind(&PolicyRuntimeNode::onCmdVelStamped, this, std::placeholders::_1));
|
||||
estop_sub_ = create_subscription<std_msgs::msg::Bool>(
|
||||
"/safety/estop", 10,
|
||||
std::bind(&PolicyRuntimeNode::onEstop, this, std::placeholders::_1));
|
||||
|
||||
// 4. Timer at 50Hz (20ms)
|
||||
policy_timer_ = create_wall_timer(20ms, std::bind(&PolicyRuntimeNode::onPolicyLoop, this));
|
||||
|
||||
last_actions_.fill(0.0f);
|
||||
}
|
||||
|
||||
void PolicyRuntimeNode::onState(const sim2real_interfaces::msg::RuntimeState::SharedPtr msg)
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(mutex_);
|
||||
latest_state_ = *msg;
|
||||
has_state_ = true;
|
||||
last_state_recv_time_ = std::chrono::steady_clock::now();
|
||||
}
|
||||
|
||||
void PolicyRuntimeNode::applyCmdVel(float vx, float vy, float vyaw)
|
||||
{
|
||||
// Velocity saturation limits (consistent with training domain)
|
||||
constexpr float kMaxLinVelX = 0.8f; // m/s
|
||||
constexpr float kMaxLinVelY = 0.3f; // m/s
|
||||
constexpr float kMaxAngVelZ = 0.5f; // rad/s
|
||||
|
||||
std::scoped_lock<std::mutex> lock(mutex_);
|
||||
raw_cmd_[0] = std::clamp(vx, -kMaxLinVelX, kMaxLinVelX);
|
||||
raw_cmd_[1] = std::clamp(vy, -kMaxLinVelY, kMaxLinVelY);
|
||||
raw_cmd_[2] = std::clamp(vyaw, -kMaxAngVelZ, kMaxAngVelZ);
|
||||
cmd_ = raw_cmd_;
|
||||
}
|
||||
|
||||
void PolicyRuntimeNode::onCmdVel(const geometry_msgs::msg::Twist::SharedPtr msg)
|
||||
{
|
||||
applyCmdVel(
|
||||
static_cast<float>(msg->linear.x),
|
||||
static_cast<float>(msg->linear.y),
|
||||
static_cast<float>(msg->angular.z));
|
||||
}
|
||||
|
||||
void PolicyRuntimeNode::onCmdVelStamped(const geometry_msgs::msg::TwistStamped::SharedPtr msg)
|
||||
{
|
||||
applyCmdVel(
|
||||
static_cast<float>(msg->twist.linear.x),
|
||||
static_cast<float>(msg->twist.linear.y),
|
||||
static_cast<float>(msg->twist.angular.z));
|
||||
}
|
||||
|
||||
void PolicyRuntimeNode::onEstop(const std_msgs::msg::Bool::SharedPtr msg)
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(mutex_);
|
||||
estop_triggered_ = msg->data;
|
||||
if (estop_triggered_) {
|
||||
RCLCPP_WARN(get_logger(), "!!! E-stop triggered via /safety/estop !!!");
|
||||
} else {
|
||||
RCLCPP_INFO(get_logger(), "E-stop reset.");
|
||||
}
|
||||
}
|
||||
|
||||
std::array<float, 53> PolicyRuntimeNode::buildObservation(
|
||||
const sim2real_interfaces::msg::RuntimeState & state,
|
||||
const std::array<float, 3> & cmd,
|
||||
const std::array<float, 16> & last_actions) const
|
||||
{
|
||||
std::array<float, 53> obs{};
|
||||
std::size_t cursor = 0;
|
||||
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
obs[cursor++] = state.imu_gyro[i] * 0.25f;
|
||||
}
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
obs[cursor++] = state.projected_gravity[i];
|
||||
}
|
||||
for (float v : cmd) {
|
||||
obs[cursor++] = v;
|
||||
}
|
||||
for (std::size_t i = 0; i < sim2real_common::DeploymentContract::kLegJointCount; ++i) {
|
||||
obs[cursor++] = state.joint_pos[i] - sim2real_common::DeploymentContract::kDefaultDofPos[i];
|
||||
}
|
||||
for (std::size_t i = 0; i < sim2real_common::DeploymentContract::kLegJointCount; ++i) {
|
||||
obs[cursor++] = state.joint_vel[i] * 0.05f;
|
||||
}
|
||||
for (std::size_t i = 12; i < sim2real_common::DeploymentContract::kActionDim; ++i) {
|
||||
obs[cursor++] = state.joint_vel[i] * 0.05f;
|
||||
}
|
||||
for (float v : last_actions) {
|
||||
obs[cursor++] = v;
|
||||
}
|
||||
|
||||
// Clip observations values to ±clip_obs_
|
||||
if (clip_obs_ > 0.0f) {
|
||||
for (float & v : obs) {
|
||||
v = std::clamp(v, -clip_obs_, clip_obs_);
|
||||
}
|
||||
}
|
||||
|
||||
return obs;
|
||||
}
|
||||
|
||||
std::array<float, 16> PolicyRuntimeNode::runPolicy(const std::array<float, 53> & obs)
|
||||
{
|
||||
std::array<float, 16> action{};
|
||||
try {
|
||||
auto input_tensor = Ort::Value::CreateTensor<float>(
|
||||
*memory_info_,
|
||||
const_cast<float*>(obs.data()),
|
||||
obs.size(),
|
||||
input_shape_.data(),
|
||||
input_shape_.size()
|
||||
);
|
||||
|
||||
auto output_tensor = Ort::Value::CreateTensor<float>(
|
||||
*memory_info_,
|
||||
action.data(),
|
||||
action.size(),
|
||||
output_shape_.data(),
|
||||
output_shape_.size()
|
||||
);
|
||||
|
||||
session_->Run(
|
||||
Ort::RunOptions{nullptr},
|
||||
input_names_char_.data(),
|
||||
&input_tensor,
|
||||
1,
|
||||
output_names_char_.data(),
|
||||
&output_tensor,
|
||||
1
|
||||
);
|
||||
} catch (const std::exception& e) {
|
||||
RCLCPP_ERROR(get_logger(), "ONNX Runtime inference exception: %s", e.what());
|
||||
action.fill(0.0f);
|
||||
}
|
||||
|
||||
for (float& v : action) {
|
||||
v = std::clamp(v, -10.0f, 10.0f);
|
||||
}
|
||||
|
||||
return action;
|
||||
}
|
||||
|
||||
bool PolicyRuntimeNode::isZeroCommand(const std::array<float, 3> & cmd, const std::array<float, 3> & imu_gyro) const
|
||||
{
|
||||
const float planar_cmd = std::sqrt(cmd[0] * cmd[0] + cmd[1] * cmd[1]);
|
||||
const bool cmd_is_zero = planar_cmd < zero_cmd_lin_thresh_ && std::abs(cmd[2]) < zero_cmd_yaw_thresh_;
|
||||
if (!zero_cmd_use_yaw_rate_) {
|
||||
return cmd_is_zero;
|
||||
}
|
||||
return cmd_is_zero && std::abs(imu_gyro[2]) < zero_yaw_rate_thresh_;
|
||||
}
|
||||
|
||||
bool PolicyRuntimeNode::isCommandActive(const std::array<float, 3> & cmd) const
|
||||
{
|
||||
const float planar_cmd = std::sqrt(cmd[0] * cmd[0] + cmd[1] * cmd[1]);
|
||||
return planar_cmd >= zero_cmd_lin_thresh_ || std::abs(cmd[2]) >= zero_cmd_yaw_thresh_;
|
||||
}
|
||||
|
||||
void PolicyRuntimeNode::onPolicyLoop()
|
||||
{
|
||||
sim2real_interfaces::msg::RuntimeState state;
|
||||
std::array<float, 3> cmd{};
|
||||
std::array<float, 3> raw_cmd{};
|
||||
std::array<float, 16> last_actions{};
|
||||
bool estop_active = false;
|
||||
bool safety_active = false;
|
||||
double state_age_ms = 0.0;
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(mutex_);
|
||||
if (!has_state_) {
|
||||
return;
|
||||
}
|
||||
state = latest_state_;
|
||||
cmd = cmd_;
|
||||
raw_cmd = raw_cmd_;
|
||||
last_actions = last_actions_;
|
||||
estop_active = estop_triggered_;
|
||||
safety_active = safety_triggered_;
|
||||
if (last_state_recv_time_.time_since_epoch().count() != 0) {
|
||||
state_age_ms = std::chrono::duration<double, std::milli>(
|
||||
std::chrono::steady_clock::now() - last_state_recv_time_).count();
|
||||
}
|
||||
}
|
||||
|
||||
// 1) Run RuntimeGuard check
|
||||
if (safety_enabled_ && !safety_active) {
|
||||
std::vector<float> extra_vals;
|
||||
extra_vals.reserve(48);
|
||||
for (float v : state.joint_pos) extra_vals.push_back(v);
|
||||
for (float v : state.joint_vel) extra_vals.push_back(v);
|
||||
for (float v : last_actions) extra_vals.push_back(v);
|
||||
|
||||
const float effective_imu_age_ms = static_cast<float>(std::max(
|
||||
static_cast<double>(state.imu_age_ms), state_age_ms));
|
||||
|
||||
auto guard_decision = runtime_guard_->check(
|
||||
state.imu_gyro, state.projected_gravity, effective_imu_age_ms, estop_active, extra_vals);
|
||||
if (guard_decision.level == sim2real_common::GuardLevel::STOP) {
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(mutex_);
|
||||
safety_triggered_ = true;
|
||||
}
|
||||
safety_active = true;
|
||||
safety_reason_ = "Runtime Guard Stop: " + guard_decision.reason;
|
||||
RCLCPP_ERROR(get_logger(), "SAFETY STOP TRIGGERED in Policy Runtime: %s", safety_reason_.c_str());
|
||||
} else if (guard_decision.level == sim2real_common::GuardLevel::WARN) {
|
||||
RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "Safety Guard Warning in Policy Runtime: %s", guard_decision.reason.c_str());
|
||||
}
|
||||
}
|
||||
|
||||
if (safety_active) {
|
||||
sim2real_interfaces::msg::RuntimeTarget target;
|
||||
target.stamp = now();
|
||||
target.sequence = sequence_++;
|
||||
target.raw_command = raw_cmd;
|
||||
target.command = cmd;
|
||||
target.raw_action.fill(0.0f);
|
||||
target.scaled_action.fill(0.0f);
|
||||
target.target = sim2real_common::DeploymentContract::kDefaultDofPos;
|
||||
target.target_source = "safety_brake";
|
||||
target.target_age_ms = 0.0f;
|
||||
target_pub_->publish(target);
|
||||
return;
|
||||
}
|
||||
|
||||
sim2real_interfaces::msg::RuntimeTarget target;
|
||||
target.stamp = now();
|
||||
target.sequence = sequence_++;
|
||||
target.raw_command = raw_cmd;
|
||||
target.raw_action.fill(0.0f);
|
||||
target.scaled_action.fill(0.0f);
|
||||
target.command = cmd;
|
||||
|
||||
const auto now_time = rclcpp::Time(target.stamp);
|
||||
|
||||
if (startup_state_ == StartupState::BOOT_HOLD) {
|
||||
// 1. Initial State Read
|
||||
start_pose_ = state.joint_pos;
|
||||
start_pose_[12] = start_pose_[13] = start_pose_[14] = start_pose_[15] = 0.0f; // Wheel starts at 0
|
||||
|
||||
// 2. Shortest periodic delta to stand pose
|
||||
float max_dev = 0.0f;
|
||||
for (std::size_t i = 0; i < 12; ++i) {
|
||||
float delta = sim2real_common::DeploymentContract::kDefaultDofPos[i] - start_pose_[i];
|
||||
delta = delta - 2.0f * static_cast<float>(M_PI) * std::floor((delta + static_cast<float>(M_PI)) / (2.0f * static_cast<float>(M_PI)));
|
||||
startup_delta_[i] = delta;
|
||||
max_dev = std::max(max_dev, std::abs(delta));
|
||||
}
|
||||
startup_delta_[12] = startup_delta_[13] = startup_delta_[14] = startup_delta_[15] = 0.0f;
|
||||
|
||||
if (max_dev > 3.0f) {
|
||||
RCLCPP_WARN(get_logger(), "Measured joint dev too large (%f rad > 3.0 rad). Aborting standup transition.", max_dev);
|
||||
target.target = start_pose_;
|
||||
target.target_source = "boot_hold";
|
||||
target_pub_->publish(target);
|
||||
return;
|
||||
}
|
||||
|
||||
// Adapt transition time: min 2s, max 6s, 1.5s per rad
|
||||
transition_time_ = std::clamp(max_dev * 1.5, 2.0, 6.0);
|
||||
startup_state_ = StartupState::STARTUP_SOFT_HOLD;
|
||||
state_start_time_ = now_time;
|
||||
RCLCPP_INFO(get_logger(), "Standup sequence started. Starting dev: %f rad, transition time: %f s", max_dev, transition_time_);
|
||||
}
|
||||
|
||||
if (startup_state_ == StartupState::STARTUP_SOFT_HOLD) {
|
||||
double elapsed = (now_time - state_start_time_).seconds();
|
||||
target.target = start_pose_;
|
||||
target.target_source = "startup_soft_hold";
|
||||
|
||||
if (elapsed >= 1.0) { // 1s soft hold
|
||||
startup_state_ = StartupState::STARTUP_TRANSITION;
|
||||
state_start_time_ = now_time;
|
||||
RCLCPP_INFO(get_logger(), "Transitioning to stand pose...");
|
||||
}
|
||||
}
|
||||
else if (startup_state_ == StartupState::STARTUP_TRANSITION) {
|
||||
double elapsed = (now_time - state_start_time_).seconds();
|
||||
double phase = std::min(1.0, elapsed / transition_time_);
|
||||
|
||||
// Cosine blend interpolation
|
||||
double blend = 0.5 - 0.5 * std::cos(M_PI * phase);
|
||||
for (std::size_t i = 0; i < 16; ++i) {
|
||||
target.target[i] = start_pose_[i] + blend * startup_delta_[i];
|
||||
}
|
||||
target.target_source = "startup_hold";
|
||||
|
||||
if (phase >= 1.0) {
|
||||
// Settle check
|
||||
float max_pos_err = 0.0f;
|
||||
for (std::size_t i = 0; i < 12; ++i) {
|
||||
float delta = sim2real_common::DeploymentContract::kDefaultDofPos[i] - state.joint_pos[i];
|
||||
delta = delta - 2.0f * static_cast<float>(M_PI) * std::floor((delta + static_cast<float>(M_PI)) / (2.0f * static_cast<float>(M_PI)));
|
||||
max_pos_err = std::max(max_pos_err, std::abs(delta));
|
||||
}
|
||||
float max_vel_err = 0.0f;
|
||||
for (std::size_t i = 0; i < 12; ++i) {
|
||||
max_vel_err = std::max(max_vel_err, std::abs(state.joint_vel[i]));
|
||||
}
|
||||
|
||||
if (max_pos_err <= 0.30f && max_vel_err <= 0.6f) {
|
||||
startup_state_ = StartupState::STARTUP_HOLD_AFTER;
|
||||
state_start_time_ = now_time;
|
||||
RCLCPP_INFO(get_logger(), "Pose settled. Holding for 1.0s...");
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (startup_state_ == StartupState::STARTUP_HOLD_AFTER) {
|
||||
double elapsed = (now_time - state_start_time_).seconds();
|
||||
|
||||
// Run stand balance controller during holding phase
|
||||
target.target = stand_balance_->computeTarget(state.projected_gravity, state.imu_gyro, cmd);
|
||||
target.target_source = "startup_hold";
|
||||
|
||||
if (elapsed >= 1.0 && stand_balance_->isStable()) {
|
||||
startup_state_ = StartupState::RUNTIME;
|
||||
RCLCPP_INFO(get_logger(), "Standup sequence completed. Entering Policy RUNTIME mode!");
|
||||
}
|
||||
}
|
||||
else if (startup_state_ == StartupState::RUNTIME) {
|
||||
// Python template uses the command directly in policy obs/release logic.
|
||||
// Upstream cmd mux may already smooth it, so do not apply an extra runtime filter here.
|
||||
filtered_cmd_ = cmd;
|
||||
|
||||
const auto target_hold = stand_balance_->computeTarget(state.projected_gravity, state.imu_gyro, std::array<float, 3>{0.0f, 0.0f, 0.0f});
|
||||
const bool zero_command = isZeroCommand(cmd, state.imu_gyro);
|
||||
|
||||
if (!runtime_released_) {
|
||||
if (require_active_command_to_release_) {
|
||||
if (isCommandActive(cmd)) {
|
||||
release_active_time_ += kPolicyDt;
|
||||
} else {
|
||||
release_active_time_ = 0.0f;
|
||||
}
|
||||
|
||||
float max_hold_err = 0.0f;
|
||||
for (std::size_t i = 0; i < sim2real_common::DeploymentContract::kLegJointCount; ++i) {
|
||||
max_hold_err = std::max(max_hold_err, std::abs(state.joint_pos[i] - target_hold[i]));
|
||||
}
|
||||
|
||||
const bool active_ready = release_active_time_ >= release_command_hold_s_;
|
||||
const bool posture_ready = max_hold_err <= release_posture_max_err_;
|
||||
if (active_ready && posture_ready) {
|
||||
runtime_released_ = true;
|
||||
}
|
||||
} else {
|
||||
runtime_released_ = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (!runtime_released_ || zero_command) {
|
||||
release_alpha_ = 0.0f;
|
||||
target.runtime_released = false;
|
||||
target.release_alpha = 0.0f;
|
||||
target.zero_command = zero_command;
|
||||
target.raw_action.fill(0.0f);
|
||||
target.scaled_action.fill(0.0f);
|
||||
last_actions.fill(0.0f);
|
||||
target.target_source = "runtime_zero_hold";
|
||||
target.target = target_hold;
|
||||
if (!runtime_released_) {
|
||||
target.target_source = "runtime_hold";
|
||||
}
|
||||
} else {
|
||||
release_alpha_ = std::min(1.0f, release_alpha_ + kPolicyDt / std::max(command_release_s_, 1.0e-3f));
|
||||
target.runtime_released = (release_alpha_ >= 1.0f);
|
||||
target.release_alpha = release_alpha_;
|
||||
target.zero_command = false;
|
||||
target.command = cmd;
|
||||
|
||||
auto raw = runPolicy(buildObservation(state, cmd, last_actions));
|
||||
for (float & v : raw) {
|
||||
v *= release_alpha_;
|
||||
}
|
||||
target.raw_action = raw;
|
||||
|
||||
const float blend = std::min(1.0f, release_alpha_ * (command_release_s_ / std::max(release_target_blend_s_, kPolicyDt)));
|
||||
for (std::size_t i = 0; i < sim2real_common::DeploymentContract::kActionDim; ++i) {
|
||||
target.scaled_action[i] = raw[i] * sim2real_common::DeploymentContract::kActionScale[i];
|
||||
const float policy_target = target.scaled_action[i] + sim2real_common::DeploymentContract::kDefaultDofPos[i];
|
||||
target.target[i] = (1.0f - blend) * target_hold[i] + blend * policy_target;
|
||||
last_actions[i] = raw[i];
|
||||
}
|
||||
target.target_source = blend < 0.999f ? "runtime_blend" : "runtime_policy";
|
||||
}
|
||||
}
|
||||
|
||||
// 2) Run SafetyMonitor check on computed target
|
||||
if (safety_enabled_) {
|
||||
auto safety_decision = safety_monitor_->check(target.target, sim2real_common::DeploymentContract::kDefaultDofPos, state.imu_gyro, state.projected_gravity, estop_active);
|
||||
if (safety_decision.level == sim2real_common::SafetyLevel::ESTOP || safety_decision.level == sim2real_common::SafetyLevel::BRAKE) {
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(mutex_);
|
||||
safety_triggered_ = true;
|
||||
}
|
||||
safety_reason_ = "Safety Monitor Stop: " + safety_decision.message;
|
||||
RCLCPP_ERROR(get_logger(), "SAFETY STOP TRIGGERED in Policy Runtime: %s", safety_reason_.c_str());
|
||||
|
||||
// Override target to safety_brake damping pose
|
||||
target.target = sim2real_common::DeploymentContract::kDefaultDofPos;
|
||||
target.target_source = "safety_brake";
|
||||
} else if (safety_decision.level == sim2real_common::SafetyLevel::CLIP) {
|
||||
target.target = safety_decision.clipped_target;
|
||||
target.target_source = "safety_clip";
|
||||
RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "Safety Monitor: Joint target clipped in Policy Runtime.");
|
||||
}
|
||||
}
|
||||
|
||||
target.target_age_ms = 0.0f;
|
||||
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(mutex_);
|
||||
last_actions_ = last_actions;
|
||||
}
|
||||
|
||||
target_pub_->publish(target);
|
||||
}
|
||||
|
||||
} // namespace sim2real_runtime
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::spin(std::make_shared<sim2real_runtime::PolicyRuntimeNode>());
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,250 @@
|
||||
#!/usr/bin/env python3
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass, field
|
||||
from typing import Optional
|
||||
|
||||
import serial
|
||||
|
||||
import rclpy
|
||||
from geometry_msgs.msg import Twist
|
||||
from rclpy.executors import ExternalShutdownException
|
||||
from rclpy.node import Node
|
||||
from std_msgs.msg import Bool
|
||||
|
||||
SBUS_FRAME_SIZE = 25
|
||||
SBUS_RC_MID = 1024
|
||||
SBUS_AXIS_SCALE = 660.0
|
||||
|
||||
SWITCH_LOW = -1
|
||||
SWITCH_MID = 0
|
||||
SWITCH_HIGH = 1
|
||||
|
||||
|
||||
@dataclass
|
||||
class RemoteSwitchState:
|
||||
ch7: int = SWITCH_MID
|
||||
|
||||
|
||||
@dataclass
|
||||
class RemoteControlState:
|
||||
ch1: int = 0
|
||||
ch2: int = 0
|
||||
ch3: int = 0
|
||||
ch4: int = 0
|
||||
switches: RemoteSwitchState = field(default_factory=RemoteSwitchState)
|
||||
frame_ok: bool = False
|
||||
|
||||
@property
|
||||
def estop_requested(self) -> bool:
|
||||
return self.switches.ch7 == SWITCH_HIGH
|
||||
|
||||
|
||||
class SbusUartReceiver:
|
||||
def __init__(self, port: str, baudrate: int, timeout: float, axis_deadzone: int):
|
||||
self.port = port
|
||||
self.baudrate = int(baudrate)
|
||||
self.timeout = float(timeout)
|
||||
self.axis_deadzone = int(axis_deadzone)
|
||||
self.serial: Optional[serial.Serial] = None
|
||||
self.buffer = bytearray()
|
||||
self.state = RemoteControlState()
|
||||
|
||||
def open(self) -> None:
|
||||
if self.serial and self.serial.is_open:
|
||||
return
|
||||
self.serial = serial.Serial(
|
||||
port=self.port,
|
||||
baudrate=self.baudrate,
|
||||
timeout=self.timeout,
|
||||
bytesize=serial.EIGHTBITS,
|
||||
parity=serial.PARITY_EVEN,
|
||||
stopbits=serial.STOPBITS_TWO,
|
||||
)
|
||||
|
||||
def close(self) -> None:
|
||||
if self.serial and self.serial.is_open:
|
||||
self.serial.close()
|
||||
|
||||
def poll(self) -> RemoteControlState:
|
||||
if not self.serial or not self.serial.is_open:
|
||||
raise RuntimeError("remote uart is not open")
|
||||
|
||||
waiting = self.serial.in_waiting
|
||||
if waiting:
|
||||
self.buffer.extend(self.serial.read(waiting))
|
||||
|
||||
while len(self.buffer) >= SBUS_FRAME_SIZE:
|
||||
start_idx = self.buffer.find(0x0F)
|
||||
if start_idx < 0:
|
||||
self.buffer.clear()
|
||||
break
|
||||
if start_idx > 0:
|
||||
del self.buffer[:start_idx]
|
||||
if len(self.buffer) < SBUS_FRAME_SIZE:
|
||||
break
|
||||
frame = bytes(self.buffer[:SBUS_FRAME_SIZE])
|
||||
del self.buffer[:SBUS_FRAME_SIZE]
|
||||
parsed = self._parse_frame(frame)
|
||||
if parsed is not None:
|
||||
self.state = parsed
|
||||
return self.state
|
||||
|
||||
def _parse_frame(self, frame: bytes) -> Optional[RemoteControlState]:
|
||||
if len(frame) != SBUS_FRAME_SIZE or frame[0] != 0x0F:
|
||||
return None
|
||||
|
||||
channels = [0] * 16
|
||||
channels[0] = (frame[1] | (frame[2] << 8)) & 0x07FF
|
||||
channels[1] = ((frame[2] >> 3) | (frame[3] << 5)) & 0x07FF
|
||||
channels[2] = ((frame[3] >> 6) | (frame[4] << 2) | (frame[5] << 10)) & 0x07FF
|
||||
channels[3] = ((frame[5] >> 1) | (frame[6] << 7)) & 0x07FF
|
||||
channels[4] = ((frame[6] >> 4) | (frame[7] << 4)) & 0x07FF
|
||||
channels[5] = ((frame[7] >> 7) | (frame[8] << 1) | (frame[9] << 9)) & 0x07FF
|
||||
channels[6] = ((frame[9] >> 2) | (frame[10] << 6)) & 0x07FF
|
||||
channels[7] = ((frame[10] >> 5) | (frame[11] << 3)) & 0x07FF
|
||||
channels[8] = (frame[12] | (frame[13] << 8)) & 0x07FF
|
||||
channels[9] = ((frame[13] >> 3) | (frame[14] << 5)) & 0x07FF
|
||||
|
||||
if channels[0] < 100:
|
||||
return None
|
||||
|
||||
state = RemoteControlState(
|
||||
ch1=self._normalize_axis(channels[0]),
|
||||
ch2=self._normalize_axis(channels[1]),
|
||||
ch3=self._normalize_axis(channels[3]),
|
||||
ch4=self._normalize_axis(channels[2]),
|
||||
switches=RemoteSwitchState(ch7=self._decode_switch(channels[6])),
|
||||
frame_ok=True,
|
||||
)
|
||||
if any(abs(value) > 800 for value in (state.ch1, state.ch2, state.ch3, state.ch4)):
|
||||
return None
|
||||
return state
|
||||
|
||||
def _normalize_axis(self, value: int) -> int:
|
||||
mapped = int(round((value - SBUS_RC_MID) * SBUS_AXIS_SCALE / 800.0))
|
||||
return 0 if abs(mapped) <= self.axis_deadzone else mapped
|
||||
|
||||
@staticmethod
|
||||
def _decode_switch(value: int) -> int:
|
||||
if value < 500:
|
||||
return SWITCH_LOW
|
||||
if value > 1500:
|
||||
return SWITCH_HIGH
|
||||
return SWITCH_MID
|
||||
|
||||
|
||||
class RemoteUartNode(Node):
|
||||
def __init__(self) -> None:
|
||||
super().__init__("sim2real_remote_uart_node", allow_undeclared_parameters=True)
|
||||
|
||||
self.enabled = bool(self.declare_parameter("remote_enabled", True).value)
|
||||
self.port = str(self.declare_parameter("remote_port", "/dev/ttyACM0").value)
|
||||
self.baudrate = int(self.declare_parameter("remote_baudrate", 100000).value)
|
||||
self.timeout = float(self.declare_parameter("remote_timeout", 0.02).value)
|
||||
self.axis_deadzone = int(self.declare_parameter("remote_axis_deadzone", 40).value)
|
||||
self.active_threshold = int(self.declare_parameter("remote_active_threshold", 40).value)
|
||||
self.axis_full_scale = max(float(self.declare_parameter("remote_axis_full_scale", 660.0).value), 1.0)
|
||||
self.max_vx = float(self.declare_parameter("remote_max_vx", 0.8).value)
|
||||
self.max_vy = float(self.declare_parameter("remote_max_vy", 0.3).value)
|
||||
self.max_yaw = float(self.declare_parameter("remote_max_yaw_rate", 0.5).value)
|
||||
self.invert_vx = bool(self.declare_parameter("remote_invert_vx", True).value)
|
||||
self.invert_vy = bool(self.declare_parameter("remote_invert_vy", False).value)
|
||||
self.invert_yaw = bool(self.declare_parameter("remote_invert_yaw", True).value)
|
||||
self.publish_inactive_zero = bool(self.declare_parameter("remote_publish_inactive_zero", True).value)
|
||||
self.estop_latch = bool(self.declare_parameter("remote_estop_latch", True).value)
|
||||
self.poll_hz = float(self.declare_parameter("remote_poll_hz", 50.0).value)
|
||||
|
||||
self.cmd_pub = self.create_publisher(Twist, "cmd_vel_remote", 10)
|
||||
self.estop_pub = self.create_publisher(Bool, "/safety/estop", 10)
|
||||
self.receiver: Optional[SbusUartReceiver] = None
|
||||
self.estop_published = False
|
||||
self.open_error_logged = False
|
||||
|
||||
if self.enabled:
|
||||
self.receiver = SbusUartReceiver(
|
||||
port=self.port,
|
||||
baudrate=self.baudrate,
|
||||
timeout=self.timeout,
|
||||
axis_deadzone=self.axis_deadzone,
|
||||
)
|
||||
try:
|
||||
self.receiver.open()
|
||||
self.get_logger().info(f"Remote UART opened on {self.port} at {self.baudrate} baud")
|
||||
except Exception as exc:
|
||||
self.get_logger().error(f"Failed to open remote UART {self.port}: {exc}")
|
||||
self.open_error_logged = True
|
||||
else:
|
||||
self.get_logger().warn("Remote UART node is disabled by parameter")
|
||||
|
||||
period = 1.0 / self.poll_hz if self.poll_hz > 0.0 else 0.02
|
||||
self.timer = self.create_timer(period, self.on_timer)
|
||||
|
||||
def destroy_node(self) -> bool:
|
||||
if self.receiver is not None:
|
||||
self.receiver.close()
|
||||
return super().destroy_node()
|
||||
|
||||
def on_timer(self) -> None:
|
||||
if not self.enabled or self.receiver is None:
|
||||
return
|
||||
|
||||
try:
|
||||
if not self.receiver.serial or not self.receiver.serial.is_open:
|
||||
self.receiver.open()
|
||||
state = self.receiver.poll()
|
||||
except Exception as exc:
|
||||
if not self.open_error_logged:
|
||||
self.get_logger().error(f"Remote UART poll failed: {exc}")
|
||||
self.open_error_logged = True
|
||||
return
|
||||
|
||||
self.open_error_logged = False
|
||||
|
||||
if state.estop_requested:
|
||||
if not self.estop_published or not self.estop_latch:
|
||||
self.estop_pub.publish(Bool(data=True))
|
||||
self.get_logger().warn("Remote E-stop requested by CH7 high")
|
||||
self.estop_published = True
|
||||
self.publish_zero_cmd()
|
||||
return
|
||||
|
||||
if not self.estop_latch and self.estop_published:
|
||||
self.estop_pub.publish(Bool(data=False))
|
||||
self.estop_published = False
|
||||
|
||||
active = any(abs(value) > self.active_threshold for value in (state.ch1, state.ch2, state.ch4))
|
||||
if active or self.publish_inactive_zero:
|
||||
cmd = Twist()
|
||||
cmd.linear.x = self.axis_to_velocity(state.ch2, self.max_vx, self.invert_vx)
|
||||
cmd.linear.y = self.axis_to_velocity(state.ch4, self.max_vy, self.invert_vy)
|
||||
cmd.angular.z = self.axis_to_velocity(state.ch1, self.max_yaw, self.invert_yaw)
|
||||
self.cmd_pub.publish(cmd)
|
||||
|
||||
def publish_zero_cmd(self) -> None:
|
||||
self.cmd_pub.publish(Twist())
|
||||
|
||||
def axis_to_velocity(self, raw_value: int, limit: float, invert: bool) -> float:
|
||||
if abs(raw_value) <= self.active_threshold:
|
||||
return 0.0
|
||||
scaled = max(-1.0, min(1.0, raw_value / self.axis_full_scale))
|
||||
if invert:
|
||||
scaled = -scaled
|
||||
return float(scaled * limit)
|
||||
|
||||
|
||||
def main(args: Optional[list[str]] = None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = RemoteUartNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except (KeyboardInterrupt, ExternalShutdownException):
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,261 @@
|
||||
#!/usr/bin/env python3
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import socket
|
||||
from typing import Any, Optional
|
||||
|
||||
import rclpy
|
||||
from geometry_msgs.msg import Twist
|
||||
from rclpy.executors import ExternalShutdownException
|
||||
from rclpy.node import Node
|
||||
from sim2real_interfaces.msg import RuntimeState, RuntimeTarget
|
||||
from std_msgs.msg import Bool, String
|
||||
|
||||
|
||||
class WebUdpBridgeNode(Node):
|
||||
def __init__(self) -> None:
|
||||
super().__init__("sim2real_web_udp_bridge_node", allow_undeclared_parameters=True)
|
||||
|
||||
self.enabled = bool(self.declare_parameter("web_bridge_enabled", True).value)
|
||||
self.listen_host = str(self.declare_parameter("web_udp_listen_host", "0.0.0.0").value)
|
||||
self.listen_port = int(self.declare_parameter("web_udp_listen_port", 15000).value)
|
||||
self.remote_host = str(self.declare_parameter("web_udp_remote_host", "").value)
|
||||
self.remote_port = int(self.declare_parameter("web_udp_remote_port", 15001).value)
|
||||
self.state_hz = float(self.declare_parameter("web_udp_state_hz", 20.0).value)
|
||||
self.cmd_timeout_ms = float(self.declare_parameter("web_udp_cmd_timeout_ms", 300.0).value)
|
||||
self.max_packet_bytes = int(self.declare_parameter("web_udp_max_packet_bytes", 8192).value)
|
||||
self.max_vx = float(self.declare_parameter("web_udp_max_vx", 0.8).value)
|
||||
self.max_vy = float(self.declare_parameter("web_udp_max_vy", 0.3).value)
|
||||
self.max_yaw = float(self.declare_parameter("web_udp_max_yaw_rate", 0.5).value)
|
||||
self.estop_on_timeout = bool(self.declare_parameter("web_udp_estop_on_timeout", False).value)
|
||||
|
||||
self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||
self.sock.setblocking(False)
|
||||
self.sock.bind((self.listen_host, self.listen_port))
|
||||
|
||||
self.client_addr: Optional[tuple[str, int]] = None
|
||||
if self.remote_host:
|
||||
self.client_addr = (self.remote_host, self.remote_port)
|
||||
|
||||
self.latest_target: Optional[RuntimeTarget] = None
|
||||
self.latest_state: Optional[RuntimeState] = None
|
||||
self.latest_cmd = Twist()
|
||||
self.latest_mode = "UNKNOWN"
|
||||
self.latest_mux_status = ""
|
||||
self.estop = False
|
||||
self.web_enabled = False
|
||||
self.last_cmd_time = self.get_clock().now()
|
||||
self.timeout_estop_sent = False
|
||||
|
||||
self.cmd_pub = self.create_publisher(Twist, "cmd_vel_web", 10)
|
||||
self.estop_pub = self.create_publisher(Bool, "/safety/estop", 10)
|
||||
self.web_enabled_pub = self.create_publisher(Bool, "web/enabled", 10)
|
||||
self.remote_enabled_pub = self.create_publisher(Bool, "remote/enabled", 10)
|
||||
self.nav_enabled_pub = self.create_publisher(Bool, "nav/enabled", 10)
|
||||
self.mode_pub = self.create_publisher(String, "control/mode", 10)
|
||||
|
||||
self.create_subscription(RuntimeTarget, "runtime/target", self.on_target, 10)
|
||||
self.create_subscription(RuntimeState, "runtime/state", self.on_state, 10)
|
||||
self.create_subscription(Twist, "cmd_vel", self.on_cmd_vel, 10)
|
||||
self.create_subscription(Bool, "/safety/estop", self.on_estop, 10)
|
||||
self.create_subscription(String, "control/mode_state", self.on_mode_state, 10)
|
||||
self.create_subscription(String, "control/mux_status", self.on_mux_status, 10)
|
||||
|
||||
self.rx_timer = self.create_timer(0.01, self.on_rx_timer)
|
||||
self.state_timer = self.create_timer(1.0 / self.state_hz if self.state_hz > 0.0 else 0.05, self.on_state_timer)
|
||||
self.guard_timer = self.create_timer(0.05, self.on_guard_timer)
|
||||
|
||||
self.get_logger().info(f"Web UDP bridge listening on {self.listen_host}:{self.listen_port}")
|
||||
|
||||
def on_target(self, msg: RuntimeTarget) -> None:
|
||||
self.latest_target = msg
|
||||
|
||||
def on_state(self, msg: RuntimeState) -> None:
|
||||
self.latest_state = msg
|
||||
|
||||
def on_cmd_vel(self, msg: Twist) -> None:
|
||||
self.latest_cmd = msg
|
||||
|
||||
def on_estop(self, msg: Bool) -> None:
|
||||
self.estop = bool(msg.data)
|
||||
|
||||
def on_mode_state(self, msg: String) -> None:
|
||||
self.latest_mode = msg.data
|
||||
|
||||
def on_mux_status(self, msg: String) -> None:
|
||||
self.latest_mux_status = msg.data
|
||||
|
||||
def on_rx_timer(self) -> None:
|
||||
if not self.enabled:
|
||||
return
|
||||
while True:
|
||||
try:
|
||||
data, addr = self.sock.recvfrom(self.max_packet_bytes)
|
||||
except BlockingIOError:
|
||||
break
|
||||
except OSError as exc:
|
||||
self.get_logger().warn(f"UDP receive failed: {exc}")
|
||||
break
|
||||
self.client_addr = addr
|
||||
try:
|
||||
payload = json.loads(data.decode("utf-8"))
|
||||
self.handle_packet(payload)
|
||||
except Exception as exc:
|
||||
self.send_packet({"type": "error", "message": str(exc)})
|
||||
|
||||
def handle_packet(self, payload: dict[str, Any]) -> None:
|
||||
msg_type = str(payload.get("type", "")).lower()
|
||||
if msg_type == "cmd_vel":
|
||||
cmd = self.parse_twist(payload)
|
||||
self.cmd_pub.publish(cmd)
|
||||
self.last_cmd_time = self.get_clock().now()
|
||||
self.timeout_estop_sent = False
|
||||
elif msg_type == "zero":
|
||||
self.cmd_pub.publish(Twist())
|
||||
self.last_cmd_time = self.get_clock().now()
|
||||
elif msg_type == "estop":
|
||||
self.estop_pub.publish(Bool(data=bool(payload.get("data", True))))
|
||||
elif msg_type == "mode":
|
||||
mode = str(payload.get("mode", "DISABLED")).upper()
|
||||
self.mode_pub.publish(String(data=mode))
|
||||
self.web_enabled = mode == "WEB"
|
||||
self.web_enabled_pub.publish(Bool(data=mode == "WEB"))
|
||||
self.remote_enabled_pub.publish(Bool(data=mode == "REMOTE"))
|
||||
self.nav_enabled_pub.publish(Bool(data=mode == "NAV"))
|
||||
elif msg_type == "web_enable":
|
||||
self.web_enabled = bool(payload.get("data", False))
|
||||
self.web_enabled_pub.publish(Bool(data=self.web_enabled))
|
||||
if self.web_enabled:
|
||||
self.mode_pub.publish(String(data="WEB"))
|
||||
elif msg_type == "remote_enable":
|
||||
enabled = bool(payload.get("data", False))
|
||||
self.remote_enabled_pub.publish(Bool(data=enabled))
|
||||
if enabled:
|
||||
self.mode_pub.publish(String(data="REMOTE"))
|
||||
elif msg_type == "nav_enable":
|
||||
enabled = bool(payload.get("data", False))
|
||||
self.nav_enabled_pub.publish(Bool(data=enabled))
|
||||
if enabled:
|
||||
self.mode_pub.publish(String(data="NAV"))
|
||||
elif msg_type == "ping":
|
||||
self.send_packet({"type": "pong", "stamp": self.now_sec()})
|
||||
else:
|
||||
self.send_packet({"type": "error", "message": f"unknown packet type: {msg_type}"})
|
||||
|
||||
def parse_twist(self, payload: dict[str, Any]) -> Twist:
|
||||
cmd = Twist()
|
||||
linear = payload.get("linear", {}) or {}
|
||||
angular = payload.get("angular", {}) or {}
|
||||
cmd.linear.x = self.clamp(float(linear.get("x", 0.0)), -self.max_vx, self.max_vx)
|
||||
cmd.linear.y = self.clamp(float(linear.get("y", 0.0)), -self.max_vy, self.max_vy)
|
||||
cmd.angular.z = self.clamp(float(angular.get("z", 0.0)), -self.max_yaw, self.max_yaw)
|
||||
return cmd
|
||||
|
||||
def on_guard_timer(self) -> None:
|
||||
age_ms = (self.get_clock().now() - self.last_cmd_time).nanoseconds / 1.0e6
|
||||
if age_ms > self.cmd_timeout_ms:
|
||||
self.cmd_pub.publish(Twist())
|
||||
if self.estop_on_timeout and not self.timeout_estop_sent:
|
||||
self.estop_pub.publish(Bool(data=True))
|
||||
self.timeout_estop_sent = True
|
||||
|
||||
def on_state_timer(self) -> None:
|
||||
if not self.enabled:
|
||||
return
|
||||
self.send_packet(self.build_state_packet())
|
||||
|
||||
def build_state_packet(self) -> dict[str, Any]:
|
||||
target = self.latest_target
|
||||
state = self.latest_state
|
||||
packet: dict[str, Any] = {
|
||||
"type": "state",
|
||||
"stamp": self.now_sec(),
|
||||
"mode": self.latest_mode,
|
||||
"mux_status": self.latest_mux_status,
|
||||
"estop": self.estop,
|
||||
"web_enabled": self.web_enabled,
|
||||
"cmd_vel": self.twist_to_dict(self.latest_cmd),
|
||||
"runtime": {},
|
||||
"robot": {},
|
||||
}
|
||||
if target is not None:
|
||||
packet["runtime"] = {
|
||||
"target_source": str(target.target_source),
|
||||
"zero_command": bool(target.zero_command),
|
||||
"runtime_released": bool(target.runtime_released),
|
||||
"release_alpha": self._f(target.release_alpha),
|
||||
"command": [self._f(v) for v in target.command],
|
||||
"raw_command": [self._f(v) for v in target.raw_command],
|
||||
}
|
||||
if state is not None:
|
||||
packet["robot"] = {
|
||||
"joint_pos": [self._f(v) for v in state.joint_pos],
|
||||
"joint_vel": [self._f(v) for v in state.joint_vel],
|
||||
"joint_torque": [self._f(v) for v in state.joint_torque],
|
||||
"imu_gyro": [self._f(v) for v in state.imu_gyro],
|
||||
"imu_accel": [self._f(v) for v in state.imu_accel],
|
||||
"projected_gravity": [self._f(v) for v in state.projected_gravity],
|
||||
"quat_wxyz": [self._f(v) for v in state.quat_wxyz],
|
||||
"imu_age_ms": self._f(state.imu_age_ms),
|
||||
"imu_fresh": bool(state.imu_fresh),
|
||||
"odom_age_ms": self._f(state.odom_age_ms),
|
||||
"odom_fresh": bool(state.odom_fresh),
|
||||
"odom_local_pos": [self._f(v) for v in state.odom_local_pos],
|
||||
"odom_local_yaw": self._f(state.odom_local_yaw),
|
||||
"fresh_count": int(state.fresh_count),
|
||||
"holdover_count": int(state.holdover_count),
|
||||
"stale_max": int(state.stale_max),
|
||||
"update_counts": [int(v) for v in state.update_counts],
|
||||
}
|
||||
return packet
|
||||
|
||||
def send_packet(self, payload: dict[str, Any]) -> None:
|
||||
if self.client_addr is None:
|
||||
return
|
||||
try:
|
||||
data = json.dumps(payload, separators=(",", ":")).encode("utf-8")
|
||||
self.sock.sendto(data, self.client_addr)
|
||||
except OSError as exc:
|
||||
self.get_logger().warn(f"UDP send failed: {exc}")
|
||||
|
||||
@staticmethod
|
||||
def _f(v: Any) -> float:
|
||||
try:
|
||||
f = float(v)
|
||||
if f != f:
|
||||
return 0.0
|
||||
return round(f, 6)
|
||||
except (TypeError, ValueError):
|
||||
return 0.0
|
||||
|
||||
def now_sec(self) -> float:
|
||||
return self.get_clock().now().nanoseconds / 1.0e9
|
||||
|
||||
@staticmethod
|
||||
def twist_to_dict(msg: Twist) -> dict[str, Any]:
|
||||
return {
|
||||
"linear": {"x": msg.linear.x, "y": msg.linear.y, "z": msg.linear.z},
|
||||
"angular": {"x": msg.angular.x, "y": msg.angular.y, "z": msg.angular.z},
|
||||
}
|
||||
|
||||
@staticmethod
|
||||
def clamp(value: float, low: float, high: float) -> float:
|
||||
return max(low, min(high, value))
|
||||
|
||||
|
||||
def main(args: Optional[list[str]] = None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = WebUdpBridgeNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except (KeyboardInterrupt, ExternalShutdownException):
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,75 @@
|
||||
#!/bin/bash
|
||||
|
||||
# Exit immediately if a command exits with a non-zero status
|
||||
set -e
|
||||
|
||||
# Define color codes for pretty output
|
||||
GREEN='\033[0;32m'
|
||||
YELLOW='\033[1;33m'
|
||||
RED='\033[0;31m'
|
||||
NC='\033[0m' # No Color
|
||||
|
||||
echo -e "${YELLOW}====================================================${NC}"
|
||||
echo -e "${GREEN} Starting Sim2Real Locomotion ROS2 Stack ${NC}"
|
||||
echo -e "${YELLOW}====================================================${NC}"
|
||||
|
||||
# 1. Source ROS2 Humble environment
|
||||
if [ -f "/opt/ros/humble/setup.bash" ]; then
|
||||
echo -e "[System] Sourcing ROS2 Humble..."
|
||||
source /opt/ros/humble/setup.bash
|
||||
else
|
||||
echo -e "${RED}[Error] ROS2 Humble not found. Please install ROS2 Humble first.${NC}"
|
||||
exit 1
|
||||
fi
|
||||
|
||||
# 2. Check if local workspace is compiled and source it
|
||||
SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
|
||||
cd "$SCRIPT_DIR"
|
||||
|
||||
if [ -f "install/setup.bash" ]; then
|
||||
echo -e "[Workspace] Sourcing local workspace..."
|
||||
source install/setup.bash
|
||||
elif [ -f "../../install/setup.bash" ]; then
|
||||
echo -e "[Workspace] Sourcing parent install/setup.bash..."
|
||||
source ../../install/setup.bash
|
||||
else
|
||||
echo -e "${YELLOW}[Warning] install/setup.bash not found. Attempting to build the workspace first...${NC}"
|
||||
if command -v colcon &> /dev/null; then
|
||||
echo -e "[Build] Running colcon build..."
|
||||
colcon build --merge-install --cmake-args -DCMAKE_BUILD_TYPE=Release
|
||||
source install/setup.bash
|
||||
else
|
||||
echo -e "${RED}[Error] 'colcon' tool not found. Please compile the workspace manually before running.${NC}"
|
||||
exit 1
|
||||
fi
|
||||
fi
|
||||
|
||||
# 3. Check for SocketCAN interfaces (in non-dry-run mode)
|
||||
# Reading dry_run parameter from yaml config
|
||||
if [ -f "src/sim2real_bringup/config/runtime.yaml" ]; then
|
||||
# Use sed for portability (busybox-compatible, avoids GNU grep -oP dependency)
|
||||
DRY_RUN=$(sed -n 's/^[[:space:]]*dry_run:[[:space:]]*//p' src/sim2real_bringup/config/runtime.yaml | head -n 1 || echo "true")
|
||||
# Trim trailing whitespace/newlines
|
||||
DRY_RUN=$(echo "$DRY_RUN" | tr -d '[:space:]')
|
||||
else
|
||||
DRY_RUN="true"
|
||||
fi
|
||||
|
||||
if [ "$DRY_RUN" = "false" ]; then
|
||||
echo -e "[Network] Checking CAN interfaces..."
|
||||
if ip link show can0 &> /dev/null && ip link show can1 &> /dev/null; then
|
||||
echo -e "[Network] can0 and can1 interfaces detected."
|
||||
else
|
||||
echo -e "${YELLOW}[Warning] CAN interfaces (can0/can1) not fully active.${NC}"
|
||||
echo -e "To configure CAN interfaces, run:"
|
||||
echo -e " sudo ip link set can0 up type can bitrate 1000000"
|
||||
echo -e " sudo ip link set can1 up type can bitrate 1000000"
|
||||
fi
|
||||
else
|
||||
echo -e "${YELLOW}[Dry-Run] Running in Dry-Run mode. SocketCAN will not be accessed.${NC}"
|
||||
fi
|
||||
|
||||
# 4. Run the ROS2 Launch file
|
||||
echo -e "${GREEN}[Launch] Starting sim2real launch file...${NC}"
|
||||
ros2 launch sim2real_bringup sim2real_system.launch.py "$@"
|
||||
|
||||
@@ -0,0 +1,107 @@
|
||||
#!/usr/bin/env python3
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import socket
|
||||
import threading
|
||||
import time
|
||||
from http.server import SimpleHTTPRequestHandler, ThreadingHTTPServer
|
||||
from pathlib import Path
|
||||
from typing import Optional
|
||||
|
||||
STATE_LOCK = threading.Lock()
|
||||
LATEST_STATE: dict = {"type": "state", "connected": False}
|
||||
NANO_ADDR: tuple[str, int]
|
||||
UDP_SOCK: socket.socket
|
||||
|
||||
|
||||
class Handler(SimpleHTTPRequestHandler):
|
||||
def do_GET(self) -> None:
|
||||
if self.path == "/api/state":
|
||||
with STATE_LOCK:
|
||||
data = json.dumps(LATEST_STATE).encode("utf-8")
|
||||
self._json(200, data)
|
||||
return
|
||||
super().do_GET()
|
||||
|
||||
def do_POST(self) -> None:
|
||||
if self.path not in ("/api/control", "/api/state"):
|
||||
self.send_error(404)
|
||||
return
|
||||
length = int(self.headers.get("Content-Length", "0"))
|
||||
body = self.rfile.read(length) if length else b"{}"
|
||||
try:
|
||||
payload = json.loads(body.decode("utf-8"))
|
||||
send_udp(payload)
|
||||
self._json(200, b'{"ok":true}')
|
||||
except Exception as exc:
|
||||
self._json(400, json.dumps({"ok": False, "error": str(exc)}).encode())
|
||||
|
||||
def _json(self, code: int, data: bytes) -> None:
|
||||
self.send_response(code)
|
||||
self.send_header("Content-Type", "application/json")
|
||||
self.send_header("Content-Length", str(len(data)))
|
||||
self.send_header("Access-Control-Allow-Origin", "*")
|
||||
self.end_headers()
|
||||
self.wfile.write(data)
|
||||
|
||||
def log_message(self, format: str, *args: object) -> None:
|
||||
return
|
||||
|
||||
|
||||
def send_udp(payload: dict) -> None:
|
||||
data = json.dumps(payload, separators=(",", ":")).encode("utf-8")
|
||||
UDP_SOCK.sendto(data, NANO_ADDR)
|
||||
|
||||
|
||||
def udp_rx_loop(sock: socket.socket) -> None:
|
||||
global LATEST_STATE
|
||||
while True:
|
||||
try:
|
||||
data, _ = sock.recvfrom(65535)
|
||||
payload = json.loads(data.decode("utf-8"))
|
||||
payload["connected"] = True
|
||||
payload["local_receive_time"] = time.time()
|
||||
with STATE_LOCK:
|
||||
LATEST_STATE = payload
|
||||
except Exception:
|
||||
time.sleep(0.01)
|
||||
|
||||
|
||||
def heartbeat_loop() -> None:
|
||||
while True:
|
||||
try:
|
||||
send_udp({"type": "ping", "stamp": time.time()})
|
||||
except Exception:
|
||||
pass
|
||||
time.sleep(0.5)
|
||||
|
||||
|
||||
def main() -> None:
|
||||
global NANO_ADDR, UDP_SOCK
|
||||
parser = argparse.ArgumentParser(description="Windows local web debug UI for sim2real_ros2")
|
||||
parser.add_argument("--nano-host", required=True, help="Nano IP address")
|
||||
parser.add_argument("--nano-port", type=int, default=15000, help="Nano UDP listen port")
|
||||
parser.add_argument("--listen-host", default="0.0.0.0", help="Local HTTP host")
|
||||
parser.add_argument("--http-port", type=int, default=8088, help="Local HTTP port")
|
||||
parser.add_argument("--udp-port", type=int, default=15001, help="Local UDP receive port")
|
||||
args = parser.parse_args()
|
||||
|
||||
NANO_ADDR = (args.nano_host, args.nano_port)
|
||||
UDP_SOCK = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||
UDP_SOCK.bind(("0.0.0.0", args.udp_port))
|
||||
|
||||
threading.Thread(target=udp_rx_loop, args=(UDP_SOCK,), daemon=True).start()
|
||||
threading.Thread(target=heartbeat_loop, daemon=True).start()
|
||||
|
||||
static_dir = Path(__file__).resolve().parent / "static"
|
||||
handler = lambda *a, **kw: Handler(*a, directory=str(static_dir), **kw)
|
||||
httpd = ThreadingHTTPServer((args.listen_host, args.http_port), handler)
|
||||
print(f"Open http://127.0.0.1:{args.http_port}")
|
||||
print(f"UDP Nano={args.nano_host}:{args.nano_port} local={args.udp_port}")
|
||||
httpd.serve_forever()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,259 @@
|
||||
'use strict';
|
||||
|
||||
const JOINT_NAMES = [
|
||||
'FL_H_ABD','FL_H_PIT','FL_KNEE',
|
||||
'FR_H_ABD','FR_H_PIT','FR_KNEE',
|
||||
'RL_H_ABD','RL_H_PIT','RL_KNEE',
|
||||
'RR_H_ABD','RR_H_PIT','RR_KNEE',
|
||||
'FL_WHEEL','FR_WHEEL','RL_WHEEL','RR_WHEEL',
|
||||
];
|
||||
|
||||
const $ = id => document.getElementById(id);
|
||||
const cmd = { vx: 0, vy: 0, yaw: 0 };
|
||||
let cmdSendTimer = null;
|
||||
let currentMode = 'UNKNOWN';
|
||||
|
||||
// ── API ──────────────────────────────────────────────────────────────────────
|
||||
async function post(payload) {
|
||||
try {
|
||||
await fetch('/api/control', {
|
||||
method: 'POST',
|
||||
headers: { 'Content-Type': 'application/json' },
|
||||
body: JSON.stringify(payload),
|
||||
});
|
||||
} catch (e) {
|
||||
appendEvent('API_ERROR', e.message, 'bad');
|
||||
}
|
||||
}
|
||||
|
||||
function sendCmd() {
|
||||
post({
|
||||
type: 'cmd_vel',
|
||||
linear: { x: cmd.vx, y: cmd.vy, z: 0 },
|
||||
angular: { x: 0, y: 0, z: cmd.yaw },
|
||||
});
|
||||
$('cmd-display').textContent =
|
||||
`vx=${cmd.vx.toFixed(2)} vy=${cmd.vy.toFixed(2)} yaw=${cmd.yaw.toFixed(2)}`;
|
||||
}
|
||||
|
||||
function zeroAll() {
|
||||
cmd.vx = 0; cmd.vy = 0; cmd.yaw = 0;
|
||||
$('cmd-vx').value = 0;
|
||||
$('cmd-vy').value = 0;
|
||||
$('cmd-yaw').value = 0;
|
||||
$('cmd-vx-v').textContent = '0.00';
|
||||
$('cmd-vy-v').textContent = '0.00';
|
||||
$('cmd-yaw-v').textContent = '0.00';
|
||||
$('cmd-display').textContent = 'vx=0.00 vy=0.00 yaw=0.00';
|
||||
$('stick').style.transform = 'translate(-50%, -50%)';
|
||||
post({ type: 'zero' });
|
||||
}
|
||||
|
||||
// ── Buttons ──────────────────────────────────────────────────────────────────
|
||||
function setMode(mode) {
|
||||
if (mode === 'WEB' && !confirm('确认切换到 WEB 控制？\n请确认机器人安全且速度为 0。')) return;
|
||||
post({ type: 'mode', mode });
|
||||
appendEvent('MODE_SET', `→ ${mode}`, 'ok');
|
||||
}
|
||||
|
||||
function highlightMode(mode) {
|
||||
for (const m of ['DISABLED', 'REMOTE', 'WEB', 'NAV']) {
|
||||
const btn = $('btn-' + m.toLowerCase());
|
||||
if (btn) btn.classList.toggle('active-mode', m === mode);
|
||||
}
|
||||
const el = $('stage');
|
||||
if (el) {
|
||||
el.textContent = mode;
|
||||
el.className = 'stage ' + mode;
|
||||
}
|
||||
currentMode = mode;
|
||||
}
|
||||
|
||||
$('btn-disabled').onclick = () => { zeroAll(); setMode('DISABLED'); };
|
||||
$('btn-remote').onclick = () => setMode('REMOTE');
|
||||
$('btn-web').onclick = () => setMode('WEB');
|
||||
$('btn-nav').onclick = () => setMode('NAV');
|
||||
$('btn-zero').onclick = zeroAll;
|
||||
$('btn-estop').onclick = () => {
|
||||
if (confirm('确认触发软急停？')) {
|
||||
post({ type: 'estop', data: true });
|
||||
zeroAll();
|
||||
appendEvent('ESTOP', '软急停已触发', 'bad');
|
||||
}
|
||||
};
|
||||
|
||||
// ── Sliders ──────────────────────────────────────────────────────────────────
|
||||
for (const [id, key] of [['cmd-vx','vx'],['cmd-vy','vy'],['cmd-yaw','yaw']]) {
|
||||
$(id).addEventListener('input', e => {
|
||||
cmd[key] = parseFloat(e.target.value);
|
||||
$(id + '-v').textContent = cmd[key].toFixed(2);
|
||||
if (currentMode === 'WEB') sendCmd();
|
||||
});
|
||||
}
|
||||
|
||||
// ── Joystick ─────────────────────────────────────────────────────────────────
|
||||
const joystick = $('joystick');
|
||||
const stick = $('stick');
|
||||
let dragging = false;
|
||||
|
||||
function updateJoystick(clientX, clientY) {
|
||||
const rect = joystick.getBoundingClientRect();
|
||||
const cx = rect.left + rect.width / 2;
|
||||
const cy = rect.top + rect.height / 2;
|
||||
const maxR = rect.width * 0.42;
|
||||
let dx = clientX - cx;
|
||||
let dy = clientY - cy;
|
||||
const dist = Math.hypot(dx, dy);
|
||||
if (dist > maxR) { dx = dx / dist * maxR; dy = dy / dist * maxR; }
|
||||
stick.style.transform = `translate(calc(-50% + ${dx}px), calc(-50% + ${dy}px))`;
|
||||
cmd.vx = parseFloat((-(dy / maxR) * 0.8).toFixed(3));
|
||||
cmd.vy = parseFloat(( (dx / maxR) * 0.3).toFixed(3));
|
||||
$('cmd-vx').value = cmd.vx;
|
||||
$('cmd-vy').value = cmd.vy;
|
||||
$('cmd-vx-v').textContent = cmd.vx.toFixed(2);
|
||||
$('cmd-vy-v').textContent = cmd.vy.toFixed(2);
|
||||
if (currentMode === 'WEB') sendCmd();
|
||||
}
|
||||
|
||||
joystick.addEventListener('pointerdown', e => {
|
||||
dragging = true;
|
||||
joystick.setPointerCapture(e.pointerId);
|
||||
updateJoystick(e.clientX, e.clientY);
|
||||
});
|
||||
joystick.addEventListener('pointermove', e => { if (dragging) updateJoystick(e.clientX, e.clientY); });
|
||||
joystick.addEventListener('pointerup', () => { dragging = false; zeroAll(); });
|
||||
joystick.addEventListener('pointercancel', () => { dragging = false; zeroAll(); });
|
||||
|
||||
// ── Joints grid init ─────────────────────────────────────────────────────────
|
||||
function initJointsGrid() {
|
||||
const grid = $('joints-grid');
|
||||
if (!grid) return;
|
||||
grid.innerHTML = JOINT_NAMES.map((name, i) => `
|
||||
<div class="motor-row" id="mi-${i}">
|
||||
<span class="stale" id="ms-${i}" style="color:#ef4444">●</span>
|
||||
<span class="name">${name}</span>
|
||||
<span class="val pos" id="mp-${i}">0.00</span>
|
||||
<span class="val vel" id="mv-${i}">0.00</span>
|
||||
<span class="val tau" id="mt-${i}">0.00</span>
|
||||
</div>`).join('');
|
||||
}
|
||||
|
||||
function updateJointsGrid(robot) {
|
||||
if (!robot) return;
|
||||
const pos = robot.joint_pos || [];
|
||||
const vel = robot.joint_vel || [];
|
||||
const tau = robot.joint_torque || [];
|
||||
const upd = robot.update_counts || [];
|
||||
for (let i = 0; i < 16; i++) {
|
||||
const dot = $('ms-' + i);
|
||||
const cnt = upd[i] ?? 0;
|
||||
if (dot) dot.style.color = cnt > 0 ? '#30d158' : '#ef4444';
|
||||
const p = $('mp-' + i); if (p) p.textContent = (pos[i] || 0).toFixed(2);
|
||||
const v = $('mv-' + i); if (v) v.textContent = (vel[i] || 0).toFixed(2);
|
||||
const t = $('mt-' + i);
|
||||
if (t) {
|
||||
t.textContent = (tau[i] || 0).toFixed(2);
|
||||
t.style.color = Math.abs(tau[i] || 0) > 16 ? '#ff453a' : '#ff9f0a';
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ── State polling ─────────────────────────────────────────────────────────────
|
||||
function setText(id, text, cls) {
|
||||
const el = $(id);
|
||||
if (!el) return;
|
||||
el.textContent = text;
|
||||
if (cls !== undefined) el.className = 'diag-value ' + cls;
|
||||
}
|
||||
|
||||
function applyState(data) {
|
||||
const connected = data.connected &&
|
||||
(!data.local_receive_time || Date.now() / 1000 - data.local_receive_time < 2.5);
|
||||
|
||||
const stage = $('stage');
|
||||
if (stage) {
|
||||
if (!connected) {
|
||||
stage.textContent = 'DISCONNECTED';
|
||||
stage.className = 'stage DISCONNECTED';
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
const mode = data.mode || 'UNKNOWN';
|
||||
if (mode !== currentMode) highlightMode(mode);
|
||||
|
||||
const rt = data.runtime || {};
|
||||
const src = rt.target_source || '--';
|
||||
const srcCls = src === 'safety_brake' ? 'bad'
|
||||
: src === 'runtime_policy' ? 'ok'
|
||||
: src === 'runtime_zero_hold' ? 'active' : '';
|
||||
setText('d-source', src, srcCls);
|
||||
setText('d-released', String(rt.runtime_released ?? '--'),
|
||||
rt.runtime_released ? 'ok' : '');
|
||||
setText('d-alpha', (rt.release_alpha ?? '--') !== '--'
|
||||
? Number(rt.release_alpha).toFixed(2) : '--');
|
||||
setText('d-zero', String(rt.zero_command ?? '--'));
|
||||
setText('d-estop', String(data.estop ?? '--'),
|
||||
data.estop ? 'bad' : 'ok');
|
||||
setText('d-mux', data.mux_status || '--');
|
||||
|
||||
const robot = data.robot || {};
|
||||
const imuAge = robot.imu_age_ms ?? null;
|
||||
setText('d-imu-fresh', String(robot.imu_fresh ?? '--'),
|
||||
robot.imu_fresh ? 'ok' : 'bad');
|
||||
setText('d-imu-age', imuAge !== null ? imuAge.toFixed(1) : '--',
|
||||
imuAge !== null ? (imuAge > 200 ? 'bad' : imuAge > 60 ? 'warn' : 'ok') : '');
|
||||
|
||||
const grav = robot.projected_gravity;
|
||||
setText('d-gravity', grav ? grav.map(v => Number(v).toFixed(2)).join(', ') : '--',
|
||||
grav && grav[2] < -0.5 ? 'ok' : 'warn');
|
||||
|
||||
setText('d-holdover', String(robot.holdover_count ?? '--'),
|
||||
(robot.holdover_count || 0) > 10 ? 'warn' : '');
|
||||
|
||||
const odomAge = robot.odom_age_ms ?? null;
|
||||
setText('d-odom-age', odomAge !== null ? odomAge.toFixed(1) : '--',
|
||||
odomAge !== null ? (odomAge > 500 ? 'bad' : odomAge > 200 ? 'warn' : 'ok') : '');
|
||||
|
||||
const lp = robot.odom_local_pos;
|
||||
setText('d-odom-pos', lp ? `x=${Number(lp[0]).toFixed(2)} y=${Number(lp[1]).toFixed(2)}` : '--');
|
||||
|
||||
const cv = data.cmd_vel || {};
|
||||
const lin = cv.linear || {};
|
||||
const ang = cv.angular || {};
|
||||
setText('cv-vx', (lin.x ?? 0).toFixed(3));
|
||||
setText('cv-vy', (lin.y ?? 0).toFixed(3));
|
||||
setText('cv-yaw', (ang.z ?? 0).toFixed(3));
|
||||
|
||||
updateJointsGrid(robot);
|
||||
}
|
||||
|
||||
async function poll() {
|
||||
try {
|
||||
const res = await fetch('/api/state');
|
||||
const data = await res.json();
|
||||
applyState(data);
|
||||
} catch (_) {}
|
||||
}
|
||||
|
||||
// ── WEB mode heartbeat ────────────────────────────────────────────────────────
|
||||
setInterval(() => {
|
||||
if (currentMode === 'WEB' && !dragging) sendCmd();
|
||||
}, 50);
|
||||
|
||||
// ── Event log ─────────────────────────────────────────────────────────────────
|
||||
function appendEvent(kind, detail, cls) {
|
||||
const el = $('events-log');
|
||||
if (!el) return;
|
||||
const div = document.createElement('div');
|
||||
const t = new Date().toLocaleTimeString();
|
||||
div.innerHTML = `<span class="ev-t">${t}</span> <span class="ev-${cls || 'ok'}">${kind}</span> <span style="color:#8e8e93">${detail || ''}</span>`;
|
||||
el.appendChild(div);
|
||||
while (el.children.length > 200) el.removeChild(el.firstChild);
|
||||
el.scrollTop = el.scrollHeight;
|
||||
}
|
||||
|
||||
// ── Init ──────────────────────────────────────────────────────────────────────
|
||||
initJointsGrid();
|
||||
setInterval(poll, 100);
|
||||
appendEvent('READY', '页面已加载，等待 Nano 连接', 'ok');
|
||||
@@ -0,0 +1,98 @@
|
||||
<!doctype html>
|
||||
<html lang="zh-CN" data-theme="dark">
|
||||
<head>
|
||||
<meta charset="utf-8">
|
||||
<meta name="viewport" content="width=device-width,initial-scale=1">
|
||||
<title>sim2real ROS2 控制台</title>
|
||||
<link rel="stylesheet" href="style.css">
|
||||
</head>
|
||||
<body>
|
||||
<header class="glass-panel top-bar">
|
||||
<div class="top-bar-left">
|
||||
<h1>sim2real ROS2</h1>
|
||||
<span class="stage" id="stage">DISCONNECTED</span>
|
||||
</div>
|
||||
<div class="top-bar-center">
|
||||
<span class="label">控制模式</span>
|
||||
<button class="btn" id="btn-disabled">DISABLED</button>
|
||||
<button class="btn btn-remote" id="btn-remote">REMOTE</button>
|
||||
<button class="btn btn-web" id="btn-web">WEB</button>
|
||||
<button class="btn btn-nav" id="btn-nav">NAV</button>
|
||||
<div class="divider"></div>
|
||||
<button class="btn" id="btn-zero">速度归零</button>
|
||||
</div>
|
||||
<div class="top-bar-right">
|
||||
<button class="btn btn-danger" id="btn-estop">软急停</button>
|
||||
</div>
|
||||
</header>
|
||||
|
||||
<div class="glass-panel side-panel left-panel">
|
||||
<div class="panel-section">
|
||||
<h2 class="panel-title">运行状态</h2>
|
||||
<div class="diag-row"><span class="diag-label">target_source</span><span class="diag-value" id="d-source">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">runtime_released</span><span class="diag-value" id="d-released">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">release_alpha</span><span class="diag-value" id="d-alpha">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">zero_command</span><span class="diag-value" id="d-zero">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">estop</span><span class="diag-value" id="d-estop">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">mux</span><span class="diag-value" id="d-mux">--</span></div>
|
||||
</div>
|
||||
|
||||
<div class="panel-section">
|
||||
<h2 class="panel-title">IMU & 里程计</h2>
|
||||
<div class="diag-row"><span class="diag-label">IMU fresh</span><span class="diag-value" id="d-imu-fresh">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">IMU age ms</span><span class="diag-value" id="d-imu-age">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">projected_gravity</span><span class="diag-value" id="d-gravity">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">holdover</span><span class="diag-value" id="d-holdover">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">odom age ms</span><span class="diag-value" id="d-odom-age">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">odom local pos</span><span class="diag-value" id="d-odom-pos">--</span></div>
|
||||
</div>
|
||||
|
||||
<div class="panel-section flex-1">
|
||||
<h2 class="panel-title">关节状态 (16轴)</h2>
|
||||
<div id="joints-grid" class="motors-grid-list"></div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div class="glass-panel side-panel right-panel">
|
||||
<div class="panel-section">
|
||||
<h2 class="panel-title">Web 手动控制</h2>
|
||||
<div class="joystick-area">
|
||||
<div class="joystick" id="joystick"><div id="stick"></div></div>
|
||||
<p class="hint">拖动控制前后(vx)和横移(vy)，松开归零</p>
|
||||
</div>
|
||||
<div class="slider-group">
|
||||
<div class="slider-row">
|
||||
<span class="slider-label">vx</span>
|
||||
<input type="range" id="cmd-vx" class="glass-slider" min="-0.8" max="0.8" step="0.01" value="0">
|
||||
<span class="slider-val" id="cmd-vx-v">0.00</span>
|
||||
</div>
|
||||
<div class="slider-row">
|
||||
<span class="slider-label">vy</span>
|
||||
<input type="range" id="cmd-vy" class="glass-slider" min="-0.3" max="0.3" step="0.01" value="0">
|
||||
<span class="slider-val" id="cmd-vy-v">0.00</span>
|
||||
</div>
|
||||
<div class="slider-row">
|
||||
<span class="slider-label">yaw</span>
|
||||
<input type="range" id="cmd-yaw" class="glass-slider" min="-0.5" max="0.5" step="0.01" value="0">
|
||||
<span class="slider-val" id="cmd-yaw-v">0.00</span>
|
||||
</div>
|
||||
</div>
|
||||
<div class="cmd-display" id="cmd-display">vx=0.00 vy=0.00 yaw=0.00</div>
|
||||
</div>
|
||||
|
||||
<div class="panel-section">
|
||||
<h2 class="panel-title">当前输出 /cmd_vel</h2>
|
||||
<div class="diag-row"><span class="diag-label">linear.x</span><span class="diag-value" id="cv-vx">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">linear.y</span><span class="diag-value" id="cv-vy">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">angular.z</span><span class="diag-value" id="cv-yaw">--</span></div>
|
||||
</div>
|
||||
|
||||
<div class="panel-section log-section flex-1">
|
||||
<h2 class="panel-title">事件流</h2>
|
||||
<div id="events-log" class="log"></div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<script src="app.js"></script>
|
||||
</body>
|
||||
</html>
|
||||
@@ -0,0 +1,263 @@
|
||||
/* sim2real ROS2 Web Debug — Apple Glass Design */
|
||||
:root {
|
||||
--bg-primary: #000000;
|
||||
--glass-bg: rgba(20, 20, 22, 0.65);
|
||||
--glass-border: rgba(255, 255, 255, 0.12);
|
||||
--glass-shadow: 0 8px 32px rgba(0, 0, 0, 0.25);
|
||||
--text-primary: #ffffff;
|
||||
--text-secondary: #ebebf5;
|
||||
--text-tertiary: #8e8e93;
|
||||
--accent: #0a84ff;
|
||||
--accent-hover: #409cff;
|
||||
--success: #30d158;
|
||||
--warning: #ffd60a;
|
||||
--danger: #ff453a;
|
||||
--blur-amount: 24px;
|
||||
--saturation: 180%;
|
||||
--spring: cubic-bezier(0.4, 0, 0.2, 1);
|
||||
--panel-radius: 16px;
|
||||
--font-family: -apple-system, BlinkMacSystemFont, "SF Pro Display", "PingFang SC", sans-serif;
|
||||
}
|
||||
|
||||
* { box-sizing: border-box; margin: 0; padding: 0; }
|
||||
|
||||
body {
|
||||
font-family: var(--font-family);
|
||||
background: radial-gradient(circle at top left, #1a1a24 0%, #000000 100%);
|
||||
color: var(--text-primary);
|
||||
-webkit-font-smoothing: antialiased;
|
||||
min-height: 100vh;
|
||||
overflow-x: hidden;
|
||||
}
|
||||
|
||||
.glass-panel {
|
||||
background: var(--glass-bg);
|
||||
backdrop-filter: blur(var(--blur-amount)) saturate(var(--saturation));
|
||||
-webkit-backdrop-filter: blur(var(--blur-amount)) saturate(var(--saturation));
|
||||
border: 0.5px solid var(--glass-border);
|
||||
box-shadow: var(--glass-shadow);
|
||||
z-index: 50;
|
||||
}
|
||||
|
||||
/* Top Bar */
|
||||
.top-bar {
|
||||
position: fixed;
|
||||
top: 16px;
|
||||
left: 50%;
|
||||
transform: translateX(-50%);
|
||||
display: flex;
|
||||
align-items: center;
|
||||
justify-content: space-between;
|
||||
padding: 8px 16px;
|
||||
border-radius: 24px;
|
||||
width: 96%;
|
||||
max-width: 1400px;
|
||||
gap: 16px;
|
||||
}
|
||||
.top-bar-left, .top-bar-center, .top-bar-right {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
gap: 10px;
|
||||
}
|
||||
.top-bar-center { flex: 1; justify-content: center; }
|
||||
.top-bar h1 {
|
||||
font-size: 16px;
|
||||
font-weight: 600;
|
||||
background: linear-gradient(45deg, #fff, #8e8e93);
|
||||
-webkit-background-clip: text;
|
||||
-webkit-text-fill-color: transparent;
|
||||
}
|
||||
.divider { width: 1px; height: 24px; background: var(--glass-border); margin: 0 4px; }
|
||||
|
||||
/* Stage badge */
|
||||
.stage {
|
||||
padding: 4px 10px;
|
||||
border-radius: 12px;
|
||||
font-size: 11px;
|
||||
font-weight: 700;
|
||||
text-transform: uppercase;
|
||||
letter-spacing: 0.5px;
|
||||
background: rgba(255,255,255,0.1);
|
||||
color: var(--text-secondary);
|
||||
}
|
||||
.stage.DISCONNECTED { background: rgba(142,142,147,0.3); color: #aaa; }
|
||||
.stage.CONNECTED { background: rgba(10,132,255,0.3); color: #82c4ff; }
|
||||
.stage.REMOTE { background: rgba(48,209,88,0.3); color: #8deda7; }
|
||||
.stage.WEB { background: rgba(0,122,255,0.3); color: #82c4ff; }
|
||||
.stage.NAV { background: rgba(255,214,10,0.3); color: #ffe680; }
|
||||
.stage.DISABLED { background: rgba(142,142,147,0.25); color: #aaa; }
|
||||
.stage.ESTOPPED { background: rgba(255,69,58,0.5); color: #ff8b86; box-shadow: 0 0 8px rgba(255,69,58,0.4); }
|
||||
|
||||
/* Buttons */
|
||||
.btn {
|
||||
background: rgba(255,255,255,0.08);
|
||||
border: 1px solid rgba(255,255,255,0.1);
|
||||
border-radius: 8px;
|
||||
color: var(--text-primary);
|
||||
font-size: 12px;
|
||||
font-weight: 500;
|
||||
padding: 6px 12px;
|
||||
cursor: pointer;
|
||||
transition: all 0.2s var(--spring);
|
||||
font-family: inherit;
|
||||
}
|
||||
.btn:hover:not(:disabled) { background: rgba(255,255,255,0.15); transform: translateY(-1px); }
|
||||
.btn:active:not(:disabled) { transform: translateY(1px); }
|
||||
.btn:disabled { opacity: 0.5; cursor: not-allowed; }
|
||||
.btn-danger { background: rgba(255,69,58,0.8); border-color: transparent; color: white; }
|
||||
.btn-remote { background: rgba(48,209,88,0.2); border-color: rgba(48,209,88,0.4); color: #8deda7; }
|
||||
.btn-web { background: rgba(10,132,255,0.2); border-color: rgba(10,132,255,0.4); color: #82c4ff; }
|
||||
.btn-nav { background: rgba(255,214,10,0.2); border-color: rgba(255,214,10,0.4); color: #ffe680; }
|
||||
.btn.active-mode { box-shadow: 0 0 0 2px white; }
|
||||
|
||||
.label { font-size: 11px; color: var(--text-tertiary); }
|
||||
|
||||
/* Side panels */
|
||||
.side-panel {
|
||||
position: fixed;
|
||||
top: 80px;
|
||||
bottom: 20px;
|
||||
width: 340px;
|
||||
border-radius: var(--panel-radius);
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
overflow: hidden;
|
||||
}
|
||||
.left-panel { left: 2%; }
|
||||
.right-panel { right: 2%; }
|
||||
.flex-1 { flex: 1; min-height: 0; overflow: hidden; display: flex; flex-direction: column; }
|
||||
|
||||
.panel-section {
|
||||
padding: 14px 16px;
|
||||
border-bottom: 0.5px solid var(--glass-border);
|
||||
}
|
||||
.panel-section:last-child { border-bottom: none; }
|
||||
|
||||
.panel-title {
|
||||
font-size: 11px;
|
||||
font-weight: 700;
|
||||
color: var(--text-tertiary);
|
||||
text-transform: uppercase;
|
||||
letter-spacing: 0.5px;
|
||||
margin-bottom: 10px;
|
||||
}
|
||||
|
||||
/* Diag rows */
|
||||
.diag-row {
|
||||
display: flex;
|
||||
justify-content: space-between;
|
||||
align-items: center;
|
||||
padding: 4px 6px;
|
||||
background: rgba(0,0,0,0.2);
|
||||
border-radius: 4px;
|
||||
margin-bottom: 3px;
|
||||
}
|
||||
.diag-label { font-size: 11px; color: var(--text-tertiary); }
|
||||
.diag-value { font-size: 11px; font-family: monospace; color: var(--text-primary); font-weight: 600; }
|
||||
.diag-value.ok { color: var(--success); }
|
||||
.diag-value.warn { color: var(--warning); }
|
||||
.diag-value.bad { color: var(--danger); }
|
||||
.diag-value.active { color: #82c4ff; }
|
||||
|
||||
/* Joints grid */
|
||||
.motors-grid-list {
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
gap: 2px;
|
||||
overflow-y: auto;
|
||||
flex: 1;
|
||||
}
|
||||
.motor-row {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
gap: 4px;
|
||||
padding: 3px 6px;
|
||||
background: rgba(0,0,0,0.25);
|
||||
border-radius: 5px;
|
||||
}
|
||||
.motor-row .name { font-size: 10px; color: var(--text-secondary); width: 72px; font-family: monospace; flex-shrink: 0; }
|
||||
.motor-row .val { font-size: 10px; font-family: monospace; text-align: right; flex: 1; }
|
||||
.motor-row .val.pos { color: #0a84ff; }
|
||||
.motor-row .val.vel { color: #30d158; }
|
||||
.motor-row .val.tau { color: #ff9f0a; }
|
||||
.motor-row .stale { font-size: 9px; width: 8px; flex-shrink: 0; }
|
||||
|
||||
/* Joystick */
|
||||
.joystick-area { display: flex; flex-direction: column; align-items: center; gap: 8px; margin-bottom: 12px; }
|
||||
.joystick {
|
||||
position: relative;
|
||||
width: 180px;
|
||||
height: 180px;
|
||||
border-radius: 50%;
|
||||
background: radial-gradient(circle, rgba(10,132,255,0.2), rgba(10,132,255,0.05));
|
||||
border: 1px solid rgba(10,132,255,0.3);
|
||||
touch-action: none;
|
||||
flex-shrink: 0;
|
||||
}
|
||||
#stick {
|
||||
position: absolute;
|
||||
left: 50%; top: 50%;
|
||||
width: 56px; height: 56px;
|
||||
border-radius: 50%;
|
||||
background: linear-gradient(135deg, #30d158, #0a84ff);
|
||||
transform: translate(-50%, -50%);
|
||||
box-shadow: 0 8px 24px rgba(0,0,0,0.4);
|
||||
}
|
||||
|
||||
/* Sliders */
|
||||
.slider-group { display: flex; flex-direction: column; gap: 8px; }
|
||||
.slider-row { display: flex; align-items: center; gap: 8px; }
|
||||
.slider-label { font-size: 11px; color: var(--text-tertiary); width: 28px; font-family: monospace; }
|
||||
.slider-val { font-size: 11px; color: var(--accent); font-family: monospace; width: 38px; text-align: right; }
|
||||
.glass-slider {
|
||||
flex: 1;
|
||||
-webkit-appearance: none;
|
||||
height: 4px;
|
||||
border-radius: 2px;
|
||||
background: rgba(255,255,255,0.2);
|
||||
outline: none;
|
||||
}
|
||||
.glass-slider::-webkit-slider-thumb {
|
||||
-webkit-appearance: none;
|
||||
width: 14px; height: 14px;
|
||||
border-radius: 50%;
|
||||
background: white;
|
||||
cursor: pointer;
|
||||
box-shadow: 0 2px 4px rgba(0,0,0,0.5);
|
||||
}
|
||||
.cmd-display {
|
||||
margin-top: 8px;
|
||||
padding: 6px 10px;
|
||||
background: rgba(0,0,0,0.3);
|
||||
border-radius: 6px;
|
||||
font-family: monospace;
|
||||
font-size: 12px;
|
||||
color: var(--accent);
|
||||
text-align: center;
|
||||
}
|
||||
.hint { font-size: 11px; color: var(--text-tertiary); text-align: center; }
|
||||
|
||||
/* Log */
|
||||
.log-section { flex: 1; overflow: hidden; display: flex; flex-direction: column; }
|
||||
.log {
|
||||
flex: 1;
|
||||
background: rgba(0,0,0,0.4);
|
||||
border: 1px solid rgba(255,255,255,0.05);
|
||||
border-radius: 6px;
|
||||
padding: 8px;
|
||||
font-family: monospace;
|
||||
font-size: 11px;
|
||||
color: var(--text-secondary);
|
||||
overflow-y: auto;
|
||||
}
|
||||
.log div { margin-bottom: 2px; line-height: 1.4; }
|
||||
.ev-t { color: var(--text-tertiary); margin-right: 4px; }
|
||||
.ev-ok { color: var(--success); }
|
||||
.ev-warn { color: var(--warning); }
|
||||
.ev-bad { color: var(--danger); }
|
||||
|
||||
@media (max-width: 960px) {
|
||||
.side-panel { position: relative; top: auto; width: 100%; left: 0; right: 0; border-radius: 0; height: auto; }
|
||||
.top-bar { width: 100%; border-radius: 0; top: 0; }
|
||||
body { padding-top: 56px; }
|
||||
}
|
||||
@@ -0,0 +1,168 @@
|
||||
# 真机部署与验证指南
|
||||
|
||||
本文档面向当前目录 `shiji/sim2real (10)/sim2real`，用于减少上机排错时间。默认不修改已验证的电机映射、方向、零位和策略观测。
|
||||
|
||||
## 1. 前期环境
|
||||
|
||||
推荐在 Orin / Linux 上运行：
|
||||
|
||||
```bash
|
||||
cd sim2real
|
||||
python -m pip install -r requirements-orin.txt
|
||||
```
|
||||
|
||||
确认设备：
|
||||
|
||||
```bash
|
||||
ip link show can0
|
||||
ip link show can1
|
||||
ls -l /dev/ttyACM0
|
||||
```
|
||||
|
||||
确认 `config.yaml`：
|
||||
|
||||
- `can1_port` 和 `can2_port` 对应实际 SocketCAN 设备。
|
||||
- `control_freq` 默认保持 `50`。
|
||||
- `remote.port` 默认 `/dev/ttyACM0`。
|
||||
- `policy.action_scale` 必须是 16 维。
|
||||
- 默认策略文件优先使用 `policies/model_rough.onnx`；如果只有 `.pt`，先用 `tools/export_onnx.py` 导出。
|
||||
- `controller.kp_leg/kd_leg/kd_wheel` 不要在未记录实验的情况下大改。
|
||||
|
||||
## 2. 无硬件/低风险检查
|
||||
|
||||
先做文件和模型契约检查：
|
||||
|
||||
```bash
|
||||
python tools/export_onnx.py --pt policies/model_rough.pt --onnx policies/model_rough.onnx
|
||||
python tools/alignment_check.py --policy policies/model_rough.onnx --manifest deployment_manifest.yaml
|
||||
python tools/standalone_check.py
|
||||
python -m py_compile tools/logger.py interface/motor_driver.py interface/imu_client.py interface/real_io.py web/session.py
|
||||
```
|
||||
|
||||
`alignment_check.py` 会同时检查：
|
||||
|
||||
- 策略 obs/action 维度。
|
||||
- action scale。
|
||||
- default pose。
|
||||
- joint order。
|
||||
- wheel indices。
|
||||
- `config.yaml` 与 `deployment_manifest.yaml` 的控制频率和 command filter 是否一致。
|
||||
- ONNX 策略的 obs/action 维度是否仍为 `53D/16D`。
|
||||
|
||||
如果有 Node 环境，可检查前端语法：
|
||||
|
||||
```bash
|
||||
node --check web/static/app.js
|
||||
```
|
||||
|
||||
## 3. 上电前检查
|
||||
|
||||
上电前确认：
|
||||
|
||||
- 机器人架空或有可靠支撑。
|
||||
- 16 个电机 CAN 线和电源线固定。
|
||||
- Odin1 连接稳定，启动时机器人尽量静止，利于重力对齐。
|
||||
- Web 急停可见，遥控软急停通道可用。
|
||||
- CAN 设备名和遥控串口名与 `config.yaml` 一致。
|
||||
|
||||
## 4. Web 启动
|
||||
|
||||
```bash
|
||||
python web/server.py --host 0.0.0.0 --port 8080
|
||||
```
|
||||
|
||||
浏览器打开：
|
||||
|
||||
```text
|
||||
http://<orin-ip>:8080
|
||||
```
|
||||
|
||||
推荐先只看状态，不急着释放遥控。
|
||||
|
||||
## 5. 标准上机流程
|
||||
|
||||
1. 点击 `Connect`，确认 IMU/Odin 和 CAN 初始化正常。
|
||||
2. 点击 `Enable Motors`，确认 16 个电机都有反馈。
|
||||
3. 点击 `Startup`，从当前实测姿态过渡到站立。
|
||||
4. 进入 `STAND_HOLD` 后观察 IMU age、Motor Fresh、Loop Profile。
|
||||
5. 点击 `Start Runtime`，进入 50Hz 策略循环。
|
||||
6. 策略 release 后再点击遥控接管。
|
||||
7. 小幅给命令，先测试前后、转向，再测试组合动作。
|
||||
|
||||
## 6. 本轮新增诊断如何看
|
||||
|
||||
`Loop Profile`：
|
||||
|
||||
- `read_state_ms` 高：优先查电机接收、CAN 队列、Odin 获取是否阻塞。
|
||||
- `policy_ms` 高：优先查策略推理和 CPU 负载。
|
||||
- `send_actions_ms` 高：优先查 CAN 发送和 USB-CAN 适配器。
|
||||
- `log_ms` 高：说明日志队列或磁盘仍可能有压力。
|
||||
|
||||
`Motor Fresh`：
|
||||
|
||||
- 理想状态是 `16/16 (cnt 16, val 0)` 或接近。
|
||||
- `cnt` 高说明 `update_count` 正常增长，这是最可靠的电机反馈证据。
|
||||
- 如果机器人静止时 `val` 为 0 是正常现象，不应据此判断丢电机。
|
||||
|
||||
`Odin Odom`：
|
||||
|
||||
- 显示 `STANDARD/HIGHFREQ/TF`、age、local x/y/yaw。
|
||||
- 当前只用于诊断和全局坐标显示，不参与策略输入。
|
||||
- odom 不可用时，当前 locomotion 仍应可以运行。
|
||||
- 如果显示 `JUMP`，说明 odom 局部位置或 yaw 出现突变，先不要把它用于闭环导航。
|
||||
|
||||
`Latest Target`：
|
||||
|
||||
- 显示当前电机目标来源，例如 `runtime_policy`、`runtime_zero_hold`、`runtime_release_hold`。
|
||||
- `age` 应随 runtime 正常刷新；如果明显超过控制周期很多，说明目标更新链路卡住。
|
||||
- `d` 是相邻目标最大变化量，可用于观察停车/起步是否有目标突变。
|
||||
|
||||
`Obs / Action`：
|
||||
|
||||
- `obs` 接近 `100` 时，说明观测可能接近 clip 边界。
|
||||
- `raw` 接近 `10` 时，说明策略输出可能接近 raw action 裁剪边界。
|
||||
- `scaled` 长期很大时，检查 action scale、目标限幅和 safety clip。
|
||||
|
||||
`cmd/raw cmd`：
|
||||
|
||||
- `raw cmd` 是 Web/遥控原始输入。
|
||||
- `cmd` 是经过 `command_filter` 限加速度后的策略命令。
|
||||
- 如果机器人响应慢，先看两者差值是否由命令滤波造成。
|
||||
|
||||
## 7. 如果出现前后晃动
|
||||
|
||||
先不要直接改控制频率。按顺序排查：
|
||||
|
||||
1. 看 `Loop Overruns` 是否增长。
|
||||
2. 看 `Loop Profile` 最慢阶段。
|
||||
3. 看 `imu_age` 是否超过 30-60ms。
|
||||
4. 看 `Motor Fresh` 是否掉到 16 以下。
|
||||
5. 看停止时 `cmd` 是否真的回到 0。
|
||||
6. 看 `runtime_released`、`release_alpha` 和 `track_err` 是否异常。
|
||||
|
||||
只有确认 50Hz 长期跑不稳时，才把 `control_freq: 40` 作为诊断实验，而不是默认方案。训练/部署频率不一致可能引入新的 sim2real gap。
|
||||
|
||||
## 8. 日志
|
||||
|
||||
每次运行会生成：
|
||||
|
||||
- `state.csv`：高频状态流，后台线程写入。
|
||||
- `events.jsonl`：事件流，关键事件会即时 flush。
|
||||
|
||||
重点搜索：
|
||||
|
||||
```bash
|
||||
grep LOOP_OVERRUN web/logs/*/events.jsonl
|
||||
grep SAFETY web/logs/*/events.jsonl
|
||||
grep GUARD web/logs/*/events.jsonl
|
||||
```
|
||||
|
||||
## 9. 当前不建议改动的内容
|
||||
|
||||
- 16 个电机映射、方向、零位。
|
||||
- 策略 53D 观测顺序和缩放。
|
||||
- 16D 动作顺序和 action scale。
|
||||
- 默认 `50Hz` 控制频率。
|
||||
- 已经真机跑通过的遥控方向配置。
|
||||
|
||||
这些内容只有在有新日志和明确现象时再改，避免把已验证链路打散。
|
||||
@@ -0,0 +1,49 @@
|
||||
# `FACTS_AND_ASSUMPTIONS`
|
||||
|
||||
## 已确认
|
||||
|
||||
- 当前部署模型：默认优先使用 `sim2real/policies/model_rough.onnx`
|
||||
- 当前源模型/fallback：`sim2real/policies/model_rough.pt`
|
||||
- 源模型：`model_2000.pt`
|
||||
- actor 输入：`53D`
|
||||
- actor 输出：`16D`
|
||||
- 当前 actor 不吃 `base_lin_vel`
|
||||
- 当前 actor 不吃 `height_scan`
|
||||
|
||||
## 当前观测顺序
|
||||
|
||||
1. `base_ang_vel * 0.25`
|
||||
2. `projected_gravity`
|
||||
3. `command`
|
||||
4. `joint_pos_rel`（12）
|
||||
5. `joint_vel_rel * 0.05`（12）
|
||||
6. `wheel_vel * 0.05`（4）
|
||||
7. `last_actions`（16）
|
||||
|
||||
## 当前控制定义
|
||||
|
||||
- 控制频率：`50Hz`
|
||||
- 腿缩放：`0.125 / 0.25`
|
||||
- 轮缩放：`5.0`
|
||||
- 腿 LPF：`5Hz`
|
||||
- 轮 LPF：`15Hz`
|
||||
|
||||
## 当前仍依赖现场一致的部分
|
||||
|
||||
- IMU 安装方向与上一版校正一致
|
||||
- 当前 MJCF / 电机参数对应这次重新训练后的模型
|
||||
- 电机零位、方向、接线已按当前硬件修正
|
||||
|
||||
## 本次实现边界
|
||||
|
||||
不再支持：
|
||||
|
||||
- `crawl` 模型
|
||||
- 多策略切换
|
||||
- `318D` 历史输入
|
||||
- 旧版 `startup.start_pose`
|
||||
|
||||
## 本次排查结论
|
||||
|
||||
代码应只围绕当前 rough 模型运行。
|
||||
如果后续模型结构再改，必须重新核对观测、动作缩放、控制频率和部署文档。
|
||||
@@ -0,0 +1,45 @@
|
||||
# `Orin Nano` 部署说明
|
||||
|
||||
## 是否必须转 ONNX
|
||||
|
||||
不必须。
|
||||
|
||||
当前优先级仍然是：
|
||||
|
||||
1. 先保证观测、动作、站立控制对齐
|
||||
2. 再测 `50Hz` 实际环路稳定性
|
||||
3. 最后才决定是否转 `ONNX/TensorRT`
|
||||
|
||||
## 当前代码重点
|
||||
|
||||
- `stand_balance` 已加入 `main.py` 和 `web/session.py`
|
||||
- 启动后先站稳，再允许策略接管
|
||||
- `PolicyRunner.step()` 仍保留零命令抑制开关，默认开启
|
||||
|
||||
## Orin 上先测什么
|
||||
|
||||
- 机器人能否在不启动策略时，仅靠 `startup + stand_balance` 稳定站住
|
||||
- `loop_dt_ms`
|
||||
- `imu_age_ms`
|
||||
- 电机 stale
|
||||
- policy forward 耗时
|
||||
|
||||
## 纯 Python 部署命令
|
||||
|
||||
默认前提：当前目录就是 `sim2real/`
|
||||
|
||||
```bash
|
||||
python3 -m pip install -r requirements-orin.txt
|
||||
python3 tools/alignment_check.py --policy policies/model_rough.pt --manifest deployment_manifest.yaml
|
||||
python3 tools/standalone_check.py
|
||||
python3 main.py --dry-run
|
||||
python3 main.py
|
||||
python3 web/server.py --host 0.0.0.0 --port 8080
|
||||
```
|
||||
|
||||
## 首轮实机建议
|
||||
|
||||
1. 先不启动策略
|
||||
2. 只验证 `startup -> stand_balance`
|
||||
3. 站稳后再启动策略
|
||||
4. 只给很小的 `vx / vy / yaw`
|
||||
@@ -0,0 +1,99 @@
|
||||
# Python Sim2Real v2
|
||||
|
||||
本目录归档 `real/sim2real_v2` 版本，对应 Python 真机部署从第一代闭环继续演进后的版本节点。该阶段保持 `53D -> 16D` 策略契约，重点完善电机反馈判断、IMU/Odin 可观测性、Web 调试和运行时诊断；后续 ROS 2/C++ 版本另行归档。
|
||||
|
||||
## 当前控制链路
|
||||
|
||||
- 策略模型：默认优先使用 `policies/model_rough.onnx`，`.pt` 仅作为导出源和 fallback
|
||||
- 策略输入：单帧 `53D`
|
||||
- 策略输出：`16D`
|
||||
- 控制频率：默认 `50Hz`
|
||||
- 电机数量：16 个 RS02/RobStride 电机
|
||||
- 电机映射、方向、零位：以 `interface/motor_mapping.py` 为准，当前视为真机验证过，不在本轮改造中修改
|
||||
- Web 入口：`python web/server.py --host 0.0.0.0 --port 8080`
|
||||
|
||||
策略观测顺序保持不变：
|
||||
|
||||
1. `base_ang_vel * 0.25`
|
||||
2. `projected_gravity`
|
||||
3. `command`
|
||||
4. `joint_pos[:12] - default_dof_pos[:12]`
|
||||
5. `joint_vel[:12] * 0.05`
|
||||
6. `wheel_vel[12:] * 0.05`
|
||||
7. `last_actions`
|
||||
|
||||
Odin odom 现在只作为诊断和全局状态显示接入，不进入策略观测，避免破坏已训练模型的输入契约。
|
||||
|
||||
## 本轮改造内容
|
||||
|
||||
- 日志写入改为后台线程，降低 `state.csv` 高频写入对 50Hz 控制循环的阻塞。
|
||||
- 电机 fresh/stale 判断优先使用驱动层 `update_count`，避免静止电机反馈正常但位置速度不变时被误判为丢电机。
|
||||
- `wait_feedback_ready()` 同步识别 `update_count`，降低使能后零位静止电机被误判 missing 的概率。
|
||||
- `IMUClient` 增加 `get_latest_odom()`，读取 Odin odom 的位置、姿态、线速度、角速度、类型和 age。
|
||||
- `RealIO.read_state()` 增加只读 `odom` 字段，但不改变策略观测。
|
||||
- 增加 `OdomTracker`，把 Odin raw odom 转成启动点局部坐标 `local x/y/yaw`，并做跳变检测。
|
||||
- 增加 `LatestTarget` 诊断，记录当前下发目标来源、age、目标变化量，为后续双 loop 解耦铺路。
|
||||
- Web runtime 状态增加 `loop_profile`、odom、电机 fresh 计数、每电机 update_count。
|
||||
- Web Diagnostics 面板增加 loop overrun、最慢阶段、电机 fresh 来源、Odin odom 状态。
|
||||
- 增加 `command_filter`，对 Web/遥控命令统一做限加速度平滑，降低起停冲击；Web 同时显示 raw cmd 和 filtered cmd。
|
||||
- 策略推理入口改为 ONNXRuntime 优先，默认查找 `policies/model_rough.onnx`；不存在时兼容回退到 `policies/model_rough.pt`。
|
||||
- ONNXRuntime 使用 CPU 单线程顺序执行，减少推理线程池和 motor/status/logger 线程抢占。
|
||||
|
||||
## 重点诊断字段
|
||||
|
||||
Web 中重点看这些项：
|
||||
|
||||
- `loop_dt`：控制循环总耗时，50Hz 下目标约 `20ms`。
|
||||
- `Loop Overruns`：runtime 超时累计次数和最大超时。
|
||||
- `Loop Profile`：显示本轮最慢阶段，例如 `read_state_ms`、`policy_ms`、`send_actions_ms`。
|
||||
- `Latest Target`：显示目标来源、age 和本次目标最大变化量；age 异常增大说明目标更新链路卡住。
|
||||
- `Obs / Action`：显示观测绝对值最大值、raw action 最大值和 scaled action 最大值，用来发现输入爆炸或动作饱和。
|
||||
- `Motor Fresh`：格式为 `fresh/16 (cnt x, val y)`；`cnt` 表示通过 `update_count` 确认的新反馈数量。
|
||||
- `Odin Odom`：显示 odom 类型、age、local x/y/yaw；没有 odom 时不影响控制。
|
||||
- `cmd/raw cmd`：`cmd` 是进入策略的滤波后命令，`raw cmd` 是 Web/遥控原始命令。
|
||||
- 电机列表状态点：绿色代表 stale count 为 0，黄色代表短时未刷新，红色代表连续 stale。
|
||||
|
||||
## 当前仍需实机重点确认
|
||||
|
||||
- 如果继续出现 `LOOP_OVERRUN`，先看 Web 的 `Loop Profile`，不要直接调低控制频率。
|
||||
- 如果 `read_state_ms` 慢，重点排查 SocketCAN/CAN 队列、Odin bridge 或电机反馈处理。
|
||||
- 如果 `policy_ms` 慢，重点排查 ONNXRuntime 推理耗时、CPU 占用和是否有后台进程抢占。
|
||||
- 如果 `send_actions_ms` 慢，重点排查 CAN 发送阻塞或 USB-CAN 适配器。
|
||||
- 如果 `Obs / Action` 中 obs 接近 `100` 或 raw action 接近 `10`，说明策略输入/输出可能在饱和边界，需要优先检查观测缩放、IMU、关节速度和命令。
|
||||
- 如果 `Motor Fresh` 不是 16/16，但 `update_counts` 在增长，需要检查 Web stale 阈值而不是电机丢失。
|
||||
- 如果 odom age 长时间不更新，只影响全局坐标/诊断，不应影响当前策略控制。
|
||||
|
||||
## 启动命令
|
||||
|
||||
在 Orin / Linux 真机上：
|
||||
|
||||
```bash
|
||||
python -m pip install -r requirements-orin.txt
|
||||
python tools/export_onnx.py --pt policies/model_rough.pt --onnx policies/model_rough.onnx
|
||||
python tools/alignment_check.py --policy policies/model_rough.onnx --manifest deployment_manifest.yaml
|
||||
python tools/standalone_check.py
|
||||
python web/server.py --host 0.0.0.0 --port 8080
|
||||
```
|
||||
|
||||
Web 流程：
|
||||
|
||||
1. Connect
|
||||
2. Enable Motors
|
||||
3. Startup
|
||||
4. Start Runtime
|
||||
5. 策略 release 后，如需要，点击遥控接管
|
||||
6. 实时观察 Diagnostics、Motors、Plots
|
||||
|
||||
## 安全边界
|
||||
|
||||
当前保留原有保护链路：
|
||||
|
||||
- Web 急停
|
||||
- 遥控软急停
|
||||
- runtime guard
|
||||
- safety monitor
|
||||
- NaN/Inf 检查
|
||||
- 电机 stale holdover
|
||||
- damping brake
|
||||
|
||||
这轮没有放宽安全边界，也没有调整电机限位、零位、方向、默认增益和策略动作缩放。
|
||||
@@ -0,0 +1,113 @@
|
||||
can1_port: "can0"
|
||||
can2_port: "can1"
|
||||
motor_model: "rs-02"
|
||||
debug: false
|
||||
|
||||
control_freq: 50
|
||||
policy_freq: 50
|
||||
motor_freq: 200
|
||||
status_freq: 10
|
||||
policy_stale_warn_ms: 60.0
|
||||
policy_timeout_ms: 150.0
|
||||
imu_lib_path: null
|
||||
|
||||
controller:
|
||||
# Runtime policy gains should stay softer than stand/startup gains. This
|
||||
# follows the same separation used by rl_sar fixed_kp/fixed_kd vs rl_kp/rl_kd
|
||||
# and avoids high-stiffness zero-command oscillation after policy release.
|
||||
kp_leg: 50.0
|
||||
kd_leg: 1.5
|
||||
hold_kp_leg: 80.0
|
||||
hold_kd_leg: 4.0
|
||||
kd_wheel: 1.0
|
||||
max_vx: 0.8
|
||||
max_vy: 0.3
|
||||
max_yaw_rate: 0.5
|
||||
|
||||
policy:
|
||||
enable_zero_cmd_suppression: true
|
||||
hold_zero_command_pose: true
|
||||
command_release_s: 0.35
|
||||
require_active_command_to_release: true
|
||||
zero_cmd_use_yaw_rate: false
|
||||
clip_obs: 100.0
|
||||
action_scale: [0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 5.0, 5.0, 5.0, 5.0]
|
||||
release_command_hold_s: 0.12
|
||||
release_posture_max_err: 0.35
|
||||
release_target_blend_s: 0.30
|
||||
|
||||
stand_balance:
|
||||
enabled: true
|
||||
height: 0.33
|
||||
kp_roll: 0.85
|
||||
pitch_compensation_enabled: false
|
||||
kp_pitch: 0.25
|
||||
kd_roll_rate: 0.03
|
||||
kd_pitch_rate: 0.03
|
||||
pitch_deadband_deg: 2.0
|
||||
pitch_corr_clip: 0.04
|
||||
pitch_corr_filter_alpha: 0.10
|
||||
pitch_front_sign: -1.0
|
||||
lateral_lean_gain: 0.0
|
||||
hip_abduction_clip: 0.45
|
||||
hip_pitch_clip: [-1.0, 2.5]
|
||||
knee_clip: [-2.6, -0.3]
|
||||
stable_roll_deg: 6.0
|
||||
stable_pitch_deg: 8.0
|
||||
stable_gyro_deg_s: 45.0
|
||||
enter_hold_s: 1.0
|
||||
profile_h: [0.157, 0.248, 0.311, 0.366, 0.411, 0.448]
|
||||
profile_hip: [1.5, 1.2, 1.0, 0.8, 0.6, 0.4]
|
||||
profile_knee: [-2.5, -2.1, -1.8, -1.5, -1.2, -0.9]
|
||||
|
||||
startup:
|
||||
enabled: true
|
||||
wait_for_enter_before_rise: false
|
||||
soft_hold_duration: 1.0
|
||||
ramp_kp_time: 1.0
|
||||
transition_time_min: 2.0
|
||||
transition_time_max: 6.0
|
||||
transition_seconds_per_rad: 1.5
|
||||
timeout_extra: 3.0
|
||||
imu_fresh_wait_s: 1.0
|
||||
hold_time: 1.0
|
||||
settle_pos_threshold: 0.30
|
||||
settle_vel_threshold: 0.6
|
||||
progress_log_interval: 0.5
|
||||
max_dev_warn: 1.5
|
||||
max_dev_abort: 3.0
|
||||
require_user_confirm: true
|
||||
|
||||
safety:
|
||||
enabled: true
|
||||
max_target_offset: 0.6
|
||||
hard_target_offset: 1.2
|
||||
max_ang_vel: 10.0
|
||||
max_tilt_z: -0.3
|
||||
clip_to_brake: 0
|
||||
imu_age_warn_ms: 60.0
|
||||
imu_age_stop_ms: 200.0
|
||||
|
||||
remote:
|
||||
enabled: true
|
||||
port: "/dev/ttyACM0"
|
||||
baudrate: 100000
|
||||
timeout: 0.02
|
||||
axis_deadzone: 50
|
||||
active_threshold: 50
|
||||
axis_full_scale: 660.0
|
||||
max_vx: 0.3
|
||||
max_vy: 0.3
|
||||
max_yaw_rate: 0.3
|
||||
invert_vx: true
|
||||
invert_vy: false
|
||||
invert_yaw: true
|
||||
|
||||
command_filter:
|
||||
enabled: true
|
||||
max_vx_acc: 1.0
|
||||
max_vy_acc: 1.0
|
||||
max_yaw_acc: 1.5
|
||||
|
||||
log_dir: "logs"
|
||||
log_every: 1
|
||||
@@ -0,0 +1,102 @@
|
||||
model:
|
||||
path: "policies/model_rough.onnx"
|
||||
source_pt: "policies/model_rough.pt"
|
||||
backend: "onnxruntime"
|
||||
obs_dim: 53
|
||||
action_dim: 16
|
||||
clip_obs: 100.0
|
||||
enable_zero_cmd_suppression: true
|
||||
|
||||
observation:
|
||||
terms:
|
||||
- name: base_ang_vel
|
||||
dim: 3
|
||||
scale: 0.25
|
||||
- name: projected_gravity
|
||||
dim: 3
|
||||
- name: command
|
||||
dim: 3
|
||||
- name: joint_pos_rel
|
||||
dim: 12
|
||||
- name: joint_vel_rel
|
||||
dim: 12
|
||||
scale: 0.05
|
||||
- name: wheel_vel
|
||||
dim: 4
|
||||
scale: 0.05
|
||||
- name: last_actions
|
||||
dim: 16
|
||||
|
||||
action:
|
||||
joint_order:
|
||||
- fl_hip_abduction
|
||||
- fl_hip_pitch
|
||||
- fl_knee
|
||||
- fr_hip_abduction
|
||||
- fr_hip_pitch
|
||||
- fr_knee
|
||||
- rl_hip_abduction
|
||||
- rl_hip_pitch
|
||||
- rl_knee
|
||||
- rr_hip_abduction
|
||||
- rr_hip_pitch
|
||||
- rr_knee
|
||||
- fl_wheel
|
||||
- fr_wheel
|
||||
- rl_wheel
|
||||
- rr_wheel
|
||||
wheel_indices: [12, 13, 14, 15]
|
||||
scale:
|
||||
- 0.125
|
||||
- 0.25
|
||||
- 0.25
|
||||
- 0.125
|
||||
- 0.25
|
||||
- 0.25
|
||||
- 0.125
|
||||
- 0.25
|
||||
- 0.25
|
||||
- 0.125
|
||||
- 0.25
|
||||
- 0.25
|
||||
- 5.0
|
||||
- 5.0
|
||||
- 5.0
|
||||
- 5.0
|
||||
default_dof_pos:
|
||||
- 0.0
|
||||
- 0.9
|
||||
- -1.8
|
||||
- 0.0
|
||||
- 0.9
|
||||
- -1.8
|
||||
- 0.0
|
||||
- 0.9
|
||||
- -1.8
|
||||
- 0.0
|
||||
- 0.9
|
||||
- -1.8
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
|
||||
control:
|
||||
control_freq_hz: 50
|
||||
runtime_kp_leg: 50.0
|
||||
runtime_kd_leg: 1.5
|
||||
hold_kp_leg: 80.0
|
||||
hold_kd_leg: 4.0
|
||||
kd_wheel: 1.0
|
||||
leg_lpf_hz: 5
|
||||
wheel_lpf_hz: 15
|
||||
command_filter:
|
||||
enabled: true
|
||||
max_vx_acc: 1.0
|
||||
max_vy_acc: 1.0
|
||||
max_yaw_acc: 1.5
|
||||
|
||||
safety:
|
||||
zero_cmd_lin_thresh: 0.05
|
||||
zero_cmd_yaw_thresh: 0.05
|
||||
zero_yaw_rate_thresh: 0.10
|
||||
@@ -0,0 +1,89 @@
|
||||
"""键盘控制器 — 兼容 sim2sim/input_dev/keyboard.py 的接口与平滑参数。"""
|
||||
import numpy as np
|
||||
|
||||
try:
|
||||
from pynput import keyboard
|
||||
PYNPUT_AVAILABLE = True
|
||||
except ImportError:
|
||||
PYNPUT_AVAILABLE = False
|
||||
keyboard = None # type: ignore
|
||||
|
||||
|
||||
class KeyboardCommandController:
|
||||
"""方向键 + AD 键的键盘指令源。
|
||||
指令: [vx, vy, yaw_rate]，平滑加减速；空格触发急停标志。
|
||||
"""
|
||||
|
||||
def __init__(self,
|
||||
max_x_vel: float = 0.8,
|
||||
max_y_vel: float = 0.3,
|
||||
max_yaw_vel: float = 0.5,
|
||||
acc_step: float = 0.05,
|
||||
dec_step: float = 0.1):
|
||||
if not PYNPUT_AVAILABLE:
|
||||
raise RuntimeError("pynput 不可用，无法使用键盘控制；改用其他输入源。")
|
||||
|
||||
self.current_cmd = np.zeros(3, dtype=np.float32)
|
||||
self.max_x_vel = max_x_vel
|
||||
self.max_y_vel = max_y_vel
|
||||
self.max_yaw_vel = max_yaw_vel
|
||||
self.acc_step = acc_step
|
||||
self.dec_step = dec_step
|
||||
|
||||
self._pressed = set()
|
||||
self._estop = False
|
||||
self.listener = keyboard.Listener(
|
||||
on_press=self._on_press, on_release=self._on_release
|
||||
)
|
||||
|
||||
def start(self):
|
||||
self.listener.start()
|
||||
print("[Keyboard] 启动。↑↓ 前后, ←→ 转向, A/D 横移, SPACE 急停")
|
||||
|
||||
def stop(self):
|
||||
try:
|
||||
self.listener.stop()
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
def _on_press(self, key):
|
||||
self._pressed.add(key)
|
||||
if key == keyboard.Key.space:
|
||||
self._estop = True
|
||||
|
||||
def _on_release(self, key):
|
||||
self._pressed.discard(key)
|
||||
|
||||
def is_estop_triggered(self) -> bool:
|
||||
return self._estop
|
||||
|
||||
def reset_estop(self):
|
||||
self._estop = False
|
||||
|
||||
def get_command(self) -> np.ndarray:
|
||||
target = np.zeros(3, dtype=np.float32)
|
||||
if keyboard.Key.up in self._pressed:
|
||||
target[0] += self.max_x_vel
|
||||
if keyboard.Key.down in self._pressed:
|
||||
target[0] -= self.max_x_vel
|
||||
if keyboard.Key.left in self._pressed:
|
||||
target[2] += self.max_yaw_vel
|
||||
if keyboard.Key.right in self._pressed:
|
||||
target[2] -= self.max_yaw_vel
|
||||
try:
|
||||
if keyboard.KeyCode.from_char('a') in self._pressed:
|
||||
target[1] += self.max_y_vel
|
||||
if keyboard.KeyCode.from_char('d') in self._pressed:
|
||||
target[1] -= self.max_y_vel
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
for i, max_v in enumerate((self.max_x_vel, self.max_y_vel, self.max_yaw_vel)):
|
||||
step = self.acc_step if target[i] != 0 else self.dec_step
|
||||
if i == 2:
|
||||
step *= 2.0
|
||||
if self.current_cmd[i] < target[i]:
|
||||
self.current_cmd[i] = min(self.current_cmd[i] + step, target[i])
|
||||
else:
|
||||
self.current_cmd[i] = max(self.current_cmd[i] - step, target[i])
|
||||
return self.current_cmd.copy()
|
||||
@@ -0,0 +1,256 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass, field
|
||||
from typing import Optional
|
||||
|
||||
import numpy as np
|
||||
import serial
|
||||
|
||||
SBUS_FRAME_SIZE = 25
|
||||
SBUS_RC_MID = 1024
|
||||
SBUS_AXIS_SCALE = 660.0
|
||||
|
||||
SWITCH_LOW = -1
|
||||
SWITCH_MID = 0
|
||||
SWITCH_HIGH = 1
|
||||
|
||||
|
||||
@dataclass
|
||||
class RemoteSwitchState:
|
||||
ch7: int = SWITCH_MID
|
||||
|
||||
|
||||
@dataclass
|
||||
class RemoteControlState:
|
||||
ch1: int = 0
|
||||
ch2: int = 0
|
||||
ch3: int = 0
|
||||
ch4: int = 0
|
||||
switches: RemoteSwitchState = field(default_factory=RemoteSwitchState)
|
||||
frame_ok: bool = False
|
||||
|
||||
def active_axes(self, threshold: int = 50) -> dict[str, bool]:
|
||||
return {
|
||||
"ch1": abs(self.ch1) > threshold,
|
||||
"ch2": abs(self.ch2) > threshold,
|
||||
"ch3": abs(self.ch3) > threshold,
|
||||
"ch4": abs(self.ch4) > threshold,
|
||||
}
|
||||
|
||||
@property
|
||||
def estop_requested(self) -> bool:
|
||||
return self.switches.ch7 == SWITCH_HIGH
|
||||
|
||||
def as_dict(self) -> dict:
|
||||
return {
|
||||
"ch1": int(self.ch1),
|
||||
"ch2": int(self.ch2),
|
||||
"ch3": int(self.ch3),
|
||||
"ch4": int(self.ch4),
|
||||
"switches": {"ch7": int(self.switches.ch7)},
|
||||
"frame_ok": bool(self.frame_ok),
|
||||
"estop_requested": bool(self.estop_requested),
|
||||
}
|
||||
|
||||
|
||||
class RemoteUartReceiver:
|
||||
def __init__(self, port: str, baudrate: int = 100000, timeout: float = 0.02, axis_deadzone: int = 50):
|
||||
self.port = port
|
||||
self.baudrate = int(baudrate)
|
||||
self.timeout = float(timeout)
|
||||
self.axis_deadzone = int(axis_deadzone)
|
||||
self.serial: Optional[serial.Serial] = None
|
||||
self._buffer = bytearray()
|
||||
self.state = RemoteControlState()
|
||||
|
||||
def open(self) -> None:
|
||||
if self.serial and self.serial.is_open:
|
||||
return
|
||||
self.serial = serial.Serial(
|
||||
port=self.port,
|
||||
baudrate=self.baudrate,
|
||||
timeout=self.timeout,
|
||||
bytesize=serial.EIGHTBITS,
|
||||
parity=serial.PARITY_EVEN,
|
||||
stopbits=serial.STOPBITS_TWO,
|
||||
)
|
||||
|
||||
def close(self) -> None:
|
||||
if self.serial and self.serial.is_open:
|
||||
self.serial.close()
|
||||
|
||||
def poll(self) -> RemoteControlState:
|
||||
if not self.serial or not self.serial.is_open:
|
||||
raise RuntimeError("remote uart is not open")
|
||||
|
||||
waiting = self.serial.in_waiting
|
||||
if waiting:
|
||||
self._buffer.extend(self.serial.read(waiting))
|
||||
|
||||
while len(self._buffer) >= SBUS_FRAME_SIZE:
|
||||
start_idx = self._buffer.find(0x0F)
|
||||
if start_idx < 0:
|
||||
self._buffer.clear()
|
||||
break
|
||||
if start_idx > 0:
|
||||
del self._buffer[:start_idx]
|
||||
if len(self._buffer) < SBUS_FRAME_SIZE:
|
||||
break
|
||||
frame = bytes(self._buffer[:SBUS_FRAME_SIZE])
|
||||
del self._buffer[:SBUS_FRAME_SIZE]
|
||||
parsed = self._parse_frame(frame)
|
||||
if parsed is not None:
|
||||
self.state = parsed
|
||||
return self.state
|
||||
|
||||
def _parse_frame(self, frame: bytes) -> Optional[RemoteControlState]:
|
||||
if len(frame) != SBUS_FRAME_SIZE or frame[0] != 0x0F:
|
||||
return None
|
||||
|
||||
channels = [0] * 16
|
||||
channels[0] = (frame[1] | (frame[2] << 8)) & 0x07FF
|
||||
channels[1] = ((frame[2] >> 3) | (frame[3] << 5)) & 0x07FF
|
||||
channels[2] = ((frame[3] >> 6) | (frame[4] << 2) | (frame[5] << 10)) & 0x07FF
|
||||
channels[3] = ((frame[5] >> 1) | (frame[6] << 7)) & 0x07FF
|
||||
channels[4] = ((frame[6] >> 4) | (frame[7] << 4)) & 0x07FF
|
||||
channels[5] = ((frame[7] >> 7) | (frame[8] << 1) | (frame[9] << 9)) & 0x07FF
|
||||
channels[6] = ((frame[9] >> 2) | (frame[10] << 6)) & 0x07FF
|
||||
channels[7] = ((frame[10] >> 5) | (frame[11] << 3)) & 0x07FF
|
||||
channels[8] = (frame[12] | (frame[13] << 8)) & 0x07FF
|
||||
channels[9] = ((frame[13] >> 3) | (frame[14] << 5)) & 0x07FF
|
||||
|
||||
if channels[0] < 100:
|
||||
return None
|
||||
|
||||
state = RemoteControlState(
|
||||
ch1=self._normalize_axis(channels[0]),
|
||||
ch2=self._normalize_axis(channels[1]),
|
||||
ch3=self._normalize_axis(channels[3]),
|
||||
ch4=self._normalize_axis(channels[2]),
|
||||
switches=RemoteSwitchState(ch7=self._decode_switch(channels[6])),
|
||||
frame_ok=True,
|
||||
)
|
||||
if any(abs(value) > 800 for value in (state.ch1, state.ch2, state.ch3, state.ch4)):
|
||||
return None
|
||||
return state
|
||||
|
||||
def _normalize_axis(self, value: int) -> int:
|
||||
mapped = int(round((value - SBUS_RC_MID) * SBUS_AXIS_SCALE / 800.0))
|
||||
return 0 if abs(mapped) <= self.axis_deadzone else mapped
|
||||
|
||||
@staticmethod
|
||||
def _decode_switch(value: int) -> int:
|
||||
if value < 500:
|
||||
return SWITCH_LOW
|
||||
if value > 1500:
|
||||
return SWITCH_HIGH
|
||||
return SWITCH_MID
|
||||
|
||||
|
||||
class RemoteCommandMapper:
|
||||
def __init__(
|
||||
self,
|
||||
*,
|
||||
max_vx: float,
|
||||
max_vy: float,
|
||||
max_yaw: float,
|
||||
active_threshold: int = 50,
|
||||
axis_full_scale: float = SBUS_AXIS_SCALE,
|
||||
invert_vx: bool = False,
|
||||
invert_vy: bool = False,
|
||||
invert_yaw: bool = False,
|
||||
):
|
||||
self.max_vx = float(max_vx)
|
||||
self.max_vy = float(max_vy)
|
||||
self.max_yaw = float(max_yaw)
|
||||
self.active_threshold = int(active_threshold)
|
||||
self.axis_full_scale = max(float(axis_full_scale), 1.0)
|
||||
self.invert_vx = bool(invert_vx)
|
||||
self.invert_vy = bool(invert_vy)
|
||||
self.invert_yaw = bool(invert_yaw)
|
||||
|
||||
def map_command(self, state: RemoteControlState) -> np.ndarray:
|
||||
vx = self._axis_to_velocity(state.ch2, self.max_vx, self.invert_vx)
|
||||
vy = self._axis_to_velocity(state.ch4, self.max_vy, self.invert_vy)
|
||||
yaw = self._axis_to_velocity(state.ch1, self.max_yaw, self.invert_yaw)
|
||||
return np.array([vx, vy, yaw], dtype=np.float32)
|
||||
|
||||
def is_command_active(self, state: RemoteControlState) -> bool:
|
||||
return any(abs(value) > self.active_threshold for value in (state.ch1, state.ch2, state.ch4))
|
||||
|
||||
def _axis_to_velocity(self, raw_value: int, limit: float, invert: bool) -> float:
|
||||
if abs(raw_value) <= self.active_threshold:
|
||||
return 0.0
|
||||
scaled = max(-1.0, min(1.0, raw_value / self.axis_full_scale))
|
||||
if invert:
|
||||
scaled = -scaled
|
||||
return float(scaled * limit)
|
||||
|
||||
|
||||
class RemoteCommandSource:
|
||||
def __init__(
|
||||
self,
|
||||
*,
|
||||
port: str,
|
||||
max_vx: float,
|
||||
max_vy: float,
|
||||
max_yaw: float,
|
||||
baudrate: int = 100000,
|
||||
timeout: float = 0.02,
|
||||
axis_deadzone: int = 50,
|
||||
active_threshold: int = 50,
|
||||
axis_full_scale: float = SBUS_AXIS_SCALE,
|
||||
invert_vx: bool = False,
|
||||
invert_vy: bool = False,
|
||||
invert_yaw: bool = False,
|
||||
):
|
||||
self.receiver = RemoteUartReceiver(
|
||||
port=port,
|
||||
baudrate=baudrate,
|
||||
timeout=timeout,
|
||||
axis_deadzone=axis_deadzone,
|
||||
)
|
||||
self.mapper = RemoteCommandMapper(
|
||||
max_vx=max_vx,
|
||||
max_vy=max_vy,
|
||||
max_yaw=max_yaw,
|
||||
active_threshold=active_threshold,
|
||||
axis_full_scale=axis_full_scale,
|
||||
invert_vx=invert_vx,
|
||||
invert_vy=invert_vy,
|
||||
invert_yaw=invert_yaw,
|
||||
)
|
||||
self.last_state = RemoteControlState()
|
||||
self.last_command = np.zeros(3, dtype=np.float32)
|
||||
|
||||
@property
|
||||
def port(self) -> str:
|
||||
return self.receiver.port
|
||||
|
||||
def open(self) -> None:
|
||||
self.receiver.open()
|
||||
|
||||
def close(self) -> None:
|
||||
self.receiver.close()
|
||||
|
||||
def poll(self) -> RemoteControlState:
|
||||
self.last_state = self.receiver.poll()
|
||||
self.last_command = self.mapper.map_command(self.last_state)
|
||||
return self.last_state
|
||||
|
||||
def get_command(self) -> np.ndarray:
|
||||
return self.last_command.copy()
|
||||
|
||||
def is_command_active(self) -> bool:
|
||||
return self.mapper.is_command_active(self.last_state)
|
||||
|
||||
def get_status(self) -> dict:
|
||||
status = self.last_state.as_dict()
|
||||
status.update(
|
||||
{
|
||||
"port": self.port,
|
||||
"cmd": self.last_command.tolist(),
|
||||
"command_active": bool(self.is_command_active()),
|
||||
}
|
||||
)
|
||||
return status
|
||||
@@ -0,0 +1,229 @@
|
||||
"""Odin1 IMU 客户端封装。
|
||||
|
||||
核心改动相对 sim_rl/odin1/python/odin1_imu.py：
|
||||
- 自动加载默认 .so 路径，调用方只需要 IMUClient(lib_path=...)
|
||||
- 启动后做一次"重力对齐" — 用静止时的加速度计读数初始化 Mahony 滤波器，
|
||||
把首步姿态偏差从可能的 5°+ 降到 0.3° 内。这是方法论 D4 的关键一步。
|
||||
- 数据老化检测：若 imu_age_ms > stale_threshold 则报警（不阻塞）。
|
||||
"""
|
||||
import sys
|
||||
import time
|
||||
from pathlib import Path
|
||||
from typing import Dict, Optional
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
class IMUClient:
|
||||
"""Odin1 IMU 包装。
|
||||
|
||||
Args:
|
||||
lib_path: libodin1_imu_bridge.so 的绝对路径；None 则按方法论 1.2 中
|
||||
约定的相对位置寻找。
|
||||
gravity_align_samples: 启动时取多少帧加速度计平均值用于姿态初始化
|
||||
stale_threshold_ms: 单帧数据超过该 age 视为陈旧
|
||||
"""
|
||||
|
||||
def __init__(self, lib_path: Optional[str] = None, gravity_align_samples: int = 50,
|
||||
stale_threshold_ms: float = 50.0, dry_run: bool = False):
|
||||
self._mock_mode = dry_run
|
||||
self._initial_gravity = None
|
||||
self._last_seq = -1
|
||||
self._last_fresh_time = 0.0
|
||||
self._last_odom_stamp = -1
|
||||
self._last_odom_fresh_time = 0.0
|
||||
|
||||
if self._mock_mode:
|
||||
print("[IMUClient] 启动 Mock IMU 模式 (不加载物理 IMU 驱动)。")
|
||||
self._client = None
|
||||
return
|
||||
|
||||
# 优先级 1: vendored/odin1_imu（独立部署模式）
|
||||
# 优先级 2: ../../odin1/odin1/python（开发模式，即 sim_rl/odin1/odin1/python）
|
||||
sim2real_root = Path(__file__).resolve().parents[1]
|
||||
candidates = [
|
||||
sim2real_root / "vendored" / "odin1_imu",
|
||||
sim2real_root.parents[1] / "odin1" / "odin1" / "python",
|
||||
]
|
||||
for cand in candidates:
|
||||
if cand.exists() and str(cand) not in sys.path:
|
||||
sys.path.insert(0, str(cand))
|
||||
break
|
||||
try:
|
||||
from odin1_imu import Odin1ImuClient # type: ignore
|
||||
# lib_path 默认查找：vendored/odin1_imu/build/libodin1_imu_bridge.so → 开发路径
|
||||
if lib_path is None:
|
||||
so_candidates = [
|
||||
sim2real_root / "vendored" / "odin1_imu" / "build" / "libodin1_imu_bridge.so",
|
||||
sim2real_root / "vendored" / "odin1_imu" / "libodin1_imu_bridge.so",
|
||||
sim2real_root.parents[1] / "odin1" / "odin1" / "build" / "libodin1_imu_bridge.so",
|
||||
]
|
||||
for so in so_candidates:
|
||||
if so.exists():
|
||||
lib_path = str(so)
|
||||
break
|
||||
|
||||
self._client = Odin1ImuClient(lib_path=lib_path)
|
||||
self._gravity_align_samples = gravity_align_samples
|
||||
self._stale_threshold_ms = stale_threshold_ms
|
||||
except Exception as e:
|
||||
print(f"[IMUClient] 错误: 无法初始化实机 IMU 驱动 ({type(e).__name__}: {e})。实机部署下拒绝启动。")
|
||||
raise
|
||||
|
||||
def version(self) -> str:
|
||||
if self._mock_mode:
|
||||
return "MockIMU-v1.0"
|
||||
return self._client.version()
|
||||
|
||||
def start(self, timeout_ms: int = 8000):
|
||||
"""启动 IMU 流，并采集若干帧用于重力对齐。"""
|
||||
if self._mock_mode:
|
||||
self._initial_gravity = np.array([0.0, 0.0, 9.81], dtype=np.float32)
|
||||
self._last_fresh_time = time.time()
|
||||
self._last_odom_fresh_time = time.time()
|
||||
return
|
||||
self._client.start(timeout_ms=timeout_ms)
|
||||
self._wait_for_stream()
|
||||
self._initial_gravity = self._collect_gravity_samples()
|
||||
self._last_fresh_time = time.time()
|
||||
|
||||
def stop(self):
|
||||
if self._mock_mode:
|
||||
return
|
||||
try:
|
||||
self._client.stop()
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
@property
|
||||
def initial_gravity(self) -> Optional[np.ndarray]:
|
||||
"""启动后的初始重力向量（机身坐标系），用于初始化 Mahony 四元数。"""
|
||||
return self._initial_gravity
|
||||
|
||||
def get_latest(self):
|
||||
"""返回 (gyro[3], accel[3], age_ms, fresh)；fresh=False 表示无新数据。"""
|
||||
if self._mock_mode:
|
||||
now = time.time()
|
||||
age_ms = float(getattr(self, "_debug_mock_age_ms", 0.0))
|
||||
fresh = True
|
||||
return (np.zeros(3, dtype=np.float32),
|
||||
np.array([0.0, 0.0, 9.81], dtype=np.float32),
|
||||
age_ms, fresh)
|
||||
|
||||
sample = self._client.get_latest()
|
||||
if sample is None:
|
||||
return (np.zeros(3, dtype=np.float32),
|
||||
np.array([0.0, 0.0, 9.81], dtype=np.float32),
|
||||
-1.0, False)
|
||||
gyro = np.array([sample.gyro_x, sample.gyro_y, sample.gyro_z], dtype=np.float32)
|
||||
accel = np.array([sample.accel_x, sample.accel_y, sample.accel_z], dtype=np.float32)
|
||||
# 用 stamp_ns 判断是否有新数据，因为 sequence 字段在 C++ 中可能没有赋值，导致永远为 0
|
||||
stamp = getattr(sample, "stamp_ns", 0)
|
||||
now = time.time()
|
||||
if stamp != self._last_seq:
|
||||
self._last_seq = stamp
|
||||
self._last_fresh_time = now
|
||||
fresh = True
|
||||
else:
|
||||
fresh = False
|
||||
age_ms = (now - self._last_fresh_time) * 1000.0
|
||||
return gyro, accel, age_ms, fresh
|
||||
|
||||
def get_latest_odom(self) -> Optional[Dict[str, object]]:
|
||||
"""Return latest Odin odom for diagnostics only; policy observations stay unchanged."""
|
||||
if self._mock_mode:
|
||||
now = time.time()
|
||||
# 模拟一个围绕 (0, 0) 的圆形轨迹，用于测试 Web UI Canvas 绘图
|
||||
theta = now * 0.2
|
||||
x = 0.5 * np.cos(theta)
|
||||
y = 0.5 * np.sin(theta)
|
||||
return {
|
||||
"type": "STANDARD",
|
||||
"stamp_ns": int(now * 1e9),
|
||||
"fresh": True,
|
||||
"age_ms": 0.0,
|
||||
"pos": [float(x), float(y), 0.33],
|
||||
"quat_wxyz": [1.0, 0.0, 0.0, 0.0],
|
||||
"linear_vel": [0.0, 0.0, 0.0],
|
||||
"angular_vel": [0.0, 0.0, 0.2],
|
||||
}
|
||||
|
||||
getter = getattr(self._client, "odom_get_latest", None)
|
||||
if getter is None:
|
||||
return None
|
||||
try:
|
||||
sample = getter()
|
||||
except Exception:
|
||||
return None
|
||||
if sample is None:
|
||||
return None
|
||||
|
||||
stamp = int(getattr(sample, "stamp_ns", 0))
|
||||
now = time.time()
|
||||
if stamp != self._last_odom_stamp:
|
||||
self._last_odom_stamp = stamp
|
||||
self._last_odom_fresh_time = now
|
||||
fresh = True
|
||||
else:
|
||||
fresh = False
|
||||
age_ms = (now - self._last_odom_fresh_time) * 1000.0 if self._last_odom_fresh_time else -1.0
|
||||
return {
|
||||
"type": _odom_type_name(int(getattr(sample, "type", -1))),
|
||||
"stamp_ns": stamp,
|
||||
"fresh": fresh,
|
||||
"age_ms": age_ms,
|
||||
"pos": [
|
||||
float(getattr(sample, "pos_x", 0.0)),
|
||||
float(getattr(sample, "pos_y", 0.0)),
|
||||
float(getattr(sample, "pos_z", 0.0)),
|
||||
],
|
||||
"quat_wxyz": [
|
||||
float(getattr(sample, "orient_w", 1.0)),
|
||||
float(getattr(sample, "orient_x", 0.0)),
|
||||
float(getattr(sample, "orient_y", 0.0)),
|
||||
float(getattr(sample, "orient_z", 0.0)),
|
||||
],
|
||||
"linear_vel": [
|
||||
float(getattr(sample, "linear_vel_x", 0.0)),
|
||||
float(getattr(sample, "linear_vel_y", 0.0)),
|
||||
float(getattr(sample, "linear_vel_z", 0.0)),
|
||||
],
|
||||
"angular_vel": [
|
||||
float(getattr(sample, "angular_vel_x", 0.0)),
|
||||
float(getattr(sample, "angular_vel_y", 0.0)),
|
||||
float(getattr(sample, "angular_vel_z", 0.0)),
|
||||
],
|
||||
}
|
||||
|
||||
# ---- 内部方法 ----
|
||||
def _wait_for_stream(self, timeout: float = 3.0):
|
||||
deadline = time.time() + timeout
|
||||
while time.time() < deadline:
|
||||
if self._client.wait_for_data(timeout_ms=200):
|
||||
# 有数据进来后清空一次队列以保证后续 get_latest 拿到的都是最新
|
||||
while self._client.pop_sample() is not None:
|
||||
pass
|
||||
return
|
||||
raise RuntimeError("IMU 启动超时，未收到任何样本")
|
||||
|
||||
def _collect_gravity_samples(self) -> np.ndarray:
|
||||
accels = []
|
||||
for _ in range(self._gravity_align_samples):
|
||||
sample = self._client.pop_sample()
|
||||
if sample is None:
|
||||
if not self._client.wait_for_data(timeout_ms=100):
|
||||
continue
|
||||
sample = self._client.pop_sample()
|
||||
if sample is None:
|
||||
continue
|
||||
accels.append([sample.accel_x, sample.accel_y, sample.accel_z])
|
||||
if not accels:
|
||||
print("[IMU] 警告: 重力对齐期间未收到样本，使用默认重力 [0,0,-9.81]")
|
||||
return np.array([0.0, 0.0, -9.81], dtype=np.float32)
|
||||
gravity = np.mean(accels, axis=0).astype(np.float32)
|
||||
print(f"[IMU] 重力对齐完成: g_body = {gravity}")
|
||||
return gravity
|
||||
|
||||
|
||||
def _odom_type_name(value: int) -> str:
|
||||
return {0: "STANDARD", 1: "HIGHFREQ", 2: "TF"}.get(value, f"UNKNOWN_{value}")
|
||||
@@ -0,0 +1,364 @@
|
||||
"""RobStride 电机驱动包装。
|
||||
|
||||
职责：
|
||||
- 封装 ik_real 中 RobStrideDriver 的 enable/disable/clear/control_mit 调用
|
||||
- **真实的丢包检测**：旧版用「value=0 启发式」会误判（电机回机械零位时也是 0）。
|
||||
新方案：
|
||||
1. 调用 process_messages 前快照所有电机的 (pos, vel, torque)
|
||||
2. 调用后比较：状态变了 → 这一帧有新反馈；状态完全没变 → 累计 stale_count
|
||||
3. stale_count 超过阈值才沿用上一帧（方法论 3.4.2）
|
||||
仍然不完美（电机长时间静止确实会有连续多帧 state 不变），但比 0 启发式可靠。
|
||||
- 通过 driver_factory 由调用方注入：远程 Linux 主机用 RobStrideDriver，
|
||||
本地 Windows 调试可用 Mock。
|
||||
"""
|
||||
from dataclasses import dataclass
|
||||
import threading
|
||||
from typing import Callable, Dict, List, Optional, Tuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
from interface.motor_mapping import MotorMapping
|
||||
|
||||
|
||||
@dataclass
|
||||
class MotorReading:
|
||||
position: float
|
||||
velocity: float
|
||||
torque: float = 0.0
|
||||
fresh: bool = False # True 表示本帧驱动板有新反馈
|
||||
|
||||
|
||||
class HardwareIO:
|
||||
"""统一的电机+IMU总线接口（不含策略），主控调用这一层。
|
||||
|
||||
Args:
|
||||
driver_factory: () -> (drv1, drv2)，由调用方注入；返回的对象需要满足：
|
||||
connect()/disconnect()/disable(name)/enable(name)/clear_warnings(name)
|
||||
add_motor(name, mid, model)/process_messages()
|
||||
control_mit(name, q, dq, kp, kd, tau)
|
||||
.motors: dict[name -> motor], motor.state.position / .velocity / .torque
|
||||
config: yaml 解析后的字典
|
||||
"""
|
||||
|
||||
def __init__(self, driver_factory: Callable[[str, str, bool], Tuple[object, object]],
|
||||
motor_model: str, can1_port: str, can2_port: str, debug: bool = False,
|
||||
stale_frames_to_holdover: int = 2):
|
||||
self.mapper = MotorMapping()
|
||||
drv1, drv2 = driver_factory(can1_port, can2_port, debug)
|
||||
self.driver_can1 = drv1
|
||||
self.driver_can2 = drv2
|
||||
self.motor_model = motor_model
|
||||
self.stale_frames_to_holdover = stale_frames_to_holdover
|
||||
|
||||
# 上一帧反馈（按 (bus, can_id) 索引），用于丢包兜底
|
||||
self._last_pos: Dict[Tuple[int, int], float] = {}
|
||||
self._last_vel: Dict[Tuple[int, int], float] = {}
|
||||
self._last_torque: Dict[Tuple[int, int], float] = {}
|
||||
self._last_real_pos: Dict[Tuple[int, int], float] = {}
|
||||
# 每个电机连续多少帧没收到新反馈
|
||||
self._stale_counts: Dict[Tuple[int, int], int] = {}
|
||||
# 第一次必须读到才能解锁，避免初始化时直接用零位发送大力矩
|
||||
self._initialized = False
|
||||
|
||||
self.lock = threading.Lock()
|
||||
|
||||
# 累计诊断
|
||||
self.holdover_total = 0 # 累计被沿用上一帧的次数
|
||||
|
||||
# ---- 总线管理 ----
|
||||
def connect(self):
|
||||
self.driver_can1.connect()
|
||||
self.driver_can2.connect()
|
||||
for jk in self.mapper.SIM_JOINT_ORDER:
|
||||
leg, joint = jk
|
||||
bus, mid = self.mapper.CAN_ID_MAP[jk]
|
||||
name = f"{leg}_{joint}"
|
||||
drv = self.driver_can1 if bus == 1 else self.driver_can2
|
||||
drv.add_motor(name, mid, self.motor_model)
|
||||
self._stale_counts[(bus, mid)] = 0
|
||||
|
||||
def disconnect(self):
|
||||
try:
|
||||
self.driver_can1.disconnect()
|
||||
finally:
|
||||
self.driver_can2.disconnect()
|
||||
|
||||
def enable_all(self):
|
||||
for drv in (self.driver_can1, self.driver_can2):
|
||||
for name in drv.motors:
|
||||
drv.clear_warnings(name)
|
||||
drv.enable(name)
|
||||
|
||||
def disable_all(self):
|
||||
for drv in (self.driver_can1, self.driver_can2):
|
||||
for name in drv.motors:
|
||||
drv.disable(name)
|
||||
|
||||
# ---- 状态读取 ----
|
||||
def _snapshot_state(self) -> Dict[Tuple[int, int], Tuple[float, float, float, int, float, int, int]]:
|
||||
"""快照所有电机的 (pos, vel, torque, update_count, temperature, fault_code, mode_state)，process_messages 前后比较即可判 fresh。"""
|
||||
snap: Dict[Tuple[int, int], Tuple[float, float, float, int, float, int, int]] = {}
|
||||
for drv_idx, drv in enumerate((self.driver_can1, self.driver_can2)):
|
||||
bus = drv_idx + 1
|
||||
for name, motor in drv.motors.items():
|
||||
parts = name.split("_", 1)
|
||||
if len(parts) != 2:
|
||||
continue
|
||||
key = (parts[0], parts[1])
|
||||
if key not in self.mapper.CAN_ID_MAP:
|
||||
continue
|
||||
_, mid = self.mapper.CAN_ID_MAP[key]
|
||||
s = motor.state
|
||||
snap[(bus, mid)] = (
|
||||
s.position,
|
||||
s.velocity,
|
||||
s.torque,
|
||||
getattr(s, "update_count", 0),
|
||||
getattr(s, "temperature", 0.0),
|
||||
getattr(s, "fault_code", 0),
|
||||
getattr(s, "mode_state", 0)
|
||||
)
|
||||
return snap
|
||||
|
||||
def read_state(self) -> Tuple[np.ndarray, np.ndarray, np.ndarray, Dict[str, object]]:
|
||||
"""返回 (sim_joint_pos[16], sim_joint_vel[16], sim_joint_torque[16], debug_info)。"""
|
||||
with self.lock:
|
||||
# 1) 抓取上一次的状态作为「pre」快照（基线）
|
||||
pre = self._snapshot_state()
|
||||
|
||||
# 2) 拉取本帧反馈
|
||||
self.driver_can1.process_messages()
|
||||
self.driver_can2.process_messages()
|
||||
|
||||
# 3) 抓取「post」快照
|
||||
post = self._snapshot_state()
|
||||
|
||||
# 4) 比较：state 元组变了 → 本帧有新反馈，stale_count 清零；否则 stale_count++
|
||||
per_motor_fresh: Dict[Tuple[int, int], bool] = {}
|
||||
fresh_by_update_count = 0
|
||||
fresh_by_value_change = 0
|
||||
update_counts: Dict[Tuple[int, int], int] = {}
|
||||
for key in post:
|
||||
pre_fields = pre.get(key)
|
||||
post_fields = post[key]
|
||||
pre_count = pre_fields[3] if pre_fields is not None and len(pre_fields) >= 4 else 0
|
||||
post_count = post_fields[3] if len(post_fields) >= 4 else 0
|
||||
update_counts[key] = int(post_count)
|
||||
if post_count > pre_count:
|
||||
fresh = True
|
||||
fresh_by_update_count += 1
|
||||
elif pre_fields is not None and post_fields[:3] != pre_fields[:3]:
|
||||
fresh = True
|
||||
fresh_by_value_change += 1
|
||||
else:
|
||||
fresh = False
|
||||
per_motor_fresh[key] = fresh
|
||||
if fresh:
|
||||
self._stale_counts[key] = 0
|
||||
else:
|
||||
self._stale_counts[key] += 1
|
||||
|
||||
# 5) 取出本帧 pos/vel；若该电机连续多帧没刷新，沿用上一帧（方法论 3.4.2）
|
||||
real_pos: Dict[Tuple[int, int], float] = {}
|
||||
real_vel: Dict[Tuple[int, int], float] = {}
|
||||
real_torque: Dict[Tuple[int, int], float] = {}
|
||||
real_temp: Dict[Tuple[int, int], float] = {}
|
||||
real_fault: Dict[Tuple[int, int], int] = {}
|
||||
real_mode: Dict[Tuple[int, int], int] = {}
|
||||
holdover_this_frame = 0
|
||||
for key, snap_val in post.items():
|
||||
pos, vel, tor, _, temp, fault, mode = snap_val
|
||||
real_temp[key] = temp
|
||||
real_fault[key] = fault
|
||||
real_mode[key] = mode
|
||||
if (not per_motor_fresh[key]) and self._stale_counts[key] >= self.stale_frames_to_holdover:
|
||||
# 长时间不刷新视作丢包：沿用上一帧
|
||||
if key in self._last_pos:
|
||||
real_pos[key] = self._last_pos[key]
|
||||
real_vel[key] = self._last_vel[key]
|
||||
real_torque[key] = self._last_torque[key]
|
||||
holdover_this_frame += 1
|
||||
else:
|
||||
real_pos[key] = pos
|
||||
real_vel[key] = vel
|
||||
real_torque[key] = tor
|
||||
else:
|
||||
real_pos[key] = pos
|
||||
real_vel[key] = vel
|
||||
real_torque[key] = tor
|
||||
|
||||
self.holdover_total += holdover_this_frame
|
||||
# 缓存本帧（即便部分是 holdover 也缓存）
|
||||
self._last_pos = real_pos.copy()
|
||||
self._last_vel = real_vel.copy()
|
||||
self._last_torque = real_torque.copy()
|
||||
self._last_real_pos = real_pos.copy()
|
||||
if not self._initialized:
|
||||
self._initialized = True
|
||||
|
||||
cur_pos = self.mapper.real_to_sim(real_pos)
|
||||
cur_vel = self.mapper.real_vel_to_sim(real_vel)
|
||||
cur_torque = self.mapper.real_vel_to_sim(real_torque)
|
||||
|
||||
# 诊断信息
|
||||
stale_max = max(self._stale_counts.values()) if self._stale_counts else 0
|
||||
n_stale_motors = sum(1 for c in self._stale_counts.values()
|
||||
if c >= self.stale_frames_to_holdover)
|
||||
# 按 SIM_JOINT_ORDER 排列的每个电机连续丢帧数
|
||||
per_motor_stale = [
|
||||
self._stale_counts.get(self.mapper.CAN_ID_MAP[jk], 99)
|
||||
for jk in self.mapper.SIM_JOINT_ORDER
|
||||
]
|
||||
return cur_pos, cur_vel, cur_torque, {
|
||||
"holdover_this_frame": holdover_this_frame,
|
||||
"stale_max": stale_max,
|
||||
"n_stale_motors": n_stale_motors,
|
||||
"fresh_count": sum(1 for v in per_motor_fresh.values() if v),
|
||||
"per_motor_stale": per_motor_stale,
|
||||
"fresh_by_update_count": fresh_by_update_count,
|
||||
"fresh_by_value_change": fresh_by_value_change,
|
||||
"update_counts": [
|
||||
update_counts.get(self.mapper.CAN_ID_MAP[jk], 0)
|
||||
for jk in self.mapper.SIM_JOINT_ORDER
|
||||
],
|
||||
"temperatures": [
|
||||
real_temp.get(self.mapper.CAN_ID_MAP[jk], 0.0)
|
||||
for jk in self.mapper.SIM_JOINT_ORDER
|
||||
],
|
||||
"fault_codes": [
|
||||
real_fault.get(self.mapper.CAN_ID_MAP[jk], 0)
|
||||
for jk in self.mapper.SIM_JOINT_ORDER
|
||||
],
|
||||
"mode_states": [
|
||||
real_mode.get(self.mapper.CAN_ID_MAP[jk], 0)
|
||||
for jk in self.mapper.SIM_JOINT_ORDER
|
||||
],
|
||||
}
|
||||
|
||||
def passive_poll(self):
|
||||
"""发送全 0 (0刚度0阻尼0力矩) 的 MIT 指令给所有电机。
|
||||
目的：在 ENABLED 状态下，不产生力矩地索要反馈（因为 RobStride 在 MIT 模式下必须有指令才反馈）。"""
|
||||
with self.lock:
|
||||
for jk in self.mapper.SIM_JOINT_ORDER:
|
||||
bus, mid = self.mapper.CAN_ID_MAP[jk]
|
||||
name = f"{jk[0]}_{jk[1]}"
|
||||
drv = self.driver_can1 if bus == 1 else self.driver_can2
|
||||
if name in drv.motors:
|
||||
drv.control_mit(name, 0.0, 0.0, 0.0, 0.0, 0.0)
|
||||
|
||||
# ---- 控制下发 ----
|
||||
def send_control(self, target_angles: np.ndarray, kp_leg: float, kd_leg: float,
|
||||
kd_wheel: float):
|
||||
"""与 sim2sim 的 PD 模型对齐：
|
||||
- 腿: position 控制，目标角度由 target_angles[:12] 给出，kp/kd 来自配置
|
||||
- 轮: velocity 控制，目标速度由 target_angles[12:] 给出，kd 阻尼
|
||||
"""
|
||||
with self.lock:
|
||||
if target_angles.shape != (16,):
|
||||
raise ValueError("target_angles must be (16,)")
|
||||
|
||||
real_targets = self.mapper.sim_to_real(
|
||||
target_angles.astype(np.float32),
|
||||
current_real_pos=self._last_real_pos,
|
||||
)
|
||||
|
||||
# 轮毂速度目标暂且用 0，如果 target_angles 里包含了速度，就在 policy 那里处理，
|
||||
# 这里的 target_angles 是 pose 目标，轮毂作为连续旋转关节其实位置控制没有意义。
|
||||
# 为了兼容旧代码，这里构造一个 16 维的 velocity array，只有后 4 个是目标（如果当作速度的话）。
|
||||
vel_targets = np.zeros(16, dtype=np.float32)
|
||||
vel_targets[12:] = target_angles[12:].astype(np.float32)
|
||||
real_wheel = self.mapper.sim_vel_to_real(vel_targets)
|
||||
|
||||
for jk in self.mapper.SIM_JOINT_ORDER:
|
||||
leg, joint = jk
|
||||
bus, mid = self.mapper.CAN_ID_MAP[jk]
|
||||
name = f"{leg}_{joint}"
|
||||
drv = self.driver_can1 if bus == 1 else self.driver_can2
|
||||
if name not in drv.motors:
|
||||
continue
|
||||
|
||||
if joint == "wheel":
|
||||
v = real_wheel[(bus, mid)]
|
||||
drv.control_mit(name, 0.0, v, 0.0, kd_wheel, 0.0)
|
||||
else:
|
||||
q = real_targets[(bus, mid)]
|
||||
drv.control_mit(name, q, 0.0, kp_leg, kd_leg, 0.0)
|
||||
|
||||
def damping_brake(self, kd_leg: float, kd_wheel: float):
|
||||
"""急停模式：所有关节卸载刚度，仅保留阻尼。
|
||||
对应 270_SimToReal 方法论 97.11 Level 2 "刹车"。
|
||||
"""
|
||||
with self.lock:
|
||||
for jk in self.mapper.SIM_JOINT_ORDER:
|
||||
leg, joint = jk
|
||||
bus, _ = self.mapper.CAN_ID_MAP[jk]
|
||||
name = f"{leg}_{joint}"
|
||||
drv = self.driver_can1 if bus == 1 else self.driver_can2
|
||||
if name not in drv.motors:
|
||||
continue
|
||||
kd = kd_wheel if joint == "wheel" else kd_leg
|
||||
drv.control_mit(name, 0.0, 0.0, 0.0, kd, 0.0)
|
||||
|
||||
def wait_feedback_ready(self, max_attempts: int = 20,
|
||||
poll_interval: float = 0.05) -> Tuple[bool, list]:
|
||||
"""enable 后调用：尝试 max_attempts 次读总线，等所有 16 个电机
|
||||
都至少给出一帧反馈。
|
||||
返回 (all_ready, missing_motors)；missing_motors 是 (bus, mid, name) 列表。
|
||||
"""
|
||||
import time
|
||||
seen: Dict[Tuple[int, int], bool] = {
|
||||
self.mapper.CAN_ID_MAP[jk]: False for jk in self.mapper.SIM_JOINT_ORDER
|
||||
}
|
||||
# 用第一次读到的 (pos, vel, torque) 三元组的"非零"或"已变化"作为反馈到达的判据。
|
||||
# 启动瞬间所有 motor.state 默认全 0，要么收到反馈让其变化，要么收到反馈但值确实是 0。
|
||||
# 退化情况下电机静止时 vel=0 且 pos=机械零位也=0，那种情况只能等多帧确认。
|
||||
snap_prev = self._snapshot_state()
|
||||
for attempt in range(max_attempts):
|
||||
with self.lock:
|
||||
self.driver_can1.process_messages()
|
||||
self.driver_can2.process_messages()
|
||||
snap_cur = self._snapshot_state()
|
||||
for key, fields_cur in snap_cur.items():
|
||||
if seen[key]:
|
||||
continue
|
||||
fields_prev = snap_prev.get(key)
|
||||
# 任一字段不为 0 → 一定有反馈（因为初始值都是 0）
|
||||
if any(v != 0.0 for v in fields_cur):
|
||||
seen[key] = True
|
||||
# 与上一次快照不同 → 一定有反馈（即便都很小）
|
||||
elif fields_prev is not None and (fields_cur[3] > fields_prev[3] or fields_cur != fields_prev):
|
||||
seen[key] = True
|
||||
snap_prev = snap_cur
|
||||
if all(seen.values()):
|
||||
return True, []
|
||||
time.sleep(poll_interval)
|
||||
|
||||
# 超时：列出仍未反馈的电机
|
||||
missing = []
|
||||
rev_can = {v: k for k, v in self.mapper.CAN_ID_MAP.items()}
|
||||
for key, ok in seen.items():
|
||||
if not ok:
|
||||
leg, joint = rev_can[key]
|
||||
missing.append((key[0], key[1], f"{leg}_{joint}"))
|
||||
return False, missing
|
||||
|
||||
def read_measured_pose(self) -> np.ndarray:
|
||||
"""返回 (16,) 当前实测 sim 坐标系下的关节位置。
|
||||
会先 process_messages 一次保证拿到本帧。
|
||||
"""
|
||||
self.driver_can1.process_messages()
|
||||
self.driver_can2.process_messages()
|
||||
real_pos: Dict[Tuple[int, int], float] = {}
|
||||
for drv_idx, drv in enumerate((self.driver_can1, self.driver_can2)):
|
||||
bus = drv_idx + 1
|
||||
for name, motor in drv.motors.items():
|
||||
parts = name.split("_", 1)
|
||||
if len(parts) != 2:
|
||||
continue
|
||||
key = (parts[0], parts[1])
|
||||
if key not in self.mapper.CAN_ID_MAP:
|
||||
continue
|
||||
_, mid = self.mapper.CAN_ID_MAP[key]
|
||||
real_pos[(bus, mid)] = motor.state.position
|
||||
self._last_real_pos = real_pos.copy()
|
||||
return self.mapper.real_to_sim(real_pos)
|
||||
@@ -0,0 +1,123 @@
|
||||
"""仿真→实机电机映射。
|
||||
|
||||
数据来源：sim_rl/ik_real/sim_to_real_deploy_beifen.py 和
|
||||
sim_rl/sim2real/motor_mapping.py 中的 sign / offset / can_id 表（已在实机上验证）。
|
||||
关节顺序与 rc_mjlab/sim2sim 完全一致：[12 个腿关节] + [4 个轮子]。
|
||||
"""
|
||||
from typing import Dict, Tuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
class MotorMapping:
|
||||
TWO_PI = float(2.0 * np.pi)
|
||||
LEG_NAMES = ("fl", "fr", "rl", "rr")
|
||||
JOINT_NAMES = ("hip_abduction", "hip_pitch", "knee", "wheel")
|
||||
|
||||
SIM_JOINT_ORDER = (
|
||||
("fl", "hip_abduction"), ("fl", "hip_pitch"), ("fl", "knee"),
|
||||
("fr", "hip_abduction"), ("fr", "hip_pitch"), ("fr", "knee"),
|
||||
("rl", "hip_abduction"), ("rl", "hip_pitch"), ("rl", "knee"),
|
||||
("rr", "hip_abduction"), ("rr", "hip_pitch"), ("rr", "knee"),
|
||||
("fl", "wheel"), ("fr", "wheel"), ("rl", "wheel"), ("rr", "wheel"),
|
||||
)
|
||||
SIM_INDEX_MAP = {jk: i for i, jk in enumerate(SIM_JOINT_ORDER)}
|
||||
|
||||
CAN_ID_MAP: Dict[Tuple[str, str], Tuple[int, int]] = {
|
||||
("fl", "hip_abduction"): (1, 1), ("fl", "hip_pitch"): (1, 2),
|
||||
("fl", "knee"): (1, 3), ("fl", "wheel"): (1, 4),
|
||||
("fr", "hip_abduction"): (1, 5), ("fr", "hip_pitch"): (1, 6),
|
||||
("fr", "knee"): (1, 7), ("fr", "wheel"): (1, 8),
|
||||
("rl", "hip_abduction"): (2, 1), ("rl", "hip_pitch"): (2, 2),
|
||||
("rl", "knee"): (2, 3), ("rl", "wheel"): (2, 4),
|
||||
("rr", "hip_abduction"): (2, 5), ("rr", "hip_pitch"): (2, 6),
|
||||
("rr", "knee"): (2, 7), ("rr", "wheel"): (2, 8),
|
||||
}
|
||||
|
||||
DIRECTION_MAP: Dict[Tuple[str, str], int] = {
|
||||
("fl", "hip_abduction"): -1, ("fl", "hip_pitch"): -1,
|
||||
("fl", "knee"): -1, ("fl", "wheel"): -1,
|
||||
("fr", "hip_abduction"): -1, ("fr", "hip_pitch"): 1,
|
||||
("fr", "knee"): 1, ("fr", "wheel"): 1,
|
||||
("rl", "hip_abduction"): 1, ("rl", "hip_pitch"): -1,
|
||||
("rl", "knee"): -1, ("rl", "wheel"): -1,
|
||||
("rr", "hip_abduction"): 1, ("rr", "hip_pitch"): 1,
|
||||
("rr", "knee"): 1, ("rr", "wheel"): 1,
|
||||
}
|
||||
|
||||
ZERO_OFFSET_MAP: Dict[Tuple[str, str], float] = {
|
||||
("fl", "hip_abduction"): 0.003, ("fl", "hip_pitch"): 0.030,
|
||||
("fl", "knee"): 0.028, ("fl", "wheel"): 0.000,
|
||||
("fr", "hip_abduction"): 0.004, ("fr", "hip_pitch"): 0.038,
|
||||
("fr", "knee"): 0.011, ("fr", "wheel"): 0.000,
|
||||
("rl", "hip_abduction"): 0.019, ("rl", "hip_pitch"): -0.034,
|
||||
("rl", "knee"): 0.025, ("rl", "wheel"): 0.000,
|
||||
("rr", "hip_abduction"): -0.001, ("rr", "hip_pitch"): 0.039,
|
||||
("rr", "knee"): 0.018, ("rr", "wheel"): 0.000,
|
||||
}
|
||||
|
||||
def __init__(self):
|
||||
self.num_motors = len(self.SIM_JOINT_ORDER)
|
||||
self._sign = np.array([self.DIRECTION_MAP[jk] for jk in self.SIM_JOINT_ORDER], dtype=np.float32)
|
||||
self._offset = np.array([self.ZERO_OFFSET_MAP[jk] for jk in self.SIM_JOINT_ORDER], dtype=np.float32)
|
||||
self._default_pose = np.array(
|
||||
[0.0, 0.9, -1.8, 0.0, 0.9, -1.8, 0.0, 0.9, -1.8, 0.0, 0.9, -1.8, 0.0, 0.0, 0.0, 0.0],
|
||||
dtype=np.float32,
|
||||
)
|
||||
|
||||
@classmethod
|
||||
def _nearest_periodic(cls, value: float, reference: float) -> float:
|
||||
return float(reference + ((value - reference + np.pi) % cls.TWO_PI - np.pi))
|
||||
|
||||
def canonicalize_sim(self, sim_angles: np.ndarray, reference: np.ndarray | None = None) -> np.ndarray:
|
||||
"""Map leg joint angles onto the 2*pi branch nearest the deployment reference pose."""
|
||||
out = np.asarray(sim_angles, dtype=np.float32).copy()
|
||||
ref = self._default_pose if reference is None else np.asarray(reference, dtype=np.float32)
|
||||
for i, (_, joint) in enumerate(self.SIM_JOINT_ORDER[:12]):
|
||||
out[i] = self._nearest_periodic(float(out[i]), float(ref[i]))
|
||||
return out
|
||||
|
||||
def sim_to_real(
|
||||
self,
|
||||
sim_angles: np.ndarray,
|
||||
current_real_pos: Dict[Tuple[int, int], float] | None = None,
|
||||
) -> Dict[Tuple[int, int], float]:
|
||||
if len(sim_angles) != 16:
|
||||
raise ValueError(f"expected 16 sim angles, got {len(sim_angles)}")
|
||||
out: Dict[Tuple[int, int], float] = {}
|
||||
for i, jk in enumerate(self.SIM_JOINT_ORDER):
|
||||
real = float(self._sign[i] * sim_angles[i] + self._offset[i])
|
||||
can_key = self.CAN_ID_MAP[jk]
|
||||
if current_real_pos is not None and i < 12 and can_key in current_real_pos:
|
||||
real = self._nearest_periodic(real, float(current_real_pos[can_key]))
|
||||
out[can_key] = real
|
||||
return out
|
||||
|
||||
def sim_vel_to_real(self, sim_vels: np.ndarray) -> Dict[Tuple[int, int], float]:
|
||||
# 速度只受方向影响，不应用 offset。
|
||||
out: Dict[Tuple[int, int], float] = {}
|
||||
for i, jk in enumerate(self.SIM_JOINT_ORDER):
|
||||
out[self.CAN_ID_MAP[jk]] = float(self._sign[i] * sim_vels[i])
|
||||
return out
|
||||
|
||||
def real_to_sim(self, real_pos: Dict[Tuple[int, int], float]) -> np.ndarray:
|
||||
out = np.zeros(16, dtype=np.float32)
|
||||
for i, jk in enumerate(self.SIM_JOINT_ORDER):
|
||||
v = real_pos.get(self.CAN_ID_MAP[jk])
|
||||
if v is None:
|
||||
continue
|
||||
out[i] = (v - self._offset[i]) / self._sign[i]
|
||||
return self.canonicalize_sim(out)
|
||||
|
||||
def real_vel_to_sim(self, real_vel: Dict[Tuple[int, int], float]) -> np.ndarray:
|
||||
out = np.zeros(16, dtype=np.float32)
|
||||
for i, jk in enumerate(self.SIM_JOINT_ORDER):
|
||||
v = real_vel.get(self.CAN_ID_MAP[jk])
|
||||
if v is None:
|
||||
continue
|
||||
out[i] = v / self._sign[i]
|
||||
return out
|
||||
|
||||
def joint_name_at(self, idx: int) -> str:
|
||||
leg, joint = self.SIM_JOINT_ORDER[idx]
|
||||
return f"{leg}_{joint}_joint"
|
||||
@@ -0,0 +1,216 @@
|
||||
import time
|
||||
from typing import Callable, Dict, Tuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
from interface.imu_client import IMUClient
|
||||
from interface.motor_driver import HardwareIO
|
||||
from tools.math_utils import LowPassFilter, MahonyFilter, get_gravity_orientation
|
||||
|
||||
|
||||
def _quat_yaw_wxyz(quat) -> float:
|
||||
w, x, y, z = [float(v) for v in quat]
|
||||
return float(np.arctan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z)))
|
||||
|
||||
|
||||
def _wrap_pi(angle: float) -> float:
|
||||
return float((angle + np.pi) % (2.0 * np.pi) - np.pi)
|
||||
|
||||
|
||||
class OdomTracker:
|
||||
def __init__(self, jump_distance_m: float = 0.5, jump_yaw_rad: float = 0.8):
|
||||
self.jump_distance_m = float(jump_distance_m)
|
||||
self.jump_yaw_rad = float(jump_yaw_rad)
|
||||
self.origin_pos = None
|
||||
self.origin_yaw = 0.0
|
||||
self.last_local_pos = None
|
||||
self.last_local_yaw = 0.0
|
||||
|
||||
def reset(self):
|
||||
self.origin_pos = None
|
||||
self.origin_yaw = 0.0
|
||||
self.last_local_pos = None
|
||||
self.last_local_yaw = 0.0
|
||||
|
||||
def update(self, odom):
|
||||
if odom is None:
|
||||
return None
|
||||
pos = np.asarray(odom.get("pos", [0.0, 0.0, 0.0]), dtype=np.float32)
|
||||
yaw = _quat_yaw_wxyz(odom.get("quat_wxyz", [1.0, 0.0, 0.0, 0.0]))
|
||||
if self.origin_pos is None:
|
||||
self.origin_pos = pos.copy()
|
||||
self.origin_yaw = yaw
|
||||
local_pos = pos - self.origin_pos
|
||||
local_yaw = _wrap_pi(yaw - self.origin_yaw)
|
||||
jump_detected = False
|
||||
jump_distance = 0.0
|
||||
jump_yaw = 0.0
|
||||
if self.last_local_pos is not None:
|
||||
jump_distance = float(np.linalg.norm(local_pos[:2] - self.last_local_pos[:2]))
|
||||
jump_yaw = abs(_wrap_pi(local_yaw - self.last_local_yaw))
|
||||
jump_detected = jump_distance > self.jump_distance_m or jump_yaw > self.jump_yaw_rad
|
||||
self.last_local_pos = local_pos.copy()
|
||||
self.last_local_yaw = local_yaw
|
||||
tracked = dict(odom)
|
||||
tracked.update(
|
||||
{
|
||||
"local_pos": local_pos.tolist(),
|
||||
"local_yaw": local_yaw,
|
||||
"jump_detected": bool(jump_detected),
|
||||
"jump_distance_m": jump_distance,
|
||||
"jump_yaw_rad": jump_yaw,
|
||||
}
|
||||
)
|
||||
return tracked
|
||||
|
||||
|
||||
class RealIO:
|
||||
def __init__(
|
||||
self,
|
||||
driver_factory: Callable[[str, str, bool], Tuple[object, object]],
|
||||
motor_model: str,
|
||||
can1_port: str,
|
||||
can2_port: str,
|
||||
imu_lib_path: str,
|
||||
control_dt: float = 0.02,
|
||||
motor_dt: float = 0.005,
|
||||
kp_leg: float = 80.0,
|
||||
kd_leg: float = 2.5,
|
||||
hold_kp_leg: float | None = None,
|
||||
hold_kd_leg: float | None = None,
|
||||
kd_wheel: float = 2.0,
|
||||
debug: bool = False,
|
||||
dry_run: bool = False,
|
||||
):
|
||||
self.control_dt = control_dt
|
||||
self.motor_dt = motor_dt
|
||||
self.kp_leg = kp_leg
|
||||
self.kd_leg = kd_leg
|
||||
self.hold_kp_leg = kp_leg if hold_kp_leg is None else float(hold_kp_leg)
|
||||
self.hold_kd_leg = kd_leg if hold_kd_leg is None else float(hold_kd_leg)
|
||||
self.kd_wheel = kd_wheel
|
||||
|
||||
print("[RealIO] 初始化电机驱动...")
|
||||
self.hw = HardwareIO(driver_factory, motor_model, can1_port, can2_port, debug)
|
||||
print("[RealIO] 初始化 IMU...")
|
||||
self.imu = IMUClient(lib_path=imu_lib_path, dry_run=dry_run)
|
||||
|
||||
# 使用 motor_dt 初始化滤波器，因为它们都在 200Hz 电机控制循环中更新
|
||||
self.imu_filter = MahonyFilter(kp=2.0, ki=0.0, dt=motor_dt)
|
||||
self.quat_wxyz = np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float32)
|
||||
|
||||
self.lpf_legs = LowPassFilter(cutoff_freq=5.0, dt=motor_dt, dim=12)
|
||||
self.lpf_wheels = LowPassFilter(cutoff_freq=15.0, dt=motor_dt, dim=4)
|
||||
|
||||
self._last_imu_age_ms = -1.0
|
||||
self._last_imu_fresh = False
|
||||
self.odom_tracker = OdomTracker()
|
||||
self._last_read_time = None
|
||||
|
||||
def connect(self, imu_timeout_ms: int = 8000):
|
||||
self.hw.connect()
|
||||
self.imu.start(timeout_ms=imu_timeout_ms)
|
||||
if self.imu.initial_gravity is not None:
|
||||
self.imu_filter.reset_with_accel(self.imu.initial_gravity)
|
||||
self.quat_wxyz = self.imu_filter.q.copy()
|
||||
self.odom_tracker.reset()
|
||||
self._last_read_time = None
|
||||
|
||||
def disconnect(self):
|
||||
try:
|
||||
self.hw.disable_all()
|
||||
finally:
|
||||
self.imu.stop()
|
||||
self.hw.disconnect()
|
||||
|
||||
def enable_motors(self):
|
||||
self.hw.enable_all()
|
||||
|
||||
def disable_motors(self):
|
||||
self.hw.disable_all()
|
||||
|
||||
def damping_brake(self):
|
||||
self.hw.damping_brake(self.kd_leg, self.kd_wheel)
|
||||
|
||||
def wait_feedback_ready(self, max_attempts: int = 20, poll_interval: float = 0.05):
|
||||
return self.hw.wait_feedback_ready(max_attempts=max_attempts, poll_interval=poll_interval)
|
||||
|
||||
def read_measured_pose(self) -> np.ndarray:
|
||||
return self.hw.read_measured_pose()
|
||||
|
||||
def read_state(self) -> Dict[str, object]:
|
||||
joint_pos, joint_vel, joint_torque, motor_diag = self.hw.read_state()
|
||||
gyro, accel, age_ms, fresh = self.imu.get_latest()
|
||||
odom = self.odom_tracker.update(self.imu.get_latest_odom())
|
||||
self._last_imu_age_ms = age_ms
|
||||
self._last_imu_fresh = fresh
|
||||
|
||||
# 动态测量 dt，以适应 POLL (5Hz) 与 RUNTIME (200Hz) 的不同频率切换
|
||||
t_now = time.perf_counter()
|
||||
if self._last_read_time is not None:
|
||||
dt = t_now - self._last_read_time
|
||||
if dt <= 0.0 or dt > 0.5:
|
||||
dt = self.motor_dt
|
||||
else:
|
||||
dt = self.motor_dt
|
||||
self._last_read_time = t_now
|
||||
|
||||
self.quat_wxyz = self.imu_filter.update(accel, gyro, dt=dt)
|
||||
projected_gravity = get_gravity_orientation(self.quat_wxyz)
|
||||
|
||||
return {
|
||||
"joint_pos": joint_pos,
|
||||
"joint_vel": joint_vel,
|
||||
"joint_torque": joint_torque,
|
||||
"imu_gyro": gyro,
|
||||
"imu_accel": accel,
|
||||
"quat_wxyz": self.quat_wxyz.copy(),
|
||||
"projected_gravity": projected_gravity,
|
||||
"imu_age_ms": age_ms,
|
||||
"imu_fresh": fresh,
|
||||
"odom": odom,
|
||||
"motor_stale": motor_diag,
|
||||
}
|
||||
|
||||
def get_obs_policy(
|
||||
self,
|
||||
state: Dict[str, object],
|
||||
command: np.ndarray,
|
||||
default_dof_pos: np.ndarray,
|
||||
last_actions_raw: np.ndarray,
|
||||
) -> np.ndarray:
|
||||
gyro = state["imu_gyro"]
|
||||
joint_pos = state["joint_pos"]
|
||||
joint_vel = state["joint_vel"]
|
||||
projected_gravity = state["projected_gravity"]
|
||||
|
||||
base_ang_vel = (gyro * 0.25).astype(np.float32)
|
||||
joint_pos_rel = (joint_pos[:12] - default_dof_pos[:12]).astype(np.float32)
|
||||
joint_vel_leg = (joint_vel[:12] * 0.05).astype(np.float32)
|
||||
wheel_vel = (joint_vel[12:] * 0.05).astype(np.float32)
|
||||
|
||||
return np.concatenate(
|
||||
[
|
||||
base_ang_vel,
|
||||
projected_gravity,
|
||||
command.astype(np.float32),
|
||||
joint_pos_rel,
|
||||
joint_vel_leg,
|
||||
wheel_vel,
|
||||
last_actions_raw,
|
||||
]
|
||||
).astype(np.float32)
|
||||
|
||||
def send_actions(self, scaled_actions: np.ndarray, default_dof_pos: np.ndarray):
|
||||
act = (scaled_actions + default_dof_pos).astype(np.float32)
|
||||
act = np.clip(act, -100.0, 100.0)
|
||||
act[:12] = self.lpf_legs.filter(act[:12])
|
||||
act[12:] = self.lpf_wheels.filter(act[12:])
|
||||
self.hw.send_control(act, self.kp_leg, self.kd_leg, self.kd_wheel)
|
||||
return act
|
||||
|
||||
def hold_pose(self, sim_target_pose: np.ndarray, kp_scale: float = 1.0):
|
||||
target = np.clip(sim_target_pose.astype(np.float32), -100.0, 100.0)
|
||||
kp_scale = float(np.clip(kp_scale, 0.0, 1.0))
|
||||
self.hw.send_control(target, self.hold_kp_leg * kp_scale, self.hold_kd_leg, self.kd_wheel)
|
||||
return target
|
||||
@@ -0,0 +1,727 @@
|
||||
"""CLI entrypoint for current sim2real deployment."""
|
||||
|
||||
import argparse
|
||||
import os
|
||||
import sys
|
||||
import threading
|
||||
import time
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
import yaml
|
||||
|
||||
sys.path.insert(0, str(Path(__file__).resolve().parent))
|
||||
|
||||
from input_dev.keyboard import KeyboardCommandController
|
||||
from interface.real_io import RealIO
|
||||
from policy.policy_runner import PolicyRunner, resolve_policy_path
|
||||
from safety.runtime_guard import GuardLevel, RuntimeGuard
|
||||
from safety.safety_monitor import SafetyLevel, SafetyMonitor
|
||||
from startup.pose_initializer import PoseInitFailed, PoseInitializer, STAND_POSE
|
||||
from startup.stand_balance import StandBalanceController
|
||||
from tools.logger import LogBundle
|
||||
from tools.math_utils import get_gravity_orientation
|
||||
|
||||
JOINT_LABELS = LogBundle.JOINT_LABELS
|
||||
|
||||
|
||||
def make_real_driver_factory():
|
||||
sim2real_root = Path(__file__).resolve().parent
|
||||
workspace_root = sim2real_root.parent.parent
|
||||
for path in (workspace_root,):
|
||||
path_str = str(path)
|
||||
if path_str not in sys.path and path.exists():
|
||||
sys.path.append(path_str)
|
||||
|
||||
from dm_socket.sim2real_factory import driver_factory
|
||||
|
||||
return driver_factory
|
||||
|
||||
|
||||
def make_dry_driver_factory():
|
||||
class MockMotor:
|
||||
def __init__(self):
|
||||
class State:
|
||||
position = 0.0
|
||||
velocity = 0.0
|
||||
torque = 0.0
|
||||
|
||||
self.state = State()
|
||||
|
||||
class MockDriver:
|
||||
def __init__(self, port, debug):
|
||||
self.port = port
|
||||
self.motors = {}
|
||||
|
||||
def connect(self): ...
|
||||
def disconnect(self): ...
|
||||
def add_motor(self, name, motor_id, model): self.motors[name] = MockMotor()
|
||||
def enable(self, name): ...
|
||||
def disable(self, name): ...
|
||||
def clear_warnings(self, name): ...
|
||||
def process_messages(self): ...
|
||||
def control_mit(self, *args, **kwargs): ...
|
||||
|
||||
def factory(can1_port, can2_port, debug):
|
||||
return MockDriver(can1_port, debug), MockDriver(can2_port, debug)
|
||||
|
||||
return factory
|
||||
|
||||
|
||||
def _sleep_to(next_exec: float) -> float:
|
||||
slack = next_exec - time.perf_counter()
|
||||
if slack > 0:
|
||||
time.sleep(slack)
|
||||
return next_exec + 0.0
|
||||
return time.perf_counter()
|
||||
|
||||
|
||||
def build_action_diag(
|
||||
*,
|
||||
joint_pos: np.ndarray,
|
||||
default_pose: np.ndarray,
|
||||
raw: np.ndarray,
|
||||
scaled: np.ndarray,
|
||||
tentative: np.ndarray,
|
||||
cmd: np.ndarray,
|
||||
zero_command: bool,
|
||||
runtime_released: bool,
|
||||
release_alpha: float,
|
||||
safety_details: dict | None = None,
|
||||
) -> dict:
|
||||
details = dict(safety_details or {})
|
||||
joint_indices = list(details.get("joint_indices", []))
|
||||
pos_err = tentative - joint_pos
|
||||
leg_offset = tentative[:12] - default_pose[:12]
|
||||
diag = {
|
||||
"joint_indices": joint_indices,
|
||||
"joint_names": [JOINT_LABELS[i] for i in joint_indices if 0 <= i < len(JOINT_LABELS)],
|
||||
"cmd": cmd.tolist(),
|
||||
"zero_command": bool(zero_command),
|
||||
"runtime_released": bool(runtime_released),
|
||||
"release_alpha": float(release_alpha),
|
||||
"max_raw": float(np.max(np.abs(raw))) if raw.size else 0.0,
|
||||
"max_scaled": float(np.max(np.abs(scaled[:12]))) if scaled.size else 0.0,
|
||||
"max_target": float(np.max(np.abs(tentative[:12]))) if tentative.size else 0.0,
|
||||
}
|
||||
if joint_indices:
|
||||
primary = int(joint_indices[0])
|
||||
diag.update(
|
||||
{
|
||||
"primary_joint_index": primary,
|
||||
"primary_joint_name": JOINT_LABELS[primary],
|
||||
"primary_target": float(tentative[primary]),
|
||||
"primary_default": float(default_pose[primary]),
|
||||
"primary_measured": float(joint_pos[primary]),
|
||||
"primary_pos_err": float(pos_err[primary]),
|
||||
"primary_raw": float(raw[primary]),
|
||||
"primary_scaled": float(scaled[primary]),
|
||||
}
|
||||
)
|
||||
if primary < 12:
|
||||
diag["primary_leg_offset"] = float(leg_offset[primary])
|
||||
details.update(diag)
|
||||
return details
|
||||
|
||||
|
||||
def policy_release_cfg(cfg: dict) -> dict[str, float]:
|
||||
policy_cfg = cfg.get("policy", {})
|
||||
return {
|
||||
"command_hold_s": max(float(policy_cfg.get("release_command_hold_s", 0.12)), 0.0),
|
||||
"posture_max_err": max(float(policy_cfg.get("release_posture_max_err", 0.35)), 0.0),
|
||||
"target_blend_s": max(float(policy_cfg.get("release_target_blend_s", 0.30)), 1e-3),
|
||||
}
|
||||
|
||||
|
||||
def compute_release_metrics(runner: PolicyRunner, state: dict, hold_target: np.ndarray, cmd: np.ndarray) -> dict:
|
||||
joint_pos = np.asarray(state["joint_pos"], dtype=np.float32)
|
||||
default_pose = np.asarray(runner.default_dof_pos, dtype=np.float32)
|
||||
hold_target = np.asarray(hold_target, dtype=np.float32)
|
||||
planar_cmd, yaw_cmd = runner.command_activation_metrics(cmd)
|
||||
return {
|
||||
"planar_cmd": float(planar_cmd),
|
||||
"yaw_cmd": float(yaw_cmd),
|
||||
"max_hold_err": float(np.max(np.abs(joint_pos[:12] - hold_target[:12]))),
|
||||
"max_default_err": float(np.max(np.abs(joint_pos[:12] - default_pose[:12]))),
|
||||
"max_hold_default_gap": float(np.max(np.abs(hold_target[:12] - default_pose[:12]))),
|
||||
}
|
||||
|
||||
|
||||
def blend_runtime_target(
|
||||
runner: PolicyRunner,
|
||||
hold_target: np.ndarray,
|
||||
policy_target: np.ndarray,
|
||||
release_alpha: float,
|
||||
target_blend_s: float,
|
||||
control_dt: float,
|
||||
) -> np.ndarray:
|
||||
blend = min(1.0, release_alpha * (runner.command_release_s / max(target_blend_s, control_dt)))
|
||||
return ((1.0 - blend) * hold_target + blend * policy_target).astype(np.float32)
|
||||
|
||||
|
||||
def compute_target_error_metrics(
|
||||
state: dict,
|
||||
hold_target: np.ndarray,
|
||||
policy_target: np.ndarray,
|
||||
) -> dict[str, float]:
|
||||
joint_pos = np.asarray(state["joint_pos"], dtype=np.float32)
|
||||
hold_target = np.asarray(hold_target, dtype=np.float32)
|
||||
policy_target = np.asarray(policy_target, dtype=np.float32)
|
||||
return {
|
||||
"hold_target_max_err": float(np.max(np.abs(joint_pos[:12] - hold_target[:12]))),
|
||||
"policy_target_max_err": float(np.max(np.abs(joint_pos[:12] - policy_target[:12]))),
|
||||
"hold_policy_max_gap": float(np.max(np.abs(hold_target[:12] - policy_target[:12]))),
|
||||
}
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("--config", default=str(Path(__file__).parent / "config.yaml"))
|
||||
parser.add_argument("--policy", default=None)
|
||||
parser.add_argument("--dry-run", action="store_true")
|
||||
args = parser.parse_args()
|
||||
|
||||
with open(args.config, "r", encoding="utf-8") as file_obj:
|
||||
cfg = yaml.safe_load(file_obj)
|
||||
|
||||
sim2real_root = Path(__file__).resolve().parent
|
||||
policy_path = resolve_policy_path(args.policy, sim2real_root)
|
||||
if not policy_path.exists():
|
||||
print(f"[Main] policy not found: {policy_path}")
|
||||
sys.exit(1)
|
||||
|
||||
control_dt = 1.0 / float(cfg["control_freq"])
|
||||
driver_factory = make_dry_driver_factory() if args.dry_run else make_real_driver_factory()
|
||||
|
||||
logger = LogBundle(cfg["log_dir"])
|
||||
logger.event(
|
||||
"CONFIG_LOADED",
|
||||
config_path=args.config,
|
||||
policy=str(policy_path),
|
||||
dry_run=args.dry_run,
|
||||
control_freq=cfg["control_freq"],
|
||||
motor_model=cfg["motor_model"],
|
||||
)
|
||||
|
||||
io = RealIO(
|
||||
driver_factory=driver_factory,
|
||||
motor_model=cfg["motor_model"],
|
||||
can1_port=cfg["can1_port"],
|
||||
can2_port=cfg["can2_port"],
|
||||
imu_lib_path=cfg.get("imu_lib_path"),
|
||||
control_dt=control_dt,
|
||||
kp_leg=cfg["controller"]["kp_leg"],
|
||||
kd_leg=cfg["controller"]["kd_leg"],
|
||||
hold_kp_leg=cfg["controller"].get("hold_kp_leg", cfg["controller"]["kp_leg"]),
|
||||
hold_kd_leg=cfg["controller"].get("hold_kd_leg", cfg["controller"]["kd_leg"]),
|
||||
kd_wheel=cfg["controller"]["kd_wheel"],
|
||||
debug=cfg.get("debug", False),
|
||||
)
|
||||
runner = PolicyRunner(
|
||||
policy_path,
|
||||
enable_zero_cmd_suppression=cfg.get("policy", {}).get("enable_zero_cmd_suppression", True),
|
||||
hold_zero_command_pose=cfg.get("policy", {}).get("hold_zero_command_pose", True),
|
||||
command_release_s=cfg.get("policy", {}).get("command_release_s", 0.35),
|
||||
action_scale=np.asarray(
|
||||
cfg.get("policy", {}).get(
|
||||
"action_scale",
|
||||
[0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 5.0, 5.0, 5.0, 5.0],
|
||||
),
|
||||
dtype=np.float32,
|
||||
),
|
||||
zero_cmd_use_yaw_rate=cfg.get("policy", {}).get("zero_cmd_use_yaw_rate", False),
|
||||
clip_obs=cfg.get("policy", {}).get("clip_obs", 100.0),
|
||||
)
|
||||
require_active_command = cfg.get("policy", {}).get("require_active_command_to_release", True)
|
||||
keyboard = KeyboardCommandController(
|
||||
max_x_vel=cfg["controller"]["max_vx"],
|
||||
max_y_vel=cfg["controller"]["max_vy"],
|
||||
max_yaw_vel=cfg["controller"]["max_yaw_rate"],
|
||||
)
|
||||
safety = SafetyMonitor(
|
||||
max_target_offset=cfg["safety"]["max_target_offset"],
|
||||
max_ang_vel=cfg["safety"]["max_ang_vel"],
|
||||
max_tilt_z=cfg["safety"]["max_tilt_z"],
|
||||
clip_to_brake=cfg["safety"].get("clip_to_brake", 0),
|
||||
hard_target_offset=cfg["safety"].get("hard_target_offset", 1.2),
|
||||
)
|
||||
safety.reset()
|
||||
guard = RuntimeGuard(
|
||||
max_ang_vel=cfg["safety"]["max_ang_vel"],
|
||||
max_tilt_z=cfg["safety"]["max_tilt_z"],
|
||||
imu_age_warn_ms=cfg["safety"].get("imu_age_warn_ms", 60.0),
|
||||
imu_age_stop_ms=cfg["safety"].get("imu_age_stop_ms", 200.0),
|
||||
)
|
||||
initializer = PoseInitializer(
|
||||
io,
|
||||
control_dt=control_dt,
|
||||
transition_time_min=cfg["startup"].get("transition_time_min", 2.0),
|
||||
transition_time_max=cfg["startup"].get("transition_time_max", 6.0),
|
||||
transition_seconds_per_rad=cfg["startup"].get("transition_seconds_per_rad", 1.5),
|
||||
hold_time=cfg["startup"]["hold_time"],
|
||||
settle_pos_threshold=cfg["startup"]["settle_pos_threshold"],
|
||||
settle_vel_threshold=cfg["startup"]["settle_vel_threshold"],
|
||||
timeout_extra=cfg["startup"].get("timeout_extra", 3.0),
|
||||
imu_fresh_wait_s=cfg["startup"].get("imu_fresh_wait_s", 1.0),
|
||||
progress_log_interval=cfg["startup"]["progress_log_interval"],
|
||||
ramp_kp_time=cfg["startup"].get("ramp_kp_time", 1.0),
|
||||
soft_hold_duration=cfg["startup"].get("soft_hold_duration", 1.0),
|
||||
max_dev_warn=cfg["startup"].get("max_dev_warn", 1.5),
|
||||
max_dev_abort=cfg["startup"].get("max_dev_abort", 3.0),
|
||||
)
|
||||
initializer.attach(logger=logger, guard=guard, keyboard=keyboard)
|
||||
stand_balance = StandBalanceController(cfg.get("stand_balance", {}), control_dt=control_dt)
|
||||
|
||||
print("\n[Main] connecting hardware...")
|
||||
keyboard.start()
|
||||
try:
|
||||
io.connect()
|
||||
logger.event("CAN_IMU_CONNECTED", initial_gravity=io.imu.initial_gravity)
|
||||
except Exception as exc:
|
||||
logger.event("HARDWARE_CONNECT_FAILED", error=str(exc))
|
||||
keyboard.stop()
|
||||
logger.close()
|
||||
raise
|
||||
|
||||
try:
|
||||
io.enable_motors()
|
||||
logger.event("MOTORS_ENABLED")
|
||||
time.sleep(0.5)
|
||||
|
||||
target_pose = initializer.transition_to_stand_from_current(target_pose=STAND_POSE) if cfg["startup"]["enabled"] else STAND_POSE.copy()
|
||||
|
||||
if stand_balance.enabled:
|
||||
logger.event("STAND_BALANCE_BEGIN")
|
||||
print("[Main] waiting for stand-balance to settle...")
|
||||
stand_balance.reset()
|
||||
next_exec = time.perf_counter()
|
||||
while True:
|
||||
state = io.read_state()
|
||||
target_pose = stand_balance.compute_target(state, np.zeros(3, dtype=np.float32))
|
||||
io.hold_pose(target_pose, kp_scale=1.0)
|
||||
debug = stand_balance.last_debug
|
||||
if stand_balance.is_stable():
|
||||
logger.event(
|
||||
"STAND_BALANCE_STABLE",
|
||||
roll_deg=float(np.degrees(debug.roll)),
|
||||
pitch_deg=float(np.degrees(debug.pitch)),
|
||||
pitch_corr=float(debug.pitch_corr),
|
||||
pitch_compensation_enabled=bool(debug.pitch_compensation_enabled),
|
||||
)
|
||||
break
|
||||
next_exec += control_dt
|
||||
next_exec = _sleep_to(next_exec)
|
||||
logger.event("STAND_BALANCE_END")
|
||||
|
||||
if cfg["startup"]["require_user_confirm"]:
|
||||
print("[Main] standing complete. Press Enter to release policy control...")
|
||||
done = threading.Event()
|
||||
|
||||
def _wait():
|
||||
try:
|
||||
input()
|
||||
except EOFError:
|
||||
pass
|
||||
done.set()
|
||||
|
||||
threading.Thread(target=_wait, daemon=True).start()
|
||||
if not initializer.hold_until_user_confirm(target_pose, done):
|
||||
raise PoseInitFailed("WAIT_USER interrupted")
|
||||
|
||||
print("[Main] priming current observation...")
|
||||
logger.event("PRIME_BEGIN")
|
||||
zero_cmd = np.zeros(3, dtype=np.float32)
|
||||
next_exec = time.perf_counter()
|
||||
for index in range(1):
|
||||
if stand_balance.enabled:
|
||||
state = io.read_state()
|
||||
target_pose = stand_balance.compute_target(state, zero_cmd)
|
||||
io.hold_pose(target_pose, kp_scale=1.0)
|
||||
else:
|
||||
io.hold_pose(target_pose, kp_scale=1.0)
|
||||
state = io.read_state()
|
||||
obs = io.get_obs_policy(state, zero_cmd, runner.default_dof_pos, runner.last_actions)
|
||||
if index == 0:
|
||||
runner.reset(prime_obs=obs)
|
||||
logger.state(
|
||||
phase="PRIME",
|
||||
joint_pos=state["joint_pos"],
|
||||
joint_vel=state["joint_vel"],
|
||||
joint_torque=state.get("joint_torque", np.zeros(16, dtype=np.float32)),
|
||||
target_pose=target_pose,
|
||||
raw_action=None,
|
||||
gyro=state["imu_gyro"],
|
||||
accel=state["imu_accel"],
|
||||
quat=state["quat_wxyz"],
|
||||
proj_gravity=state["projected_gravity"],
|
||||
command=zero_cmd,
|
||||
imu_age_ms=float(state["imu_age_ms"]),
|
||||
loop_dt_ms=0.0,
|
||||
kp_scale=1.0,
|
||||
)
|
||||
next_exec += control_dt
|
||||
next_exec = _sleep_to(next_exec)
|
||||
logger.event("PRIME_END")
|
||||
|
||||
print("[Main] entering 50Hz control loop... (space = estop)")
|
||||
logger.event("RUNTIME_BEGIN")
|
||||
next_exec = time.perf_counter()
|
||||
loop_count = 0
|
||||
last_print = next_exec
|
||||
log_every = int(cfg.get("log_every", 1))
|
||||
recent_dt_ms = []
|
||||
runtime_released = not require_active_command
|
||||
release_cfg = policy_release_cfg(cfg)
|
||||
release_active_time = 0.0
|
||||
|
||||
while True:
|
||||
loop_t0 = time.perf_counter()
|
||||
cmd = keyboard.get_command()
|
||||
state = io.read_state()
|
||||
obs = io.get_obs_policy(state, cmd, runner.default_dof_pos, runner.last_actions)
|
||||
zero_command = runner._is_zero_command(cmd, state["imu_gyro"])
|
||||
|
||||
obs_nan = bool(np.any(np.isnan(obs)) or np.any(np.isinf(obs)))
|
||||
if obs_nan:
|
||||
logger.event("OBS_NAN", obs_max=float(np.nanmax(obs)))
|
||||
io.damping_brake()
|
||||
break
|
||||
|
||||
if not runtime_released and zero_command:
|
||||
raw = np.zeros(16, dtype=np.float32)
|
||||
scaled = np.zeros(16, dtype=np.float32)
|
||||
target_hold = stand_balance.compute_target(state, np.zeros(3, dtype=np.float32)) if stand_balance.enabled else runner.default_dof_pos.copy()
|
||||
actual_target = io.hold_pose(target_hold, kp_scale=1.0)
|
||||
policy_target = runner.default_dof_pos.copy()
|
||||
release_metrics = compute_release_metrics(runner, state, target_hold, cmd)
|
||||
target_metrics = compute_target_error_metrics(state, target_hold, policy_target)
|
||||
release_active_time = 0.0
|
||||
safety_decision = SafetyMonitor().check(
|
||||
target_pose=target_hold,
|
||||
default_pose=runner.default_dof_pos,
|
||||
imu_gyro=state["imu_gyro"],
|
||||
projected_gravity=state["projected_gravity"],
|
||||
estop_triggered=keyboard.is_estop_triggered(),
|
||||
)
|
||||
guard_decision = guard.check(
|
||||
imu_gyro=state["imu_gyro"],
|
||||
projected_gravity=state["projected_gravity"],
|
||||
imu_age_ms=float(state["imu_age_ms"]),
|
||||
estop_triggered=keyboard.is_estop_triggered(),
|
||||
extra_nan_arrays=(target_hold,),
|
||||
)
|
||||
else:
|
||||
target_hold = stand_balance.compute_target(state, np.zeros(3, dtype=np.float32)) if stand_balance.enabled else runner.default_dof_pos.copy()
|
||||
release_metrics = compute_release_metrics(runner, state, target_hold, cmd)
|
||||
if not runtime_released:
|
||||
release_active_time += control_dt if runner.is_command_active(cmd) else 0.0
|
||||
active_ready = release_active_time >= release_cfg["command_hold_s"]
|
||||
posture_ready = release_metrics["max_hold_err"] <= release_cfg["posture_max_err"]
|
||||
if active_ready and posture_ready:
|
||||
runtime_released = True
|
||||
logger.event(
|
||||
"RUNTIME_COMMAND_RELEASED",
|
||||
cmd=cmd.tolist(),
|
||||
active_hold_s=release_active_time,
|
||||
max_hold_err=release_metrics["max_hold_err"],
|
||||
max_default_err=release_metrics["max_default_err"],
|
||||
max_hold_default_gap=release_metrics["max_hold_default_gap"],
|
||||
)
|
||||
else:
|
||||
reasons = []
|
||||
if not active_ready:
|
||||
reasons.append(f"cmd_hold<{release_cfg['command_hold_s']:.2f}s")
|
||||
if not posture_ready:
|
||||
reasons.append(f"hold_err>{release_cfg['posture_max_err']:.3f}")
|
||||
logger.event(
|
||||
"RUNTIME_RELEASE_BLOCKED",
|
||||
reason=",".join(reasons),
|
||||
cmd=cmd.tolist(),
|
||||
active_hold_s=release_active_time,
|
||||
max_hold_err=release_metrics["max_hold_err"],
|
||||
max_default_err=release_metrics["max_default_err"],
|
||||
max_hold_default_gap=release_metrics["max_hold_default_gap"],
|
||||
)
|
||||
raw = np.zeros(16, dtype=np.float32)
|
||||
scaled = np.zeros(16, dtype=np.float32)
|
||||
actual_target = io.hold_pose(target_hold, kp_scale=1.0)
|
||||
policy_target = runner.default_dof_pos.copy()
|
||||
target_metrics = compute_target_error_metrics(state, target_hold, policy_target)
|
||||
safety_decision = SafetyMonitor().check(
|
||||
target_pose=target_hold,
|
||||
default_pose=runner.default_dof_pos,
|
||||
imu_gyro=state["imu_gyro"],
|
||||
projected_gravity=state["projected_gravity"],
|
||||
estop_triggered=keyboard.is_estop_triggered(),
|
||||
)
|
||||
guard_decision = guard.check(
|
||||
imu_gyro=state["imu_gyro"],
|
||||
projected_gravity=state["projected_gravity"],
|
||||
imu_age_ms=float(state["imu_age_ms"]),
|
||||
estop_triggered=keyboard.is_estop_triggered(),
|
||||
extra_nan_arrays=(target_hold,),
|
||||
)
|
||||
loop_dt_ms = (time.perf_counter() - loop_t0) * 1000.0
|
||||
if log_every and (loop_count % log_every == 0):
|
||||
motor_diag = state.get("motor_stale", {})
|
||||
logger.state(
|
||||
phase="RUNTIME",
|
||||
joint_pos=state["joint_pos"],
|
||||
joint_vel=state["joint_vel"],
|
||||
joint_torque=state.get("joint_torque", np.zeros(16, dtype=np.float32)),
|
||||
target_pose=actual_target,
|
||||
raw_action=raw,
|
||||
gyro=state["imu_gyro"],
|
||||
accel=state["imu_accel"],
|
||||
quat=state["quat_wxyz"],
|
||||
proj_gravity=state["projected_gravity"],
|
||||
command=cmd,
|
||||
imu_age_ms=float(state["imu_age_ms"]),
|
||||
loop_dt_ms=loop_dt_ms,
|
||||
safety_level=int(safety_decision.level),
|
||||
guard_level=int(guard_decision.level),
|
||||
holdover=int(motor_diag.get("holdover_this_frame", 0)),
|
||||
stale_max=int(motor_diag.get("stale_max", 0)),
|
||||
fresh_count=int(motor_diag.get("fresh_count", 16)),
|
||||
kp_scale=1.0,
|
||||
nan_flag=0,
|
||||
kp_leg_cmd=float(io.kp_leg),
|
||||
kd_leg_cmd=float(io.kd_leg),
|
||||
kd_wheel_cmd=float(io.kd_wheel),
|
||||
runtime_release_alpha=0.0,
|
||||
runtime_release_hold_s=release_active_time,
|
||||
runtime_blend_ratio=0.0,
|
||||
hold_target_max_err=target_metrics["hold_target_max_err"],
|
||||
policy_target_max_err=target_metrics["policy_target_max_err"],
|
||||
hold_policy_max_gap=target_metrics["hold_policy_max_gap"],
|
||||
target_source="runtime_hold",
|
||||
clip_primary_joint="",
|
||||
safety_reason=f"release_blocked:{','.join(reasons)}",
|
||||
guard_reason=guard_decision.reason,
|
||||
)
|
||||
next_exec += control_dt
|
||||
next_exec = _sleep_to(next_exec)
|
||||
loop_count += 1
|
||||
continue
|
||||
scaled, raw = runner.step(obs)
|
||||
act_nan = bool(np.any(np.isnan(raw)) or np.any(np.isinf(raw)))
|
||||
if act_nan:
|
||||
logger.event("ACTION_NAN")
|
||||
io.damping_brake()
|
||||
break
|
||||
|
||||
policy_target = (scaled + runner.default_dof_pos).astype(np.float32)
|
||||
tentative = blend_runtime_target(
|
||||
runner,
|
||||
target_hold,
|
||||
policy_target,
|
||||
float(getattr(runner, "_command_release_alpha", 0.0)),
|
||||
release_cfg["target_blend_s"],
|
||||
control_dt,
|
||||
)
|
||||
scaled = tentative - runner.default_dof_pos
|
||||
target_metrics = compute_target_error_metrics(state, target_hold, policy_target)
|
||||
runtime_blend_ratio = min(
|
||||
1.0,
|
||||
float(getattr(runner, "_command_release_alpha", 0.0))
|
||||
* (runner.command_release_s / max(release_cfg["target_blend_s"], control_dt)),
|
||||
)
|
||||
projected_gravity = get_gravity_orientation(state["quat_wxyz"])
|
||||
|
||||
guard_decision = guard.check(
|
||||
imu_gyro=state["imu_gyro"],
|
||||
projected_gravity=projected_gravity,
|
||||
imu_age_ms=float(state["imu_age_ms"]),
|
||||
estop_triggered=keyboard.is_estop_triggered(),
|
||||
extra_nan_arrays=(raw, tentative),
|
||||
)
|
||||
if guard_decision.level == GuardLevel.STOP:
|
||||
logger.event("GUARD_STOP", phase="RUNTIME", reason=guard_decision.reason)
|
||||
io.damping_brake()
|
||||
break
|
||||
|
||||
safety_decision = safety.check(
|
||||
target_pose=tentative,
|
||||
default_pose=runner.default_dof_pos,
|
||||
imu_gyro=state["imu_gyro"],
|
||||
projected_gravity=projected_gravity,
|
||||
estop_triggered=keyboard.is_estop_triggered(),
|
||||
)
|
||||
if safety_decision.level == SafetyLevel.ESTOP:
|
||||
logger.event("SAFETY_ESTOP", reason=safety_decision.message)
|
||||
io.damping_brake()
|
||||
break
|
||||
if safety_decision.level == SafetyLevel.BRAKE:
|
||||
safety_diag = build_action_diag(
|
||||
joint_pos=state["joint_pos"],
|
||||
default_pose=runner.default_dof_pos,
|
||||
raw=raw,
|
||||
scaled=scaled,
|
||||
tentative=tentative,
|
||||
cmd=cmd,
|
||||
zero_command=zero_command,
|
||||
runtime_released=runtime_released,
|
||||
release_alpha=float(getattr(runner, "_command_release_alpha", 0.0)),
|
||||
safety_details=safety_decision.details,
|
||||
)
|
||||
logger.event(
|
||||
"SAFETY_BRAKE",
|
||||
reason=safety_decision.message,
|
||||
details=safety_diag,
|
||||
primary_joint=safety_diag.get("primary_joint_name"),
|
||||
primary_offset=safety_diag.get("primary_leg_offset"),
|
||||
primary_target=safety_diag.get("primary_target"),
|
||||
primary_measured=safety_diag.get("primary_measured"),
|
||||
primary_raw=safety_diag.get("primary_raw"),
|
||||
primary_scaled=safety_diag.get("primary_scaled"),
|
||||
cmd=cmd.tolist(),
|
||||
release_alpha=float(getattr(runner, "_command_release_alpha", 0.0)),
|
||||
)
|
||||
io.damping_brake()
|
||||
break
|
||||
if safety_decision.level == SafetyLevel.CLIP and safety_decision.clipped_target is not None:
|
||||
scaled = safety_decision.clipped_target - runner.default_dof_pos
|
||||
safety_diag = build_action_diag(
|
||||
joint_pos=state["joint_pos"],
|
||||
default_pose=runner.default_dof_pos,
|
||||
raw=raw,
|
||||
scaled=scaled,
|
||||
tentative=tentative,
|
||||
cmd=cmd,
|
||||
zero_command=zero_command,
|
||||
runtime_released=runtime_released,
|
||||
release_alpha=float(getattr(runner, "_command_release_alpha", 0.0)),
|
||||
safety_details=safety_decision.details,
|
||||
)
|
||||
logger.event(
|
||||
"SAFETY_CLIP",
|
||||
reason=safety_decision.message,
|
||||
details=safety_diag,
|
||||
primary_joint=safety_diag.get("primary_joint_name"),
|
||||
primary_offset=safety_diag.get("primary_leg_offset"),
|
||||
primary_target=safety_diag.get("primary_target"),
|
||||
primary_measured=safety_diag.get("primary_measured"),
|
||||
primary_raw=safety_diag.get("primary_raw"),
|
||||
primary_scaled=safety_diag.get("primary_scaled"),
|
||||
max_raw=float(np.max(np.abs(raw))),
|
||||
cmd=cmd.tolist(),
|
||||
release_alpha=float(getattr(runner, "_command_release_alpha", 0.0)),
|
||||
)
|
||||
|
||||
actual_target = io.send_actions(scaled, runner.default_dof_pos)
|
||||
loop_dt_ms = (time.perf_counter() - loop_t0) * 1000.0
|
||||
|
||||
if log_every and (loop_count % log_every == 0):
|
||||
motor_diag = state.get("motor_stale", {})
|
||||
logger.state(
|
||||
phase="RUNTIME",
|
||||
joint_pos=state["joint_pos"],
|
||||
joint_vel=state["joint_vel"],
|
||||
joint_torque=state.get("joint_torque", np.zeros(16, dtype=np.float32)),
|
||||
target_pose=actual_target,
|
||||
raw_action=raw,
|
||||
gyro=state["imu_gyro"],
|
||||
accel=state["imu_accel"],
|
||||
quat=state["quat_wxyz"],
|
||||
proj_gravity=projected_gravity,
|
||||
command=cmd,
|
||||
imu_age_ms=float(state["imu_age_ms"]),
|
||||
loop_dt_ms=loop_dt_ms,
|
||||
safety_level=int(safety_decision.level),
|
||||
guard_level=int(guard_decision.level),
|
||||
holdover=int(motor_diag.get("holdover_this_frame", 0)),
|
||||
stale_max=int(motor_diag.get("stale_max", 0)),
|
||||
fresh_count=int(motor_diag.get("fresh_count", 16)),
|
||||
kp_scale=1.0,
|
||||
nan_flag=int(obs_nan or act_nan),
|
||||
kp_leg_cmd=float(io.kp_leg),
|
||||
kd_leg_cmd=float(io.kd_leg),
|
||||
kd_wheel_cmd=float(io.kd_wheel),
|
||||
runtime_release_alpha=float(getattr(runner, "_command_release_alpha", 0.0)),
|
||||
runtime_release_hold_s=release_active_time,
|
||||
runtime_blend_ratio=runtime_blend_ratio,
|
||||
hold_target_max_err=target_metrics["hold_target_max_err"],
|
||||
policy_target_max_err=target_metrics["policy_target_max_err"],
|
||||
hold_policy_max_gap=target_metrics["hold_policy_max_gap"],
|
||||
target_source="runtime_blend" if runtime_blend_ratio < 0.999 else "runtime_policy",
|
||||
clip_primary_joint=str((safety_decision.details or {}).get("primary_joint_name", "")),
|
||||
clip_primary_target=float((safety_decision.details or {}).get("primary_target", 0.0) or 0.0),
|
||||
clip_primary_measured=float((safety_decision.details or {}).get("primary_measured", 0.0) or 0.0),
|
||||
clip_primary_default=float((safety_decision.details or {}).get("primary_default", 0.0) or 0.0),
|
||||
clip_primary_pos_err=float((safety_decision.details or {}).get("primary_pos_err", 0.0) or 0.0),
|
||||
clip_primary_raw=float((safety_decision.details or {}).get("primary_raw", 0.0) or 0.0),
|
||||
clip_primary_scaled=float((safety_decision.details or {}).get("primary_scaled", 0.0) or 0.0),
|
||||
safety_reason=(
|
||||
f"{safety_decision.message};zero_cmd={int(zero_command)};"
|
||||
f"released={int(runtime_released)};alpha={getattr(runner, '_command_release_alpha', 0.0):.2f};"
|
||||
f"max_raw={float(np.max(np.abs(raw))):.2f};"
|
||||
f"clip={((safety_decision.details or {}).get('joint_indices', []))}"
|
||||
),
|
||||
guard_reason=guard_decision.reason,
|
||||
)
|
||||
|
||||
next_exec += control_dt
|
||||
slack = next_exec - time.perf_counter()
|
||||
if slack > 0:
|
||||
coarse = slack - 0.002
|
||||
if coarse > 0:
|
||||
time.sleep(coarse)
|
||||
while time.perf_counter() < next_exec:
|
||||
pass
|
||||
elif slack < -control_dt:
|
||||
logger.event("LOOP_OVERRUN", over_ms=-slack * 1000.0)
|
||||
next_exec = time.perf_counter()
|
||||
|
||||
recent_dt_ms.append(loop_dt_ms)
|
||||
if len(recent_dt_ms) > 50:
|
||||
recent_dt_ms.pop(0)
|
||||
if len(recent_dt_ms) == 50:
|
||||
median_dt = float(np.median(recent_dt_ms))
|
||||
if median_dt > 22.0:
|
||||
logger.event("SLOW_LOOP_TREND", median_dt_ms=median_dt)
|
||||
recent_dt_ms.clear()
|
||||
|
||||
loop_count += 1
|
||||
if time.perf_counter() - last_print > 1.0:
|
||||
print(
|
||||
f"[Loop] cmd=[{cmd[0]:+.2f},{cmd[1]:+.2f},{cmd[2]:+.2f}] "
|
||||
f"|raw|={float(np.max(np.abs(raw))):.2f} "
|
||||
f"zero={int(zero_command)} rel={int(runtime_released)} "
|
||||
f"alpha={getattr(runner, '_command_release_alpha', 0.0):.2f} "
|
||||
f"imu_age={state['imu_age_ms']:.1f}ms "
|
||||
f"holdover={io.hw.holdover_total} "
|
||||
f"safety={int(safety_decision.level)}"
|
||||
)
|
||||
last_print = time.perf_counter()
|
||||
|
||||
except PoseInitFailed as exc:
|
||||
print(f"[Main] startup aborted: {exc}")
|
||||
logger.event("POSE_INIT_FAILED", error=str(exc))
|
||||
except KeyboardInterrupt:
|
||||
print("\n[Main] Ctrl+C received, stopping...")
|
||||
logger.event("KEYBOARD_INTERRUPT")
|
||||
except Exception as exc:
|
||||
import traceback
|
||||
|
||||
print(f"\n[Main] exception: {exc}")
|
||||
traceback.print_exc()
|
||||
logger.event("UNEXPECTED_ERROR", error=str(exc), traceback=traceback.format_exc())
|
||||
finally:
|
||||
print("[Main] cleaning up...")
|
||||
try:
|
||||
io.damping_brake()
|
||||
time.sleep(0.05)
|
||||
logger.event("DAMPING_BRAKE_APPLIED")
|
||||
except Exception as exc:
|
||||
logger.event("DAMPING_BRAKE_FAILED", error=str(exc))
|
||||
try:
|
||||
io.disconnect()
|
||||
logger.event("HARDWARE_DISCONNECTED")
|
||||
finally:
|
||||
keyboard.stop()
|
||||
logger.close()
|
||||
os._exit(0)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,22 @@
|
||||
<mujoco model="wheelleg_scene">
|
||||
<include file="wheelleg.xml"/>
|
||||
|
||||
<option timestep="0.002" gravity="0 0 -9.81" integrator="implicitfast"/>
|
||||
|
||||
<visual>
|
||||
<headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3"/>
|
||||
<global azimuth="120" elevation="-20"/>
|
||||
</visual>
|
||||
|
||||
<asset>
|
||||
<texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072"/>
|
||||
<texture type="2d" name="groundplane" builtin="checker" mark="edge"
|
||||
rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3" markrgb="0.8 0.8 0.8" width="300" height="300"/>
|
||||
<material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2"/>
|
||||
</asset>
|
||||
|
||||
<worldbody>
|
||||
<light pos="0 0 3" dir="0 0 -1" directional="true"/>
|
||||
<geom name="floor" size="0 0 0.05" type="plane" material="groundplane" friction="0.8 0.05 0.01"/>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
@@ -0,0 +1,157 @@
|
||||
<mujoco model="wheelleg">
|
||||
<compiler angle="radian" meshdir="meshes/"/>
|
||||
|
||||
<default>
|
||||
<geom margin="0"/>
|
||||
</default>
|
||||
<asset>
|
||||
<mesh name="base_link" content_type="model/stl" file="base_link.STL"/>
|
||||
<mesh name="fl_hip_abduction_Link" content_type="model/stl" file="fl_hip_abduction_Link.STL"/>
|
||||
<mesh name="fl_hip_pitch_Link" content_type="model/stl" file="fl_hip_pitch_Link.STL"/>
|
||||
<mesh name="fl_knee_Link" content_type="model/stl" file="fl_knee_Link.STL"/>
|
||||
<mesh name="fl_wheel_Link" content_type="model/stl" file="fl_wheel_Link.STL"/>
|
||||
<mesh name="fr_hip_abduction_Link" content_type="model/stl" file="fr_hip_abduction_Link.STL"/>
|
||||
<mesh name="fr_hip_pitch_Link" content_type="model/stl" file="fr_hip_pitch_Link.STL"/>
|
||||
<mesh name="fr_knee_Link" content_type="model/stl" file="fr_knee_Link.STL"/>
|
||||
<mesh name="fr_wheel_Link" content_type="model/stl" file="fr_wheel_Link.STL"/>
|
||||
<mesh name="rl_hip_abduction_Link" content_type="model/stl" file="rl_hip_abduction_Link.STL"/>
|
||||
<mesh name="rl_hip_pitch_Link" content_type="model/stl" file="rl_hip_pitch_Link.STL"/>
|
||||
<mesh name="rl_knee_Link" content_type="model/stl" file="rl_knee_Link.STL"/>
|
||||
<mesh name="rl_wheel_Link" content_type="model/stl" file="rl_wheel_Link.STL"/>
|
||||
<mesh name="rr_hip_abduction_Link" content_type="model/stl" file="rr_hip_abduction_Link.STL"/>
|
||||
<mesh name="rr_hip_pitch_Link" content_type="model/stl" file="rr_hip_pitch_Link.STL"/>
|
||||
<mesh name="rr_knee_Link" content_type="model/stl" file="rr_knee_Link.STL"/>
|
||||
<mesh name="rr_wheel_Link" content_type="model/stl" file="rr_wheel_Link.STL"/>
|
||||
</asset>
|
||||
|
||||
<worldbody>
|
||||
<body name="base_link">
|
||||
<inertial pos="0.1517 0.0002 0.0542" mass="3.5" diaginertia="0.0215 0.0904 0.0985"/>
|
||||
<joint type="free"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="base_link"/>
|
||||
<geom size="0.178 0.1175 0.073" pos="0.1518 0 0.054" type="box" rgba="0.75294 0.75294 0.75294 1"/>
|
||||
<body name="fl_hip_abduction_Link" pos="0.32826 0.066172 0.053981">
|
||||
<inertial pos="0.0488 -0.0026 0.0007" mass="0.5" diaginertia="0.0003 0.0006 0.0005"/>
|
||||
<joint name="fl_hip_abduction_joint" pos="0 0 0" axis="1 0 0" range="-0.436 0.611" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="fl_hip_abduction_Link"/>
|
||||
<body name="fl_hip_pitch_Link" pos="0.06389 -0.027344 0.00010727" quat="0.999997 -0.0025023 0 0">
|
||||
<inertial pos="0.0019 0.1119 -0.048" mass="0.935" diaginertia="0.0062 0.0064 0.001"/>
|
||||
<joint name="fl_hip_pitch_joint" pos="0 0 0" axis="0 1 0" range="-2.58 2.58" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="fl_hip_pitch_Link"/>
|
||||
<geom size="0.046 0.048" pos="0 0.048 0" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.75294 0.75294 0.75294 1"/>
|
||||
<geom size="0.0435 0.0115 0.06" pos="0 0.1155 -0.06" type="box" rgba="0.75294 0.75294 0.75294 1"/>
|
||||
<body name="fl_knee_Link" pos="0 0.1035 -0.25" quat="0.999997 0.0025023 0 0">
|
||||
<inertial pos="0.0002 0.0242 -0.1539" mass="0.651" fullinertia="0.0042 0.0045 0.0005 0 0 0.0002"/>
|
||||
<joint name="fl_knee_joint" pos="0 0 0" axis="0 1 0" range="-2.65 2.65" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="fl_knee_Link"/>
|
||||
<geom size="0.0475 0.015" pos="0 0.025 -0.20011" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.75294 0.75294 0.75294 1"/>
|
||||
<geom size="0.015 0.0125 0.06" pos="0 0.0125 -0.09" type="box" rgba="0.75294 0.75294 0.75294 1"/>
|
||||
<body name="fl_wheel_Link" pos="0 0.014699 -0.20011">
|
||||
<inertial pos="-0.0002 0.0407 -0.0001" mass="0.53" diaginertia="0.0017 0.0032 0.0017"/>
|
||||
<joint name="fl_wheel_joint" pos="0 0 0" axis="0 1 0" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="fl_wheel_Link"/>
|
||||
<geom size="0.1 0.015" pos="0 0.04074 0" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.75294 0.75294 0.75294 1"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
<body name="fr_hip_abduction_Link" pos="0.32826 -0.065853 0.054034">
|
||||
<inertial pos="0.0488 0.0026 0.0008" mass="0.5" diaginertia="0.0003 0.0006 0.0005"/>
|
||||
<joint name="fr_hip_abduction_joint" pos="0 0 0" axis="1 0 0" range="-0.611 0.436" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="fr_hip_abduction_Link"/>
|
||||
<body name="fr_hip_pitch_Link" pos="0.06389 0.027311 -0.00036027" quat="0.999976 -0.00686995 0 0">
|
||||
<inertial pos="-0.0019 -0.1119 -0.048" mass="0.935" diaginertia="0.0062 0.0064 0.001"/>
|
||||
<joint name="fr_hip_pitch_joint" pos="0 0 0" axis="0 1 0" range="-2.58 2.58" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="fr_hip_pitch_Link"/>
|
||||
<geom size="0.046 0.048" pos="0 -0.048 0" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.75294 0.75294 0.75294 1"/>
|
||||
<geom size="0.0435 0.0115 0.06" pos="0 -0.1155 -0.06" type="box" rgba="0.75294 0.75294 0.75294 1"/>
|
||||
<body name="fr_knee_Link" pos="-0.00075079 -0.1035 -0.25" quat="0.999976 0.00686995 0 0">
|
||||
<inertial pos="-0.0002 -0.0242 -0.1539" mass="0.651" fullinertia="0.0042 0.0045 0.0005 0 0 0.0001"/>
|
||||
<joint name="fr_knee_joint" pos="0 0 0" axis="0 1 0" range="-2.65 2.65" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="fr_knee_Link"/>
|
||||
<geom size="0.0475 0.015" pos="0 -0.025 -0.1998" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.75294 0.75294 0.75294 1"/>
|
||||
<geom size="0.015 0.0125 0.06" pos="0 -0.0125 -0.09" type="box" rgba="0.75294 0.75294 0.75294 1"/>
|
||||
<body name="fr_wheel_Link" pos="0 -0.018447 -0.1998">
|
||||
<inertial pos="0.0002 -0.0407 -0.0001" mass="0.53" diaginertia="0.0017 0.0032 0.0017"/>
|
||||
<joint name="fr_wheel_joint" pos="0 0 0" axis="0 1 0" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="fr_wheel_Link"/>
|
||||
<geom size="0.1 0.015" pos="0 -0.040735 0" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.75294 0.75294 0.75294 1"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
<body name="rl_hip_abduction_Link" pos="-0.024743 0.066141 0.054034">
|
||||
<inertial pos="-0.0488 -0.0026 -0.0008" mass="0.5" diaginertia="0.0003 0.0006 0.0005"/>
|
||||
<joint name="rl_hip_abduction_joint" pos="0 0 0" axis="1 0 0" range="-0.436 0.611" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="rl_hip_abduction_Link"/>
|
||||
<body name="rl_hip_pitch_Link" pos="-0.06389 -0.027309 0.00045509">
|
||||
<inertial pos="0.0019 0.1119 -0.048" mass="0.935" diaginertia="0.0062 0.0064 0.001"/>
|
||||
<joint name="rl_hip_pitch_joint" pos="0 0 0" axis="0 1 0" range="-2.58 2.58" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="rl_hip_pitch_Link"/>
|
||||
<geom size="0.046 0.048" pos="0 0.048 0" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.75294 0.75294 0.75294 1"/>
|
||||
<geom size="0.0435 0.0115 0.06" pos="0 0.1155 -0.06" type="box" rgba="0.75294 0.75294 0.75294 1"/>
|
||||
<body name="rl_knee_Link" pos="0 0.099459 -0.25163">
|
||||
<inertial pos="0.0002 0.0242 -0.1539" mass="0.651" fullinertia="0.0042 0.0045 0.0005 0 0 -0.0003"/>
|
||||
<joint name="rl_knee_joint" pos="0 0 0" axis="0 1 0" range="-2.65 2.65" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="rl_knee_Link"/>
|
||||
<geom size="0.0475 0.015" pos="0 0.025 -0.20027" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.75294 0.75294 0.75294 1"/>
|
||||
<geom size="0.015 0.0125 0.06" pos="0 0.0125 -0.09" type="box" rgba="0.75294 0.75294 0.75294 1"/>
|
||||
<body name="rl_wheel_Link" pos="0 0.012475 -0.20027">
|
||||
<inertial pos="-0.0002 0.0407 -0.0001" mass="0.53" diaginertia="0.0017 0.0032 0.0017"/>
|
||||
<joint name="rl_wheel_joint" pos="0 0 0" axis="0 1 0" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="rl_wheel_Link"/>
|
||||
<geom size="0.1 0.015" pos="0 0.040737 0" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.75294 0.75294 0.75294 1"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
<body name="rr_hip_abduction_Link" pos="-0.024743 -0.065884 0.053981">
|
||||
<inertial pos="-0.0488 0.0026 0.0008" mass="0.5" diaginertia="0.0003 0.0006 0.0005"/>
|
||||
<joint name="rr_hip_abduction_joint" pos="0 0 0" axis="1 0 0" range="-0.611 0.436" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="rr_hip_abduction_Link"/>
|
||||
<body name="rr_hip_pitch_Link" pos="-0.06389 0.027341 0.00041625">
|
||||
<inertial pos="-0.002 -0.1111 -0.0498" mass="0.935" diaginertia="0.0062 0.0064 0.001"/>
|
||||
<joint name="rr_hip_pitch_joint" pos="0 0 0" axis="0 1 0" range="-2.58 2.58" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="rr_hip_pitch_Link"/>
|
||||
<geom size="0.046 0.048" pos="0 -0.048 0" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.75294 0.75294 0.75294 1"/>
|
||||
<geom size="0.0435 0.0115 0.06" pos="0 -0.1155 -0.06" type="box" rgba="0.75294 0.75294 0.75294 1"/>
|
||||
<body name="rr_knee_Link" pos="-0.00075079 -0.099408 -0.25165">
|
||||
<inertial pos="-0.0002 -0.0225 -0.1541" mass="0.651" fullinertia="0.0042 0.0045 0.0005 0 0 -0.0001"/>
|
||||
<joint name="rr_knee_joint" pos="0 0 0" axis="0 1 0" range="-2.65 2.65" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="rr_knee_Link"/>
|
||||
<geom size="0.0475 0.015" pos="0 -0.025 -0.20027" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.75294 0.75294 0.75294 1"/>
|
||||
<geom size="0.015 0.0125 0.06" pos="0 -0.0125 -0.09" type="box" rgba="0.75294 0.75294 0.75294 1"/>
|
||||
<body name="rr_wheel_Link" pos="0 -0.012435 -0.20027">
|
||||
<inertial pos="0.0002 -0.0407 -0.0005" mass="0.53" diaginertia="0.0017 0.0032 0.0017"/>
|
||||
<joint name="rr_wheel_joint" pos="0 0 0" axis="0 1 0" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="rr_wheel_Link"/>
|
||||
<geom size="0.1 0.015" pos="0 -0.040737 0" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.75294 0.75294 0.75294 1"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
<body name="imu_link" pos="0.1518 0 0.127">
|
||||
<inertial pos="0 0 0" mass="0" diaginertia="0 0 0"/>
|
||||
</body>
|
||||
</body>
|
||||
</worldbody>
|
||||
|
||||
<actuator>
|
||||
<general name="fl_hip_abduction_joint" joint="fl_hip_abduction_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="120" biasprm="0 -8 -8"/>
|
||||
<general name="fl_hip_pitch_joint" joint="fl_hip_pitch_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="120" biasprm="0 -8 -8"/>
|
||||
<general name="fl_knee_joint" joint="fl_knee_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="120" biasprm="0 -8 -8"/>
|
||||
<general name="fl_wheel_joint" joint="fl_wheel_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="0.5"/>
|
||||
<general name="fr_hip_abduction_joint" joint="fr_hip_abduction_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="120" biasprm="0 -8 -8"/>
|
||||
<general name="fr_hip_pitch_joint" joint="fr_hip_pitch_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="120" biasprm="0 -8 -8"/>
|
||||
<general name="fr_knee_joint" joint="fr_knee_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="120" biasprm="0 -8 -8"/>
|
||||
<general name="fr_wheel_joint" joint="fr_wheel_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="0.5"/>
|
||||
<general name="rl_hip_abduction_joint" joint="rl_hip_abduction_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="120" biasprm="0 -8 -8"/>
|
||||
<general name="rl_hip_pitch_joint" joint="rl_hip_pitch_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="120" biasprm="0 -8 -8"/>
|
||||
<general name="rl_knee_joint" joint="rl_knee_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="120" biasprm="0 -8 -8"/>
|
||||
<general name="rl_wheel_joint" joint="rl_wheel_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="0.5"/>
|
||||
<general name="rr_hip_abduction_joint" joint="rr_hip_abduction_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="120" biasprm="0 -8 -8"/>
|
||||
<general name="rr_hip_pitch_joint" joint="rr_hip_pitch_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="120" biasprm="0 -8 -8"/>
|
||||
<general name="rr_knee_joint" joint="rr_knee_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="120" biasprm="0 -8 -8"/>
|
||||
<general name="rr_wheel_joint" joint="rr_wheel_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="0.5"/>
|
||||
</actuator>
|
||||
</mujoco>
|
||||
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,243 @@
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
import torch
|
||||
import torch.nn as nn
|
||||
|
||||
|
||||
def resolve_policy_path(policy_path: Path | str | None, root: Path | None = None) -> Path:
|
||||
"""Prefer ONNX for deployment while keeping the PT file as source/fallback."""
|
||||
root = root or Path(__file__).resolve().parents[1]
|
||||
if policy_path is None:
|
||||
onnx_path = root / "policies" / "model_rough.onnx"
|
||||
pt_path = root / "policies" / "model_rough.pt"
|
||||
return onnx_path if onnx_path.exists() else pt_path
|
||||
|
||||
path = Path(policy_path)
|
||||
if not path.is_absolute():
|
||||
path = root / path
|
||||
if path.suffix.lower() == ".pt":
|
||||
onnx_path = path.with_suffix(".onnx")
|
||||
if onnx_path.exists():
|
||||
return onnx_path
|
||||
return path
|
||||
|
||||
|
||||
class PolicyMLP(nn.Module):
|
||||
def __init__(self, obs_dim: int, action_dim: int):
|
||||
super().__init__()
|
||||
self.register_buffer("obs_mean", torch.zeros(obs_dim))
|
||||
self.register_buffer("obs_std", torch.ones(obs_dim))
|
||||
self.net = nn.Sequential(
|
||||
nn.Linear(obs_dim, 512),
|
||||
nn.ELU(),
|
||||
nn.Linear(512, 256),
|
||||
nn.ELU(),
|
||||
nn.Linear(256, 128),
|
||||
nn.ELU(),
|
||||
nn.Linear(128, action_dim),
|
||||
)
|
||||
|
||||
def forward(self, x: torch.Tensor) -> torch.Tensor:
|
||||
x = (x - self.obs_mean) / torch.clamp(self.obs_std, min=1e-6)
|
||||
return self.net(x)
|
||||
|
||||
|
||||
class OnnxPolicy:
|
||||
def __init__(self, model_path: Path):
|
||||
try:
|
||||
import onnxruntime as ort
|
||||
except ImportError as exc:
|
||||
raise ImportError(
|
||||
"onnxruntime is required for ONNX policy inference. "
|
||||
"Install it on Orin with `python -m pip install onnxruntime`."
|
||||
) from exc
|
||||
|
||||
opts = ort.SessionOptions()
|
||||
opts.intra_op_num_threads = 1
|
||||
opts.inter_op_num_threads = 1
|
||||
opts.execution_mode = ort.ExecutionMode.ORT_SEQUENTIAL
|
||||
self.session = ort.InferenceSession(
|
||||
str(model_path),
|
||||
sess_options=opts,
|
||||
providers=["CPUExecutionProvider"],
|
||||
)
|
||||
self.input_name = self.session.get_inputs()[0].name
|
||||
self.output_name = self.session.get_outputs()[0].name
|
||||
input_shape = self.session.get_inputs()[0].shape
|
||||
output_shape = self.session.get_outputs()[0].shape
|
||||
self.expected_obs_dim = int(input_shape[1]) if len(input_shape) >= 2 and isinstance(input_shape[1], int) else 53
|
||||
self.expected_action_dim = int(output_shape[1]) if len(output_shape) >= 2 and isinstance(output_shape[1], int) else 16
|
||||
self.backend = "onnxruntime"
|
||||
|
||||
self.obs_mean = torch.zeros(self.expected_obs_dim)
|
||||
self.obs_std = torch.ones(self.expected_obs_dim)
|
||||
|
||||
def __call__(self, x: torch.Tensor) -> torch.Tensor:
|
||||
obs = x.detach().cpu().numpy().astype(np.float32, copy=False)
|
||||
action = self.session.run([self.output_name], {self.input_name: obs})[0]
|
||||
return torch.from_numpy(np.asarray(action, dtype=np.float32)).to(x.device)
|
||||
|
||||
|
||||
def load_policy(model_path: Path, device: torch.device):
|
||||
if model_path.suffix.lower() == ".onnx":
|
||||
return OnnxPolicy(model_path)
|
||||
|
||||
checkpoint = torch.load(model_path, map_location=device, weights_only=False)
|
||||
state_dict = checkpoint["actor_state_dict"]
|
||||
|
||||
input_key = "mlp.0.weight" if "mlp.0.weight" in state_dict else "net.0.weight"
|
||||
output_key = "mlp.6.weight" if "mlp.6.weight" in state_dict else "net.6.weight"
|
||||
obs_dim = int(state_dict[input_key].shape[1])
|
||||
action_dim = int(state_dict[output_key].shape[0])
|
||||
|
||||
model = PolicyMLP(obs_dim=obs_dim, action_dim=action_dim)
|
||||
remapped_state_dict: dict[str, torch.Tensor] = {}
|
||||
for key, value in state_dict.items():
|
||||
if key.startswith("mlp."):
|
||||
remapped_state_dict[key.replace("mlp.", "net.")] = value
|
||||
elif key.startswith("net."):
|
||||
remapped_state_dict[key] = value
|
||||
elif key == "obs_normalizer._mean":
|
||||
remapped_state_dict["obs_mean"] = value.squeeze()
|
||||
elif key == "obs_normalizer._var":
|
||||
remapped_state_dict["obs_std"] = torch.sqrt(value.squeeze() + 1e-5)
|
||||
|
||||
model.load_state_dict(remapped_state_dict, strict=False)
|
||||
model.eval()
|
||||
model.to(device)
|
||||
model.expected_obs_dim = obs_dim
|
||||
model.expected_action_dim = action_dim
|
||||
model.backend = "torch"
|
||||
return model
|
||||
|
||||
|
||||
class PolicyRunner:
|
||||
BASE_OBS_DIM = 53
|
||||
DEFAULT_STAND_POSE = np.array(
|
||||
[
|
||||
0.0, 0.9, -1.8,
|
||||
0.0, 0.9, -1.8,
|
||||
0.0, 0.9, -1.8,
|
||||
0.0, 0.9, -1.8,
|
||||
0.0, 0.0, 0.0, 0.0,
|
||||
],
|
||||
dtype=np.float32,
|
||||
)
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
policy_path: Path,
|
||||
device: torch.device | None = None,
|
||||
enable_zero_cmd_suppression: bool = True,
|
||||
hold_zero_command_pose: bool = True,
|
||||
command_release_s: float = 0.35,
|
||||
action_scale: np.ndarray | None = None,
|
||||
zero_cmd_use_yaw_rate: bool = True,
|
||||
clip_obs: float = 100.0,
|
||||
):
|
||||
self.device = device or torch.device("cuda" if torch.cuda.is_available() else "cpu")
|
||||
self.policy_path = Path(policy_path)
|
||||
self.enable_zero_cmd_suppression = bool(enable_zero_cmd_suppression)
|
||||
self.hold_zero_command_pose = bool(hold_zero_command_pose)
|
||||
self.command_release_s = max(float(command_release_s), 1e-3)
|
||||
self.clip_obs = max(float(clip_obs), 0.0)
|
||||
self.policy_path = resolve_policy_path(self.policy_path)
|
||||
if not self.policy_path.exists():
|
||||
raise FileNotFoundError(f"policy file not found: {self.policy_path}")
|
||||
print(f"[PolicyRunner] device={self.device}, policy={self.policy_path}")
|
||||
self.policy = load_policy(self.policy_path, self.device)
|
||||
if self.policy.expected_obs_dim != self.BASE_OBS_DIM:
|
||||
raise ValueError(
|
||||
f"Unsupported policy obs dim {self.policy.expected_obs_dim}. "
|
||||
f"Current sim2real only supports {self.BASE_OBS_DIM}-D actor observations."
|
||||
)
|
||||
|
||||
self.default_dof_pos = self.DEFAULT_STAND_POSE.copy()
|
||||
self.last_actions = np.zeros(16, dtype=np.float32)
|
||||
|
||||
self.action_scale = np.asarray(
|
||||
action_scale
|
||||
if action_scale is not None
|
||||
else [
|
||||
0.125, 0.25, 0.25,
|
||||
0.125, 0.25, 0.25,
|
||||
0.125, 0.25, 0.25,
|
||||
0.125, 0.25, 0.25,
|
||||
5.0, 5.0, 5.0, 5.0,
|
||||
],
|
||||
dtype=np.float32,
|
||||
)
|
||||
if self.action_scale.shape != (16,):
|
||||
raise ValueError(f"action_scale must be shape (16,), got {self.action_scale.shape}")
|
||||
|
||||
self.zero_cmd_lin_thresh = 0.05
|
||||
self.zero_cmd_yaw_thresh = 0.05
|
||||
self.zero_yaw_rate_thresh = 0.10
|
||||
self.zero_cmd_use_yaw_rate = bool(zero_cmd_use_yaw_rate)
|
||||
self._command_release_alpha = 0.0
|
||||
print(
|
||||
f"[PolicyRunner] obs_dim={self.policy.expected_obs_dim}, "
|
||||
f"base_obs_dim={self.BASE_OBS_DIM}, history=1, "
|
||||
f"action_dim={self.policy.expected_action_dim}, "
|
||||
f"backend={getattr(self.policy, 'backend', 'unknown')}, "
|
||||
f"clip_obs={self.clip_obs:.1f}, "
|
||||
f"zero_cmd_suppression={self.enable_zero_cmd_suppression}, "
|
||||
f"hold_zero_command_pose={self.hold_zero_command_pose}"
|
||||
)
|
||||
|
||||
def reset(self, prime_obs: np.ndarray | None = None) -> None:
|
||||
self.last_actions = np.zeros(16, dtype=np.float32)
|
||||
self._command_release_alpha = 0.0
|
||||
|
||||
def _is_zero_command(self, command: np.ndarray, base_ang_vel: np.ndarray) -> bool:
|
||||
cmd_is_zero = (
|
||||
np.linalg.norm(command[:2]) < self.zero_cmd_lin_thresh
|
||||
and abs(command[2]) < self.zero_cmd_yaw_thresh
|
||||
)
|
||||
if not self.zero_cmd_use_yaw_rate:
|
||||
return cmd_is_zero
|
||||
return cmd_is_zero and abs(base_ang_vel[2]) < self.zero_yaw_rate_thresh
|
||||
|
||||
def command_activation_metrics(self, command: np.ndarray) -> tuple[float, float]:
|
||||
command = np.asarray(command, dtype=np.float32)
|
||||
planar = float(np.linalg.norm(command[:2]))
|
||||
yaw = float(abs(command[2]))
|
||||
return planar, yaw
|
||||
|
||||
def is_command_active(self, command: np.ndarray) -> bool:
|
||||
planar, yaw = self.command_activation_metrics(command)
|
||||
return planar >= self.zero_cmd_lin_thresh or yaw >= self.zero_cmd_yaw_thresh
|
||||
|
||||
def step(self, obs: np.ndarray, dt: float = 0.02) -> tuple[np.ndarray, np.ndarray]:
|
||||
obs = np.asarray(obs, dtype=np.float32)
|
||||
expected_obs_dim = int(self.policy.expected_obs_dim)
|
||||
if obs.shape[0] != expected_obs_dim:
|
||||
raise ValueError(
|
||||
f"Observation dim mismatch: got {obs.shape[0]}, expected {expected_obs_dim}."
|
||||
)
|
||||
if self.clip_obs > 0.0:
|
||||
obs = np.clip(obs, -self.clip_obs, self.clip_obs).astype(np.float32, copy=False)
|
||||
|
||||
obs_tensor = torch.tensor(obs, dtype=torch.float32, device=self.device).unsqueeze(0)
|
||||
with torch.no_grad():
|
||||
raw_actions = self.policy(obs_tensor).squeeze(0).cpu().numpy()
|
||||
|
||||
raw_actions = np.clip(raw_actions, -10.0, 10.0).astype(np.float32)
|
||||
command = obs[6:9]
|
||||
base_ang_vel = obs[0:3] / 0.25
|
||||
zero_command = self._is_zero_command(command, base_ang_vel)
|
||||
if zero_command:
|
||||
self._command_release_alpha = 0.0
|
||||
if self.hold_zero_command_pose:
|
||||
raw_actions[:] = 0.0
|
||||
elif self.enable_zero_cmd_suppression:
|
||||
raw_actions[12:16] = 0.0
|
||||
raw_actions[:12] *= 0.5
|
||||
else:
|
||||
self._command_release_alpha = min(1.0, self._command_release_alpha + dt / self.command_release_s)
|
||||
raw_actions *= self._command_release_alpha
|
||||
|
||||
self.last_actions = raw_actions.copy()
|
||||
scaled_actions = raw_actions * self.action_scale
|
||||
return scaled_actions, raw_actions
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user