urdf、rviz2、ros2_control、Moveit2、Gazebo仿真综合ROS2仿真机械臂项目实战笔记

声明：该文是本人在b站学习up主 后来老师 和 路一直都在456 共创的 机械臂仿真实战课程的一篇笔记，仅作整理思路和复习用，写的不太严谨，如有侵权请联系。

推荐各位去看看两位up主的b站学习链接：https://www.bilibili.com/video/BV1SBunzJEdE?vd_source=85ae0151c01007a142f774fee35d0e85

Moveit2介绍

功能

这是一个内部集成了多种轨迹规划器、碰撞检测、规划组的运动控制库，主要用于机械臂的控制。

得益于moveit2配置助手，我们仅需一个含有基本标签（visual、collision、internia）的urdf就可以通过一步步的简单图形化界面配置得到一个带有ros2_control控制器以及moveit规划器的机械臂，我认为这是moveit2最强大的地方。在配置助手中，我们还可以可视化地配置一些预设动作，方便测试和完成简单任务。

它还预留了编程接口，使得我们能在代码中使用几个简单的控制函数即可控制机械臂。

通过moveit配置得到一堆配置文件之后，再将xacro插件和launch稍加改动即可在gazebo中联动。

补充：ros2_control是一个集成了资源管理器、控制器、底层硬件接口的控制库。它以资源管理器为中心（controller_manager），连接了上层的控制器（controllers）以及下层的硬件接口（hardware_interface），核心设计思想是在接入不同硬件的时候只需要修改hardware_interface的函数与下位机联系，而上层应用（控制器）能够被复用，高层常用算法不用再“造轮子”。

原理

moveit2是基于ros2_control编写的高层规划运动库，实际的控制执行器是由ros2_control提供的。相当于ros2_control是moveit2的下位机，moveit2负责更高层的每一个move_group运动规划和碰撞检测，而ros2_control负责控制信号（关节状态）的发出。

Gazebo介绍

功能

用于进行模型的仿真，运动仿真等。内部集成了物理引擎。

使用方法

将带有gazebo标签的urdf文件通过launch导入到节点中即可

通过定义sdf模型可以自定义gazebo中的模型（sdf文件可定义摩擦力）

综合的仿真机械臂项目介绍

环境与ROS2库

ubuntu24.04、ros2 jazzy、moveit2、rviz2、gazebo（默认最新版）、joint_state_publisher_gui

学习大纲

各模块介绍

urdf

（连杆：碰撞、惯性、可视部分（形状、颜色）origin；关节：旋转、origin）

连杆：

    <link name="link1">
        <inertial>
            <origin xyz="0.0 0.0 0.05" rpy="0.0 0.0 0.0"/>
            <mass value="1.0"/>
            <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.001"/>
        </inertial>
        <visual>
            <geometry>
                <cylinder radius="0.05" length="0.1"/>
            </geometry>
            <origin xyz="0.0 0.0 0.05" rpy="0.0 0.0 0.0"/>
            <material name="pink">
                <color rgba="1.0 0.0 1.0 1.0"/>
            </material>
        </visual>
        <collision>
            <geometry>
                <cylinder radius="0.05" length="0.1"/>
            </geometry>
            <origin xyz="0.0 0.0 0.05" rpy="0.0 0.0 0.0"/>
        </collision>
    </link>

关节：

<joint name="joint1" type="revolute">
        <parent link="base_link"/>
        <child link="link1"/>
        <origin xyz="0.0 0.0 0.1" rpy="0.0 0.0 0.0"/>
        <axis xyz="0.0 0.0 1.0"/>
        <limit lower="-3.14" upper="3.14" effort="100.0" velocity="1.0"/>
    </joint>

launch：在launch中打开带有配置参数的多节点，通过launch启动多节点

rviz2：将模型可视化、moveit可视化

moveit2：配置urdf为能动的机械臂

gazebo：夹取物体仿真（未做）

ros2_control: controller_manager、hardware_interface(略讲)、controllers、在urdf中的插件、yaml配置

各部分的关联性

