MuJoCo逆运动学实操：从末端目标到关节力矩-编程实验室

MuJoCo逆运动学实操：从末端目标到关节力矩

【免费下载链接】mujocoMulti-Joint dynamics with Contact. A general purpose physics simulator.项目地址: https://gitcode.com/GitHub_Trending/mu/mujoco

写机械臂的关节控制时，末端要落在毫米级的目标点上，手推正运动学和动力学方程很费劲。把目标坐标交给 MuJoCo 的逆运动学链路，它会替你算出每个关节该出的力。本文只解决一件事：用 C API 跑通"末端目标 → 关节力"的最小闭环；路径规划和多自由度解析解不在范围内。

IK 在这条链路里到底干什么

逆运动学（IK，Inverse Kinematics）的输入是末端执行器在世界坐标系里的目标位置，输出是让末端朝目标运动的关节力向量。注意：它不直接吐出关节角度，角度是力矩经物理积分后"长出来"的结果，所以必须放进一个带时间推进的闭环里用。MuJoCo 在这件事上的定位很明确：它是带接触的高性能物理求解器，不是纯运动学库——mj_inverse、mj_jacSite这些函数全部基于当前完整的物理状态计算，而不是只读几何。

上图就是仓库里自带的两连杆臂（model/tendon_arm/arm26.xml），那些白点是 site。site 是整条 IK 链路的核心锚点：你给它一个名字，之后就能随时查询它的世界坐标，整个闭环都围着它转。

从 0 到跑通：最小可运行链路

① 准备 MJCF 模型

先把关节链骨架和末端 site 写进 MJCF（MuJoCo 的 XML 格式），下面的骨架是两自由度平面臂的最简形态：

<mujoco model="ik-arm"> <option timestep="0.002" integrator="RK4"/> <worldbody> <body name="link1" pos="0 0 0.5"> <joint name="j1" type="hinge" axis="0 1 0" range="-1.5 1.5" damping="1"/> <geom type="capsule" fromto="0 0 0 0.4 0 0" size="0.05"/> <body name="link2" pos="0.4 0 0"> <joint name="j2" type="hinge" axis="0 1 0" range="-2.5 0.5" damping="1"/> <geom type="capsule" fromto="0 0 0 0.35 0 0" size="0.04"/> <site name="tip" pos="0.35 0 0" size="0.02"/> </body> </body> </worldbody> <actuator> <motor joint="j1" gear="10"/><motor joint="j2" gear="8"/> </actuator> </mujoco>

site就是你要控制的"那个点"，IK 闭环全部围绕tip的世界坐标展开
两个 hinge（铰链）关节构成 2 自由度臂，关节数与后面ctrl的维度对应
gear是电机的传动比，后面要把关节力矩换算成电机输入时会用到
每个关节都写了range和damping，行程限制是后文一个高频坑的解药

② 加载与初始化

这一步把 XML 编译成可仿真模型，并准备好承载运行时状态的mjData：

char errmsg[200]; mjModel* m = mj_loadXML("ik_arm.xml", 0, errmsg, sizeof(errmsg)); if (!m) { printf("load failed: %s\n", errmsg); return 1; } mjData* d = mj_makeData(m); mj_resetData(m, d); // 复位到 keyframe 初始姿态 mj_forward(m, d); // 先跑一次正运动学，把 qacc 等量同步好

第二个参数是 vfs（虚拟文件系统）句柄，传0表示走默认本地磁盘，读内存或网络模型才需要自定义
末尾如果也想传0做错误缓冲长度，加载失败就只剩一句笼统报错；像上面这样传errmsg长度，XML 写错时能看到具体原因
mjData是纯内存结构，同一个模型可以开多份，方便并排比较不同状态
这里先调一次mj_forward，保证qacc等字段是"算过的"而不是脏数据，后面的mj_inverse才可靠

③ 控制循环

控制回调注册到全局钩子mjcb_control，mj_step每步推进前会自动调它，这是 MuJoCo 写控制器的标准位置：

