ROS机械臂篇

算法之名

发布于 2023-10-16 14:47:48

4990

发布于 2023-10-16 14:47:48

MoveIt安装

由于我这里是Ubuntu 20.04的，所以会安装noetic版本的软件包

sudo apt install ros-noetic-moveit

如果是Ubuntu 18.04，则运行

sudo apt install ros-melodic-moveit

此处安装需要一点时间。

安装完成后输入

moveit_version

有输出表示安装成功，我这里输出为

1.1.13

安装ccd

从https://github.com/danfis/libccd下载源码，解压缩后进入主文件夹下的src，编译

cd src

修改Makefile，修改内容如下

CFLAGS += -I. -fvisibility=hidden -fPIC

继续执行

make
sudo make install

安装fcl库

从https://github.com/flexible-collision-library/fcl下载源码，解压缩后进入主文件夹进行编译安装

mkdir build
cd build
cmake ..
make
sudo make install

创建机械臂模型

URDF建模原理

机器人主要分为控制系统、驱动系统、执行机构、传感系统。

控制系统：相当于人的大脑，实现任务及信息的处理，输出控制命令信号。
驱动系统：相当于人的肌肉和经络，负责驱动执行机构，将控制系统下达的命令转换成执行机构需要的信号。
执行机构：相当于人的手和脚，直接干活的机械装置。
传感系统：相当于人的感官和神经，完成信号的输入和反馈，包括内部传感系统(感知机器人内部状态，如姿态传感器，里程计)和外部传感系统(感知外部环境，如激光雷达)。

这里说的建模指的是对执行机构(机械装置)进行建模。

URDF：一种使用XML格式描述的机器人模型文件。URDF 不能单独使用，需要结合 Rviz 或 Gazebo，URDF 只是一个文件，需要在 Rviz 或 Gazebo 中渲染成图形化的机器人模型。

<robot>为最顶层标签，如

<robot name="pipi">
    <link>......</link>
    <link>......</link>
    
    <join>......</join>
    <join>......</join>
</robot>

对上图的机械臂来讲，分为大臂、小臂以及中间的关节。无论是大臂、小臂，我们都称为刚体连杆(links)，中间的关节称为joint。在模型文件中包含

Links：坐标系与几何关系。
Joints：Links之间的连接关系。

URDF不仅可以对机械臂进行建模，还可以对外接场景进行建模。比如上图中的桌子，它其实就是一个刚体部分，所以也是一个link。

<link>

描述机器人某个刚体部分的外观和物理属性；
描述连杆尺寸(size)，颜色(color)，形状(shape)，惯性矩阵(inertial matrix)，碰撞参数(collsion properties)等；
每个Link会成为一个坐标系

<link name="link_4">
    <visual>
        <geometry>
             <mesh filename="link_4.stl" />
        </geometry>
    </visual>
    <collision>
        <geometry>
            <cylinder length="0.5" radius="0.1" />
        </geometry>
        <origin xyz="0 0 -0.05" rpy="0 0 0" />
    </collision>
</link>

这里visual是描述外观的，即可视的；

geometry设置连杆的形状

标签1: box(盒状)，属性:size=长(x) 宽(y) 高(z)
标签2: cylinder(圆柱)，属性:radius=半径 length=高度
标签3: sphere(球体)，属性:radius=半径
标签4: mesh(为连杆添加皮肤)，属性: filename=资源路径(格式:package://<packagename>/<path>/文件)

collision表示连杆的碰撞属性

origin 设置偏移量与倾斜弧度

属性1: xyz=x偏移 y偏移 z偏移
属性2: rpy=x翻滚 y俯仰 z偏航 (单位是弧度)

metrial 设置材料属性(颜色)

属性: name
标签: color
1. 属性: rgba=红绿蓝权重值与透明度 (每个权重值以及透明度取值[0,1])

<joint>

描述两个link之间的关系，有6种类型，主要使用revolute类型，还可以描述关节的位置限制和速度限制以及动力学等属性。

关节类型	描述
continuous	旋转关节，可以围绕单轴无线旋转
revolute	旋转关节，类似于continuous，但是有旋转的角度限制
prismatic	滑动关节，沿某一轴线移动的关节，带有位置极限
planar	平面关节，允许在平面正交方向上平移或者旋转
floating	浮动关节，允许进行平移，旋转运动
fixed	固定关节，不允许运动的特殊关节

<joint name="joint_2" type="revolute">
    <parent link="link_1" />
    <child link="link_2" />
    <origin xyz="0.2 0.2 0" rpy="0 0 0" />
    <axis xyz="0 0 1" />
    <limit lower="-3.14" upper="3.14" velocity="1.0" />
</joint>

parent(必需的)，parent link的名字是一个强制的属性，link:父级连杆的名字，是这个link在机器人结构树中的名字。
child(必需的)，child link的名字是一个强制的属性，link:子级连杆的名字，是这个link在机器人结构树中的名字。
origin，属性: xyz=各轴线上的偏移量 rpy=各轴线上的偏移弧度。
axis，属性: xyz用于设置围绕哪个关节轴运动。

xacro模型

对URDF模型的改进，以定义class为主

用法一：宏定义

<xacro:macro name="ur5_robot" params="prefix joint_limited" ...>
......
</xacro:macro>

这里就是定义一个类(class)，然后在其他文件中去调用。

用法二：文件包含

<xacro:include filename="$(find ur_desciption)/urdf/ur5.urdf.xacro" />
<xacro:ur5_robot prefix="" joint_limited="false" />

这里就是调用了用法一中的类ur5_robot。

用法三：属性与数学运算

<xacro:property name="d1" value="0.089159" />
<xacro:property name="shoulder_height" value="${d1}" />
<joint name="${prefix}shoulder_pan_joint" type="revolute">
    ...
    <origin xyz="0.0 0.0 ${shoulder_height}" rpy="0.0 0.0 0.0" />
</joint>

这里是预先定义一些参数，然后在link或者joint中去使用，会更加的便捷。

HelloWorld

进入工作空间

cd Documents/catkin_ws/src

创建一个新的包

catkin_create_pkg learning_urdf urdf xacro

在该功能包下创建四个目录

cd learning_urdf
mkdir urdf
mkdir meshes
mkdir config
mkdir launch

我们来创建一个盒装机器人

cd urdf
vim box.urdf

内容如下

<robot name="mybox">
    <link name="base_link">
        <visual>
            <geometry>
                <box size="0.5 0.2 0.1" />
            </geometry>
        </visual>
    </link>
</robot>

进入launch文件夹，创建运行脚本文件

cd ../launch
vim box.launch

内容如下

<launch>
    <param name="robot_description" textfile="$(find learning_urdf)/urdf/box.urdf" />
    <node pkg="rviz" type="rviz" name="rviz" />
</launch>

运行该脚本

roslaunch learning_urdf box.launch

运行完成启动Rviz，点击Add按钮，添加RobotModel，

再将Fixed Frame改为base_link。此时我们就可以看见一个红色的小方块机器人了。

保存rviz的配置

进入config文件夹

cd Documents/catkin_ws/src/learning_urdf/config

新增配置文件

touch default.rviz

选择Rviz菜单

选中刚才目录下新建的default.rviz文件，并替换

将launch目录下的box.launch文件内容替换如下

<launch>
    <param name="robot_description" textfile="$(find learning_urdf)/urdf/box.urdf" />
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find learning_urdf)/config/default.rviz" />
</launch>

