Compare commits

..
4 Commits
Author SHA1 Message Date
caowangrenbo 46bbb06978 feat(hardware): 标定归硬件;按 name 选关节 + kSetZero;toml++ 改 vendored
标定边界上移到 invicta_hardware:
- raw↔model 的 dir/offset 由 hardware 独家施加(话题收发都是模型帧),
  deploy 的 InvictaInterface 退化为透传,invicta_deploy.toml 不再有 dir/offset_deg。
  这样 [joint_limits](模型帧)与被限幅的指令同帧,/joint_states 也是模型角。

关节选择(部分接入 / 单腿调试):
- JointCommandArray 的 name 即本次要控制的关节子集(空 = 全部已配置关节)。
- 未选中的关节写「零力矩自由」块,而不是留全 0——按协议 (0,0,0,0,0) 会解码成
  P_MIN/V_MIN/T_MIN,其中 tau=T_MIN(摆臂 -100 N·m)是危险的前馈力矩。
- 新增 control_word kSetZero(6) -> 0xEF,只作用于选中的摆臂(轮毂不支持,跳过)。

调试脚本 invicta_hardware/scripts/hw_debug_test.py:
- 只起 hardware 节点调试:monitor/damping/angle/wheel/offset/zero 六种模式 + --joints。
- 限幅参数硬编码;发指令时默认回显模型角。

构建:toml++ 改为 vendored single-header(三个包各 third_party/tomlplusplus/toml++/toml.hpp),
   去掉 find_package(tomlplusplus) 与 -DTOML_HEADER_ONLY(单头自带 header-only)。

已在容器(localhost/custom-ros-noetic)中全新 catkin_make + smoke_test 验证通过。
2026-10-23 13:44:27 +08:00
caowangrenbo 594885b9e8 feat(hardware): 接入 SocketCAN 与智身模组协议,补温度诊断和指令看门狗
- hardware: 新增 can_bus.h / joint_module_protocol.h,实现使能/失能/运行帧与
  12 字节反馈帧编解码;ReadHardware/WriteHardware 落地,控制字按协议语义映射
  (kErrorReset 按重新使能处理,kGetStatus 无需请求)
- hardware: 温度/驱动状态/电压经 /joint_diagnostics 发布(默认 10 Hz,可配置)
- hardware: 指令看门狗,100 ms 无指令转阻尼,避免上游挂掉后重发最后一帧
- hardware: 限幅与归一化边界分离——[joint_limits] 是唯一实际限幅,加载期校验
  其不超过 [motor.*] 的归一化边界;力矩按峰值扭矩夹紧,kp/kd 越界夹紧并告警
- hardware: require_can 开关,无 CAN 时可只发状态(冒烟测试用)
- remote: 1002/4 关节按 M20 的 16 槽上报(swing→HipY、wheel→Wheel,其余占位 0),
  1002/5 温度改由 /joint_diagnostics 提供
- deploy: 启动时序等订阅者出现再发(ROS1 无 latch,否则整段丢失)
- msgs: 新增 JointDiagnostics
- docs: 智身模组 CANFD 协议整理、待办清单
2026-10-22 11:05:49 +08:00
caowangrenbo a173383756 refactor(config) + feat(model): 每节点一份 TOML 取代 ROS yaml;8 关节两自由度腿与单连杆 IK
配置(节点自治,跨包不共享):
- 新增各节点自己的 config/<节点>.toml(deploy / remote / hardware),只放本节点参数;删除三份 *.yaml
- 各节点自带配置结构与加载器(deploy_config.h / remote_config.h / hardware_config.h),
  删除 state_machine/parameters/ 与 ControlParameters
- TOML 解析库 toml++ 改为系统安装:find_package(tomlplusplus REQUIRED),以 header-only 方式编译,
  库源码不进仓库、目标机不需部署运行时 .so
- launch 只传 config_file;改 TOML 不需重编,改配置头自动触发该节点重编

