Dofbot机械臂从零部署笔记（1）——ROS之创建URDF模型、配置Moveit和MotionPlanning

Yahboom公司机械臂Dofbot从零部署笔记，看这篇的同学首先需要购买他们公司的Dofbot机械臂+Jetson Nano板子组合，出厂已经配置好部署完毕的镜像。

本文从新建一个ROS工作空间开始从零再部署一遍，顺便把踩的坑都记录下来。

需要适当的前置知识：什么是URDF模型、什么是moveit和轨迹规划。

从零部署之创建URDF模型

0 创建新工作空间

home/jetson目录下
创建 xiaok_ws 目录，在该目录下创建src目录
进入src目录，catkin_init_workspace
回到xiaok_ws 目录，编译catkin_make

1 创建URDF模型测试功能包

进入src目录：
catkin_create_pkg xiaok_description urdf xacro
进入xiaok_description目录，建立四个文件夹：

・urdf：存放机器人模型的URDF或xacro文件
・meshes：存放URDF中引用的模型渲染文件
・launch：保存相关启动文件
・config：保存rviz的配置文件

进入rudf目录，创建模型文件：
touch xiaok.urdf

2 dofbot机械臂的urdf模型

<?xml version="1.0" encoding="UTF-8" ?>
<robot name="dofbot">
    <link name="base_link">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <geometry>
                <mesh filename="package://xiaok_description/meshes/base_link.STL" />
            </geometry>
            <material name="">
                <color rgba="0.592156862745098 0.666666666666667 0.682352941176471 1" />
            </material>
        </visual>
        <collision>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <geometry>
                <mesh filename="package://xiaok_description/meshes/base_link.STL" />
            </geometry>
        </collision>
    </link>
    <link name="link1">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <geometry>
                <mesh filename="package://xiaok_description/meshes/link1.STL" />
            </geometry>
            <material name="">
                <color rgba="0 0.627450980392157 0.235294117647059 1" />
            </material>
        </visual>
        <collision>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <geometry>
                <mesh filename="package://xiaok_description/meshes/link1.STL" />
            </geometry>
        </collision>
    </link>
    <joint name="joint1" type="revolute">
        <origin xyz="0 0 0.06605" rpy="-0.010805 0 0" />
        <parent link="base_link" />
        <child link="link1" />
        <axis xyz="0 0 1" />
        <limit effort="30" velocity="1.0" lower="-1.5708" upper="1.5708"/>
    </joint>
    <link name="link2">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <geometry>
                <mesh filename="package://xiaok_description/meshes/link2.STL" />
            </geometry>
            <material name="">
                <color rgba="0.592156862745098 0.666666666666667 0.682352941176471 1" />
            </material>
        </visual>
        <collision>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <geometry>
                <mesh filename="package://xiaok_description/meshes/link2.STL" />
            </geometry>
        </collision>
    </link>
    <joint name="joint2" type="revolute">
        <origin xyz="0 -0.00031873 0.04145" rpy="0 1.5708 0" />
        <parent link="link1" />
        <child link="link2" />
        <axis xyz="0 0 1" />
        <limit effort="30" velocity="1.0" lower="-1.5708" upper="1.5708"/>
    </joint>
    <link name="link3">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <geometry>
                <mesh filename="package://xiaok_description/meshes/link3.STL" />
            </geometry>
            <material name="">
                <color rgba="0 0.627450980392157 0.235294117647059 1" />
            </material>
        </visual>
        <collision>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <geometry>
                <mesh filename="package://xiaok_description/meshes/link3.STL" />
            </geometry>
        </collision>
    </link>
    <joint name="joint3" type="revolute">
        <origin xyz="-0.08285 0 0" rpy="0 0 0" />
        <parent link="link2" />
        <child link="link3" />
        <axis xyz="0 0 1" />
        <limit effort="30" velocity="1.0" lower="-1.5708" upper="1.5708"/>
    </joint>
    <link name="link4">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <geometry>
                <mesh filename="package://xiaok_description/meshes/link4.STL" />
            </geometry>
            <material name="">
                <color rgba="0.592156862745098 0.666666666666667 0.682352941176471 1" />
            </material>
        </visual>
        <collision>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <geometry>
                <mesh filename="package://xiaok_description/meshes/link4.STL" />
            </geometry>
        </collision>
    </link>
    <joint name="joint4" type="revolute">
        <origin xyz="-0.08285 0 0" rpy="0 0 0.0083081" />
        <parent link="link3" />
        <child link="link4" />
        <axis xyz="0 0 1" />
        <limit effort="30" velocity="1.0" lower="-1.5708" upper="1.5708"/>
    </joint>
    <!-- link5 -->
    <link name="link5">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <geometry>
                <mesh filename="package://xiaok_description/meshes/link5.STL"/>
            </geometry>
            <material name="">
                <color rgba="1 1 1 1" />
            </material>
        </visual>
        <collision>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <geometry>
                <mesh filename="package://xiaok_description/meshes/link5.STL" />
            </geometry>
        </collision>
    </link>
    <!-- joint5 -->
    <joint name="joint5" type="revolute">
        <origin xyz="-0.07385 -0.001 0" rpy="0 -1.57 0"/>
        <axis xyz="0 0 1"/>
        <parent link="link4"/>
        <child link="link5"/>
        <limit effort="30" velocity="1.0" lower="-1.5708" upper="3.1416"/>
    </joint>