这里的-d为配置文件路径

关闭Rviz，重新执行box.launch文件

roslaunch learning_urdf box.launch

此时会直接打开Rviz，并有了一个红色小方块机器人，无需再去选择RobotModel了。

四轮圆柱机器人

我们在之前的功能包的urdf目录下新建一个新的urdf文件

vim car.urdf

内容如下

<robot name="mycar">
    <!-- 设置 base_footprint  -->
    <link name="base_footprint">
        <visual>
            <geometry>
                <!-- 创建一个球体，半径为0.001 -->
                <sphere radius="0.001" />
            </geometry>
        </visual>
    </link>

    <!-- 添加底盘 -->
    <link name="base_link">
        <visual>
            <geometry>
                <!-- 创建一个圆柱，半径为0.1，高度为0.08  -->
                <cylinder radius="0.1" length="0.08" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <!-- 颜色为黄色，红绿蓝透明权重分别为0.8,0.3,0.1,0.5  -->
            <material name="yellow">
                <color rgba="0.8 0.3 0.1 0.5" />
            </material>
        </visual>
    </link>
    <!-- 创建一个固定关节连接base_link2base_footprint和base_link，z偏移0.055  -->
    <joint name="base_link2base_footprint" type="fixed">
        <parent link="base_footprint" />
        <child link="base_link"/>
        <origin xyz="0 0 0.055" />
    </joint>

    <!-- 添加驱动轮 -->
    <link name="left_wheel">
        <visual>
            <geometry>
                <cylinder radius="0.0325" length="0.015" />
            </geometry>
            <origin xyz="0 0 0" rpy="1.5705 0 0" />
            <material name="black">
                <color rgba="0.0 0.0 0.0 1.0" />
            </material>
        </visual>

    </link>
    <!-- 创建一个旋转关节 -->
    <joint name="left_wheel2base_link" type="continuous">
        <parent link="base_link" />
        <child link="left_wheel" />
        <origin xyz="0 0.1 -0.0225" />
        <axis xyz="0 1 0" />
    </joint>


    <link name="right_wheel">
        <visual>
            <geometry>
                <cylinder radius="0.0325" length="0.015" />
            </geometry>
            <origin xyz="0 0 0" rpy="1.5705 0 0" />
            <material name="black">
                <color rgba="0.0 0.0 0.0 1.0" />
            </material>
        </visual>

    </link>

    <joint name="right_wheel2base_link" type="continuous">
        <parent link="base_link" />
        <child link="right_wheel" />
        <origin xyz="0 -0.1 -0.0225" />
        <axis xyz="0 1 0" />
    </joint>

    <!-- 添加万向轮(支撑轮) -->
    <link name="front_wheel">
        <visual>
            <geometry>
                <!-- 创建一个球体 -->
                <sphere radius="0.0075" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <material name="black">
                <color rgba="0.0 0.0 0.0 1.0" />
            </material>
        </visual>
    </link>

    <joint name="front_wheel2base_link" type="continuous">
        <parent link="base_link" />
        <child link="front_wheel" />
        <origin xyz="0.0925 0 -0.0475" />
        <axis xyz="1 1 1" />
    </joint>

    <link name="back_wheel">
        <visual>
            <geometry>
                <sphere radius="0.0075" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <material name="black">
                <color rgba="0.0 0.0 0.0 1.0" />
            </material>
        </visual>
    </link>

    <joint name="back_wheel2base_link" type="continuous">
        <parent link="base_link" />
        <child link="back_wheel" />
        <origin xyz="-0.0925 0 -0.0475" />
        <axis xyz="1 1 1" />
    </joint>
</robot>

对于该urdf文件，我们可以进行检查

check_urdf car.urdf

