UNI-Controller

介绍

本项目目的是构建一套通用机械臂控制框架，包含6轴/7轴机械臂运动求解与轨迹规划，支持对真机/MuJoCo仿真的控制，能够接入不同的VLA模型进行控制，以及支持遥操作，可能支持简单场景的全自动数采，可能兼容不同的机械臂厂商SDK

TODO List

• 运动求解
• 支持正运动学
• 支持数值逆解 - 牛顿拉夫逊法
• 支持符合Pieper准则的机械臂的解析解
• 支持正逆动力学
• 轨迹规划
• 支持关节空间轨迹规划
• 支持笛卡尔控制轨迹规划 (三次样条插值)
• 接入开源库，支持更顺滑的轨迹规划
• 停止轨迹规划
• 控制接口
• 支持MuJoco仿真(zmq通讯)
• 支持多种伺服控制
• 兼容多种机械臂SDK
• 模型接口
• 兼容多种VLA模型控制
• 软件架构
• 完善分层CMake
• 移除当前print_helper写法，支持spdlog
• 支持日志存储
• Callback 函数实时控制接口

快速开始

前置要求

1. 安装 CMake (>= 3.10)
2. 安装 vcpkg 并配置好环境变量
3. 编译器 (GCC/Clang/MSVC)

编译步骤

在命令行中执行以下命令，请将 <path/to/vcpkg.cmake> 替换为本地 vcpkg 的实际路径：

# 1. 安装依赖库 (vcpkg manifest 模式)
vcpkg install

# 2. 删除已有构建(可选)
Remove-Item -Recurse -Force build-debug

# 3. 创建构建目录
mkdir build 
cd build

# 4. 配置项目 (指定 vcpkg 工具链文件)
# Windows 示例:
cmake .. -DCMAKE_TOOLCHAIN_FILE="F:/dev-libs/vcpkg/scripts/buildsystems/vcpkg.cmake"

# Linux/Mac 示例:
cmake .. -DCMAKE_TOOLCHAIN_FILE="/home/user/vcpkg/scripts/buildsystems/vcpkg.cmake"

# 5. 编译
cmake --build .
## 编译特定文件
cmake --build . --target <file_name>

功能设计说明

MoveJPath

多段轨迹规划设计说明：

【输入数据结构】

• waypoints: 路径点序列，每个点包含6个关节的目标位置 (rad)
• segments: 段参数序列，segments[i] 描述 waypoints[i] -> waypoints[i+1] 的运动 speed_ratio: 该段的速度比例 [0.0, 1.0]，相对于 MAX_JOINT_VELOCITY_USER
• 约束: segments.size() == waypoints.size() - 1 (n个点定义n-1段)

【中间点速度计算方法】 目标：保证轨迹在中间点处速度连续（C1连续性），避免加速度突变

策略：基于前后段的实际运动时间进行加权平均

1. 计算前后两段各自的运动时间 T_in, T_out (基于最大关节位移和段速度)
2. 计算每个关节在两段中的平均速度 v_in = Δq/T_in, v_out = Δq/T_out
3. 短边权重更大：weight = 1/T (运动时间短的段对中间点速度影响更大)
4. 加权平均得到中间点速度：v_mid = (v_inw_in + v_outw_out) / (w_in + w_out)

边界条件：

• 起点速度 = 0 (从静止开始)
• 终点速度 = 0 (平稳停止)
• 中间点速度 ≠ 0 (保证平滑过渡)

【各段插值方法】 采用三次多项式插值 (Cubic Polynomial Interpolation):

• 形式: q(t) = a0 + a1t + a2t² + a3*t³
• 边界条件: q(0)=q_start, q(T)=q_end, dq(0)=v_start, dq(T)=v_end
• 优点: 保证位置、速度连续，加速度平滑，无抖动

【时间戳计算】

• 每段独立生成轨迹点，时间从0到T (段内时间)
• 转换为全局时间戳: cmd.time = segment_time + accumulated_time
• accumulated_time 累积各段时长，表示"从轨迹起点到当前段起点的时间"
• 最终 HardJointCmd.time 表示相对于整条轨迹起点的绝对时间戳 (秒)

【采样频率】

• 控制周期: CONTROL_PERIOD = 10ms (100Hz)
• 每段点数: num_points = motion_time / CONTROL_PERIOD
• 保证与硬件控制频率同步

部署方案

仿真测试

[C++ 控制器]  ---- TCP/UDP Socket ---->  [Python Mujoco 仿真器]
    cmd(q, qd, qdd)       cmd.q               apply action
                                              mj_step()

钱江真机控制

[C++ 控制器 & 伺服控制器]  ---- EtherCAT ---->  [QJR7-9000机械臂]
    cmd(q, qd, qdd)                              apply action
    cmd.q --> 电机编码