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

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>
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142

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>
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

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
1

点击“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
1

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

<link name="base_footprint" />
1

<joint name="base_footprint_joint" type="fixed">
  <parent link="base_footprint" />
  <child link="base_link" />
</joint>
1
2
3
4

②报警告：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可直接打开。

解决！