Dofbot机械臂学习（1）

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

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

一、创建URDF模型

1、创造新的工作空间

在根目录下创建工作空间

mkdir -p catkin_ws/src
cd catkin_ws/
catkin_make

进入src目录：

catkin_create_pkg dofbot_description urdf xacro

2、文件配置

进入dofbot_description目录，建立四个文件夹：

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

2.1 urdf文件

进入urdf目录，创建模型文件：

touch dofbot.urdf

  dofbot机械臂的urdf模型

<?xml version="1.0" encoding="utf-8" ?>
<robot name="yahboom_dofbot">
    <link name="base_link">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <geometry>
                <mesh filename="package://dofbot_description/meshes/base_link.STL" />
            </geometry>
            <material name="">
                <color rgba="0.2 0.2 0.2 1" />
            </material>
        </visual>
        <collision>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <geometry>
                <mesh filename="package://dofbot_description/meshes/base_link.STL" />
            </geometry>
        </collision>
    </link>
    
    <link name="arm_link1">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <geometry>
                <mesh filename="package://dofbot_description/meshes/visual/arm_link1.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://dofbot_description/meshes/collision/arm_link1.STL" />
            </geometry>
        </collision>
    </link>
    
    <joint name="arm_joint1" type="revolute">
        <origin xyz="0 0 0.06605" rpy="-0.010805 0 1.5" />
        <parent link="base_link" />
        <child link="arm_link1" />
        <axis xyz="0 0 1" />
        <limit effort="30" velocity="10.0" lower="-1.5708" upper="1.5708" />
    </joint>
    
    
    <link name="arm_link2">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <geometry>
                <mesh filename="package://dofbot_description/meshes/visual/arm_link2.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://dofbot_description/meshes/collision/arm_link2.STL" />
            </geometry>
        </collision>
    </link>
    <joint name="arm_joint2" type="revolute">
        <origin xyz="0 0 0.0405" rpy="-1.5708 0 0" />
        <parent link="arm_link1" />
        <child link="arm_link2" />
        <axis xyz="0 0 -1" />
        <limit lower="-1.5708" upper="1.5708" effort="100" velocity="10" />
    </joint>
    <link name="arm_link3">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <geometry>
                <mesh filename="package://dofbot_description/meshes/visual/arm_link3.STL" />
            </geometry>
            <material name="">
                <color rgba="1 1 1 1.0" />
            </material>
        </visual>
        <collision>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <geometry>
                <mesh filename="package://dofbot_description/meshes/collision/arm_link3.STL" />
            </geometry>
        </collision>
    </link>
    <joint name="arm_joint3" type="revolute">
        <origin xyz="0 -0.0829 0" rpy="0 0 0" />
        <parent link="arm_link2" />
        <child link="arm_link3" />
        <axis xyz="0 0 -1" />
        <limit lower="-1.5708" upper="1.5708" effort="100" velocity="10" />
    </joint>
    <link name="arm_link4">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <geometry>
                <mesh filename="package://dofbot_description/meshes/visual/arm_link4.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://dofbot_description/meshes/collision/arm_link4.STL" />
            </geometry>
        </collision>
    </link>
    <joint name="arm_joint4" type="revolute">
        <origin xyz="0 -0.0829 0" rpy="0 0 0" />
        <parent link="arm_link3" />
        <child link="arm_link4" />
        <axis xyz="0 0 -1" />
        <limit lower="-1.5708" upper="1.5708" effort="100" velocity="10" />
    </joint>
    <link name="arm_link5">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <geometry>
                <mesh filename="package://dofbot_description/meshes/visual/arm_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://dofbot_description/meshes/collision/arm_link5.STL" />
            </geometry>
        </collision>
    </link>
    <joint name="arm_joint5" type="revolute">
        <origin xyz="-0.00215 -0.17455 0" rpy="1.5708 0 0" />
        <parent link="arm_link4" />
        <child link="arm_link5" />
        <axis xyz="0 0 1" />
        <limit lower="-1.5708" upper="3.14159" effort="100" velocity="10" />
    </joint>
    <link name="rlink1">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <geometry>
                <mesh filename="package://dofbot_description/meshes/visual/rlink1.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://dofbot_description/meshes/visual/rlink1.STL" />
            </geometry>
        </collision>
    </link>
    <joint name="grip_joint" type="revolute">
        <origin xyz="-0.0035 -0.012625 -0.0685" rpy="0 -1.5708 0" />
        <parent link="arm_link5" />
        <child link="rlink1" />
        <axis xyz="0 0 1" />
        <limit lower="-1.54" upper="0" effort="100" velocity="10" />
    </joint>
    <link name="rlink2">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <geometry>
                <mesh filename="package://dofbot_description/meshes/visual/rlink2.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://dofbot_description/meshes/visual/rlink2.STL" />
            </geometry>
        </collision>
    </link>
    <joint name="rlink_joint2" type="continuous">
        <origin xyz="0.03 -0.00075429 0" rpy="0 0 0" />
        <parent link="rlink1" />
        <child link="rlink2" />
        <axis xyz="0 0 1" />
        <mimic joint="grip_joint" multiplier="-1" />
    </joint>
    <link name="rlink3">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <geometry>
                <mesh filename="package://dofbot_description/meshes/visual/rlink3.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://dofbot_description/meshes/visual/rlink3.STL" />
            </geometry>
        </collision>
    </link>
    <joint name="rlink_joint3" type="continuous">
        <origin xyz="-0.0035 -0.0045 -0.0505" rpy="0 -1.5708 0" />
        <parent link="arm_link5" />
        <child link="rlink3" />
        <axis xyz="0 0 1" />
        <mimic joint="grip_joint" multiplier="1" />
    </joint>
    <link name="llink1">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <geometry>
                <mesh filename="package://dofbot_description/meshes/visual/llink1.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://dofbot_description/meshes/visual/llink1.STL" />
            </geometry>
        </collision>
    </link>
    <joint name="llink_joint1" type="continuous">
        <origin xyz="-0.0035 0.012375 -0.0685" rpy="0 -1.5708 0" />
        <parent link="arm_link5" />
        <child link="llink1" />
        <axis xyz="0 0 1" />
        <mimic joint="grip_joint" multiplier="-1" />
    </joint>
    <link name="llink2">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <geometry>
                <mesh filename="package://dofbot_description/meshes/visual/llink2.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://dofbot_description/meshes/visual/llink2.STL" />
            </geometry>
        </collision>
    </link>
    <joint name="llink_joint2" type="continuous">
        <origin xyz="0.03 0.00065104 0" rpy="0 0 0" />
        <parent link="llink1" />
        <child link="llink2" />
        <axis xyz="0 0 1" />
        <mimic joint="grip_joint" multiplier="1" />
    </joint>
    <link name="llink3">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <geometry>
                <mesh filename="package://dofbot_description/meshes/visual/llink3.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://dofbot_description/meshes/visual/llink3.STL" />
            </geometry>
        </collision>
    </link>
    <joint name="llink_joint3" type="continuous">
        <origin xyz="-0.0035 0.0045 -0.0505" rpy="0 -1.5708 0" />
        <parent link="arm_link5" />
        <child link="llink3" />
        <axis xyz="0 0 1" />
        <mimic joint="grip_joint" multiplier="-1" />
    </joint>
 
