【ROS机械臂入门教程】

首先声明一下，此项目是参考B站哈萨克斯坦UP的【ROS机械臂入门教程】，前期以复现【机械臂视觉抓取从理论到实战】
此内容为他研究生生涯的阶段性成果展示和技术分享，所有数据和代码均开源。所以鹏鹏我又特此来复现一下，我采用的硬件与之有所不同，UP主使用UR5，我实验室采用的是UR3，下面列出相关材料
UR3CB3.12:https://www.universal-robots.cn/cb3/
【UR3系统升级到CB3.12附带URcap1.05】

硬件支持

序号	名称	功能
1	UR3机械臂	一切行动的执行者
2	D435i	执行者的眼睛
3	Ubuntu深度学习环境	执行者思维的大脑
4	平面抓取平台	实验操作环境
5	6*6黑白棋盘标定板	相机眼在手外标定
6	3x3x3cm 3D打印小方块	工件演员

代码支持
ros机械臂入门教程代码 version3.0链接：
链接：https://pan.baidu.com/s/1KFLQXVWShG5KfroCd6eM0A=8888
提取码：8888
【ROS机械臂入门教程-小五】PPT
链接：https://pan.baidu.com/s/18ierKnf8OJPPvGn8uOf1hw=8888
提取码：8888

1. 概述

【Autolabor初级教程】ROS机器人入门

机器人操作系统 ROS 快速入门教程

2. ROS简介

2.1 ROS介绍

ROS官网：http://wiki.ros.org/

autolabor地址:http://www.autolabor.com.cn/book/ROSTutorials/

Moveit api:https://moveit.ros.org/documentation/source-code-api/

melodic:http://docs.ros.org/en/melodic/api/moveit_tutorials/html/index.html

2.2 ROS机器人包

ros-melodic:git clone https://github.com/ros-industrial/universal_robot.git
ros-noetic:https://blog.csdn.net/Dawn_yc/article/details/114791755

3. 机器人URDF建模

3.1 URDF简介

URDF是ROS中机器人模型的描述格式,包含对机器人刚体外观、物理属性、关节类型等方面的描述

3.2 查看模型

$ check_urdf xxx.urdf 检查xxx.urdf文件,发现错误
$ urdf_to_graphiz xxx.urdf
将urdf文件生成pdf文件,进行可视化

$ roslaunch ur_description view_ur5.launch 展示ur5模型

3.3 配置保存Rviz

roslaunch ur_description view_ur5_with_gripper.launch 展示ur5+robotiq85模型

roslaunch ur_description view_ur5.launch
rviz

rviz保存路径在universal_robot/ur_description/cfg/2222.rviz

<launch>
  <include file="$(find ur_description)/launch/ur5_upload.launch"/>

  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" >
    <param name="use_gui" value="true"/>
  node>
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find ur_description)/cfg/2222.rviz" required="true" />
launch>

4. Moveit!核心功能介绍及Rviz控制

4.1 Moveit!简介

MoveIt! 是机械臂相关的工具集软件,
集成了各种功能:
Ø 运动学(Kinematics)
Ø 运动规划(Motion Planning)
Ø 碰撞检测(Collision Checking)
Ø 3D 感知(Perception)
Ø 操作(Manipulation)

4.2 机器人ROS包架构

rosrun moveit_setup_assistant moveit_setup_assistant
加载urdf模型
检查关节之间是否有碰撞

选择求解器

选择基准连接和末端求解

配置相关姿态位置

平躺全零

up姿态0,-1.5708,0-1.5708,0,0

配置末端夹爪

填写个人信息
最后创建模型

直接导入模型

4.3 电脑设置

moveit配置之后有对应的Rviz demo

roslaunch ur5_moveit_config demo.launch

随机目标位置，点击plan&execute，即可运动到对应位置

添加障碍物
夹爪demo

roslaunch ur5_gripper_moveit_config demo.launch

5. Gazebo仿真or控制真实机器人

超级终端

sudo apt install terminator

5.1 Moveit与控制器的连接

使用方式