</robot>

3 meshes

/home/jetson/dofbot_ws/src/dofbot_moveit/meshes 文件夹拷贝到 xiaok_description 下。
这些文件本身由Yahboom公司的Dofbot机械臂设备出厂自带。

4 launch文件

xiaok.launch

<launch>
    <arg name="model" default="xiaok.urdf"/>
    <arg name="gui" default="false"/>
    
    <!-- 设置机器人模型路径参数 -->
    <param name="robot_description" command="$(find xacro)/xacro --inorder $(find xiaok_description)/urdf/$(arg model)"/>
    
    <param name="use_gui" value="$(arg gui)"/>
    
    <!-- 运行joint_state_publisher节点，发布机器人的关节状态  -->
    <node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui"/>

    <!-- 运行robot_state_publisher节点，发布tf  -->
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>

    <!-- 运行rviz可视化界面 -->
    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find xiaok_description)/config/dofbot.rviz" required="true"/>
</launch>

5 编译并运行

进入工作空间目录：
catkin_make
运行：
source ~/xiaok_ws/devel/setup.bash
roslaunch xiaok_description xiaok.launch
（之后可以把 source /home/jetson/xiaok_ws/devel/setup.bash 这句话添加加到.bashrc文件里，后面一直要用）

6 运行效果

ADD → RobotModel
在Fixed Frame处改为 base_link
保存配置，以便下次继续查看：
顶端的菜单栏 File → Save Config As，保存至刚才建立的config文件夹下
取名为 xiaok.rviz

添加坐标系：
ADD → TF

显示joint转动效果：
单击左侧joint_state_publisher_gui图标

7 检查模型

从 home/jetson/xiaok_ws/src/xiaok_description/urdf 打开终端，输入：
urdf_to_graphiz xiaok.urdf
会生成模型检查文档，打开：

从零部署之配置MoveIt

1 启动Setup Assistant

roslaunch moveit_setup_assistant setup_assistant.launch

点击“Create New MoveIt Configuration Package”
载入之前的 xiaok.urdf

2 配置自碰撞检测（Self-Collisions）

点击“Generate Collision Matrix”

可以看到有些连杆之间没有碰撞。

3 添加虚拟关节（Virtual Joints）

添加虚拟关节与外界的某一个参考系建立连接。
点击 Add Virtual Joints：
命名为 virtual_joint
Child Link选择 base_link
Parent Frame Name即世界坐标的名称，命名为 world
关节类型选择 Fixed

选择Save保存

4 创建运动规划组（Planning Groups）

点击 Add Group。
Group Name，我命名为 xiaok
Kinematic Solver 运动学求解器，用来求解正运动学（Forward Kinematics）和逆运动学（IK），选择 kdl_kinematics_plugin
Kin. Search Resolution 运动学求解的精度（关节空间采样密度）
Kin. Search Timeout 求解时间，这个规划的时间可以适当拉长（设为0.5也可以），也可以默认 0.005

OMPL Planning 运动规划
Group Default Planner 规划器，可以选择 RRTConnect 规划算法

接着为以上的规划设置选择针对的关节Joints。
可点击 Add Kin.Chain 直接添加运动链
选择base_link为 Base Link，选择link5为 Tip Link，点击save。
这样就生成了一个规划组。


我们还可以添加新的规划组，比如我们还可以为夹爪添加一组规划组（这次就掠过了）。
我们规划机械臂运动是按规划组为单位进行规划的。

5 预定义机器人位姿（Robot Poses）

