使用 ROS2 Control和Gazebo控制机械臂包含夹爪

概述

在本教程中，我将向您展示如何使用 ROS 2 Control 和 Gazebo (经典版本也就是classic)创建和控制机械臂。

前提依赖

1. ROS2版本：humble
2. Gazebo版本：gazebo classic 11

sudo apt-get update
sudo apt install ros-$ROS_DISTRO-gazebo-ros-pkgs

3. 安装ROS2 Control

sudo apt-get install ros-${ROS_DISTRO}-ros2-control ros-${ROS_DISTRO}-ros2-controllers ros-${ROS_DISTRO}-gripper-controllers ros-${ROS_DISTRO}-gazebo-ros2-control

创建机械臂描述文件URDF

创建URDF文件
显示如下：

需要注意的部分

World Link

<link name="world" />

<joint name="world_joint" type="fixed">
    <parent link="world" />
    <child link = "base_link" />
    <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
</joint>

它表示固定的世界坐标系。、
如果此机械臂连接到其他机器人比如AGV或者人形机器人，则World Link可能会替换为表示该机械臂所连接的机器人部分的Link。这可能是：

1. 人形机器人的躯干或胸部
2. 肩关节或肩部结构
3. AGV的主Link

末端夹爪

这里末端夹爪使用了panda_hand，和机械臂最后一个Link也就是wrist3_Link使用panda_hand_joint通过fixed的方式相连，其中<origin rpy="0 0 0" xyz="0 0 0.01" />表示panda_hand和wrist3_Link之间的相对位姿关系，可以根据实际情况进行调整。

<joint name="panda_hand_joint" type="fixed">
    <parent link="wrist3_Link" />
    <child link="panda_hand" />
    <origin rpy="0 0 0" xyz="0 0 0.01" />
</joint>

机械臂控制

使用ros2_control来控制机械臂的关节。
创建各关节的ros2_control.xacro描述文件

需要注意的部分

hardware

这里使用gazebo classic进行仿真，因此hardware使用gazebo_ros2_control/GazeboSystem，如果使用gazebo ign或者gazebo gz进行仿真，或者其他仿真软件或者真实机器人控制，请修改这一部分。

<hardware>
    <plugin>gazebo_ros2_control/GazeboSystem</plugin>
</hardware>

控制器配置

使用gazebo classic进行控制仿真，需要使用libgazebo_ros2_control.so，如果使用gazebo ign或者gazebo gz进行仿真，或者其他仿真软件或者真实机器人控制，请修改这一部分。在aubo_i10.ros2_control.xacro的文件末尾可以看到：

<gazebo>
    <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
        <parameters>$(find my_arm)/config/ros2_controllers.yaml</parameters>
    </plugin>
</gazebo>

其中ros2_controllers.yaml为ros2 control的配置文件，参考ros2_controllers.yaml。其中arm_controller和gripper_controller为机械臂和夹爪的控制配置项，joint_state_broadcaster为每一个关节的状态广播配置。

创建仿真世界环境

创建一个gazebo的仿真世界。
重力设置为模拟地球的重力。其他模型从外部来源通过include引入世界：
太阳，提供光线
地面，为对象提供一个平坦的表面。

创建launch.py启动文件

创建launch.py文件。使用下面的命令来启用仿真。

ros2 launch my_arm robot_gazebo.launch.py

控制机械臂运动

查看控制器状态

ros2 control list_controllers

正常的话输出为

joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active
arm_controller      [joint_trajectory_controller/JointTrajectoryController] active
gripper_controller  [joint_trajectory_controller/JointTrajectoryController] active

也可以使用下面的命令来查看更为详细的控制器信息。

ros2 control list_controllers -v

控制机械臂

输入下面的命令来查看所有的话题

ros2 topic list

在输出中可以看到

/arm_controller/controller_state
/arm_controller/joint_trajectory
/arm_controller/state
/arm_controller/transition_event
/gripper_controller/controller_state
/gripper_controller/joint_trajectory
/gripper_controller/state
/gripper_controller/transition_event
/joint_state_broadcaster/transition_event
/joint_states

要控制机械臂我们需要使用其中的 /arm_controller/joint_trajectory和/gripper_controller/joint_trajectory。
可以发布该主题的轨迹来移动机械臂。让我们了解有关该主题的更多信息：

ros2 topic info /arm_controller/joint_trajectory

输出

Type: trajectory_msgs/msg/JointTrajectory
Publisher count: 0
Subscription count: 1

从中我们知道需要发布消息的类型为trajectory_msgs/msg/JointTrajectory。可以尝试以下命令来控制机械臂。

ros2 topic pub --once /arm_controller/joint_trajectory trajectory_msgs/msg/JointTrajectory "{
        joint_names: [
            'shoulder_joint', 
            'upperArm_joint', 
            'foreArm_joint', 
            'wrist1_joint', 
            'wrist2_joint', 
            'wrist3_joint'
        ], 
        points: [
            {
                positions: [-1.345, -1.23, 0.264, -0.296, 0.389, -1.5], 
                time_from_start: {sec: 5, nanosec: 0}
            }
        ]
    }"

命令输入后在gazebo中可以看到机械臂运动到如下位置

现在也可以执行下面的命令将其移动回开始的位姿：

ros2 topic pub --once /arm_controller/joint_trajectory trajectory_msgs/msg/JointTrajectory "{
        joint_names: [
            'shoulder_joint', 
            'upperArm_joint', 
            'foreArm_joint', 
            'wrist1_joint', 
            'wrist2_joint', 
            'wrist3_joint'
        ], 
        points: [
            {
                positions: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], 
                time_from_start: {sec: 5, nanosec: 0}
            }
        ]
    }"

控制夹爪

要合拢夹爪，可以输入：

ros2 topic pub --once /gripper_controller/joint_trajectory trajectory_msgs/msg/JointTrajectory "{
        joint_names: [
        'panda_finger_joint1',
        'panda_finger_joint2'
        ], 
        points: [
            {
                positions: [0.0, 0.0],
                time_from_start: {sec: 5, nanosec: 0}
            }
        ]
    }"

要打开夹爪，可以输入：

ros2 topic pub --once /gripper_controller/joint_trajectory trajectory_msgs/msg/JointTrajectory "{
        joint_names: [
        'panda_finger_joint1',
        'panda_finger_joint2'
        ], 
        points: [
            {
                positions: [0.8, 0.8], 
                time_from_start: {sec: 5, nanosec: 0}
            }
        ]
    }"