# gazebo仿真中控制机器人
roslaunch ur_gazebo ur5.launch
roslaunch ur5_moveit_config ur5_moveit_planning_execution.launch sim:=true
roslaunch ur5_moveit_config moveit_rviz.launch config:=true
# 控制真实UR5机器人
一键启动: 
roslaunch ur_planning start_ur5.launch
分开启动:
roslaunch ur_modern_driver ur5_bringup.launch
roslaunch ur5_moveit_config ur5_moveit_planning_execution.launch
roslaunch ur5_moveit_config moveit_rviz.launch config:=true
# rviz 仅作可视化显示
roslaunch ur5_moveit_config demo.launch
rosrun ur_planning moveitServer.py

5.2 Gazebo仿真

5.3 控制真实UR5机器人

一键启动:

roslaunch ur_planning start_ur5.launch

分开启动:

roslaunch ur_modern_driver ur5_bringup.launch
roslaunch ur5_moveit_config ur5_moveit_planning_execution.launch
roslaunch ur5_moveit_config moveit_rviz.launch config:=true

6. Moveit基础（python）

6.1 Moveit!回顾

6.2 关节空间运动

Python中安装moveit_commander

sudo apt-get update
sudo apt-get install ros-melodic-moveit-commander

# 关节规划，输入6个关节角度（单位：弧度）
    def move_j(self, joint_configuration=None,a=1,v=1):
        # 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
        if joint_configuration==None:
            joint_configuration = [0, -1.5707, 0, -1.5707, 0, 0]
        self.arm.set_max_acceleration_scaling_factor(a)
        self.arm.set_max_velocity_scaling_factor(v)
        self.arm.set_joint_value_target(joint_configuration)
        rospy.loginfo("move_j:"+str(joint_configuration))
        self.arm.go()
        rospy.sleep(1)

    # 空间规划，输入xyzRPY
    def move_p(self, tool_configuration=None,a=1,v=1):
        if tool_configuration==None:
            tool_configuration = [0.3,0,0.3,0,-np.pi/2,0]
        self.arm.set_max_acceleration_scaling_factor(a)
        self.arm.set_max_velocity_scaling_factor(v)

        target_pose = PoseStamped()
        target_pose.header.frame_id = self.reference_frame
        target_pose.header.stamp = rospy.Time.now()
        target_pose.pose.position.x = tool_configuration[0]
        target_pose.pose.position.y = tool_configuration[1]
        target_pose.pose.position.z = tool_configuration[2]
        q = quaternion_from_euler(tool_configuration[3],tool_configuration[4],tool_configuration[5])
        target_pose.pose.orientation.x = q[0]
        target_pose.pose.orientation.y = q[1]
        target_pose.pose.orientation.z = q[2]
        target_pose.pose.orientation.w = q[3]

        self.arm.set_start_state_to_current_state()
        self.arm.set_pose_target(target_pose, self.end_effector_link)
        rospy.loginfo("move_p:" + str(tool_configuration))
        traj = self.arm.plan()
        self.arm.execute(traj)
        rospy.sleep(1)

6.3 笛卡尔空间运动