点击 Add Pose
我们把全角度为0的位姿定义为 up，点击 Save

我们再定义一个 down，joint2为-1.5708，其余角度不变，再保存。

6 设置终端（End Effectors）

可以设置夹爪，这里跳过

7 设置消极关节（Passive Joints）

设置规划时不让动的关节，这里没有，5个关节都要动。

8 设置 ROS Control

ROS是通过ROS Control中的一个个Controller接口，与gazebo仿真环境、或者实机环境进行连接的。
点击 Auto Add FollowJointTrajectory…
这样自动就为5个关节配置了一个Controller，Type叫做 FollowJointTrajectory（不是的话要改成这个type）
配置该Controller的作用就是把MoveIt输出的接口给封装好发出去；同时发出去还需要接收回来。

点击 Add Controller
添加一个 controller_gazebo，类型选择 position_controller/JointTrajectoryController

下面设置对哪些关节进行信息的接收，点击 Add Planning Group Joints
把xiaok放进来，点击 Save 保存

这样就添加了一组新的Controller，上面的Controller负责把MoveIt的数据发出去，下面的Controller负责把发出去的数据收进来，让gazebo中的仿真机械臂产生运动（实机也是一个道理）。

9 Simulation

可自动产生URDF模型，其中会加入各种gazebo仿真的属性，方便将这个机械臂模型放入gazebo。
点击 Generate URDF。

将这段代码覆盖到 xiaok.urdf 中。
注意将代码中 EffortJointInterface 改成 PositionJointInterface，因为我们前面建的是position_controller。

10 建立3D信息（Setup 3D Perception Sensors）

如果有3D感知的相机，可以设置
这里可先略过

11 添加作者信息

添加Name和邮箱地址

12 生成配置文件（Generate Configuration Files）

在工作空间 xiaok_ws/src 下新建一个文件夹 xiaok_moveit_config
选择该目录作为 Package Save Path
点击 Generate Package 完成

点击 Exit… 退出。

我们打开 xiaok_moveit_config/config 下的 dofbot.srdf 查看模型文件。文件名为 dofbot 是与我们之前urdf文件里定义的robot的名字一致。
其他文件还有一些参数文件：
chomp_planning.yaml 运动规划的参数
joint_limits.yaml 配置关节约束
kinematics.yaml 运动学算法的配置
ompl_planning.yaml 运动规划算法的配置
ros_controllers.yaml ROS Controller配置
sensors_3d.yaml 3D视觉接收器配置

13 运行demo

我们运行demo，来看看通过moveit可视化手臂运动，并进行轨迹规划。

roslaunch xiaok_moveit_config demo.launch

这里肯定有一堆问题，一一解决：
①报警告：The root link_base has an inertia specified in the URDF, but KDL does not support …
解决：
在原先的urdf模型文件中，base_link前分别加上

<link name="base_footprint" />

<joint name="base_footprint_joint" type="fixed">
  <parent link="base_footprint" />
  <child link="base_link" />
</joint>

②报警告：Skipping virtual joint ‘fixed_base’ because its child frame ‘base_link’ does not match the URDF…
解决：
打开config下的dofbot.srdf
修改virtual_joint下的child_link：
<virtual_joint name=“fixed_base” type=“fixed” parent_frame=“world” child_link=“base_footprint” />

③报错：
Unable to connect to move_group action server ‘move_group‘ within allotted time (30s)
模型加载时间过长，导致rviz启动有问题
解决：
先将demo.launch文件中rviz节点注释掉：
然后运行roslaunch xiaok_moveit_config demo.launch，等待启动，直到显示绿字：
You can start planning now!
然后运行：rosrun rviz rviz手动打开rviz。
先把Fixed Frame改成 base_link；然后Add → Robot Model；最后Add → MotionPlanning。
就可以完整显示rviz了，并进行MotionPlanning了。

④rviz中运动规划的轨迹球无法拖动。
勾选：
这样就可以拖动轨迹球到目标位置，然后点击Plan进行轨迹规划了，再点击Execute可以让机械臂从规划好的轨迹移动。

⑤如何启动实时控制互动界面：joint_state_publisher_gui
方法1：打开 launch/demo.launch：
<arg name="use_gui" default="false" />改成<arg name="use_gui" default="true" />，回工作空间catkin_make一下再启动。
方法2（临时打开，不推荐）：新打开一个终端，输入：rosrun joint_state_publisher_gui joint_state_publisher_gui可直接打开。

解决！