robot name is: mycar
---------- Successfully Parsed XML ---------------
root Link: base_footprint has 1 child(ren)
    child(1):  base_link
        child(1):  back_wheel
        child(2):  front_wheel
        child(3):  left_wheel
        child(4):  right_wheel

如果不抛异常，证明是成功的。

进入launch文件夹，创建launch文件

vim car.launch

内容如下

<launch>
    <!-- 将 urdf 文件内容设置进参数服务器 -->
    <param name="robot_description" textfile="$(find learning_urdf)/urdf/car.urdf" />

    <!-- 启动 rivz -->
    <node pkg="rviz" type="rviz" name="rviz_test" args="-d $(find learning_urdf)/config/default.rviz" />

    <!-- 启动机器人状态和关节状态发布节点 -->
    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />
    <node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" />
    <!-- 启动图形化的控制关节运动节点 -->
    <node pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" name="joint_state_publisher_gui" />
</launch>

安装两个Python组件

conda activate py39
pip install PyQt5 -i https://pypi.tuna.tsinghua.edu.cn/simple
pip install PySide2 -i https://pypi.tuna.tsinghua.edu.cn/simple

安装一个ROS组件

sudo apt-get install ros-noetic-joint-state-publisher-gui

执行命令

roslaunch learning_urdf car.launch

使用Xacro优化四轮圆柱机器人

进入之前的功能包

cd Documents/catkin_ws/src/learning_urdf

新建文件夹xacro

mkdir xacro
cd xacro

创建xacro文件

vim car.urdf.xacro

内容如下

<!--
    使用 xacro 优化 URDF 版的小车底盘实现：

    实现思路:
    1.将一些常量、变量封装为 xacro:property
      比如:PI 值、小车底盘半径、离地间距、车轮半径、宽度 ....
    2.使用 宏 封装驱动轮以及支撑轮实现，调用相关宏生成驱动轮与支撑轮

-->
<!-- 根标签，必须声明 xmlns:xacro -->
<robot name="my_base" xmlns:xacro="http://www.ros.org/wiki/xacro">
    <!-- 封装变量、常量 -->
    <xacro:property name="PI" value="3.141"/>
    <!-- 宏:黑色设置 -->
    <material name="black">
        <color rgba="0.0 0.0 0.0 1.0" />
    </material>
    <!-- 底盘属性 -->
    <xacro:property name="base_footprint_radius" value="0.001" /> <!-- base_footprint 半径  -->
    <xacro:property name="base_link_radius" value="0.1" /> <!-- base_link 半径 -->
    <xacro:property name="base_link_length" value="0.08" /> <!-- base_link 长 -->
    <xacro:property name="earth_space" value="0.015" /> <!-- 离地间距 -->

    <!-- 底盘 -->
    <link name="base_footprint">
      <visual>
        <geometry>
	  <!-- 创建一个球体，半径为0.001米 -->
          <sphere radius="${base_footprint_radius}" />
        </geometry>
      </visual>
    </link>
    <!-- 添加底盘 -->
    <link name="base_link">
      <visual>
        <geometry>
	  <!-- 创建一个圆柱，半径为0.1，高度为0.08  -->
          <cylinder radius="${base_link_radius}" length="${base_link_length}" />
        </geometry>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <!-- 颜色为黄色，红绿蓝透明权重分别为0.8,0.3,0.1,0.5  -->
        <material name="yellow">
          <color rgba="0.5 0.3 0.0 0.5" />
        </material>
      </visual>
    </link>
    <!-- 创建一个固定关节连接base_link2base_footprint和base_link，z偏移0.055  -->
    <joint name="base_link2base_footprint" type="fixed">
      <parent link="base_footprint" />
      <child link="base_link" />
      <origin xyz="0 0 ${earth_space + base_link_length / 2 }" />
    </joint>

    <!-- 驱动轮 -->
    <!-- 驱动轮属性 -->
    <xacro:property name="wheel_radius" value="0.0325" /><!-- 半径 -->
    <xacro:property name="wheel_length" value="0.015" /><!-- 宽度 -->
    <!-- 驱动轮宏实现 -->
    <xacro:macro name="add_wheels" params="name flag">
      <link name="${name}_wheel">
        <visual>
          <geometry>
            <cylinder radius="${wheel_radius}" length="${wheel_length}" />
          </geometry>
          <origin xyz="0.0 0.0 0.0" rpy="${PI / 2} 0.0 0.0" />
          <material name="black" />
        </visual>
      </link>

      <joint name="${name}_wheel2base_link" type="continuous">
        <parent link="base_link" />
        <child link="${name}_wheel" />
        <origin xyz="0 ${flag * base_link_radius} ${-(earth_space + base_link_length / 2 - wheel_radius) }" />
        <axis xyz="0 1 0" />
      </joint>
    </xacro:macro>
    <xacro:add_wheels name="left" flag="1" />
    <xacro:add_wheels name="right" flag="-1" />
    <!-- 支撑轮 -->
    <!-- 支撑轮属性 -->
    <xacro:property name="support_wheel_radius" value="0.0075" /> <!-- 支撑轮半径 -->

    <!-- 支撑轮宏 -->
    <xacro:macro name="add_support_wheel" params="name flag" >
      <link name="${name}_wheel">
        <visual>
            <geometry>
                <sphere radius="${support_wheel_radius}" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <material name="black" />
        </visual>
      </link>

      <joint name="${name}_wheel2base_link" type="continuous">
          <parent link="base_link" />
          <child link="${name}_wheel" />
          <origin xyz="${flag * (base_link_radius - support_wheel_radius)} 0 ${-(base_link_length / 2 + earth_space / 2)}" />
          <axis xyz="1 1 1" />
      </joint>
    </xacro:macro>

    <xacro:add_support_wheel name="front" flag="1" />
    <xacro:add_support_wheel name="back" flag="-1" />