</robot>

2.2 meshes文件

可以从以下链接提取

链接: https://pan.baidu.com/s/16a1roVT0-WjkZqEdYmZdpg 提取码: pdkg

2.3 launch文件

dofbot.launch

<launch>
    <arg name="model" default="dofbot.urdf"/>
    <arg name="gui" default="false"/>
    
    <!-- 设置机器人模型路径参数 -->
    <param name="robot_description" command="$(find xacro)/xacro --inorder $(find dofbot_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 dofbot_description)/config/dofbot.rviz" required="true"/>
</launch>

3、 编译并运行

进入工作空间目录编译：catkin_make
运行：

source ./devel/setup.bash
roslaunch dofbot_description dofbot.launch

4、 运行效果

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

 添加坐标系：
ADD → TF

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

5、检查模型

从 ~/catkin_ws/src/dofbot_description/urdf 打开终端，输入：

urdf_to_graphiz dofbot.urdf

会生成模型检查文档，打开：

二、配置MoveIt

1、启动Setup Assistant

roslaunch moveit_setup_assistant setup_assistant.launch

注意： 运行代码前需要确认是否打开roscore，建议进入工作空间内输入source ./devel/setup.bash刷新环境变量再打开moveit。

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

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

点击“Generate Collision Matrix”

3、添加虚拟关节（Virtual Joints）

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

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

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

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

添加夹爪规划组
点击Add Group，添加夹爪规划组。
创建组名设置为gripper_group，无需设置运动学求解器；点击Add Links添加夹爪连杆。 

 选中夹爪部分的连杆，点击【>】，自动添加右侧，点击Save保存。

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

点击 Add Pose
我们把全角度为0的位姿定义为 up，点击 Save，我们再定义一个 down，joint2为-1.5708，其余角度不变，再保存。

 添加init姿态

 添加夹爪close姿态

 添加夹爪open姿态

6、设置终端（End Effectors）

可以设置夹爪

7、设置消极关节（Passive Joints）

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

8、设置 ROS Control

ROS是通过ROS Control中的一个个Controller接口，与gazebo仿真环境、或者实机环境进行连接的。
点击 Auto Add FollowJointTrajectory…

9、Simulation

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

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

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

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

11、添加作者信息

添加Name和邮箱地址

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

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

点击 Exit… 退出。

我们打开 dofbot_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 dofbot_moveit_config demo.launch