1. 从0手搓可以动的机械臂：urdf ->加入ros2_control控制插件以及配置yaml文件 得到ros2_control.urdf -> 运行在rviz2中（可用joint_state_publisher_gui功能包进行初步的可视化关节控制） -> 自己编写控制节点代替掉joint_state_publisher_gui（通过/arm_controller/joint_trajectory话题进行控制，也可以通过动作控制（未完成）） -> 最终得到能在rviz中通过代码动起来的机械臂。

2. **moveit2搓出可以动的机械臂：**urdf -> 使用moveit2得到已经集成了控制器的xacro模型 -> 在rviz2中使用（moveit插件）可视化执行运行。

相当于moveit给我们省去了配置ros2_control的工作，在此基础上还多了规划组分类、预设动作、可视化显示、简化编程接口等功能，非常强大。

3. 由从moveit2配置后的模型得到能在gazebo中运行的机械臂： 只需要在xacro、launch里面增加gazebo与ros2_control通讯的插件、gazebo启动节点、gazebo与ros的时钟桥接，gazebo中加载机械臂的节点，即可。下文会仔细讲解。

正式开始

文件目录：

ARM_WS/
├── build/
├── install/
├── log/
└── src/
    ├── arm1/                      ---->从0手搓可以动的机械臂 功能包
    │   ├── __pycache__/
    │   ├── __init__.py
    │   ├── test_arm_topic_control.py    ---> 使用话题通讯控制arm规划组
    │   ├── test_hand_control.py		---> 使用话题通讯控制hand规划组
    │   ├── config/
    │   │   ├── arm_controllers.yaml	---> ros2_control控制器配置文件
    │   │   └── rviz.rviz				---> rviz2页面配置文件
    │   ├── launch/
    │   │   ├── arm_ros2_control_urdf_launch.py  ---> 在rviz2中打开带有ros2_control控制器的机械臂，并打开手臂控制节点
    │   │   ├── arm_urdf_launch.py               ---> 在rviz2中打开原始的机械臂模型
    │   │   └── hand_control_launch.py			---> 在rviz2中打开带有ros2_control控制器的机械手，并打开夹爪控制节点
    │   ├── resource/
    │   ├── test/
    │   ├── urdf/
    │   │   ├── arm_ros2_control.urdf         ---> 带有ros2_control控制器的机械臂模型
    │   │   ├── arm.urdf					  ---> 原始的机械臂模型
    │   │   └── robot.urdf					  ---> 简单的urdf模型示例
    │   ├── LICENSE
    │   ├── package.xml
    │   ├── setup.cfg
    │   └── setup.py						 ---> 编译配置文件
    ├── moveit_config/            ----> moveit2搓出可以动的机械臂(由moveit配置助手得到的文件)
    │   ├── config/
    │   │   ├── initial_positions.yaml
    │   │   ├── joint_limits.yaml         --->关节限位，需要将整数改为浮点数、并把a、v、p限位改为true！！！
    │   │   ├── kinematics.yaml
    │   │   ├── moveit_controllers.yaml
    │   │   ├── moveit.rviz
    │   │   ├── pilz_cartesian_limits.yaml
    │   │   ├── robot_name.gazebo.ros2_control.xacro
    │   │   ├── robot_name.ros2_control.xacro   ----> moveit2生成的带ros2_control标签的模型
    │   │   ├── robot_name.srdf					---->机械臂描述文件
    │   │   ├── robot_name.urdf.xacro			----> moveit2生成的原始模型
    │   │   └── ros2_controllers.yaml			---> ros2_control控制器配置文件
    │   ├── launch/
    │   │   ├── demo.launch.py                 ---->配置完moveit后就可以跑的机械臂（集成了下面其他的launch）
    │   │   ├── gazebo.launch.py			  ----->自己编写改造的，用于打开gazebo中跑机械臂模型
    │   │   ├── move_group.launch.py
    │   │   ├── moveit_rviz.launch.py
    │   │   ├── rsp.launch.py
    │   │   ├── setup_assistant.launch.py
    │   │   ├── spawn_controllers.launch.py
    │   │   ├── static_virtual_joint_tfs.launch.py
    │   │   └── warehouse_db.launch.py
    │   ├── .setup_assistant
    │   ├── CMakeLists.txt
    │   └── package.xml
    └── moveit_motion_api/      ----> 使用moveit2的python编程接口完成控制
        ├── config/
        │   └── moveit_cpp.yaml  ---> 使用编程接口前所需的配置文件（基本是固定模板）
        ├── moveit_motion_api/
        │   ├── __pycache__/
        │   ├── __init__.py
        │   └── test1.py        ---> 使用moveit接口编程实现对机械臂的控制（基本是固定模板）
        ├── resource/
        ├── test/
        ├── LICENSE
        ├── package.xml
        ├── setup.cfg
        └── setup.py          ---> 节点编译配置

