企业官网建设流程全解析

机械臂如何避障绕障？mujoco-learning中OMPL RRT路径规划完全指南

【免费下载链接】mujoco-learning项目地址: https://gitcode.com/gh_mirrors/mu/mujoco-learning

🤖 机械臂避障，为什么需要路径规划？

mujoco-learning是一个基于 MuJoCo 的机器人学习开源项目，其中用OMPL + RRT（RRTConnect）算法实现了机械臂关节空间的避障、绕障路径规划，并提供可交互的可视化仿真。

直接让机械臂从 A 位姿插值到 B 位姿，中间关节很容易撞上桌子、障碍物甚至自己的躯干。路径规划要回答的问题是：在关节空间中找出一条全程不碰撞的关节角序列，让末端执行器安全地从起点绕到终点。

MuJoCo 中的机器人仿真场景：机械臂需要在充满障碍物的工作空间中安全运动

📌 本指南聚焦两个核心脚本：

• rrt_obstacle.py：完整流程——IK 求起点/终点 → RRTConnect 避障规划 → 逐步执行
• path_plan_ompl_rrtconnect.py：最小示例——给定两个关节角，直接 RRTConnect 规划

📐 RRT 与 RRTConnect：绕障原理 3 分钟讲清

RRT（Rapidly-exploring Random Tree，快速扩展随机树）的核心思想非常直观：

1. 从起点出发，在关节空间内不断随机采样新点；
2. 找到离它最近的已有节点，朝它扩展一小步；
3. 新点如果无碰撞（通过 MuJoCo 碰撞检测判定）就保留，否则丢弃；
4. 树不断生长，直到"碰到"终点。

RRTConnect是它的加速版：从起点和终点同时生长两棵树，相遇即成路径。对"从当前位姿绕障去目标"这类双向任务，收敛速度比单向 RRT 快得多，所以项目中选用了它。

📦 快速开始：环境安装步骤

项目已用uv管理好依赖，安装只需两步：

git clone https://gitcode.com/gh_mirrors/mu/mujoco-learning cd mujoco-learning uv sync

uv sync会自动安装 pyproject.toml 中的 mujoco、ompl、numpy 等全部依赖。

💡 若 rrt_obstacle.py 使用 IK 求解器，还需按 README 中的可选步骤编译带 CasADi 的 Pinocchio（bash scripts/install_pinocchio.sh）。

🧩 代码拆解：rrt_obstacle.py 的完整避障流程

打开 rrt_obstacle.py，整个流程由 4 步组成，正好对应"求位姿 → 建空间 → 避障规划 → 执行"。

第 1 步：IK 求解起点与终点关节角（getBipolarJoints）

runBefore中设定了末端目标位置（示例中为0.4, 0.3, 0.4），脚本调用 src/pinocchio_kinematic.py 中的Kinematics.ik()做逆运动学，把"末端想去哪里"翻译成"每个关节应该转到多少"，得到start_dof和goal_dof。

第 2 步：建立带关节限位的状态空间（pathPlaning）

state_space = ob.RealVectorStateSpace(self.model.nq) bounds = ob.RealVectorBounds(self.model.nq) for i in range(min(self.model.nq, self.model.jnt_range.shape[0])): bounds.setLow(i, self.model.jnt_range[i, 0]) bounds.setHigh(i, self.model.jnt_range[i, 1]) state_space.setBounds(bounds)

这一段把 MuJoCo 模型中每个关节的物理限位（jnt_range）交给 OMPL 的RealVectorStateSpace——采样永远发生在合法关节范围内，这是绕障的第一道保险。

第 3 步：碰撞检测器——避障的灵魂

def is_state_valid(state): self.data.qpos[:7] = [state[i] for i in range(7)] mujoco.mj_step(self.model, self.data) return self.data.ncon == 0

这是整个项目最关键的一段：把候选关节角写进 MuJoCo，前向仿真一步，然后检查data.ncon（当前接触对数量）是否为 0。

✅ 没有碰撞 → 状态合法，节点保留 ❌ 碰到障碍物/地面/自身 → 状态非法，节点丢弃

RRT 的"绕障"能力，本质上就是这个验证器在成千上万次采样中反复筛选的结果。

第 4 步：RRTConnect 求解与路径执行

planner = og.RRTConnect(si) planner.setRange(0.01) # 每次扩展步长 planner.setIntermediateStates(True) # 输出中间密集路径点 planner.setProblemDefinition(pdef) solved = planner.solve(3.0) # 最多 3 秒

求解成功后，runFunc每帧把路径点写回qpos，机械臂就沿这条绕开障碍物的轨迹运动；按下↓键还能随机生成新任务重新规划。

⚙️ 3 个关键参数调优：setRange / setIntermediateStates / timeout

另外，rrt_obstacle.py 在createTask中做了10 次重试：RRT 是基于随机采样的算法，单次失败不代表无解，重试是标准做法。

🚧 障碍物从哪来？动态添加障碍物

rrt_obstacle.py 启动时会调用addObstacles()，向模型动态注入一个半径 0.06 的球形障碍（位于机械臂运动路径上）：

self.obstacles_type.append("sphere") self.obstacles_size.append([0.06]) self.obstacles_pos.append([0.3, 0.2, 0.5])

它的实现在 src/mujoco_viewer.py 的addObstacles方法中：直接解析场景 XML 的<worldbody>，插入带contype="1" conaffinity="1" mass="0.0"的<geom>节点——质量设为 0 保证障碍是静态的，contype/conaffinity 设为 1 保证参与碰撞计算，这两点缺一不可。

如果想要更多、更随机的障碍，项目还提供了 add_random_obstacles.py：在工作空间内随机生成 N 个球体障碍。生成的场景 XML（如 model/franka_emika_panda/scene_with_obstacles.xml）可直接作为规划场景使用。

⚖️ 两个脚本怎么选？

项目里还有进阶方案：path_plan_pyroboplan_rrt.py 用 PyRoboPlan 库做 RRT + 轨迹优化，适合追求平滑轨迹的场景，可作下一步学习。

🐛 避障常见失败排查清单

1. "No solution found."：起点或终点本身就是碰撞状态；超时太短。→ 先单独验证起终点ncon == 0，再调大 timeout 并多重试几次。
2. 规划"成功"但手臂仍撞障碍：多半是障碍物<geom>少了contype="1" conaffinity="1"，碰撞根本没开。
3. 路径抖/不平滑：setRange太大或没开setIntermediateStates。→ 调小 range（如 0.01）、开启中间状态。
4. 规划极慢：setRange太小导致扩展太碎。→ 适当放大，或缩小工作空间（限位范围）。
5. IK 解不合理导致绕大圈：IK 是多解问题。→ 换随机初值重试，或结合关节限位筛选解。

🎯 总结

mujoco-learning 用不到 200 行代码，展示了一条机械臂避障路径规划的完整链路：

末端目标 → Pinocchio IK 解出关节角 → OMPL RRTConnect 在关节空间随机采样 → MuJoCo 碰撞检测筛选无碰撞节点 → 得到避障路径 → 逐点执行

核心结论只有三条：状态空间用关节限位约束、合法性用 MuJoCo 的ncon判定、参数用 range + 超时 + 重试组合调。读懂 rrt_obstacle.py，你就能把这套避障框架迁移到任意 MuJoCo 机器人模型上。

【免费下载链接】mujoco-learning项目地址: https://gitcode.com/gh_mirrors/mu/mujoco-learning