</robot>

这里我们同样需要检查该文件，但不是检查xacro文件，而是将xacro文件转换成urdf文件进行检查

rosrun xacro xacro car.urdf.xacro > car.urdf

再使用check_urdf进行检查

check_urdf car.urdf

进入 launch 文件夹，创建 launch 文件

vim car.xacro.launch

内容如下

<launch>
    <param name="robot_description" command="$(find xacro)/xacro $(find learning_urdf)/xacro/car.urdf.xacro" />

    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find learning_urdf)/config/default.rviz" />
    <!-- 添加关节状态发布节点 -->
    <node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" output="screen" />
    <!-- 添加机器人状态发布节点 -->
    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen" />
    <node pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" name="joint_state_publisher_gui" output="screen" />

</launch>

执行命令

conda activate py39
roslaunch learning_urdf car.xacro.launch

这里的效果跟之前是一样的。

给四轮圆柱机器人增加雷达和摄像头

进入功能包的xacro文件夹

cd Documents/catkin_ws/src/learning_urdf/xacro

分别创建雷达和摄像头两个部件的xacro文件

vim camera.urdf.xacro

内容如下

<!-- 摄像头相关的 xacro 文件 -->
<robot name="my_camera" xmlns:xacro="http://wiki.ros.org/xacro">
    <!-- 摄像头属性 -->
    <xacro:property name="camera_length" value="0.01" /> <!-- 摄像头长度(x) -->
    <xacro:property name="camera_width" value="0.025" /> <!-- 摄像头宽度(y) -->
    <xacro:property name="camera_height" value="0.025" /> <!-- 摄像头高度(z) -->
    <xacro:property name="camera_x" value="0.08" /> <!-- 摄像头安装的x坐标 -->
    <xacro:property name="camera_y" value="0.0" /> <!-- 摄像头安装的y坐标 -->
    <xacro:property name="camera_z" value="${base_link_length / 2 + camera_height / 2}" /> <!-- 摄像头安装的z坐标:底盘高度 / 2 + 摄像头高度 / 2  -->

    <!-- 摄像头关节以及link -->
    <link name="camera">
        <visual>
            <geometry>
                <box size="${camera_length} ${camera_width} ${camera_height}" />
            </geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
            <material name="black" />
        </visual>
    </link>

    <joint name="camera2base_link" type="fixed">
        <parent link="base_link" />
        <child link="camera" />
        <origin xyz="${camera_x} ${camera_y} ${camera_z}" />
    </joint>
</robot>

vim laser.urdf.xacro

内容如下

<!--
    小车底盘添加雷达
-->
<robot name="my_laser" xmlns:xacro="http://wiki.ros.org/xacro">

    <!-- 雷达支架 -->
    <xacro:property name="support_length" value="0.15" /> <!-- 支架长度 -->
    <xacro:property name="support_radius" value="0.01" /> <!-- 支架半径 -->
    <xacro:property name="support_x" value="0.0" /> <!-- 支架安装的x坐标 -->
    <xacro:property name="support_y" value="0.0" /> <!-- 支架安装的y坐标 -->
    <xacro:property name="support_z" value="${base_link_length / 2 + support_length / 2}" /> <!-- 支架安装的z坐标:底盘高度 / 2 + 支架高度 / 2  -->

    <link name="support">
        <visual>
            <geometry>
                <cylinder radius="${support_radius}" length="${support_length}" />
            </geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
            <material name="red">
                <color rgba="0.8 0.2 0.0 0.8" />
            </material>
        </visual>
    </link>

    <joint name="support2base_link" type="fixed">
        <parent link="base_link" />
        <child link="support" />
        <origin xyz="${support_x} ${support_y} ${support_z}" />
    </joint>


    <!-- 雷达属性 -->
    <xacro:property name="laser_length" value="0.05" /> <!-- 雷达长度 -->
    <xacro:property name="laser_radius" value="0.03" /> <!-- 雷达半径 -->
    <xacro:property name="laser_x" value="0.0" /> <!-- 雷达安装的x坐标 -->
    <xacro:property name="laser_y" value="0.0" /> <!-- 雷达安装的y坐标 -->
    <xacro:property name="laser_z" value="${support_length / 2 + laser_length / 2}" /> <!-- 雷达安装的z坐标:支架高度 / 2 + 雷达高度 / 2  -->

    <!-- 雷达关节以及link -->
    <link name="laser">
        <visual>
            <geometry>
                <cylinder radius="${laser_radius}" length="${laser_length}" />
            </geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
            <material name="black" />
        </visual>
    </link>

    <joint name="laser2support" type="fixed">
        <parent link="support" />
        <child link="laser" />
        <origin xyz="${laser_x} ${laser_y} ${laser_z}" />
    </joint>
</robot>

添加一个组装的xacro文件

vim make.urdf.xacro

内容如下

<!-- 组合小车底盘与摄像头与雷达 -->
<robot name="make" xmlns:xacro="http://wiki.ros.org/xacro">
    <xacro:include filename="car.urdf.xacro" />
    <xacro:include filename="camera.urdf.xacro" />
    <xacro:include filename="laser.urdf.xacro" />
</robot>

进入launch文件夹，创建launch文件

vim make.urdf.launch

内容如下