arm功能包——从launch入手介绍由urdf、rviz、ros2_control完成对仿真机械臂的建模与控制（moveit的基础知识）

我将从launch出发来一个个介绍节点各自的作用，以及各组件之间如何连接产生通讯（什么话题、服务、joint_state）,为了方便学习，我以行内注释的方式来介绍各个节点作用：

from launch import LaunchDescription
from ament_index_python import get_package_share_directory
from launch_ros.actions import Node

package_path = get_package_share_directory('arm1')
print("package_path:",package_path)
urdf_path = package_path + "/urdf/arm_ros2_control.urdf"
ros2_control_yaml_path = package_path + "/config/arm_controllers.yaml"
rviz_config_path = package_path + "/config/rviz.rviz"
robot_desc = open(urdf_path).read()          #得到urdf描述文件

def generate_launch_description():
    robot_desc_node = Node(                 #该节点将urdf描述文件解析，得到并发布每个关节的状态
        package="robot_state_publisher",
        executable="robot_state_publisher",
        name = "robot_state_publisher",
        output = "both",
        parameters=[
            {"use_sim_time":True},
            {"robot_description":robot_desc}
        ],
    )
    
    rviz_node = Node(                   #以打开个性化配置好之后的rviz界面，和直接打开rviz没什么区别。
        package="rviz2",
        executable="rviz2",
        name= "rviz",
        arguments=["-d",rviz_config_path],
    )
     
    contorller_manager_node = Node(    #打开ros2_control的中心管理节点，使用ros2_control的控制器时固定需要打开
        package="controller_manager",
        executable="ros2_control_node",
        output = "both",
        parameters=[ros2_control_yaml_path],
    )
    
              #生成由yaml配置好的ros2_control控制器，argument里面的控制器需要与arm_controllers.yaml内的名称一致
    contorller_node = Node( 
        package="controller_manager",
        executable="spawner",
        name = "controller_node",
        output = "both",
        arguments=['arm_controller','hand_controller','joint_state_broadcaster'],
    )
    
    test_arm_topic_control = Node(     #打开自己编写的控制节点（通过ros2_control控制器暴露出来的话题进行通讯）
        package="arm1",
        executable="test_arm_topic_control",
    )
    return LaunchDescription(
        [robot_desc_node,          #解析urdf、发布关节信息
         rviz_node,				   #打开rviz
         contorller_manager_node,  #打开ros2_control
         contorller_node,		   #生成ros2_control控制器
         test_arm_topic_control    #机械臂控制节点
        ]
    )

moveit_config——使用moveit2配置助手快速将urdf配置为可以在rviz2中可视化运动的机械臂

这位博主写的蛮好的，参考这个即可：【Moveit2】使用moveit_setup_assistant配置自己的机械臂功能包-CSDN博客

注意：

1. 需要所有规划组相关的配置同名

2. 目前moveit2官方存在的问题：需要手动修改joint_limit.yaml的整数为浮点数并将各个关节的加速度、速度、位置限制改为true

---

拓展：

添加gazebo相关的插件，实现通过在rviz2中的控制，控制gazebo中的机械臂运动。

1. 需要修改ros_control.urdf的hardware中的插件为gz_ros2_control/GazeboSimSystem

