gravity_comp

基于 Pinocchio RNEA 的机械臂重力补偿库，提供纯重力前馈、PD + 重力补偿与位置伺服三种控制律，并可选 MuJoCo 仿真验证（UR5e）。

核心特性

• Pinocchio RNEA 重力前馈（τ = g(q)）
• PD + 重力 / 位置伺服
• MuJoCo UR5e 仿真验证（可选）
• 交互式 viewer 演示

演示

无补偿（下塌）	PD + 重力补偿

重力 hold	扰动 push

快速开始

依赖

• C++17 编译器、CMake ≥ 3.16
• Eigen3、Pinocchio
• 可选（MuJoCo 仿真与 viewer）：MuJoCo、Python mujoco / pinocchio、Catch2

推荐在已安装 Pinocchio 与 MuJoCo 的 Conda 环境中构建（CMake 会自动使用 $CONDA_PREFIX）。

构建

git clone https://github.com/kyrovia/gravity_comp.git
cd gravity_comp

# MuJoCo 仿真需要 Menagerie UR5e 模型
./tools/setup_menagerie.sh

cmake -B build -DGRAVITY_COMP_ENABLE_MUJOCO=ON
cmake --build build

仅构建核心库（不含 MuJoCo）：

cmake -B build
cmake --build build

运行测试

cd build && ctest --output-on-failure

启动 MuJoCo 演示

cmake --build build --target ur5e_mujoco_viewer        # 依次演示三种模式
cmake --build build --target ur5e_mujoco_viewer_hold   # 仅 hold
cmake --build build --target ur5e_mujoco_viewer_push   # 扰动恢复

或直接运行 Python viewer：

python examples/ur5e_mujoco_viewer.py --mode compare --real-time

最小 API 示例

#include "gravity_comp/gravity_compensator.hpp"
#include "gravity_comp/pin_model.hpp"

gravity_comp::PinModel model("/path/to/robot.urdf");
gravity_comp::GravityCompensator comp(model);

const gravity_comp::VectorXd q = /* 当前关节角 */;
const gravity_comp::VectorXd tau_g = comp.gravity_torque(q);           // τ = g(q)
const gravity_comp::VectorXd tau_pd = comp.pd_plus_gravity(q, v, q_des, v_des);

文档

资源	说明
include/gravity_comp/	核心 API：PinModel、GravityCompensator
include/gravity_comp_mujoco/	MuJoCo 仿真与重力验证 API
examples/ur5e_mujoco_viewer.py	交互式 UR5e 演示脚本
Pinocchio 文档	动力学与 RNEA
MuJoCo Menagerie UR5e	仿真模型来源

许可证

本项目采用 Apache License 2.0 发布。