机器人模型(按本机构型改):
- 16 关节三关节腿 → 8 关节(每腿 摆臂 + 轮)
- IK 改为单连杆反解 θ = splay_sign·acos(h/L);趴下 h=0 → 摆臂水平(±90°)
- 步态倒向:趴下时前腿向前、后腿向后(splay_sign 前 +1 / 后 −1),与 M20 相反
- 起立/趴下用样条 + 速度前馈;轮 kp 恒 0;限位改为逐关节查表
- 精简死参数:机身长宽、髋偏置、轮半径、力矩限制、通用策略参数,以及 auto_calibrate 与 ±360° 回绕标定
- 硬件侧参数入库:电机量程(P65MAXS/W190S)、CAN 通道与仲裁 ID、使能帧数/间隔、每腿 bus/canid、台架增益

保留(未改动 HEAD 已有功能):多步态模型预加载与运行中切换、站立后自动进 RL 与就绪自检、
遥测四路上报、阻尼态单调时钟计时、策略输出 NaN 保护

验证(ROS Noetic 容器,干净构建):
- 四个包 catkin_make rc=0
- /joint_states 200 Hz(8 关节名与配置一致)、/motion_state 20 Hz
- IK 目标角:站立 ±16.3°、收腿 ±66.4°、趴下 ±90.0°;趴下指令实测峰值 1.5708 rad
- 协议 MotionParam=1/4 触发 idle→standup→liedown;自动进 RL 自检在零反馈下正确拦住

待实测:摆臂长度/站趴高度、各关节 dir/offset_deg;invicta_hardware 的 CAN 读写仍为 TODO
2026-10-21 21:39:49 +08:00
caowangrenbo cf70b1a1d7 Initial commit: extract M20 sim2real into ROS1 invicta project
Source: DeepRoboticsLab/sdk_deploy (BSD-3-Clause), M20 sim2real part only.

Packages:
- invicta_msgs: JointCommandArray, MotionCommand, MotionState; standard messages
  (sensor_msgs/JointState, sensor_msgs/Imu) elsewhere; shared motion value defs.
- invicta_deploy: state machine (idle/standup/rl/joint-damping/lie-down), ONNX
  policy with per-gait models, auto RL entry after standup with readiness checks,
  safety loop, gait->model table; no protocol/network code.
- invicta_remote: vendor basic_server protocol (UDP + JSON, proto.md V1.2.0),
  header/MsgId, 0xE0xx error codes, motion/usage-mode/gait translation, telemetry
  1002/3,4,5,6 (2/10/2/2 Hz); publishes /motion_command, subscribes /motion_state.
- invicta_hardware: joint/IMU bridge skeleton (SocketCAN I/O is TODO), publishes
  sensor_msgs/JointState + sensor_msgs/Imu, consumes /joint_commands.