void ik_control(const mjModel* m, mjData* d) { int tipid = mj_name2id(m, mjOBJ_SITE, "tip"); mjtNum endpos[3], err[3]; mj_sitePosition(m, d, tipid, endpos); for (int i = 0; i < 3; i++) err[i] = target[i] - endpos[i]; for (int i = 0; i < m->nv; i++) d->qacc[i] = 20 * err[2] - 8 * d->qvel[i]; // 先写期望加速度 mj_inverse(m, d); // 再反解所需关节力 for (int j = 0; j < m->nu; j++) d->ctrl[j] = d->qfrc_inverse[j] / m->actuator_gain[j]; } mjcb_control = ik_control;

说白了，mj_inverse是"逆动力学"而不是角度求解器：它把d->qacc当成"系统必须达到的加速度"，反算出让整条链恰好产生这个加速度的各关节力，结果存在d->qfrc_inverse。所以顺序不能反——先想清楚要什么加速度，再让引擎算出实现它需要的力，力转成电机输入后，由物理推进让关节角自己跟上。

为什么先设qacc再调mj_inverse：函数的提问方向是"达成这个加速度要多少力"，qacc是它唯一的输入条件
qfrc_inverse维度是m->nv（每个关节一份），ctrl维度是m->nu（每个执行器一份），两者不总是一比一
电机执行器里ctrl会乘上传动比生效，所以除以actuator_gain把关节力矩换算回电机输入
这里把末端误差简化映射到各自由度加速度；要严格映射可用mj_jacSite取雅可比再解伪逆

④ 渲染验证

最后套一个 GLFW 窗口把仿真画出来，结构照抄仓库示例，几行就能跑：

mjvCamera cam; mjvOption opt; mjvScene scn; mjrContext con; mjv_defaultCamera(&cam); mjv_defaultOption(&opt); mjv_defaultScene(&scn); mjr_defaultContext(&con); mjv_makeScene(m, &scn, 5000); mjr_makeContext(m, &con, mjFONTSCALE_150); while (!glfwWindowShouldClose(window)) { while (d->time - t0 < 1.0/60.0) mj_step(m, d); // 按实时推进 mjv_updateScene(m, d, &opt, NULL, &cam, mjCAT_ALL, &scn); mjr_render(viewport, &scn, &con); glfwSwapBuffers(window); glfwPollEvents(); }

内层while把仿真按 1/60 秒的帧预算跑满，模拟快于实时时正好对齐 60 fps
mjv_updateScene把当前状态搬进场景，mjr_render只负责把场景画进缓冲
想拖鼠标看视角，回调接法见sample/basic.cc，不展开

参数调错了会怎样：3 个高频坑

坑 1：timestep 取 0.05，末端一动就抖

症状：臂一启动就高频抖动，几个步长后数值炸成 NaN，tip飞出视野
根因：积分器一步迈太大，步内接触力和 PD 误差的突变采不到，离散误差被增益放大成发散
改法：timestep收到 0.002~0.005；嫌 CPU 不够看就用子步长（substeping）而不是加大步长

坑 2：积分器留 Euler，末端总在小圈里飘

症状：目标能追到，但tip永远在误差 1~2 mm 的圈上打转，长时间运行缓慢漂移
根因：Euler 是一阶积分，截断误差随步数线性累积，接触和冲击状态会把误差进一步放大
改法：XML 里integrator="RK4"，计算量翻几倍但控制代码一行不用动

坑 3：关节没设 range，目标一偏就"跑飞"

症状：目标稍微超出工作空间，臂就把关节顶到极限甚至反向猛转，IK 解彻底失控
根因：没有limited="true"加range时，mj_inverse会老老实实算出任何加速度对应的力，物理上没有挡杆阻止关节无限转
改法：所有 hinge 都补上行程限制；同时把目标坐标钳位到可达工作空间内，双保险

一个完整场景：仓储机械臂货架取放

仓储机械臂在货架间做取放：视觉系统每次给出一个取点（世界坐标，容差 ±1 mm），机械臂要把夹爪末端从货架上方任意姿态带到该点并保持 200 ms 再放行。取点每单都变，代码里不可能写死关节角，只能吃"目标坐标 + 到位判据"——正好就是第 2 节闭环的输入形态。控制回路骨架如下：

void pick_control(const mjModel* m, mjData* d) { mjtNum endpos[3], err[3]; mj_sitePosition(m, d, gripper_site, endpos); for (int i = 0; i < 3; i++) err[i] = pick_pt[i] - endpos[i]; mjtNum r2 = 0; for (int i = 0; i < 3; i++) r2 += err[i] * err[i]; if (r2 > 1e-4) { // 未到位：继续跟踪 for (int i = 0; i < m->nv; i++) d->qacc[i] = 20 * err[0] - 8 * d->qvel[i]; mj_inverse(m, d); for (int j = 0; j < m->nu; j++) d->ctrl[j] = d->qfrc_inverse[j] / m->actuator_gain[j]; } else { // 已到位：松力保持 for (int j = 0; j < m->nu; j++) d->ctrl[j] = 0; } }