<launch>
    <param name="robot_description" command="$(find xacro)/xacro $(find learning_urdf)/xacro/make.urdf.xacro" />

    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find learning_urdf)/config/default.rviz" />
    <node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" output="screen" />
    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen" />
    <node pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" name="joint_state_publisher_gui" output="screen" />

</launch>

执行命令

conda activate py39
roslaunch learning_urdf make.urdf.launch

控制机器人

安装 Arbotix

sudo apt-get install ros-noetic-arbotix

如果是Ubuntu 18.04则为

sudo apt-get install ros-melodic-arbotix

进入功能包的config文件夹

cd Documents/catkin_ws/src/learning_urdf/config

新建Arbotix配置文件

vim control.yaml

内容如下

# 该文件是控制器配置,一个机器人模型可能有多个控制器，比如: 底盘、机械臂、夹持器(机械手)....
# 因此，根 name 是 controller
controllers: {
   # 单控制器设置
   base_controller: {
       #类型: 差速控制器
       type: diff_controller,
       #参考坐标
       base_frame_id: base_footprint, 
       #两个轮子之间的间距
       base_width: 0.2,
       #控制频率
       ticks_meter: 2000, 
       #PID控制参数，使机器人车轮快速达到预期速度
       Kp: 12, 
       Kd: 12, 
       Ki: 0, 
       Ko: 50, 
       #加速限制
       accel_limit: 1.0 
    }
}

由于我们之前的机器人是两个底轮，要想让机器人直线运动就得保持两个轮子的转速相同；如果想让机器人转弯，就得让其中的一个轮子转速增大，一个轮子的转速减慢。因为是通过速度来控制机器人的，所以叫做差速控制器。Arbotix想让机器人运动起来得发布消息，在发布消息时需要用到机器人的坐标系，而这里用的是base_footprint的坐标系。轮间距可以保证机器人按照某个固定的角速度转动。Arbotix在ROS中其实是一个控制节点，它需要通过消息与机器人交互，控制频率就是1秒钟发送的消息的次数，这里为2000次。PID控制参数是为了控制机器人达到最终的稳定速度，例如120km/h，这里会有一个算法，暂略。加速限制就是限制机器人的加速度的。更多关于Arbotix的配置内容可以参考http://wiki.ros.org/arbotix_python/diff_controller

进入launch文件夹，修改make.urdf.launch

vim make.urdf.launch

内容如下

<launch>
    <param name="robot_description" command="$(find xacro)/xacro $(find learning_urdf)/xacro/make.urdf.xacro" />

    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find learning_urdf)/config/default.rviz" />
    <node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" output="screen" />
    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen" />
    <node pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" name="joint_state_publisher_gui" output="screen" />
    <!-- 集成arbotix运动控制节点，并且加载参数 -->
    <node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen">
     <rosparam file="$(find learning_urdf)/config/control.yaml" command="load" />
     <param name="sim" value="true" />
    </node>

</launch>

安装一个Python组件

conda activate py39
pip install serial -i https://pypi.tuna.tsinghua.edu.cn/simple

执行命令

roslaunch learning_urdf make.urdf.launch

配置rviz

这里我们只需要将Fixed Frame设置成odom

查看话题列表，新开一个终端窗口

rostopic list

/clicked_point
/cmd_vel
/diagnostics
/initialpose
/joint_states
/move_base_simple/goal
/odom
/rosout
/rosout_agg
/tf
/tf_static

这里/cmd_vel就是我们要用到的话题

输入

rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: 0.2, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0.5}}'

此时就可以看到机器人在做圆周运动了。

URDF集成Gazebo

进入工作空间

cd Documents/catkin_ws/src

创建一个新的包

catkin_create_pkg learning_gazebo urdf xacro gazebo_ros gazebo_ros_control gazebo_plugins

在该功能包下创建四个目录

cd learning_gazebo
mkdir urdf
mkdir meshes
mkdir config
mkdir launch

我们来创建一个盒装机器人

cd urdf
vim box.urdf

内容如下

<!-- 
    创建一个机器人模型(盒状即可)，显示在 Gazebo 中 
-->

<robot name="mybox">
    <link name="base_link">
        <visual>
            <geometry>
                <!-- 创建一个盒装物体 -->
                <box size="0.5 0.2 0.1" />
            </geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
            <!-- 该盒装物体的颜色为黄色 -->
            <material name="yellow">
                <color rgba="0.5 0.3 0.0 1" />
            </material>
        </visual>
        <collision>
            <geometry>
                <box size="0.5 0.2 0.1" />
            </geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
        </collision>
        <inertial>
            <origin xyz="0 0 0" />
            <mass value="6" />
            <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1" />
        </inertial>
    </link>
    <gazebo reference="base_link">
        <material>Gazebo/Black</material>
    </gazebo>

</robot>

注意，当 URDF 需要与 Gazebo 集成时，和 Rviz 有明显区别:

必须使用 collision 标签，因为既然是仿真环境，那么必然涉及到碰撞检测，collision 提供碰撞检测的依据。
必须使用 inertial 标签，此标签标注了当前机器人某个刚体部分的惯性矩阵，用于一些力学相关的仿真计算。
颜色设置，也需要重新使用 gazebo 标签标注，因为之前的颜色设置为了方便调试包含透明度，仿真环境下没有此选项

进入 launch 文件夹，创建运行脚本文件

vim box.launch

内容如下

<launch>

    <!-- 将 Urdf 文件的内容加载到参数服务器 -->
    <param name="robot_description" textfile="$(find learning_gazebo)/urdf/box.urdf" />

    <!-- 启动 gazebo -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch" />

    <!-- 在 gazebo 中显示机器人模型 -->
    <node pkg="gazebo_ros" type="spawn_model" name="model" args="-urdf -model mybox -param robot_description"  />