Verified on ROS Noetic (Ubuntu 20.04) in docker: clean catkin_make, 200 Hz joint
state / 50 Hz RL commands / 20 Hz state report, protocol conformance (MotionParam,
gait preconditions, error codes), telemetry push, and end-to-end switching of two
policy models with a real remote controller.
2026-10-21 00:46:29 +08:00
4514 changed files with 63164 additions and 102311 deletions
+10 -4
View File
@@ -1,5 +1,11 @@
# 编译产物
build/
cmake-build-debug/
data/*
!data/.gitkeep
.idea/
devel/
install/
logs/
src/CMakeLists.txt # catkin_make 生成的软链
*.pyc
__pycache__/
# onnxruntime 动态库装到 devel/lib 下会被重复安装,不纳入版本管理
src/invicta_deploy/third_party/onnxruntime/*/lib/libonnxruntime.so.1.22.0
-97
View File
@@ -1,97 +0,0 @@
cmake_minimum_required(VERSION 3.10)
project(m20_sdk_deploy)
# 设置C++标准
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
# 默认构建类型
if(NOT CMAKE_BUILD_TYPE)
set(CMAKE_BUILD_TYPE Release)
endif()
# 警告忽略和编译选项
add_definitions(-w)
add_compile_options(-fPIC)
# 配置选项
set(BUILD_PLATFORM "x86" CACHE STRING "Target platform (x86 or arm)")
if(BUILD_PLATFORM STREQUAL "arm")
set(CMAKE_C_COMPILER "aarch64-linux-gnu-gcc")
set(CMAKE_CXX_COMPILER "aarch64-linux-gnu-g++")
endif()
# 查找依赖包
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(drdds REQUIRED)
set(THIRD_PARTY ${CMAKE_CURRENT_SOURCE_DIR}/third_party)
include_directories(
include/types
include/utils
interface/robot
interface/user_command
interface/network
state_machine
run_policy
${THIRD_PARTY}/eigen/
${THIRD_PARTY}/gamepad/include
${THIRD_PARTY}/onnxruntime/${BUILD_PLATFORM}/include
)
# 子目录
add_subdirectory(interface)
# Keep the default RUNPATH behavior so a correctly sourced ROS environment can
# override stale build-machine library paths when necessary.
set(CMAKE_BUILD_WITH_INSTALL_RPATH TRUE)
set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
set(CMAKE_INSTALL_RPATH
${THIRD_PARTY}/onnxruntime/${BUILD_PLATFORM}/lib/
../lib/onnxruntime/
)
file(GLOB_RECURSE STATE_MACHINE_SRC "state_machine/*.c*")
add_executable(rl_deploy main.cpp ${STATE_MACHINE_SRC})
add_library(onnxruntime SHARED IMPORTED)
set_target_properties(onnxruntime PROPERTIES IMPORTED_LOCATION "${THIRD_PARTY}/onnxruntime//${BUILD_PLATFORM}/lib/libonnxruntime.so")
ament_target_dependencies(rl_deploy
rclcpp
drdds
)
if(BUILD_PLATFORM STREQUAL "arm")
target_link_libraries(rl_deploy
interface
onnxruntime
-lpthread
-lm
-lrt
-ldl
-lstdc++
)
else()
target_link_libraries(rl_deploy
interface
onnxruntime
-lpthread
-lm
-lrt
-ldl
-lstdc++
)
endif()
install(TARGETS rl_deploy
DESTINATION lib/${PROJECT_NAME}
)
ament_package()
+28
View File
@@ -0,0 +1,28 @@
BSD 3-Clause License
Copyright (c) 2025, DEEPRoboticsLab
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
3. Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
@@ -1,140 +0,0 @@
<mujoco model="fire_ant">
<compiler angle="radian" autolimits="true" balanceinertia="true"/>
<option timestep="0.001" gravity="0 0 -9.81" integrator="implicitfast">
<flag multiccd="enable"/>
</option>
<default>
<geom density="0" condim="3" solref="0.005 1" solimp="0.95 0.99 0.001"
friction="0.6 0.005 0.0001" contype="1" conaffinity="1"/>
<joint damping="0" armature="0" frictionloss="0"/>
<default class="wheel">
<!-- Wheel-ground friction is intentionally independent from the other links. -->
<geom type="cylinder" size="0.096 0.0415" quat="0.70710678 0.70710678 0 0"
friction="1.0 0.005 0.0001" rgba="0.08 0.08 0.08 1"/>
<joint axis="0 -1 0" armature="0.00243216" frictionloss="1.0"/>
</default>
</default>
<asset>
<texture name="floor_tex" type="2d" builtin="checker" rgb1="0.22 0.27 0.31"
rgb2="0.12 0.15 0.18" width="512" height="512"/>
<material name="floor_mat" texture="floor_tex" texrepeat="2 2" texuniform="true"/>
</asset>
<worldbody>
<light directional="true" pos="-2 -1 4" dir="0.5 0.2 -1"/>
<geom name="floor" type="plane" size="30 8 0.1" material="floor_mat"
friction="1.0 0.005 0.0001" rgba="0.55 0.6 0.65 1"/>
<body name="base_link" pos="0 0 0.306">
<freejoint name="floating_base"/>
<inertial pos="0.00355106 0.00165076 -0.00346462" mass="15.882"
diaginertia="0.0880787 0.533816 0.563915"/>
<site name="base_site" pos="0 0 0" size="0.005"/>
<site name="imu_site" pos="0 0 0" size="0.005"/>
<geom name="base_collision" type="box" size="0.30 0.16 0.06" rgba="0.35 0.43 0.50 1"/>
<geom name="base_left_rail" type="cylinder" size="0.055 0.30" pos="0 0.13 0"
quat="0.70710678 0 0.70710678 0" rgba="0.22 0.28 0.34 1"/>
<geom name="base_right_rail" type="cylinder" size="0.055 0.30" pos="0 -0.13 0"
quat="0.70710678 0 0.70710678 0" rgba="0.22 0.28 0.34 1"/>
<body name="fl_knee" pos="0.30 0.14 0">
<inertial pos="0.00341917 0.00269124 -0.18126550" mass="0.612"
diaginertia="0.005202 0.005202 0.0007344"/>
<joint name="fl_knee_joint" axis="0 -1 0" range="-2.792 2.809"
actuatorfrcrange="-75 75"/>
<geom name="fl_knee_collision" type="box" size="0.025 0.012 0.10"
pos="0.012 -0.012 -0.10" rgba="0.82 0.31 0.16 1"/>
<geom name="fl_motor_collision" type="cylinder" size="0.04 0.045"
pos="0 0.018 -0.21" quat="0.70710678 0.70710678 0 0" rgba="0.65 0.20 0.10 1"/>
<body name="fl_wheel" pos="0 0.11 -0.21">
<inertial pos="0 0 0" mass="1.34" diaginertia="0.003856632 0.006174720 0.003856632"/>
<joint name="fl_wheel_joint" class="wheel" actuatorfrcrange="-28.68 28.68"/>
<geom name="fl_wheel_collision" class="wheel"/>
</body>
</body>
<body name="fr_knee" pos="0.30 -0.14 0">
<inertial pos="0.00354945 -0.00247884 -0.18008924" mass="0.612"
diaginertia="0.005202 0.005202 0.0007344"/>
<joint name="fr_knee_joint" axis="0 -1 0" range="-2.792 2.809"
actuatorfrcrange="-75 75"/>
<geom name="fr_knee_collision" type="box" size="0.025 0.012 0.10"
pos="0.012 0.012 -0.10" rgba="0.82 0.31 0.16 1"/>
<geom name="fr_motor_collision" type="cylinder" size="0.04 0.045"
pos="0 -0.018 -0.21" quat="0.70710678 0.70710678 0 0" rgba="0.65 0.20 0.10 1"/>
<body name="fr_wheel" pos="0 -0.11 -0.21">
<inertial pos="0 0 0" mass="1.34" diaginertia="0.003856632 0.006174720 0.003856632"/>
<joint name="fr_wheel_joint" class="wheel" actuatorfrcrange="-28.68 28.68"/>
<geom name="fr_wheel_collision" class="wheel"/>
</body>
</body>
<body name="hl_knee" pos="-0.30 0.14 0">
<inertial pos="-0.00354945 0.00247885 -0.18008926" mass="0.612"
diaginertia="0.005202 0.005202 0.0007344"/>
<joint name="hl_knee_joint" axis="0 -1 0" range="-2.809 2.792"
actuatorfrcrange="-75 75"/>
<geom name="hl_knee_collision" type="box" size="0.025 0.012 0.10"
pos="-0.012 0.012 -0.10" rgba="0.82 0.31 0.16 1"/>
<geom name="hl_motor_collision" type="cylinder" size="0.04 0.045"
pos="0 0.018 -0.21" quat="0.70710678 0.70710678 0 0" rgba="0.65 0.20 0.10 1"/>
<body name="hl_wheel" pos="0 0.11 -0.21">
<inertial pos="0 0 0" mass="1.34" diaginertia="0.003856632 0.006174720 0.003856632"/>
<joint name="hl_wheel_joint" class="wheel" actuatorfrcrange="-28.68 28.68"/>
<geom name="hl_wheel_collision" class="wheel"/>
</body>
</body>
<body name="hr_knee" pos="-0.30 -0.14 0">
<inertial pos="-0.00341053 -0.00270202 -0.18128749" mass="0.612"
diaginertia="0.005202 0.005202 0.0007344"/>
<joint name="hr_knee_joint" axis="0 -1 0" range="-2.809 2.792"
actuatorfrcrange="-75 75"/>
<geom name="hr_knee_collision" type="box" size="0.025 0.012 0.10"
pos="-0.012 0.012 -0.10" rgba="0.82 0.31 0.16 1"/>
<geom name="hr_motor_collision" type="cylinder" size="0.04 0.045"
pos="0 -0.018 -0.21" quat="0.70710678 0.70710678 0 0" rgba="0.65 0.20 0.10 1"/>
<body name="hr_wheel" pos="0 -0.11 -0.21">
<inertial pos="0 0 0" mass="1.34" diaginertia="0.003856632 0.006174720 0.003856632"/>
<joint name="hr_wheel_joint" class="wheel" actuatorfrcrange="-28.68 28.68"/>
<geom name="hr_wheel_collision" class="wheel"/>
</body>
</body>
</body>
</worldbody>
<!-- Robot/DDS order: FL knee/wheel, FR knee/wheel, HL knee/wheel, HR knee/wheel. -->
<actuator>
<motor name="fl_knee_motor" joint="fl_knee_joint" gear="1" ctrlrange="-75 75"/>
<motor name="fl_wheel_motor" joint="fl_wheel_joint" gear="1" ctrlrange="-28.68 28.68"/>
<motor name="fr_knee_motor" joint="fr_knee_joint" gear="1" ctrlrange="-75 75"/>
<motor name="fr_wheel_motor" joint="fr_wheel_joint" gear="1" ctrlrange="-28.68 28.68"/>
<motor name="hl_knee_motor" joint="hl_knee_joint" gear="1" ctrlrange="-75 75"/>
<motor name="hl_wheel_motor" joint="hl_wheel_joint" gear="1" ctrlrange="-28.68 28.68"/>
<motor name="hr_knee_motor" joint="hr_knee_joint" gear="1" ctrlrange="-75 75"/>
<motor name="hr_wheel_motor" joint="hr_wheel_joint" gear="1" ctrlrange="-28.68 28.68"/>
</actuator>
<contact>
<exclude body1="base_link" body2="fl_knee"/>
<exclude body1="fl_knee" body2="fl_wheel"/>
<exclude body1="base_link" body2="fr_knee"/>
<exclude body1="fr_knee" body2="fr_wheel"/>
<exclude body1="base_link" body2="hl_knee"/>
<exclude body1="hl_knee" body2="hl_wheel"/>
<exclude body1="base_link" body2="hr_knee"/>
<exclude body1="hr_knee" body2="hr_wheel"/>
</contact>
<sensor>
<framequat name="base_quat" objtype="site" objname="base_site"/>
<accelerometer name="base_acc" site="imu_site"/>
<gyro name="base_gyro" site="imu_site"/>
</sensor>
<keyframe>
<key name="home" qpos="0 0 0.306 1 0 0 0 0.5 0 0.5 0 -0.5 0 -0.5 0"/>
</keyframe>
</mujoco>
@@ -1,18 +0,0 @@
<mujoco model="fire_ant_rough">
<include file="fire_ant.xml"/>
<worldbody>
<!-- A deterministic mixed obstacle course for Stage-2 sim2sim checks. -->
<geom name="step_08cm" type="box" pos="1.30 0 0.04" size="0.18 1.0 0.04"
friction="1.0 0.005 0.0001" solref="0.005 1" rgba="0.42 0.48 0.55 1"/>
<geom name="step_12cm" type="box" pos="1.66 0 0.06" size="0.18 1.0 0.06"
friction="1.0 0.005 0.0001" solref="0.005 1" rgba="0.46 0.52 0.59 1"/>
<geom name="step_16cm" type="box" pos="2.02 0 0.08" size="0.18 1.0 0.08"
friction="1.0 0.005 0.0001" solref="0.005 1" rgba="0.50 0.56 0.63 1"/>
<geom name="top_platform" type="box" pos="2.72 0 0.08" size="0.52 1.0 0.08"
friction="1.0 0.005 0.0001" solref="0.005 1" rgba="0.54 0.60 0.67 1"/>
<geom name="descending_step_10cm" type="box" pos="3.42 0 0.05" size="0.18 1.0 0.05"
friction="1.0 0.005 0.0001" solref="0.005 1" rgba="0.46 0.52 0.59 1"/>
<geom name="isolated_box" type="box" pos="4.30 0 0.07" size="0.16 0.55 0.07"
friction="1.0 0.005 0.0001" solref="0.005 1" rgba="0.73 0.42 0.20 1"/>
</worldbody>
</mujoco>
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.
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.
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.
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.
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.
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.

Some files were not shown because too many files have changed in this diff Show More