2. 除此之外，需要增加gazebo组件，来建立和ros2_control的链接。

3. 在launch中，需要打开gazebo的节点，需要将机械臂传入gazebo的节点，需要一个转化的时钟，以及其他的配置如图：
   ，前两个就是gazebo相关的节点、第三个是连接ros2和gazebo的时钟信息类型转化节点、第四个是将urdf转化为关节信息的节点、第五个是打开rviz，第六个是打开ros2_control中的controller_manager节点的，第七个是ros2_control生成控制器的，第八个是moveit的算法节点。

4. 注意：第七个是ros2_control生成控制器的参数配置要与yaml中的相对应：

moveit_motion_api——使用moveit2的Python编程接口，用代码实现对机械臂的控制

1. moveit_cpp.yaml，要规范声明各种规划器、planner_id等

planning_scene_monitor_options:
  robot_description: "robot_description"
  joint_state_topic: "/joint_states"
  attached_collision_object_topic: "/moveit_cpp/planning_scene_monitor"
  publish_planning_scene_topic: "/moveit_cpp/publish_planning_scene"
  monitored_planning_scene_topic: "/moveit_cpp/monitored_planning_scene"
  wait_for_initial_state_timeout: 10.0

planning_pipelines:
  pipeline_names: ["ompl", "pilz_industrial_motion_planner", "chomp"]

plan_request_params:
  planning_attempts: 1
  planning_pipeline: ompl
  max_velocity_scaling_factor: 1.0
  max_acceleration_scaling_factor: 1.0

ompl_rrtc: # Namespace for individual plan request
  plan_request_params: # PlanRequestParameters similar to the ones that are used by the single pipeline planner
    planning_attempts: 5 # Number of attempts the planning pipeline tries to solve a given motion planning problem
    planning_pipeline: ompl # Name of the pipeline that is being used
    planner_id: "RRTConnectkConfigDefault" # Name of the specific planner to be used by the pipeline
    max_velocity_scaling_factor: 1.0 # Velocity scaling parameter for the trajectory generation algorithm that is used.
    max_acceleration_scaling_factor: 1.0 # Acceleration scaling parameter for the trajectory generation algorithm that is used.
    planning_time: 5.0 # Time budget for the motion plan request. If the planning problem cannot be solved within this time, the request will be aborted.

pilz_lin:
  plan_request_params:
    planning_attempts: 1
    planning_pipeline: pilz_industrial_motion_planner
    planner_id: "PTP"
    max_velocity_scaling_factor: 1.0
    max_acceleration_scaling_factor: 1.0
    planning_time: 0.8

chomp_planner:
  plan_request_params:
    planning_attempts: 1
    planning_pipeline: chomp
    max_velocity_scaling_factor: 1.0
    max_acceleration_scaling_factor: 1.0
    planning_time: 1.5

2. Python中导入接口相关的包、写固定格式的moveit2代码框架。

3. 常用的控制函数示例。 2. 3. 的完整代码如下：

import time
import rclpy
from rclpy.logging import get_logger
from moveit.core.robot_state import RobotState
from moveit.planning import (
    MoveItPy,
    MultiPipelinePlanRequestParameters,
)
from moveit_configs_utils import MoveItConfigsBuilder
from geometry_msgs.msg import PoseStamped


def plan_and_execute(
    robot,
    planning_component,
    logger,
    single_plan_parameters=None,
    multi_plan_parameters=None,
):
    """一个辅助函数，用于规划和执行轨迹。"""
    logger.info("Starting trajectory planning.")
    
    plan_result = None
    if multi_plan_parameters is not None:
        plan_result = planning_component.plan(
            multi_plan_parameters=multi_plan_parameters
        )
    elif single_plan_parameters is not None:
        plan_result = planning_component.plan(
            single_plan_parameters=single_plan_parameters
        )
    else:
        logger.info("No specific parameters provided. Planning with defaults.")
        plan_result = planning_component.plan()

    if plan_result and plan_result.trajectory:
        logger.info("Planning succeeded. Executing plan.")
        robot_trajectory = plan_result.trajectory
        
        execution_result = robot.execute(robot_trajectory, controllers=[])
        
        if execution_result:
            logger.info("Plan executed successfully.")
        else:
            logger.error("Plan execution failed!")
            
    else:
        logger.error("Planning failed. No valid trajectory found.")