# 空间直线运动，输入(x,y,z,R,P,Y,x2,y2,z2,R2,...)
    # 默认仅执行一个点位，可以选择传入多个点位
    def move_l(self, tool_configuration,waypoints_number=1,a=0.5,v=0.5):
        if tool_configuration==None:
            tool_configuration = [0.3,0,0.3,0,-np.pi/2,0]
        self.arm.set_max_acceleration_scaling_factor(a)
        self.arm.set_max_velocity_scaling_factor(v)

        # 设置路点
        waypoints = []
        for i in range(waypoints_number):
            target_pose = PoseStamped()
            target_pose.header.frame_id = self.reference_frame
            target_pose.header.stamp = rospy.Time.now()
            target_pose.pose.position.x = tool_configuration[6*i+0]
            target_pose.pose.position.y = tool_configuration[6*i+1]
            target_pose.pose.position.z = tool_configuration[6*i+2]
            q = quaternion_from_euler(tool_configuration[6*i+3],tool_configuration[6*i+4],tool_configuration[6*i+5])
            target_pose.pose.orientation.x = q[0]
            target_pose.pose.orientation.y = q[1]
            target_pose.pose.orientation.z = q[2]
            target_pose.pose.orientation.w = q[3]
            waypoints.append(target_pose.pose)
        rospy.loginfo("move_l:" + str(tool_configuration))
        self.arm.set_start_state_to_current_state()
        fraction = 0.0  # 路径规划覆盖率
        maxtries = 100  # 最大尝试规划次数
        attempts = 0  # 已经尝试规划次数

        # 设置机器臂当前的状态作为运动初始状态
        self.arm.set_start_state_to_current_state()

        # 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
        while fraction < 1.0 and attempts < maxtries:
            (plan, fraction) = self.arm.compute_cartesian_path(
                waypoints,  # waypoint poses，路点列表
                0.001,  # eef_step，终端步进值
                0.00,  # jump_threshold，跳跃阈值
                True)  # avoid_collisions，避障规划
            attempts += 1
        if fraction == 1.0:
            rospy.loginfo("Path computed successfully. Moving the arm.")
            self.arm.execute(plan)
            rospy.loginfo("Path execution complete.")
        else:
            rospy.loginfo(
                "Path planning failed with only " + str(fraction) +
                " success after " + str(maxtries) + " attempts.")
        rospy.sleep(1)

6.4 与环境交互

# 在机械臂下方添加一个table，使得机械臂只能够在上半空间进行规划和运动
    # 避免碰撞到下方的桌子等其他物体
    def set_scene(self):
        ## set table
        self.scene = PlanningSceneInterface()
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5)
        self.colors = dict()
        rospy.sleep(1)
        table_id = 'table'
        self.scene.remove_world_object(table_id)
        rospy.sleep(1)
        table_size = [2, 2, 0.01]
        table_pose = PoseStamped()
        table_pose.header.frame_id = self.reference_frame
        table_pose.pose.position.x = 0.0
        table_pose.pose.position.y = 0.0
        table_pose.pose.position.z = -table_size[2]/2 -0.02
        table_pose.pose.orientation.w = 1.0
        self.scene.add_box(table_id, table_pose, table_size)
        self.setColor(table_id, 0.5, 0.5, 0.5, 1.0)
        self.sendColors()

7. Moveit基础（C++） 实现带约束的路径规划

8. 运动规划-如何选择Moveit中的规划算法？

8.1 为什么要路径规划

Ø 避障:避免与桌子等机械臂附近的静态物体发生碰撞;避免与(突然走近的)人
等动态物体发生碰撞
Ø 任务对运动的路径有要求:具有运动学或动力学约束,如焊接、抓取装有水的
杯子等
n 路径规划分类
Ø 基于搜索,Dijkstra,A*,Anytime A*、ARA*、D*
Ø 基于采样,PRM,RRT,RRT-connect,RRT*,Kinodynamic-
RRT*(符合动力学),Anytime RRT*,Informed RRT*
Ø 智能算法如遗传算法、蚁群算法
基于采样的路径规划算法
Ø 基础知识:概论完备:时间无穷大必能找到解;
Ø PRM:1.先随机采样n个点构成图 2.基于图进行搜索
Ø RRT:快速搜索随机树,非最优解;查询near可用kdtree等数据结构,
快速找到树中最近结点
Ø RRT-connect:双向扩展的RRT,从start和goal同时扩展,搜索速度比
较快,ros-moveit默认算法
Ø RRT*:作了两个改进,一是改变了新节点连接到树的规则,二是对搜
索树进行“剪枝”操作,使之更接近于真实的最优路径

8.2 如何选择路径规划算法

1. Rviz
   
2. Moveit_setup_assistant
   
   
3. 程序中

9. 视觉避障

9.1 引入

9.2 官方demo

Ø 只需要有一个点云/深度图输入,即可由此生成栅格图;

$ roslaunch moveit_tutorials obstacle_avoidance_demo.launch

Ø 由栅格图又可生成圆柱体障碍物

$ roslaunch moveit_tutorials detect_and_add_cylinder_collision_object_demo.launch

(注:运行此语句时,需要关闭上一条命令)
功能包:

https://github.com/ros-planning/moveit_tutorials
https://github.com/ros-planning/panda_moveit_config

