简介：面向机器人开发者的六轴机械臂运动规划与控制工程包，基于ROS2 Humble与MoveIt2框架搭建，专为fairino3_v6型号UR机械臂优化适配，适合具备一定ROS基础、希望快速上手工业机械臂规划的中高级学习者。压缩包共55个文件、约3.6MB，以Python规划脚本、YAML参数配置、STL网格模型、xacro/urdf/srdf机器人描述及rviz可视化配置为主，另含少量C++与srv服务定义，覆盖模型描述、规划配置与核心应用三层结构。资源包含fairino_description模型描述包与arm_planner-main规划主目录，提供逆运动学求解、碰撞检测、路径优化等插件能力，并附说明文件.txt讲解安装、配置与使用方法。目前已有101人学习，模块化解耦设计便于维护升级，可帮助读者省去底层搭建、直接开展定制化规划与控制开发。
1. 六轴机械臂上 ROS2 Humble + MoveIt2：为什么 fairino3_v6 值得单独拎出来做
很多人第一次接触六轴机械臂运动规划，都是从一个 URDF 文件开始的。你拿到一个fairino_description包，里面塞了七八个型号的模型描述，每个型号又有 visual、collision、关节限位、mesh 网格一大堆文件，结果ros2 launch一跑，RViz 里要么模型缺胳膊少腿，要么 MoveIt2 规划组报错说找不到 link。我踩过这个坑之后，习惯是把描述包做减法——只保留一个型号，把它的运动学链、碰撞体、规划组配置全部对齐。这次标题里的fairino3_v6就是被单独拎出来的那个六轴型号，配合 ROS2 Humble 和 MoveIt2，能跑通从模型加载、规划组配置到轨迹执行的完整链路。
这套东西解决的核心问题是：让一个六轴串联机械臂在 ROS2 生态里具备可规划、可仿真、可接真实控制器的能力。适合两类人——一类是刚拿到机械臂描述包、想在仿真里先验证运动规划的新手；另一类是要把 MoveIt2 接到真实控制器、需要一套干净配置做底座的熟手。下面按「模型怎么裁、MoveIt2 怎么配、规划怎么调、坑在哪」的顺序讲透。
2. 把 fairino_description 裁到只剩 fairino3_v6：URDF 与 mesh 的取舍
2.1 为什么不能直接删文件夹了事
fairino_description这类包通常长这样：urdf/下每个型号一个.urdf或.xacro，meshes/下按型号分文件夹放 STL 或 DAE，launch/里还有一堆display_xxx.launch.py。你如果直接把别的型号文件夹删掉，大概率会遇到两个问题：一是某个xacro里用<xacro:include>引了公共文件，删了之后解析直接报错；二是package.xml或CMakeLists.txt里 install 规则还指着已经不存在的路径，colcon build时警告刷屏。
常见做法是保留公共宏文件，只裁型号专属的 urdf 和 mesh。我一般会先跑一遍xacro解析，确认依赖关系再动手。
# 先看 fairino3_v6 的 xacro 到底 include 了哪些文件 cd ~/ros2_ws/src/fairino_description grep -rn "xacro:include" urdf/fairino3_v6*.xacro # 用 xacro 单独解析，确认没有断链 ros2 run xacro xacro urdf/fairino3_v6.urdf.xacro > /tmp/fairino3_v6_check.urdf check_urdf /tmp/fairino3_v6_check.urdfcheck_urdf是urdfdom提供的工具，输出里会打印整棵运动学树。如果它报Failed to find root link或者某个 joint 的 parent/child 对不上，说明裁剪时把公共 link 删了。参数上注意：xacro解析时如果用了arg，命令行要补name:=value，否则默认值不对会导致 mesh 路径拼错。
2.2 mesh 路径与 collision 简化
裁 mesh 时最容易翻车的是路径。URDF 里<mesh filename="package://fairino_description/meshes/fairino3_v6/link1.stl"/>这种写法，一旦你把 mesh 挪了位置，RViz 里模型就变成一堆白色方块或者干脆不显示。我一般保持meshes/fairino3_v6/这个层级不动，只删其他型号的文件夹。
另一个关键是 collision 几何体。很多描述包为了省事，visual 用精细 STL，collision 也直接用同一个 STL，结果 MoveIt2 做碰撞检测时计算量大得离谱，规划一次要好几秒。正确做法是 collision 用简化几何体，比如圆柱或包围盒。
<!-- visual 保留精细 mesh --> <visual> <geometry> <mesh filename="package://fairino_description/meshes/fairino3_v6/link3.stl"/> </geometry> </visual> <!-- collision 换成简化圆柱，半径和长度按连杆实际尺寸填 --> <collision> <geometry> <cylinder radius="0.06" length="0.28"/> </geometry> <origin xyz="0 0 0.14" rpy="0 0 0"/> </collision>radius和length要对着连杆实际包络填，宁可稍微大一点也别小，否则规划出的轨迹可能在实际执行时撞到。origin的xyz是 collision 几何体相对 link 坐标系的偏移，六轴臂的连杆通常沿 z 轴延伸，所以偏移量一般落在 z 上。
2.3 关节限位与规划组的对应关系
裁完模型后，URDF 里每个 joint 的<limit>必须和真实机械臂一致。fairino3_v6是六轴串联，joint1 到 joint6 的lower、upper、velocity、effort四个值一个都不能错。我见过有人直接抄了别的型号的限位，结果 MoveIt2 规划出的姿态在仿真里没问题，上真机直接触发硬限位报警。
<joint name="joint1" type="revolute"> <limit lower="-3.14159" upper="3.14159" effort="100" velocity="1.5"/> </joint>effort单位是 N·m，velocity是 rad/s。这两个值会影响 MoveIt2 的时间参数化，填太小会导致轨迹执行时速度被压得很低，填太大则可能让控制器超调。建议对着型号手册填，没有手册就用保守值先跑通。
3. MoveIt2 配置包怎么搭：从 SRDF 到规划组
3.1 用 MoveIt Setup Assistant 生成骨架
MoveIt2 在 Humble 里还是靠moveit_setup_assistant起配置包。命令是：
ros2 launch moveit_setup_assistant setup_assistant.launch.py进去之后加载你裁好的fairino3_v6.urdf，然后按顺序做几件事：生成自碰撞矩阵、定义虚拟关节（如果机械臂要固定在工作台上，就加一个virtual_joint把base_link连到world）、定义规划组、定义预设姿态、定义末端执行器、生成配置文件。
规划组这块是重点。六轴臂通常建一个arm_group，类型选Kinematic Chain，base link 选base_link，tip link 选tool0或link6。如果你后面要加夹爪，再建一个gripper_group，类型选Joint Model Group。
3.2 SRDF 里必须手改的三处
Setup Assistant 生成的 SRDF 不是万能的，有三处我每次都会手改。
第一处是disable_collisions。自碰撞矩阵生成时如果采样不够，会漏掉一些实际会碰的 link 对，或者多禁用了一些不该禁的。六轴臂的 link3 和 link5、link4 和 link6 在特定姿态下是会碰的，要确认这些对没被错误禁用。
第二处是group_state。预设姿态比如home、ready，关节角度要填实际值，不能全填 0，因为全 0 姿态对六轴臂来说往往不是自然姿态，甚至可能自碰撞。
第三处是end_effector。如果你有夹爪，parent_link要填link6，group填gripper_group，parent_group填arm_group。
<!-- SRDF 片段：规划组与预设姿态 --> <group name="arm_group"> <chain base_link="base_link" tip_link="tool0"/> </group> <group_state name="home" group="arm_group"> <joint name="joint1" value="0"/> <joint name="joint2" value="-0.785"/> <joint name="joint3" value="1.57"/> <joint name="joint4" value="0"/> <joint name="joint5" value="0.785"/> <joint name="joint6" value="0"/> </group_state>home姿态的关节值要保证机械臂处于一个舒展、不自碰的状态。joint2和joint3是决定大臂和小臂角度的，填的时候想象一下机械臂是往前伸还是往上抬。
3.3 控制器配置与 ros2_control
MoveIt2 要驱动仿真或真机，得配ros2_control。在配置包里改config/ros2_controllers.yaml，定义joint_state_broadcaster和arm_controller。
controller_manager: ros__parameters: update_rate: 100 # Hz joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster arm_controller: type: joint_trajectory_controller/JointTrajectoryController joints: - joint1 - joint2 - joint3 - joint4 - joint5 - joint6 command_interfaces: - position state_interfaces: - position - velocityupdate_rate一般设 100 到 500 Hz，仿真里 100 够用，真机要看控制器性能。command_interfaces选position还是velocity取决于你的驱动器支持哪种模式，六轴臂常见的是位置控制。state_interfaces至少要position，有velocity更好，MoveIt2 的时间参数化会用到。
4. 运动规划跑通：从 RViz 手动拖拽到 Python 脚本
4.1 启动文件与 RViz 插件
配置包生成后，启动 MoveIt2 的典型命令是：
ros2 launch fairino3_v6_moveit_config demo.launch.py这个 launch 会起move_group、robot_state_publisher、ros2_control的仿真节点，以及 RViz。RViz 里要确认 Motion Planning 插件加载了，Planning Group 选arm_group，然后就可以拖拽末端交互标记做规划。
如果 RViz 里模型显示正常但规划一直失败，先看move_group的终端输出。常见的是No motion plan found，原因可能是起始状态不在规划组里，或者目标姿态超出工作空间。
4.2 用 Python 发一个规划请求
手动拖拽只能验证链路，真正做系统要写代码。MoveIt2 的 Python 接口在 Humble 里是moveit_py，但更稳的是用rclpy加moveit_msgs直接发请求。下面是一个最小可复现的规划脚本。
import rclpy from rclpy.node import Node from moveit_msgs.srv import GetMotionPlan from moveit_msgs.msg import MotionPlanRequest, Constraints, JointConstraint from sensor_msgs.msg import JointState class PlanClient(Node): def __init__(self): super().__init__('plan_client') self.cli = self.create_client(GetMotionPlan, '/plan_kinematic_path') while not self.cli.wait_for_service(timeout_sec=1.0): self.get_logger().info('等待 /plan_kinematic_path 服务...') self.req = GetMotionPlan.Request() def build_request(self, joint_values): req = MotionPlanRequest() req.group_name = 'arm_group' req.num_planning_attempts = 10 req.allowed_planning_time = 5.0 req.max_velocity_scaling_factor = 0.3 req.max_acceleration_scaling_factor = 0.3 # 目标约束：每个关节给一个目标角度，容差 0.01 rad constraints = Constraints() for name, value in zip( ['joint1','joint2','joint3','joint4','joint5','joint6'], joint_values): jc = JointConstraint() jc.joint_name = name jc.position = value jc.tolerance_above = 0.01 jc.tolerance_below = 0.01 jc.weight = 1.0 constraints.joint_constraints.append(jc) req.goal_constraints.append(constraints) return req def send(self, joint_values): self.req.motion_plan_request = self.build_request(joint_values) future = self.cli.call_async(self.req) rclpy.spin_until_future_complete(self, future) return future.result() def main(): rclpy.init() node = PlanClient() result = node.send([0.0, -0.785, 1.57, 0.0, 0.785, 0.0]) if result and result.motion_plan_response.error_code.val == 1: print('规划成功，轨迹点数量:', len(result.motion_plan_response.trajectory.joint_trajectory.points)) else: print('规划失败，错误码:', result.motion_plan_response.error_code.val) rclpy.shutdown() if __name__ == '__main__': main()num_planning_attempts设 10 是给规划器多次随机采样机会，六轴臂在复杂约束下一次采样经常失败。allowed_planning_time设 5 秒是上限，超过就返回失败，别设太大否则卡住整个节点。max_velocity_scaling_factor和max_acceleration_scaling_factor设 0.3 是保守值，真机上先慢速验证，确认轨迹没问题再往上调。
error_code.val == 1是SUCCESS，其他值对应不同失败原因，比如-1是FAILURE，-2是PLANNING_FAILED。排查时先看这个码，再去move_group日志里找细节。
4.3 规划器选择与参数
MoveIt2 默认用 OMPL，规划器是 RRTConnect。六轴臂在空旷环境下 RRTConnect 够用，但如果工作空间里有障碍物、需要更平滑的轨迹，可以换RRTstar或PRM。在ompl_planning.yaml里改：
arm_group: default_planner_config: RRTConnect planner_configs: - RRTConnect - RRTstar - PRMRRTstar会花更多时间优化路径，适合对轨迹质量要求高的场景。PRM适合多次规划同一环境，可以预计算路线图。切换后记得把allowed_planning_time调大，否则优化还没收敛就超时了。
5. 避坑与排查：六轴臂 MoveIt2 最常见的五个翻车点
5.1 模型在 RViz 里显示但规划组是空的
现象：RViz 里机械臂模型正常，但 Motion Planning 插件的 Planning Group 下拉框是空的，选不了arm_group。
原因：SRDF 没被正确加载，或者move_group启动时找不到 SRDF 文件。常见于配置包路径没装进install/，或者 launch 里robot_description_semantic参数指向了错误路径。
解决：确认colcon build后install/fairino3_v6_moveit_config/share/下有config/fairino3_v6.srdf，然后检查 launch 文件里robot_description_semantic的textfile路径。用ros2 param get /move_group robot_description_semantic看实际加载的内容。
5.2 规划成功但轨迹执行时关节跳变
现象：规划返回成功，轨迹点数量也正常，但仿真里机械臂执行时某个关节突然跳一下，或者真机上触发报警。
原因：关节限位在 URDF 和控制器配置里不一致，或者轨迹里相邻点的关节角度差超过控制器能接受的范围。六轴臂的 joint4 和 joint6 是腕部关节，容易在奇异点附近出现大跳变。
解决：先对齐 URDF 的<limit>和ros2_controllers.yaml里的关节顺序、限位。然后在规划请求里把max_velocity_scaling_factor降到 0.1 再试，如果跳变消失说明是速度问题。还可以在ompl_planning.yaml里给规划组加longest_valid_segment_fraction: 0.005，让规划器更细地检查路径。
5.3 自碰撞矩阵误禁用导致规划撞自己
现象：规划出的轨迹在仿真里机械臂自己撞自己，但 MoveIt2 没报碰撞。
原因：Setup Assistant 生成自碰撞矩阵时采样不足，把实际会碰的 link 对标记成了disable_collisions。
解决：打开 SRDF，检查disable_collisions列表里有没有link3和link5、link4和link6这种腕部附近的组合。如果有，删掉对应的<disable_collisions>行，重新启动move_group。代价是规划变慢，但安全。
5.4 ros2_control 控制器起不来
现象：demo.launch.py跑起来后，ros2 control list_controllers显示arm_controller是unconfigured或inactive。
原因：ros2_controllers.yaml里控制器类型写错，或者joints列表里的关节名和 URDF 对不上。Humble 里joint_trajectory_controller的类型字符串是joint_trajectory_controller/JointTrajectoryController，少写斜杠或大小写错都会失败。
解决：用ros2 control list_hardware_interfaces看接口有没有加载，再用ros2 param get /controller_manager arm_controller.type确认类型。关节名要逐个对着 URDF 里的<joint name="...">核对。
5.5 规划时间忽长忽短，偶尔超时
现象：同样的起始和目标姿态，有时 0.5 秒规划出来，有时 5 秒超时。
原因：OMPL 是随机采样算法，RRTConnect 的成功率和时间本身就有波动。如果工作空间里有窄通道，波动会更明显。
解决：把num_planning_attempts从 10 提到 20，allowed_planning_time从 5 提到 10，用时间换成功率。如果还是不稳定，考虑换PRM并预计算路线图，或者把障碍物简化，减少窄通道。
6. 进阶：把规划轨迹接到真实控制器的验证方法
仿真跑通之后，下一步是接真实控制器。这里最容易出问题的是坐标系和单位。MoveIt2 规划出的轨迹是关节空间的位置序列，单位是弧度，时间戳是 ROS 时间。真实控制器如果用的是度或者脉冲数，中间要加一层转换。
我一般会先做一个「空跑验证」：把真实控制器设成不使能电机，只接收轨迹并打印每个点的关节角度和时间戳，和仿真里的轨迹逐点对比。确认一致后，再使能电机，用 0.05 的速度缩放因子跑一遍，观察实际关节是否跟随。
# 轨迹点对比：仿真轨迹 vs 控制器回传 def compare_trajectories(sim_points, real_points, tol=0.02): if len(sim_points) != len(real_points): print(f'点数不一致: sim={len(sim_points)}, real={len(real_points)}') return False for i, (s, r) in enumerate(zip(sim_points, real_points)): for j in range(6): diff = abs(s.positions[j] - r.positions[j]) if diff > tol: print(f'第{i}点 joint{j+1} 偏差 {diff:.4f} rad') return False print('轨迹一致') return Truetol设 0.02 弧度大约是 1.1 度，超过这个值说明控制器跟随有问题，可能是 PID 参数没调好或者机械臂有背隙。六轴臂的 joint2 和 joint3 负载大，背隙影响更明显，必要时在控制器里加前馈补偿。
还有一个技巧是给轨迹加velocity和acceleration字段。MoveIt2 默认只发位置，控制器自己做差分算速度，噪声大。如果控制器支持，在规划请求里让 MoveIt2 做时间参数化，把速度和加速度一起发下去，跟随会平滑很多。
最后说个我自己的习惯：每次改完 URDF 或 SRDF，先跑check_urdf和ros2 launch ... demo.launch.py确认仿真没问题，再动控制器配置。顺序反了，出了问题你都不知道是模型的事还是控制器的事。这套 fairino3_v6 的配置我前后调了大概两周，大部分时间花在自碰撞矩阵和控制器参数上，模型裁剪本身反而最快。希望帮到你。
本文还有配套的精品资源，点击获取