def main():
    # MoveItPy Setup
    rclpy.init()
    logger = get_logger("moveit_py_pose_goal")

    path = "/home/lzy/arm_ws/src/moveit_motion_api/config/moveit_cpp.yaml"
    print(f'moveit cpp config path is :{path}')

    moveit_config = (
        MoveItConfigsBuilder(
            robot_name="arm", package_name="moveit_config"
        )
        .moveit_cpp(path)
        .to_moveit_configs()
    )

    params = moveit_config.to_dict()  # 节点moveitpy的参数

    # instantiate MoveItPy instance and get planning component
    robot = MoveItPy(node_name="moveit_py", config_dict=params)
    arm_group = robot.get_planning_component("arm")
    hand_group = robot.get_planning_component("hand")

    logger.info("MoveItPy instance created")
    print(robot)
    
    # 延时时间
    sleep_duration = 5.0

    # 1. 规划到预定义的“ready”位置
    arm_group.set_start_state(configuration_name="stand")
    hand_group.set_start_state(configuration_name="open")
    arm_group.set_goal_state(configuration_name="ready")
    hand_group.set_goal_state(configuration_name="close")
    plan_and_execute(robot, arm_group, logger)
    time.sleep(1.0)
    plan_and_execute(robot, hand_group, logger)
    time.sleep(sleep_duration)
    print("-------------------- First plan executed. --------------------")
    
    # 2. 规划到指定的关节角位置
    robot_model = robot.get_robot_model()
    robot_state = RobotState(robot_model)
    robot_state.set_joint_group_positions('arm', [0, -0.4, -0.79, -0.79, -1.10, 1.55])
    arm_group.set_goal_state(robot_state=robot_state)
    arm_group.set_start_state_to_current_state()
    plan_and_execute(robot, arm_group, logger)
    time.sleep(sleep_duration)
    print("-------------------- Second plan executed. --------------------")
    
    # 3. 规划到指定的笛卡尔坐标(i can't run it ,because i can't find a vaild goal)
    pose_goal = PoseStamped()
    pose_goal.header.frame_id = "base_link"
    pose_goal.pose.orientation.x = 0.71
    pose_goal.pose.orientation.y = 0.71
    pose_goal.pose.position.x = 0.01
    pose_goal.pose.position.y = 0.01
    pose_goal.pose.position.z = 0.01
    
    arm_group.set_start_state_to_current_state()
    arm_group.set_goal_state(pose_stamped_msg=pose_goal, pose_link="gripper_base_link")
    plan_and_execute(robot, arm_group, logger)
    time.sleep(sleep_duration)
    print("-------------------- Third plan executed. --------------------")

    rclpy.shutdown()

if __name__ == "__main__":
    main()

4. 配置setup、编译.

Bug_list（简记）

ros2_control

1. yaml文件的缩进很严格；以及冒号之后需要加个空格才写对应的值；如果值换行了，则需要缩进并且写上“-”符号，再加一个空格，再写值
2. 每在launch中添加了一个文件路径，都要记得在setup中的data_file中声明；如果是可执行的控制节点，则还需要在entry_point里面声明入口
3. interface_name没有s；
4. 使用colcon build --symlink_install之后如果有文件路径的修改，需要先rm -r log/ install/ build/
5. 在写launch的时候 ，注意parameter和argument的区别，前者一般是键值对，也可能不是（如果只有一个参数的话可以直接写值），是会加载到ros节点里面的；后者相当于在终端命令行中输入命令
6. 在写node的时候，package、executable，之间要有逗号
7. 在写launch的controller_manager_node的时候，不要写name，似乎是内部已经写死了的，不能改名
8. 如果不写name，默认是和executable的名字一致
9. ros_control yaml文件中的type可以去github上找（先找对应版本的分支，然后找对应的控制器），，type中斜杠前的部分是文件名，斜杠后是类名，，而参数大全可以在github上的src的yaml里面找到所有描述
10. hardware_interface一般需要自己编写，github上只找到了仿真用的mock_components/GenericSystem插件。