</launch>

运行该脚本

conda activate py39
roslaunch learning_gazebo box.launch

四轮圆柱机器人(Gazebo版)

进入包的urdf目录

cd Documents/catkin_ws/src/learning_gazebo/urdf

新增封装惯性矩阵算法的xacro文件

vim head.xacro

内容如下

<robot name="base" xmlns:xacro="http://wiki.ros.org/xacro">
    <!-- Macro for inertia matrix -->
    <xacro:macro name="sphere_inertial_matrix" params="m r">
        <inertial>
            <mass value="${m}" />
            <inertia ixx="${2*m*r*r/5}" ixy="0" ixz="0"
                iyy="${2*m*r*r/5}" iyz="0" 
                izz="${2*m*r*r/5}" />
        </inertial>
    </xacro:macro>

    <xacro:macro name="cylinder_inertial_matrix" params="m r h">
        <inertial>
            <mass value="${m}" />
            <inertia ixx="${m*(3*r*r+h*h)/12}" ixy = "0" ixz = "0"
                iyy="${m*(3*r*r+h*h)/12}" iyz = "0"
                izz="${m*r*r/2}" /> 
        </inertial>
    </xacro:macro>

    <xacro:macro name="Box_inertial_matrix" params="m l w h">
       <inertial>
               <mass value="${m}" />
               <inertia ixx="${m*(h*h + l*l)/12}" ixy = "0" ixz = "0"
                   iyy="${m*(w*w + l*l)/12}" iyz= "0"
                   izz="${m*(w*w + h*h)/12}" />
       </inertial>
   </xacro:macro>
</robot>

该文件内容具有较强的专业性，可以暂时先不关注。

创建底盘的xacro文件

vim car.urdf.xacro

内容如下

<!--
    使用 xacro 优化 URDF 版的小车底盘实现：

    实现思路:
    1.将一些常量、变量封装为 xacro:property
      比如:PI 值、小车底盘半径、离地间距、车轮半径、宽度 ....
    2.使用 宏 封装驱动轮以及支撑轮实现，调用相关宏生成驱动轮与支撑轮

-->
<!-- 根标签，必须声明 xmlns:xacro -->
<robot name="my_base" xmlns:xacro="http://www.ros.org/wiki/xacro">
    <!-- 封装变量、常量 -->
    <!-- PI 值设置精度需要高一些，否则后续车轮翻转量计算时，可能会出现肉眼不能察觉的车轮倾斜，从而导致模型抖动 -->
    <xacro:property name="PI" value="3.1415926"/>
    <!-- 宏:黑色设置 -->
    <material name="black">
        <color rgba="0.0 0.0 0.0 1.0" />
    </material>
    <!-- 底盘属性 -->
    <xacro:property name="base_footprint_radius" value="0.001" /> <!-- base_footprint 半径  -->
    <xacro:property name="base_link_radius" value="0.1" /> <!-- base_link 半径 -->
    <xacro:property name="base_link_length" value="0.08" /> <!-- base_link 长 -->
    <xacro:property name="earth_space" value="0.015" /> <!-- 离地间距 -->
    <xacro:property name="base_link_m" value="0.5" /> <!-- 质量  -->

    <!-- 底盘 -->
    <link name="base_footprint">
      <visual>
        <geometry>
          <sphere radius="${base_footprint_radius}" />
        </geometry>
      </visual>
    </link>

    <link name="base_link">
      <visual>
        <geometry>
          <cylinder radius="${base_link_radius}" length="${base_link_length}" />
        </geometry>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <material name="yellow">
          <color rgba="0.5 0.3 0.0 0.5" />
        </material>
      </visual>
      <collision>
        <geometry>
          <cylinder radius="${base_link_radius}" length="${base_link_length}" />
        </geometry>
        <origin xyz="0 0 0" rpy="0 0 0" />
      </collision>
      <xacro:cylinder_inertial_matrix m="${base_link_m}" r="${base_link_radius}" h="${base_link_length}" />

    </link>


    <joint name="base_link2base_footprint" type="fixed">
      <parent link="base_footprint" />
      <child link="base_link" />
      <origin xyz="0 0 ${earth_space + base_link_length / 2 }" />
    </joint>
    <gazebo reference="base_link">
        <material>Gazebo/Yellow</material>
    </gazebo>

    <!-- 驱动轮 -->
    <!-- 驱动轮属性 -->
    <xacro:property name="wheel_radius" value="0.0325" /><!-- 半径 -->
    <xacro:property name="wheel_length" value="0.015" /><!-- 宽度 -->
    <xacro:property name="wheel_m" value="0.05" /> <!-- 质量  -->

    <!-- 驱动轮宏实现 -->
    <xacro:macro name="add_wheels" params="name flag">
      <link name="${name}_wheel">
        <visual>
          <geometry>
            <cylinder radius="${wheel_radius}" length="${wheel_length}" />
          </geometry>
          <origin xyz="0.0 0.0 0.0" rpy="${PI / 2} 0.0 0.0" />
          <material name="black" />
        </visual>
        <collision>
          <geometry>
            <cylinder radius="${wheel_radius}" length="${wheel_length}" />
          </geometry>
          <origin xyz="0.0 0.0 0.0" rpy="${PI / 2} 0.0 0.0" />
        </collision>
        <xacro:cylinder_inertial_matrix m="${wheel_m}" r="${wheel_radius}" h="${wheel_length}" />

      </link>

      <joint name="${name}_wheel2base_link" type="continuous">
        <parent link="base_link" />
        <child link="${name}_wheel" />
        <origin xyz="0 ${flag * base_link_radius} ${-(earth_space + base_link_length / 2 - wheel_radius) }" />
        <axis xyz="0 1 0" />
      </joint>

      <gazebo reference="${name}_wheel">
        <material>Gazebo/Red</material>
      </gazebo>

    </xacro:macro>
    <xacro:add_wheels name="left" flag="1" />
    <xacro:add_wheels name="right" flag="-1" />
    <!-- 支撑轮 -->
    <!-- 支撑轮属性 -->
    <xacro:property name="support_wheel_radius" value="0.0075" /> <!-- 支撑轮半径 -->
    <xacro:property name="support_wheel_m" value="0.03" /> <!-- 质量  -->

    <!-- 支撑轮宏 -->
    <xacro:macro name="add_support_wheel" params="name flag" >
      <link name="${name}_wheel">
        <visual>
            <geometry>
                <sphere radius="${support_wheel_radius}" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <material name="black" />
        </visual>
        <collision>
            <geometry>
                <sphere radius="${support_wheel_radius}" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />
        </collision>
        <xacro:sphere_inertial_matrix m="${support_wheel_m}" r="${support_wheel_radius}" />
      </link>

      <joint name="${name}_wheel2base_link" type="continuous">
          <parent link="base_link" />
          <child link="${name}_wheel" />
          <origin xyz="${flag * (base_link_radius - support_wheel_radius)} 0 ${-(base_link_length / 2 + earth_space / 2)}" />
          <axis xyz="1 1 1" />
      </joint>
      <gazebo reference="${name}_wheel">
        <material>Gazebo/Red</material>
      </gazebo>
    </xacro:macro>

    <xacro:add_support_wheel name="front" flag="1" />
    <xacro:add_support_wheel name="back" flag="-1" />


