三机械臂gazebo+rviz+moveit仿真过程问题及解决
本文参考代码来源：https://github.com/AntoBrandi/Robotics-and-ROS-2-Learn-by-Doing-Manipulators/tree/main
问题一：gazebo界面时间卡顿导致控制无法连接
解决方案：
（1）用meshlab减少stl文件面数
1.下载与安装
-
官网下载
-
Windows/Mac/Linux 都有对应版本，安装即可。我下的windows版本。
2. 导入模型
-
打开 MeshLab。
-
直接把你的 STL 文件（比如 armA_link1.stl）拖进去，或者 File -> Import Mesh。
-
查看当前面数：看界面底部的信息栏，找到 Faces。
-
如果显示 75,000，那就是太大了。
-
我们的目标是把它降到 1,000 ~ 3,000 左右（或者原大小的 5%）。
-
3. 执行简化操作 (Decimation)
-
在顶部菜单栏找到：
Filters (滤镜) -> Remeshing, Simplification and Reconstruction (重网格化与简化) -> Quadric Edge Collapse Decimation (二次误差边折叠简化)。
(这是最常用的保留形状的简化算法)
4. 设置参数 (关键步骤)
弹出的窗口里有几个参数，只需要关注这几个：
-
Target number of faces (目标面数)：你可以直接填一个数字。我设置的都是1000-2000
-
Percentage reduction (百分比)：或者填 0.05 (代表保留 5%，即减少 95%)。通常建议先填 0.1 (10%) 试试。
-
Preserve Boundary of the mesh (保留边界)：一定要勾选！ 这能防止模型边缘破损。
-
Preserve Topology (保留拓扑结构)：一定要勾选！ 防止模型出现破洞。
5. 应用与检查
-
点击 Apply。
-
此时你会发现模型表面的网格变得很稀疏，原来的圆滑曲面变成了有棱角的。
-
看底部的 Faces 数量，确认已经变小了。
6. 导出文件
-
File -> Export Mesh As...
-
文件名：建议改为 armA_link1_collision.stl（加上后缀以示区分）。
-
格式：选择 .stl。
-
保存时会弹出一个框，取消勾选 "Binary encoding" (可选，文本格式方便调试，二进制更小，通常默认即可)，其他保持默认，点击 OK。
每次只调整第一行的数字即可。
把减少面数的文件保存到原本的文件夹路径下，然后更改urdf.xacro文件
<link name="agv_base_link">
<inertial>
<origin xyz="0.0057835 0.0046209 -0.0611" rpy="0 0 0" />
<mass value="36.922" />
<inertia ixx="1.865" ixy="0.045437" ixz="-6.9959E-06" iyy="3.7561" iyz="-0.00014461" izz="5.5837" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<!-- Visual 使用原始高精度模型 -->
<mesh filename="${base_mesh_path}agv_base_link.STL" />
</geometry>
<material name="white">
<color rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<!-- Collision 使用 MeshLab 生成的低模 -->
<mesh filename="${base_mesh_path}agv_base_link_collision.stl" />
</geometry>
</collision>
</link>（2）controller.launch.py中正确设置较长的控制器加载时长
终端报错提示：spawner: error: unrecognized arguments: --timeout 120。
这意味着你修改 controller.launch.py 时用的参数名不对（ROS 2 版本差异）。
请回到 VS Code，修改 src/three_arms_controller/launch/controller.launch.py。
查找：
"--timeout", "120"
全部替换为：
"--controller-manager-timeout", "120"
joint_state_broadcaster_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=[
"joint_state_broadcaster",
"--controller-manager",
"/controller_manager",
# 增加超时时间，确保所有控制器都加载完成,我添加的
"--controller-manager-timeout", "120"
],
)
这条报错消失
（3）gazebo界面物理引擎设置
gazebo版本是Ignition Gazebo (6.16.0)
右下角0.55%意味着仿真世界的时间流逝速度只有现实世界的 0.5%（慢了200倍）
点击右上角菜单：
找到界面最右上角的三个垂直小圆点（⋮）图标（在关闭按钮下方）。
搜索插件：
在弹出的搜索框里输入：Physics。
添加插件：
你会看到一个叫 Physics 的搜索结果。点击它。
调节参数：
现在屏幕右侧应该会出现一个新的面板（或者混在右侧列表中）。找到它，尝试修改以下数值：（1）Max Step Size (最大步长)：默认通常是 0.001。请尝试改成 0.004 或 0.01。改完后按回车，观察右下角的百分比是否飙升。
我最后设置为0.01
（2）Real Time Update Rate (实时更新率)：默认是 1000。请尝试改成 0（意思是让它全力跑，不限制速度）。
我设置0了但是好像只能设置为0.000001？
如果这一步做完，百分比能上升到 60% 以上，你的机械臂马上就会“硬”起来，控制器也会连接成功！
我操作后确实看到百分比最高接近百分之六十，控制器也连接成功了。
这两个数值根据实际仿真效果调节！不固定
有时候不调节只是会连接控制器速度变慢，也能正常连控制器
问题二：solidworks导出惯性参数有问题
解决方案；
全使用default_inertial宏
目前没观察到有什么不良影响
问题三：solidworks 导出urdf 时未勾选的结构也成为了stl文件的一部分
之前曾经对这个三机械臂结构用solidworks导出过一个urdf文件，之前就是一个固定底座连接三个六自由度机械臂，但是后来因为底座结构改变，并且底座下面连接了一个agv小车，所以我用新的solidworks文件生成了一个新的urdf文件，生成urdf文件的时候我以这个agv小车为base_Link，link_components选择的时候我也只勾选了小车的部分，然后我选择固定平台作为base_link的child_link，然后在固定平台下我定义了三个child_link分别为三个六自由度机械臂的第一关节，到此为止我导出了一个新的urdf文件，然后结合我上一次导出urdf文件进行整合，通过meshlab查看的时候发现这个文件几乎包含我的新solidworks文件的所有结构，不仅有小车结构，还有三个机械臂的完整结构甚至包含机械臂末端夹持的结构，但是明明在第二次生成urdf文件的时候没有勾选过机械臂除了第一关节之外的部分...
解决方案：
这是 SolidWorks URDF Exporter 插件的一个常见“坑”：
当你定义 base_link 时，如果你在 Feature Tree（左侧特征树）里选择的是顶层装配体，或者没有显式地排除其他部件，插件有时会默认“该链接包含了当前可见的所有模型”。即便你后续定义了 child links，第一步生成的 STL 往往是个“大杂烩”。
使用 MeshLab 进行“手术切除”
-
重置选择：
-
如果现在画面上一片红，先点击菜单栏 Filters -> Selection -> None（或者按快捷键 Ctrl + Shift + D，不同版本可能不同），确保所有东西都变回灰白色，没有任何东西被选中。
-
-
使用连接选择工具：
-
再次点击工具栏上的 "Select Connected Components in a region" 图标。
-
-
点选垃圾（机械臂）：
-
直接点击第一条多余的机械臂。
-
观察：只有那条手臂变红了，小车还是白的。
-
按 Delete 键（删除）。
-
-
重复操作：
-
点击第二条机械臂 -> 变红 -> 删除。
-
点击第三条机械臂 -> 变红 -> 删除。
-
-
清理残余（如果有）：
-
如果删掉手臂后，发现空中还悬浮着一些细碎的小零件或夹爪碎片，继续用这个工具点选并删除。
-
如果有一些很难点中的小碎渣，可以使用 "Select Vertices in a rectangular region"（矩形框选工具），框住它们，然后删除。
-
问题四：机械臂无力下垂
解决方案：
（1）修改 URDF 中的力矩限制 (Effort)
我们需要把所有关节的最大力气改大。
请打开 src/three_arms_description/urdf/three_arms.urdf.xacro。
利用 查找/替换 功能：
-
查找：effort="100" (或者 SolidWorks 导出的其他默认值，可能是 30, 50 等)
-
替换为：effort="10000" (直接给个大数，仿真里不用担心电机烧坏)
-
同时建议：把 velocity="1" 改成 velocity="5" 或 10，不然机器人动起来像树懒。
貌似是solidworks生成的urdf里面就是没有这一部分内容的？所以要自己补一下。
因为目前在仿真阶段所以我设置数值很大
<limit effort="10000" velocity="10"/>
这个改完机械臂就不下垂了
（2）在 YAML 文件中添加 PID 增益 (Gains)
我们需要告诉控制器：“请用很大的力气去保持位置”。
请修改 src/three_arms_controller/config/three_arms_controllers.yaml。
我们需要在每个手臂控制器的 ros__parameters 下面，添加一个 gains 模块。
这个改完才可以用指令控制机械臂移动到相应位置
ros2 topic pub --once /armA_controller/joint_trajectory trajectory_msgs/msg/JointTrajectory "{joint_names: [armA_joint1, armA_joint2, armA_joint3, armA_joint4, armA_joint5, armA_joint6], points: [{positions: [0.5, 0.5, 0.5, 0.5, 0.5, 0.5], time_from_start: {sec: 2, nanosec: 0}}]}"可以source之后用这个检查机械臂能不能按照预期动作
注意区分：effort模式和position模式（controllers.yaml文件里修改）
情况 A：Effort 模式👉 必须要有 PID
-
原理：
-
Effort 模式 意味着 ROS 端的控制器（JointTrajectoryController）负责计算“为了到达目标位置，电机需要输出多大的力矩”。
-
计算公式：力矩=P×(目标位置−实际位置)+D×(目标速度−实际速度)+…
-
-
如果你在 YAML 里删掉了 gains (PID)：
-
控制器找不到 P, I, D 参数，默认就会把它们当成 0。
-
计算结果：力矩=0×误差=0。
-
后果：控制器虽然在运行，但它发给电机的指令永远是 0 牛顿米。你的机械臂会瞬间失去力量，再次像面条一样瘫软倒地。
-
结论：只要你的 command_interfaces 写的是 effort，YAML 里的 PID 就是机器人的“灵魂”，绝对不能删。
情况 B：Position 模式（我目前用的模式）👉 YAML 里非必须，但 URDF 里必须有刚度
-
原理：
-
Position 模式 意味着 ROS 控制器只负责发号施令：“去 30 度！”。
-
它不负责计算力矩，直接把“30度”这个数字扔给 Gazebo。
-
Gazebo 内部的物理引擎 负责施加力气把关节以此固定住。
-
-
为什么 YAML 里的 PID 没用？
-
因为在 Position 模式下，JointTrajectoryController 只是做轨迹插值，不进行力矩闭环运算，所以它常常忽略 YAML 里的 gains。
-
-
为什么之前会下垂？
-
因为 Gazebo 内部默认的“虚拟弹簧”太软了（或者默认刚度太低）。
-
如果要解决 Position 模式的下垂，你通常需要去 URDF 文件的 <gazebo> 标签里写底层的 PID 参数（而不是在 YAML 里）。
-
问题五：关于moveit文件的自动配置
解决方案：
（1）准备与启动
复杂结构不适合手写srdf文件，应该用MoveIt Setup Assistant解决，先安装
sudo apt install ros-humble-moveit-setup-assistant
对应工作空间下source之后
ros2 launch moveit_setup_assistant setup_assistant.launch.py（2）加载模型
点击左侧 Start。点击 Create New MoveIt Configuration Package。
Browse：找到src/three_arms_description/urdf/three_arms.urdf.xacro。
点击 Load Files。如果加载成功，右侧会出现机器人模型。
（3）生成碰撞矩阵 (Self-Collisions) —— 关键
这一步能大幅提高运行效率。
-
点击左侧 Self-Collisions。
-
Sampling Density (采样密度) 保持默认即可。
-
点击 Generate Collision Matrix。会看到进度条在跑，助手正在计算几万次随机碰撞，来确定哪些关节天生就是挨着的。
两个名词
Adjacent Links (相邻连杆)
-
定义：在 URDF 结构树中，直接通过关节（Joint）连接在一起的两个连杆（父 Link 和子 Link）。例如 armA_link1 和 armA_link2。
-
状态：因为它们由关节物理连接，所以在数学模型上它们永远是接触的。
-
处理方式：MoveIt 必须禁用这两者之间的碰撞检测。如果不禁用，MoveIt 会认为机器人一直处于“碰撞”状态，从而导致你永远无法规划任何路径。
-
可靠性：100% 可靠。这是由 URDF 拓扑结构决定的，助手绝不会搞错。
Never in Collision (永不碰撞)
-
定义：这两个连杆虽然没有直接相连，但在机器人的物理运动范围内，无论如何扭动，它们都碰不到对方。
-
例子：
-
armA_link1（Arm A 的根部）和 armB_link1（Arm B 的根部）。因为它们固定在底座的不同位置，距离很远，动不了，所以永远碰不到。
-
有时候 link1 和 link3 也可能碰不到，因为关节角度有限制。
-
-
处理方式：MoveIt 建议禁用这两者之间的检测。
-
目的：为了省电、省时间。如果不禁用，每次规划路径时，计算机都要傻傻地算一遍 Arm A 的根部有没有撞到 Arm B 的根部，纯属浪费算力。
助手生成矩阵是否百分百可靠？
自动生成的矩阵原理上是随机采样 (Random Sampling)。
它会在后台把你的机器人随机摆成几万个姿势（比如 10,000 次），然后检查每一对连杆是否发生了碰撞。如果 10,000 次随机姿势里，Link A 和 Link B 一次都没碰到过，助手就会大胆地推断：“这俩肯定永远碰不到，标记为 Never in Collision！”
对于复杂结构最好自己再检查一遍，尤其是跨手臂之间的碰撞关系。
检查步骤：
-
在 Setup Assistant 界面中：
点击 Self-Collisions 标签页，生成矩阵后，你会看到一个列表。 -
重点关注“跨手臂”的对子：
-
比如：armA_linkX 和 armB_linkY。
-
或者：armA_linkX 和 armC_linkY。
-
-
如何判断是否需要修改？
请运用你的物理直觉：-
问自己：Arm A 的末端如果不小心，能打到 Arm B 的这一节吗？
-
如果答案是“能打到”：而在列表里这一行被勾选了（即被禁用了检测），这就是错误的！
-
修改方法：点击复选框，取消勾选（Uncheck）。
-
选中（Disable） = 不检测碰撞（省性能，有风险）。
-
未选中（Enable） = 开启检测（安全，费性能）。
-
-
原则：宁可错杀，不可放过。 如果你不确定两个部件能不能碰到，那就不要禁用检测（保持未选中状态）。虽然计算慢一点，但至少安全。
-
特别需要保留检测（不要勾选）的关系：
-
Arm A 所有 Link <-> Arm B 所有 Link （除了根部那些离得十万八千里的）。
-
Arm A 所有 Link <-> Arm C 所有 Link。
-
机械臂末端 <-> AGV 底座 / Platform （防止手臂打到自己的底座）。
检查了一下只有一个地方要注意：
助手识别到platform_base_link和armB_link2是collision by default的关系，意思是当你的机器人处于初始姿态（所有关节角度为 0）时，这两个零件的网格（Mesh）就已经穿模（互相重叠）了。
要勾选吗？ 必须勾选（Disable）。如果不勾选，MoveIt 会认为机器人一开机就已经“撞车”了，从而拒绝规划任何路径。
出现这个红色警告的原因通常有两个：
结构紧凑：你的机械臂设计非常紧凑，Link 2 在 0 度位置时，物理距离离底座平台太近，导致两个零件的 Collision 模型（哪怕是低模）发生了轻微的重叠。
零位姿态问题：SolidWorks 导出时的“零位”可能是一个折叠姿态，在这个姿态下，机械臂是“缩”在底座里的。
虽然勾选可以解决报错，但你最好在 RViz 里看一眼：
-
在 Setup Assistant 右侧的 3D 视图里，把关节都设为 0。
-
看一看 armB_link2 是不是严重插入了 platform_base_link 内部？
-
如果是轻微蹭到：直接勾选禁用，没问题。（是这种情况）
-
如果是严重穿模（比如 Link 2 一半都在底座里）：说明你的 URDF 原点（Origin） 或者 SolidWorks 装配关系 可能有错。这种情况下，虽然勾选能跑，但仿真效果是不真实的。
-
如果你已经生成了包，关闭了助手，后来发现不仅需要检测，或者想优化性能禁用某些检测，你可以直接修改文件，不用重跑助手。
文件位置：
src/three_arms_moveit/config/three_arms_description.srdf
修改方法：
打开 .srdf 文件，你会看到底部的 <disable_collisions> 标签：
<disable_collisions link1="armA_link1" link2="armA_link2" reason="Adjacent" />
<disable_collisions link1="armA_link1" link2="armB_link1" reason="Never" />-
如果你想开启碰撞检测：直接删除对应的那一行代码。
-
如果你想禁用碰撞检测：手动添加一行代码。
（4）定义虚拟关节
-
点击左侧 Virtual Joints。
-
Virtual Joint Name: 输入 virtual_joint。
-
Child Link: 选择 world (如果你上一轮加了 world link) 或者 agv_base_link（用的这个）。
-
Parent Frame Name: 输入 world。
-
Joint Type: 选择 fixed (因为你的底座暂时不动)。
-
点击 Save。
（5）定义规划组——最核心步骤
建立三个独立的规划组
虽然你的机器人是一个整体，但在 MoveIt 中，我们通常把**每一条可以独立运动的运动链（Kinematic Chain）**定义为一个单独的组。这样你既可以单独控制 Arm A，也可以以后再建一个“大组”把它们合起来。
在 MoveIt Setup Assistant 的 Planning Groups 界面点击下方的 Add Group 按钮：
1. Basic Settings (基本设置)
-
Group Name (组名): 输入 arm_a
-
这是你在代码里调用它的 ID，建议用小写下划线。
-
-
Kinematic Solver (运动学解算器): 必须选择！
-
点击下拉菜单，选择 kdl_kinematics_plugin/KDLKinematicsPlugin。
-
解释：默认是 None，必须手动选。KDL 是 ROS 默认的数值解算器，支持任意关节数量的机械臂，虽然速度不是最快（相比 IKFast），但兼容性最好，配置最简单，非常适合初次配置。
-
-
Kin. Search Resolution (搜索分辨率): 保持默认 0.005。
-
Kin. Search Timeout (超时时间): 保持默认 0.005。
-
Kin. Solver Attempts (尝试次数): 保持默认 3。
-
Group Default Planner (默认规划器): 保持 None 即可。
-
解释：MoveIt 默认使用 OMPL (Open Motion Planning Library) 作为规划后端。如果你选 None，它会自动加载 OMPL 的默认配置（通常是 RRTConnect）。这完全没问题，以后可以在 yaml 文件里改。
-
2. Add Kinematic Chain (添加运动链) —— 最关键的一步
既然你的结构是 Platform 连接三个手臂，我们需要定义这条链的**“起点”和“终点”**。MoveIt 会自动把中间的所有关节和连杆包含进来。
-
点击右侧的蓝色按钮 Add Kin. Chain。
-
Base Link (基座连杆):
-
点击 Choose Selected 按钮，在树状列表里选择。
-
建议选择：platform_base_link。
-
原因：这是三条手臂共同的“根”。虽然选 agv_base_link 也可以，但 platform 离手臂更近，作为运动学的参考系更直观。
-
-
Tip Link (末端连杆):
-
点击 Choose Selected。
-
选择：armA_link6 (或者你定义的 Arm A 的最后一个 Link)。
-
-
点击 Save。
现在 arm_a 已经建好了，请重复上面的步骤建立另外两个：
配置 Arm B
-
Add Group。
-
Name: arm_b。
-
Solver: kdl_kinematics_plugin/KDLKinematicsPlugin。
-
Add Kin. Chain:
-
Base Link: platform_base_link。
-
Tip Link: armB_link6。
-
-
Save。
配置 Arm C
-
Add Group。
-
Name: arm_c。
-
Solver: kdl_kinematics_plugin/KDLKinematicsPlugin。
-
Add Kin. Chain:
-
Base Link: platform_base_link。
-
Tip Link: armC_link6。
-
-
Save。
Q: 我的 AGV 小车需要建一个组吗？
-
A: 暂时不需要。 因为你的 AGV 是固定在 world 上的（通过 fixed joint），它是不可移动的基座。如果你以后给 AGV 加了轮子控制（Diff Drive），那时候再单独处理。现在只关注机械臂即可。
Q: 为什么不选 armA_link1 作为 Base Link？
-
A: 也可以选。但是通常建议 Base Link 选择不动的那个父物体（Platform）。这样规划的时候，MoveIt 知道 Link 1 是相对于 Platform 转动的。如果选 Link 1 做 Base，有时候坐标系变换会少一截（虽然 KDL 也能处理，但 Platform 更符合逻辑）。
Q: 为什么 Planner 选 None？
-
A: 这里的 Planner 指的是特定的算法配置。选 None 并不代表没有规划器，而是代表使用 MoveIt 默认的 OMPL 管道。这是最标准、最稳妥的做法。
做完这一步后，你的 Planning Groups 列表里应该有三行：
-
arm_a | KDL | platform_base_link -> armA_link6
-
arm_b | KDL | platform_base_link -> armB_link6
-
arm_c | KDL | platform_base_link -> armC_link6
（6）预设姿态 (Robot Poses) —— 可选（这步我设置了）
定义一些常用的姿势，方便调试。
-
点击左侧 Robot Poses。
-
点击 Add Pose。
-
Name: home_a。
-
Planning Group: 选择 arm_a。
-
拖动滑块设置一个姿势（或者全 0）。
-
点击 Save。
(建议为三条手臂都设置一个 home 姿势)
（7）配置 ROS 2 控制器 (ROS 2 Controllers) —— 连接仿真的关键
-
点击左侧 ROS 2 Controllers。
-
点击 Auto Add FollowJointTrajectoryControllers (自动添加)。助手通常会自动读取你的 URDF 结构，尝试帮你生成。
-
检查生成结果：应该看到类似 armA_controller, armB_controller, armC_controller 的项。
-
如果自动添加失败或不全，请手动编辑 (点击 Edit Selected):
Name: 必须填 armA_controller (必须和你 controllers.yaml 里的名字一模一样！)。Type: 选择 FollowJointTrajectory。Action Namespace: 输入 follow_joint_trajectory (这非常重要，如果不填或填错，MoveIt 发不出指令)。Joints: 在右侧列表里，选中属于 Arm A 的所有关节(对 Arm B 和 C 重复此检查)
我看到生成arm_a_controller之类，类别都是joint_trajectory_controller/JointTrajectoryController
改名字的时候出现报错invalid controller name。原因可能是：格式限制：某些版本的 Setup Assistant 对名称检查非常严格（可能不喜欢大写字母，虽然 ROS 2 支持）。类型不匹配：自动生成的 Type 填的是 joint_trajectory_controller/JointTrajectoryController，这其实是控制器的插件类型，而不是 MoveIt 需要的接口类型（MoveIt 需要的是 FollowJointTrajectory）。
关于接口，我的MoveIt Setup Assistant 版本较新，所以接口类型选择joint_trajectory_controller/JointTrajectoryController就可以
接下来操作：
第一步：在助手里“骗”过它 (填小写)
请按照这个配置添加控制器（以 Arm A 为例）：
-
Controller Name: arma_controller
-
(注意：这里故意填全小写，为了让助手不报错，能顺利生成文件)
-
-
Type: joint_trajectory_controller/JointTrajectoryController
-
(选你看到的那个长名字)
-
-
Action Namespace: follow_joint_trajectory
-
(这个必须填对，全小写)
-
我的界面没有这一项，这一项在moveit controllers里
-
Joints: 选中 Arm A 的所有关节。
(请同样用小写 armb_controller 和 armc_controller 把另外两个加进去)
第二步：完成后续步骤并生成包
-
配置 MoveIt Controllers（自动添加即可，它会映射刚才的小写名字）。
-
填写作者信息。
-
点击 Generate Package 生成代码到 src/three_arms_moveit。
-
关闭 Setup Assistant。
第三步：手动修正名字 (连接的关键！)
文件生成后，我们需要去修改生成出来的配置文件，把名字改回大写，这样 MoveIt 才能找到 Gazebo 里的真身。
请用 VS Code 打开新生成的 src/three_arms_moveit/config/ros2_controllers.yaml。
你会看到类似这样的内容：
# MoveIt 生成的（错误的，因为是小写）
controller_manager:
ros__parameters:
update_rate: 100 # 或者其他频率
arma_controller: # <--- 这里是小写，要改！
type: joint_trajectory_controller/JointTrajectoryController
armb_controller: # <--- 要改！
type: joint_trajectory_controller/JointTrajectoryController
# ...
arma_controller: # <--- 这里也是小写，要改！
ros__parameters:
# ...相应的地方改一下
（8）MoveIt 控制器 (MoveIt Controllers)
-
点击左侧 MoveIt Controllers。
-
点击 Auto Add FollowJointTrajectoryControllers。
-
这会自动把刚才定义的 ROS 2 控制器映射到 MoveIt 内部。
-
确保 arm_a 规划组 对应 armA_controller。
-
我这一步的界面长的和上一步差不多。
（9）作者信息 (Author Information)
-
点击左侧 Author Information。
-
随便填个名字和邮箱 (比如 xhn, xhn@todo.com)，不填不让生成。
（10）生成文件 (Configuration Files)
-
点击左侧 Configuration Files。
-
Configuration Package Save Path:
-
点击 Browse。
-
进入你的 src 文件夹。
-
新建一个文件夹，命名为 three_arms_moveit。
-
点击 Open。
-
-
点击 Generate Package。
-
等待生成完毕，点击 OK，退出助手。
选择文件的时候所有文件都保持勾选
（11）添加一个launch文件联合仿真
手动创建文件src/three_arms_moveit/launch/moveit_gazebo.launch.py
内容如下：（后面有更新）
import os
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from ament_index_python.packages import get_package_share_directory
def generate_launch_description():
# 获取包路径
moveit_config_pkg = get_package_share_directory('three_arms_moveit')
description_pkg = get_package_share_directory('three_arms_description')
# 声明仿真时间参数 (关键！让 MoveIt 和 RViz 与 Gazebo 同步)
use_sim_time = LaunchConfiguration('use_sim_time', default='true')
# 1. 启动 Gazebo 环境 (包含模型加载 + 控制器启动)
# 注意：这里假设你的 gazebo.launch.py 里已经包含了 controller.launch.py 的调用
# 如果没有，你需要在这里额外 include controller.launch.py
gazebo_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(description_pkg, 'launch', 'gazebo.launch.py')
)
)
# 2. 启动 MoveIt 的核心节点 (move_group)
# MoveIt Setup Assistant 自动生成的 launch 文件通常叫 move_group.launch.py
move_group_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(moveit_config_pkg, 'launch', 'move_group.launch.py')
),
launch_arguments={'use_sim_time': use_sim_time}.items()
)
# 3. 启动 RViz (可视化界面)
# MoveIt 自动生成的 RViz 启动文件
rviz_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(moveit_config_pkg, 'launch', 'moveit_rviz.launch.py')
),
launch_arguments={'use_sim_time': use_sim_time}.items()
)
return LaunchDescription([
gazebo_launch,
move_group_launch,
rviz_launch
])build和source之后运行ros2 launch three_arms_moveit moveit_gazebo.launch.py
发现报错
parameter 'robot_description_planning.joint_limits.armA_joint1.max_velocity' has invalid type: expected [double] got [integer]
MoveIt配置文件中关节限制参数类型错误
同时
Could not parse the SRDF XML File. Error=XML_ERROR_EMPTY_DOCUMENT
Unable to parse SRDF
Robot model not loaded
说srdf文件为空，无法读取，但是检查发现这个文件就存在于对应路径下。
解决第一个问题src/three_arms_moveit/config/joint_limits.yaml这个文件是自动生成的，手动把整数都改成小数再运行不报错，成功。
问题六：gazebo仿真时间没对齐
点击rviz界面的plan可以看到机械臂动作了，但是点击execute看不到gazebo同步动作。终端看到下面报错：
[moveit_ros.current_state_monitor]: Didn't receive robot state (joint angles) with recent timestamp within 1.000000 seconds.
[moveit_ros.trajectory_execution_manager]: Failed to validate trajectory: couldn't receive full current joint state within 1s
MoveIt 发现收到的关节状态数据（从 /joint_states 话题来的）的时间戳，比当前系统时间慢了太多（或者因为仿真时间同步问题，MoveIt 觉得那是“过期的”旧数据），所以为了安全，它拒绝执行。
这在 Gazebo 仿真中非常常见，因为仿真时间通常比真实时间慢，或者两者没对齐。
解决方案：
一开始添加了一些原版代码没有的东西，比如controller_manager_node_name插件节点、手动指定 use_sim_time、IMU 插件等。但是始终没有解决问题。
后来仍然使用原版代码思想：
three_arms_gazebo.xacro这个代码保持原版结构不动
gazebo.launch.py虽然原版是分开启动的。但我的代码进行了一些修改：
（1）新增：自动包含并启动 three_arms_controller/launch/controller.launch.py。这样你只需要运行这一个文件，Gazebo、机器人模型、控制器就会全部跑起来
（2）参数传递：在启动控制器时，自动传入 is_sim:=True，确保控制器知道现在是仿真环境
src/three_arms_controller/config/three_arms_controllers.yaml也是和原版保持一致
然后运行发现问题还是没有被解决。
最终决定强制重启Broadcaster
当 Gazebo 启动时，有时候硬件接口（Hardware Interface）还没完全准备好，或者仿真时间（Clock）**还没开始走，joint_state_broadcaster 就已经急匆匆地启动了。结果它读不到数据，就进入了“假死”状态。
我们要在仿真正在运行的时候，手动把它关掉再重开。
仿真运行且已点击播放后，另开一个终端：
（1）检查目前话题：
ros2 control list_controllers可以看到
armC_controller joint_trajectory_controller/JointTrajectoryController active
armB_controller joint_trajectory_controller/JointTrajectoryController active
joint_state_broadcaster joint_state_broadcaster/JointStateBroadcaster active
armA_controller joint_trajectory_controller/JointTrajectoryController active
（2）依次输入：停用——卸载——重新加载并激活
ros2 control set_controller_state joint_state_broadcaster inactive看到：Successfully deactivated joint_state_broadcaster
ros2 control unload_controller joint_state_broadcaster看到：
[INFO] [1764229589.977113969] [_ros2cli_1675422]: waiting for service /controller_manager/unload_controller to become available...
Successfully unloaded controller joint_state_broadcaster
ros2 control load_controller joint_state_broadcaster --set-state active看到：Successfully loaded controller joint_state_broadcaster into state active
（3）检查话题是否正常发布：
ros2 topic echo /joint_states --once看到输出：
header:
stamp:
sec: 9
nanosec: 680000000
frame_id: base_link
name:
- armA_joint1
- armA_joint3
- armA_joint4
- armA_joint5
- armC_joint6
- armC_joint5
- armC_joint4
- armC_joint1
- armA_joint2
- armB_joint2
- armB_joint6
- armC_joint3
- armB_joint5
- armC_joint2
- armA_joint6
- armB_joint4
- armB_joint3
- armB_joint1
position:
- -6.7657326955668565e-06
- -4.051983736426617e-06
- -1.2807300280586095e-05
- -1.2537599979046168e-05
- 7.172104635510067e-07
- 1.0104492905360621e-05
- 9.56276739529717e-06
- -9.632513181245359e-06
- 7.975296515841719e-06
- 0.009614815019778164
- -8.025437121469618e-07
- 6.249623959058608e-06
- -2.620704939434351e-06
- -6.7408790610594205e-06
- 1.2940628347339058e-05
- 1.0861867749836579e-07
- -0.01395508984821419
- -0.007131254300364624
velocity:
- -1.734723475976807e-18
- 0.0
- -3.469446951953614e-18
- 1.734723475976807e-18
- -7.589415207398531e-19
- -1.734723475976807e-18
- 0.0
- 0.0
- 0.0
- 0.00098464360475543
- -3.0938793194046355e-15
- -8.673617379884035e-19
- 4.331170838645093e-15
- 0.0
- 0.0
- 5.9380417249954576e-12
- -0.001387882402360572
- -0.0007445472029915141
effort:
- .nan
- .nan
- .nan
- .nan
- .nan
- .nan
- .nan
- .nan
- .nan
- .nan
- .nan
- .nan
- .nan
- .nan
- .nan
- .nan
- .nan
- .nan
---
这次看到header.stamp.sec > 0即时间戳不再是 0 了
两个小问题：
（1）这此，点击plan之后rviz里面的机械臂按照预期动作了，然后点击execute之后gazebo界面迟迟不动作，execute按钮也一直处于被按下的灰色。然后我看到gazebo界面右下角的百分比是百分之五以下，手动调节了右侧栏中的physics中的step size使得右下角百分比接近百分之百，gazebo里机械臂就相应动作了。
（2）切换到arm_b之后
第一次execute没有反应，又重复plan+execute之后gazebo里机械臂有反应了
第一次日志报错：
Invalid Trajectory: start point deviates from current robot state more than 0.01
joint 'armB_joint1': expected: -0.0124765, current: -0.124776
原因如下：
初始抖动：Gazebo 刚启动时，机械臂可能会因为重力或物理碰撞稍微“下沉”一点点（从完美的 0.0 变成 -0.12）。
状态不同步：当你点击 Plan 时，MoveIt 是基于那一瞬间的状态规划的。但在你点击 Execute 之前，机械臂可能又轻微晃动了一点点。
结果：MoveIt 发现规划的起点和当前实际位置差了 0.1弧度，为了安全，它拒绝执行。
解决方法：
重新 Plan 一次：这会基于最新的稳定位置重新规划，然后 Execute 就能成功了。
或者放宽容忍度：在 moveit_controllers.yaml 里把 allowed_start_tolerance 设大一点（比如 0.05 或 0.1）。
避免每次都手动输入这三条指令所以直接写入src/three_arms_moveit/launch/moveit_gazebo.launch.py
在 Launch 文件启动后，等待一段足够长的时间（比如 20 秒，等 Gazebo 彻底稳定），然后自动在后台执行你刚才那三条命令。
import os
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, ExecuteProcess, TimerAction # <--- 引入了 ExecuteProcess 和 TimerAction
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
from launch_ros.actions import Node
from moveit_configs_utils import MoveItConfigsBuilder
def generate_launch_description():
# ========================================================================
# 1. 加载 MoveIt 配置
# ========================================================================
moveit_config = MoveItConfigsBuilder("three_arms_description", package_name="three_arms_moveit").to_moveit_configs()
# ========================================================================
# 2. 启动 Gazebo 环境
# ========================================================================
gazebo_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(get_package_share_directory('three_arms_description'), 'launch', 'gazebo.launch.py')
)
)
# ========================================================================
# 3. 手动定义 Move Group 节点
# ========================================================================
run_move_group_node = Node(
package="moveit_ros_move_group",
executable="move_group",
output="screen",
parameters=[
moveit_config.to_dict(),
{'use_sim_time': True},
{'trajectory_execution.allowed_execution_duration_scaling': 2.0},
{'trajectory_execution.allowed_goal_duration_margin': 0.5},
{'publish_robot_description_semantic': True}
],
)
# ========================================================================
# 4. 手动定义 RViz 节点
# ========================================================================
rviz_config_file = os.path.join(
get_package_share_directory("three_arms_moveit"), "config", "moveit.rviz"
)
rviz_node = Node(
package="rviz2",
executable="rviz2",
name="rviz2",
output="log",
arguments=["-d", rviz_config_file],
parameters=[
moveit_config.robot_description,
moveit_config.robot_description_semantic,
moveit_config.planning_pipelines,
moveit_config.robot_description_kinematics,
{'use_sim_time': True}
],
)
# ========================================================================
# 5. 静态 TF 发布
# ========================================================================
static_tf = Node(
package="tf2_ros",
executable="static_transform_publisher",
name="static_transform_publisher",
output="log",
arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "agv_base_link"],
parameters=[{'use_sim_time': True}]
)
# ========================================================================
# 6. [新增] 自动重启 joint_state_broadcaster (你的手动操作自动化版)
# ========================================================================
# 逻辑：等待 25 秒 (给足时间让 Gazebo 启动并变卡)，然后自动执行“重启三连”
auto_fix_joint_state = TimerAction(
period=25.0, # 如果你的电脑启动 Gazebo 很慢，可以把这个时间改得更长，比如 30.0 或 40.0
actions=[
ExecuteProcess(
cmd=[
'bash', '-c',
# 这就是你刚才手动输的那三行命令，用分号隔开
'echo ">>> 开始自动修复 joint_state_broadcaster..."; ' +
'ros2 control set_controller_state joint_state_broadcaster inactive; ' +
'ros2 control unload_controller joint_state_broadcaster; ' +
'ros2 control load_controller joint_state_broadcaster --set-state active; ' +
'echo ">>> 修复完成！"'
],
output='screen',
name='joint_state_fixer'
)
]
)
return LaunchDescription([
gazebo_launch,
run_move_group_node,
rviz_node,
static_tf,
auto_fix_joint_state # <--- 别忘了把这个加进去
])问题七：gazebo里physics里的step size不调节就看不到gazebo界面机械臂同步动作
其实是如果不调节，机械臂其实也在动，但它处于“慢动作回放”模式，慢到以为它静止了。
现象解析：
-
默认状态：你的 Gazebo 右下角显示 RTF (Real Time Factor) 是 0.55% 左右（或者更低）。这意味着现实世界过 100秒，仿真世界才过 0.55秒。
-
你的错觉：你点击 Execute 后，MoveIt 确实把指令发过去了，电机也开始转了。但是因为时间流逝太慢，机械臂可能每分钟才转动 0.01 度。肉眼看起来就像“不动”。
-
手动调节后：你把 Step Size 调大，或者把 Update Rate 调成 0，RTF 飙升到了 60% 甚至 100%。这时候仿真速度接近现实速度，你自然就看到机械臂“正常”运动了。
为什么 RTF 这么低？
尽管我们做了这么多优化（低模、惯性重置），但对于你的 18 轴机器人 加上 三条手臂互相连接的复杂结构，默认的物理步长（Step Size = 0.001s）对你的 CPU 来说依然太吃力了。
-
原因：物理引擎需要在每个 0.001 秒内，解算 18 个关节的动力学方程 + 碰撞检测。如果你的 CPU 算这一步需要 0.1 秒，那么仿真速度就只能是现实速度的 1% (0.001 / 0.1)。
终极解决方案：把你的“手动调节”固化到代码里（我没改）
可以修改 Gazebo 启动参数，打开 src/three_arms_description/launch/gazebo.launch.py
找到启动 Gazebo 的部分，把 -r (run) 后面加上物理参数的配置。
Ignition Gazebo 允许通过 --physics-engine 参数或者直接加载包含物理配置的 SDF 文件。但最简单的办法是指定更新频率。
或者，更推荐的方法是创建一个 empty.sdf 配置文件，放在你的包里。
问题八：如何
-
同时规划三条手臂（建立一个 all_arms 规划组）。
-
或者编写 Python 脚本，让三条手臂自动执行复杂的协作任务。
有“AI”的1024 = 2048，欢迎大家加入2048 AI社区
更多推荐
19
0- 0
已为社区贡献1条内容
所有评论(0)