基于ros2_control的gazebo仿真：

！！！在使用最新版的gazebo的时候，在虚拟机中记得设置关闭3d渲染加速，否则就会黑屏/白屏

Moveit2

1. 在配置的时候，记得在perception 3d sensor中选择none
2. 保证每个关节组、控制器的名字都是一样的
3. 加个虚拟关节 父关节是world 子关节是base_link
4. hand选择gripper控制器，arm选择follow trajectory控制器，arm选择k开头的规划器，选择bizzrt算法；hand选择none
5. 在生成文件之后，生成的joint_limit.yaml有bug，需要进去把整数改为浮点数，并且全部改成true，并且需要给每个值一个非零的值。
6. 在最开始选择文件的时候，选择的是没有其他额外插件的原始urdf版本

moveit2编程接口

1. 使用末端执行器坐标的方法还没成功，可能与坐标设置有问题，需要设置一个合理的position 或者是base_link不存在。
2. 都有大致的框架，照抄，省得出错。

小结

简单由moveit2得到一个能动的机械臂

1. 创建功能包，建立urdf

2. 打开moveit_setup_assistant setup (tab键补全)节点，进入可视化界面配置

3. 修改有bug的joint_limit.yaml，需要进去把整数改为浮点数，并且全部改成true，并且需要给每个值一个非零的值。

4. 运行demo.launch.py打开rviz2进行可视化控制

5. 或者创建功能包，建立config文件夹，配置moveit_cpp.yaml文件，然后编写py节点进行代码编程控制。

将moveit2配置好的机械臂显示到gazebo中进行控制

1. 复制一份robot_name.ros2_control.xacro文件，然后将hardware标签里的plugin插件由mock_components/GenericSystem改为gz_ros2_control/GazeboSimSystem

2. 复制一份robot_name.urdf.xacro文件，然后按图修改内容

3. 修改launch，增加gazebo_node，robot_to_gazebo_node, clock_bridge_node,节点。

不使用moveit，使用ros2_control控制机械臂

1. 建立原始urdf
2. 配置arm_controllers.yaml的ros2_control控制器配置文件
3. 复制一份原始urdf，增加ros2_control的标签
4. 在launch中增加contorller_manager_node, contorller_node,节点来打开ros2_control的控制接口
5. 编写控制节点代码py文件
6. 把控制节点也写入launch
7. 运行

如果需要学习代码，请私聊我，最近还没整理好，整理好之后会把代码放到github上

未来展望

1. 以动作通讯的方式完成基于ros2_control对机械臂的控制

2. 自己编写一个hardware_interface，搭建实物机械臂进行舵机控制

3. 写入一些原子动作（action），以机械臂为载体，实验一些强化学习算法
   png" alt=“image-20250908191028425” style=“zoom: 80%;” />

4. 修改launch，增加gazebo_node，robot_to_gazebo_node, clock_bridge_node,节点。

不使用moveit，使用ros2_control控制机械臂

1. 建立原始urdf
2. 配置arm_controllers.yaml的ros2_control控制器配置文件
3. 复制一份原始urdf，增加ros2_control的标签
4. 在launch中增加contorller_manager_node, contorller_node,节点来打开ros2_control的控制接口
5. 编写控制节点代码py文件
6. 把控制节点也写入launch
7. 运行

如果需要学习代码，请私聊我，最近还没整理好，整理好之后会把代码放到github上

未来展望

1. 以动作通讯的方式完成基于ros2_control对机械臂的控制
2. 自己编写一个hardware_interface，搭建实物机械臂进行舵机控制
3. 写入一些原子动作（action），以机械臂为载体，实验一些强化学习算法
4. 自己编写一个controller，将学到的控制理论写成代码