</robot>

它跟Rivz版本的内容只需要注意之前提到的注意事项即可，大致内容都是相同的。

创建摄像头xacro文件

vim camera.urdf.xacro

内容如下

<!-- 摄像头相关的 xacro 文件 -->
<robot name="my_camera" xmlns:xacro="http://wiki.ros.org/xacro">
    <!-- 摄像头属性 -->
    <xacro:property name="camera_length" value="0.01" /> <!-- 摄像头长度(x) -->
    <xacro:property name="camera_width" value="0.025" /> <!-- 摄像头宽度(y) -->
    <xacro:property name="camera_height" value="0.025" /> <!-- 摄像头高度(z) -->
    <xacro:property name="camera_x" value="0.08" /> <!-- 摄像头安装的x坐标 -->
    <xacro:property name="camera_y" value="0.0" /> <!-- 摄像头安装的y坐标 -->
    <xacro:property name="camera_z" value="${base_link_length / 2 + camera_height / 2}" /> <!-- 摄像头安装的z坐标:底盘高度 / 2 + 摄像头高度 / 2  -->

    <xacro:property name="camera_m" value="0.01" /> <!-- 摄像头质量 -->

    <!-- 摄像头关节以及link -->
    <link name="camera">
        <visual>
            <geometry>
                <box size="${camera_length} ${camera_width} ${camera_height}" />
            </geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
            <material name="black" />
        </visual>
        <collision>
            <geometry>
                <box size="${camera_length} ${camera_width} ${camera_height}" />
            </geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
        </collision>
        <xacro:Box_inertial_matrix m="${camera_m}" l="${camera_length}" w="${camera_width}" h="${camera_height}" />
    </link>

    <joint name="camera2base_link" type="fixed">
        <parent link="base_link" />
        <child link="camera" />
        <origin xyz="${camera_x} ${camera_y} ${camera_z}" />
    </joint>
    <gazebo reference="camera">
        <material>Gazebo/Blue</material>
    </gazebo>
</robot>

创建雷达xacro文件

vim laser.urdf.xacro

内容如下

<!--
    小车底盘添加雷达
-->
<robot name="my_laser" xmlns:xacro="http://wiki.ros.org/xacro">

    <!-- 雷达支架 -->
    <xacro:property name="support_length" value="0.15" /> <!-- 支架长度 -->
    <xacro:property name="support_radius" value="0.01" /> <!-- 支架半径 -->
    <xacro:property name="support_x" value="0.0" /> <!-- 支架安装的x坐标 -->
    <xacro:property name="support_y" value="0.0" /> <!-- 支架安装的y坐标 -->
    <xacro:property name="support_z" value="${base_link_length / 2 + support_length / 2}" /> <!-- 支架安装的z坐标:底盘高度 / 2 + 支架高度 / 2  -->

    <xacro:property name="support_m" value="0.02" /> <!-- 支架质量 -->

    <link name="support">
        <visual>
            <geometry>
                <cylinder radius="${support_radius}" length="${support_length}" />
            </geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
            <material name="red">
                <color rgba="0.8 0.2 0.0 0.8" />
            </material>
        </visual>

        <collision>
            <geometry>
                <cylinder radius="${support_radius}" length="${support_length}" />
            </geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
        </collision>

        <xacro:cylinder_inertial_matrix m="${support_m}" r="${support_radius}" h="${support_length}" />

    </link>

    <joint name="support2base_link" type="fixed">
        <parent link="base_link" />
        <child link="support" />
        <origin xyz="${support_x} ${support_y} ${support_z}" />
    </joint>

    <gazebo reference="support">
        <material>Gazebo/White</material>
    </gazebo>

    <!-- 雷达属性 -->
    <xacro:property name="laser_length" value="0.05" /> <!-- 雷达长度 -->
    <xacro:property name="laser_radius" value="0.03" /> <!-- 雷达半径 -->
    <xacro:property name="laser_x" value="0.0" /> <!-- 雷达安装的x坐标 -->
    <xacro:property name="laser_y" value="0.0" /> <!-- 雷达安装的y坐标 -->
    <xacro:property name="laser_z" value="${support_length / 2 + laser_length / 2}" /> <!-- 雷达安装的z坐标:支架高度 / 2 + 雷达高度 / 2  -->

    <xacro:property name="laser_m" value="0.1" /> <!-- 雷达质量 -->

    <!-- 雷达关节以及link -->
    <link name="laser">
        <visual>
            <geometry>
                <cylinder radius="${laser_radius}" length="${laser_length}" />
            </geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
            <material name="black" />
        </visual>
        <collision>
            <geometry>
                <cylinder radius="${laser_radius}" length="${laser_length}" />
            </geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
        </collision>
        <xacro:cylinder_inertial_matrix m="${laser_m}" r="${laser_radius}" h="${laser_length}" />
    </link>

    <joint name="laser2support" type="fixed">
        <parent link="support" />
        <child link="laser" />
        <origin xyz="${laser_x} ${laser_y} ${laser_z}" />
    </joint>
    <gazebo reference="laser">
        <material>Gazebo/Black</material>
    </gazebo>