9.3 实战

roslaunch ur5_moveit_config demo_with_obstacle_avoidance.launch

主要步骤
Ø 1.启动相机ros程序

$ sudo apt-get install ros-melodic-realsense2-camera
$ sudo apt-get install ros-melodic-realsense2-description
$ roslaunch realsense2_camera demo_pointcloud.launch

Ø 2.修改moveit配置文件
(1)修改xxx_moveit_config/config/sensors_kinect_pointcloud.yaml的
point_cloud_topic为自己相机点云发布的话题
(2)或者修改xxx_moveit_config/config/sensors_kinect_depthmap.yaml的
image_topic为自己相机深度图发布的话题
Ø 3(可选)修改视觉避障参数,如栅格分辨率octomap_resolution等
Ø 4(可选)修改相机参数,如分辨率、更新频率等
Ø 5.发布相机相对于机械臂基坐标的位姿

<node pkg="tf2_ros" type="static_transform_publisher"
name="camera_to_robot" args="0.72032458 -0.21936176 0.90925255
0.98422218 0.0969083 -0.04779138 0.14011233 camera_color_optical_frame
world" />

roslaunch ur5_moveit_config demo_with_obstacle_avoidance.launch

参考https://blog.csdn.net/ssw_1990/article/details/104053041
参考https://blog.csdn.net/ssw_1990/article/details/104053041

10. ROS与深度学习

10.1 引入

ROS-Moveit!

ROS-melodic以及之前版本Moveit!默认使用python2.7
ROS-noetic以及ROS2支持python3

深度学习
深度学习均使用python3,且深度学习一般需要在虚拟环境中配置环境
如何将深度学习融入ROS呢?

10. 2 实战

n ROS与Anaconda共存
Ø 1.一键安装ROS

$ wget http://fishros.com/install -O fishros && bash fishros

Ø 2.安装anaconda
https://blog.csdn.net/KIK9973/article/details/118772450
百度搜索对应ubuntu版本的anaconda安装教程
Ø 3.anaconda中创建虚拟环境

conda create -n YOUR_ENV_NAME
pyhton=3.7

Ø 4.在虚拟环境中安装ros库

$ conda activate
$ pip install rospkg/rospy

注:不管是不是虚拟环境,只要安装了rospkg 就一定能找到ros的包如rospy,
std_msgs等,因此便可进行话题通信和服务通信
Ø 5.将moveit功能封装成一个server (第6讲-Moveit基础-python/ros服务通信)

$ rosrun ur_planning moveitServer.py

Ø 6.环境中脚本中写一个client,通过服务通信与moveit功能连接

11. 基于ROS-Moveit实现6-DOF视觉抓取

11.1 视觉抓取

1. 平面抓取
   Ø Cornell数据集:1035张RGBD,8019个标签
   Ø Jacquard数据集:54K张RGBD,110万个标签
   Ø 经典平面抓取算法:GGCNN[1]、GRCNN[2]、
   Swin-Transformer[3]
2. 六自由度抓取
   GraspNet-1Billion数据集

11.系统框架设计

n 运动规划
Ø 作用:到达算法指定
位置和姿态
Ø 路径规划:从当前位
置到期望位置规划一
条无碰撞轨迹
n 手眼标定
Ø 作用:获得机器人坐标系和相机
坐标系的关系
Ø 效果:目前可达到误差在±5mm

11. 3 抓取实验

n graspness[6]单物体抓取实验
抓取物品清单:
共11个物体:1.苹果 2.香蕉 3.手电筒 4.棒球
5.鞋刷 6.羽毛球盒 7.收音机 8.羽绒服清洁剂
9.小白鞋清洁剂 10.U盘 11.遥控器

11.3 抓取实验

n graspness[6]复杂场景连续抓取实验
Ø 结论:与其他算法相比,graspness[6]抓取姿态预测结果更准确;对大多数小物体
和与背景颜色相同的物体也能检测出抓取姿态,鲁棒性更强。
Ø 存在的问题:
l 过于“平坦”且高度较低的物体,很难检测到抓取姿态
l 抓取过程中,可能产生对其他物体的碰撞