</robot>

添加一个组装的 xacro 文件

vim make.urdf.xacro

内容如下

<!-- 组合小车底盘与摄像头 -->
<robot name="make" xmlns:xacro="http://wiki.ros.org/xacro">
    <xacro:include filename="head.xacro" />
    <xacro:include filename="car.urdf.xacro" />
    <xacro:include filename="camera.urdf.xacro" />
    <xacro:include filename="laser.urdf.xacro" />
</robot>

进入 launch 文件夹，创建 launch 文件

vim make.urdf.launch

内容如下

<launch>
    <!-- 将 Urdf 文件的内容加载到参数服务器 -->
    <param name="robot_description" command="$(find xacro)/xacro $(find learning_gazebo)/urdf/make.urdf.xacro" />
    <!-- 启动 gazebo -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch" />

    <!-- 在 gazebo 中显示机器人模型 -->
    <node pkg="gazebo_ros" type="spawn_model" name="model" args="-urdf -model make -param robot_description"  />
</launch>

运行该脚本

conda activate py39
roslaunch learning_gazebo make.urdf.launch

MoveIt核心功能以及Rivz控制

MoveIt简介

机械臂抓取分为——视觉、规划和控制三个部分。在视觉中输入的为相机数据，该数据可以为彩色图(RGB)或者深度图，通过图像生成点云，再去做抓取姿态的预测。这里的输入可以是点云或者是RGB深度图，根据算法的不同而不同。姿态预测的结果就是上图中右上的部分，包含物体本身和抓取姿态。抓取姿态是相对于相机的，而机器臂要达到相应的位置，需要通过手眼标定来完成。它会传递位姿的坐标系的坐标给机械臂，通过控制让机械臂到达这个坐标位置进行真实的抓取。

如果直接从视觉到控制会存在一些问题，譬如其他物体的影响，会产生碰撞。所以我们需要将视觉部分引入到ROS中去做规划，规划出一条无碰撞的路径。会通过碰撞检测检测路径中是否会发生碰撞。

MoveIt的主要功能包括：

运动学(Kinematics)
运动规划(Motion Planning)
碰撞检测(Collision Checking)
3D感知(Pecreption)
操作(Manipulation)

其中最主要的就是前3点。

在ROS中，它最重要的节点为Move Group，它的输入可以是人也可以是AI，通过Rviz中的插件进行控制；也可以通过程序作为输入；另外它可以订阅相机的点云或者是深度图，将其转化为ROS中的格式，从而对物体进行避障。它会使用一些开源库(最主要的为OMPL库)对接，进行路径规划。然后会调用碰撞检测的开源库(FCL)去进行碰撞检测。得到路径之后还要做轨迹规划，比如说路径上的几个点，得到这几个点后还要加上到达每个点的时间、速度、加速度，都需要规划出来。最后就是发送指令给机械臂，让机械臂去执行这条轨迹。

上图是move group跟用户和机器人的接口。首先它会获取ROS参数服务器中的一些参数。在跟用户交互的接口中包含C++、Python以及Rviz的部分。这些接口主要就是提供一些服务，让move group节点可以使用这些服务。在跟机器人的接口部分，主要是把规划出来的路径给到机器人，让机器人去执行。机器人可以通过传感器去检测每个关节的θ角度再传递给move group节点，从而确定机器人当前的状态。假设我们的机械臂是6关节的，那么6个关节的角度确定了就可以确定整个机械臂的姿态。有了这个姿态就可以把每个关节的位姿发布出来。然后就是相机的输入，通过点云或者深度图传送给move group节点。

在不同的命令行窗口执行

roscore
rosrun moveit_setup_assistant moveit_setup_assistant

会弹出一个窗口，上面有两个按钮——Create New MoveIt Configuration Package(创建一个新的包)、Edit Existing MoveIt Configuration Package(编辑已有的包)。

本文参与腾讯云自媒体分享计划，分享自作者个人站点/博客。

原始发表：2023-10-11，如有侵权请联系 cloudcommunity@tencent.com 删除

机器人

本文分享自作者个人站点/博客前往查看

如有侵权，请联系 cloudcommunity@tencent.com 删除。

本文参与腾讯云自媒体分享计划，欢迎热爱写作的你一起参与！