UR5-Drawing

光头明明

发布时间 2021.02.28阅读数 7774 评论数 6

首先感谢实验课的助教工作，然我看到了学生们的热情以及大家的智慧。这是我在其中绘图机器人实验的基础上进行复现完成的一次demo演示。基于ROS，我们利用gcode与moveit结合产生了一种离线生成路点，并利用笛卡尔路径规划进行运动学规划的绘图方式。

本系统在 Ubuntu 16.04 + ros kinetic下测试通过，机械臂控制采用moveit

整体方案：

图片转换成svg格式->svg图片边缘轮廓->转为gcode->解析gcode ->运动学逆解执行->笛卡尔路径规划->机械臂空间绘图

具体原理：

1，图片转换成svg格式

这里我利用了在线软件对原始图像去除了背景，这样可以简化简笔画的绘图，然后我们利用在线软件将图片转换成了svg图像。

2，解析svg图像

我们将svg图片边缘轮廓进行提取，算法主要过程包含以下三个部分

1.对SVG图像进行解析，寻找里面的几何图形

2.针对几何图形进行边缘路径生成

3.针对弧线、圆形、椭圆，使用贝塞尔曲线进行拟合，并进行离散化采样这里我们参照了 ArtLine 2020 -11 月开源的代码，主要完成了简笔画的生成部分。

3，转为gcode

从维基百科中，我们知道，G代码（G-code，又称RS-274），是最为广泛使用的数控（numerical control）编程语言，有多个版本，主要在计算机辅助制造中用于控制自动机床。G代码有时候也称为G编程语言。这一步我们将生成的轨迹，通过采样转换成了gcode代码。

针对于2，3两部分内容，总结成SVG图像边缘轨迹采样转换算法，如下：

1.对SVG图像进行解析，寻找里面的几何图形

2.针对几何图形进行边缘路径生成

3.针对弧线、圆形、椭圆，使用贝塞尔曲线进行拟合，并进行离散化采样

4.每个图形结束时候加入抬笔动作 5.生成通用路径代码 G-code

4，对gcode代码进行解析，进行逆运动学规划，生成路点

我们将gcode代码，转换成了一系列的离散路点，用于机械臂笛卡尔路径规划，主要流程图如下：

5，笛卡尔路径规划

MoveIt！是一个主要用于执行“自由空间”运动的框架，该框架的目标是将机器人从A点移动到B点，而您并不特别关心它是如何完成的。这些类型的问题只是频繁执行任务的一部分。想象一下任何制造“过程”，例如焊接或喷漆。您非常关心该工具在机器人整个工作期间所指向的位置。笛卡尔运动规划器，旨在使机器人沿某个过程路径移动。它只是解决此类问题的多种方法中的一种，但它具有一些简洁的属性：

它是确定性的且全局最佳（对于特定的搜索分辨率）。
它可以搜索问题中的多余自由度（例如您有7个机器人关节，或者您的过程中工具的Z轴旋转无关紧要）。

由于我们通过gcode解析，逆运动学规划获得的路点，存在距离大的特点，这会引起实际运行机械臂的卡顿问题，所以我们利用笛卡尔运动规划，对两点路点，进行采样，来实现更柔顺的机械臂控制。

具体实现：

1，图片转换成svg格式

1-1，去除图片背景

https://www.remove.bg/zh/upload

1-2，图片转svg格式

https://www.pngtosvg.com/ 调整参数可以保证svg文件可以转换成gcode代码

2，解析svg图像，转为gcode

运行：

roscd draw_core/
cd scripts/
python svg_convert.py img/**.svg

转化出来的gcode代码保存在gcode_output文件下，以下是转化出来的部分代码

G28
G1 Z0.0
M05
G4 P0.2
G0 X26.0 Y135.6
M03
G0 X25.6 Y135.4
G0 X24.5 Y134.8
G0 X23.3 Y133.9
G0 X22.1 Y132.9
G0 X20.5 Y131.4
G0 X18.1 Y128.6
G0 X16.6 Y126.3
G0 X16.3 Y125.5
G0 X15.9 Y124.1
G0 X15.6 Y121.2
G0 X15.5 Y117.7
G0 X15.6 Y113.9
G0 X15.8 Y110.0
G0 X16.3 Y106.4
G0 X16.8 Y103.3
G0 X17.3 Y101.6
G0 X17.7 Y102.1
G0 X18.5 Y103.7
G0 X19.2 Y104.8
G0 X19.4 Y105.0

可以看到是一系列的平面点，然后前面带有相应的gcode命令。这里G0代表到这一点指令;M3代表了落笔指令;M5代表了提笔指令。这里提供一个在线软件，可以查看我们的gcode代码是否与实际图片一致，并且可以通过这个软件查看gcode代码的运行过程，这里需要注意的是，我们在图形过程中，加入了提笔和落笔的过程，这样是为了保证我们在绘制不同图形时，不会因起点不一致，错误的绘制出多余的轨迹。

3，查看gcode代码

通过以下软件,可以进行查看

https://ncviewer.com/

4,解析gcode指令

这里将相应的gcode指令取出，并转化为机械臂的路点我们主要是在gcode_excute.py定义了gcode_excute类，其具体代码如下：

class gcode_excute():
    def __init__(self, x0, y0,Controller):
        self.x0, self.y0 = x0, y0 # coordinate offset

        self.s = -0.001 # scale factor (mm -> m)

        # self.amax = 0.3

        # instanlize controller
        self.controller = Controller

    def G0(self, x, y): # the "just go there" instruction
        xn, yn = self.tf(x, y)
        print "G0",xn,yn
        # self.path = line.Line(self.x, self.y, xn, yn, 0.5, self.amax)
        target_point = self.controller.get_current_pos()
        target_point.position.x = xn
        target_point.position.y = yn
        
        self.controller.draw_line(target_point)

    def G1(self, x, y, fr): # move to x, y in a line at a speed in mm/minute
        fr /= 1000 * 60 # mm/minute -> m/sec
        xn, yn = self.tf(x, y)
        print "G1",xn,yn
        # self.path = line.Line(self.x, self.y, xn, yn, fr, self.amax)
        target_point = self.controller.get_current_pos()
        target_point.position.x = xn
        target_point.position.y = yn
        self.controller.draw_line(target_point)

    def M3(self):   # Down the pen
        self.controller.down_pen()
    
    def M5(self):   # Up the pen
        self.controller.up_pen()

将gcode点赋给target_point.position变量

5,笛卡尔路径规划

主要程序在arm_controller.py下首先我们对具体参数进行初始化，包括，机械臂规划组名; 参考坐标系; 最大速度; 加速度范围; 方位及位置容忍度; 规划最长时间及笛卡尔路径规划最小间隔，这里我们定义成了1cm，最后对提笔高度和落笔高度进行了定义。然后我们进行原点设定： 使用示教器将机械臂运行到绘制的原点(原点定义为绘图平面的0,0点并且该点高度为落笔绘制高度)

roslaunch ur_modern_driver ur5_bringup.launch robot_ip:=192.168.20.200 （实际IP） 
roslaunch ur5_moveit_config ur5_moveit_planning_execution.launch
roslaunch ur5_moveit_config moveit_rviz.launch config:=true
rosrun draw_core arm_controller.py

记录下终端中 current_joint_values的值填入draw_core/scripts/arm_controller.py 中go_home函数joint_positions 数组中最后我们定义了具体执行指令，如下

    def move(self, goal_state):
        # get current state
        current_state = geometry_msgs.msg.Pose()
        current_state = self.group.get_current_pose().pose
        self.group.set_pose_target(goal_state)

        # print("111111111111111")

        # plan and execute
        plan = self.group.go(wait = True)
        time.sleep(5.0)
        self.group.stop()
        self.group.clear_pose_targets()
        print("achieve move goal")

    def draw_line(self, end_point):
        # get current state
        current_state = geometry_msgs.msg.Pose()
        current_state = self.group.get_current_pose().pose

        line_points = []
        line_points.append(copy.deepcopy(current_state))
        line_points.append(end_point)
        # print("111111111111111")       
        # draw the line
        (line_traj, fraction) = self.group.compute_cartesian_path(line_points, self.line_gap, 0,avoid_collisions= False)
        # print("line_traj pose", line_traj.joint_trajectory.points)
        self.group.execute(line_traj, wait=True)
        # print("2222222222222222")
        time.sleep(0.25)
        # time.sleep(q)
                
    def up_pen(self):   #Up the pen
        current_state_ = geometry_msgs.msg.Pose()
        current_state_ = self.group.get_current_pose().pose
        current_state_.position.z = self.up_pen_height
        self.draw_line(current_state_)
        # self.move(current_state_)        

    def down_pen(self):   #Down the pen
        current_state_ = geometry_msgs.msg.Pose()
        current_state_ = self.group.get_current_pose().pose
        current_state_.position.z = self.down_pen_height
        self.draw_line(current_state_)
        # self.move(current_state_)

6,执行

我们将上述的操作，融合到了gcode_draw.py中。然后创建了start_draw.launch文件，文件内容如下，需要重新修改这里的文件的绝对路径，找到gcode代码。

<?xml version="1.0"?>
<launch>
 <!-- Run the gcode draw with the gcode path -->
  <arg name="file" default="$(find draw_core)/scripts/img/gcode_output/22.gcode"/>
  <node pkg="draw_core" type="gcode_draw.py" name="gcode_draw" args="$(arg file)" output="screen"/>
  
</launch>

最后实验：

我们分别在gazebo环境下和实际机械臂环境下，对我们的绘制图形过程，进行了实现

1,首先在模拟环境下：

运行以下语句，完成了机械臂的轨迹规划

roslaunch ur_gazebo ur5.launch limited:=true
roslaunch ur5_moveit_config ur5_moveit_planning_execution.launch sim:=true limited:=true
roslaunch ur5_moveit_config moveit_rviz.launch config:=true
roslaunch draw_core start_draw.launch

演示效果：即可看到UR5机械臂在gazebo下正在空中绘制图形

2,实际环境中，进行的实验：

运行实际的机械臂绘图：

roslaunch ur_modern_driver ur5_bringup.launch robot_ip:=192.168.20.200 （实际IP）
roslaunch ur5_moveit_config ur5_moveit_planning_execution.launch 
roslaunch ur5_moveit_config moveit_rviz.launch config:=true
roslaunch draw_core start_draw.launch

演示效果图及视频：

当然由于提笔程序出现问题，也出现了下面的画面：

解决错误：

1,我们可以利用网线通讯代替无线通讯的方式，可以一定的程度解决机械臂的卡顿问题。 2,在运行过程中，由于笛卡尔坐标系中语句初始化不对，分别在不同终端下出现过以下错误

error: Trajectory message contains waypoints that are not strictly increasing in time

[ WARN] [1610200121.539126017, 53.226000000]: Controller  failed with error code INVALID_GOAL
[ WARN] [1610200121.539203351, 53.226000000]: Controller handle  reports status FAILED
###############
[ INFO] [1610200121.259221852, 52.946000000]: ABORTED: Solution found but controller failed during execution

解决方法： https://answers.ros.org/question/253004/moveit-problem-error-trajectory-message-contains-waypoints-that-are-not-strictly-increasing-in-time/ Not adding start points into waypoints solved this problem for me. In the Move Group Interface Tutorial they've added start point for visualization: You can plan a cartesian path directly by specifying a list of waypoints for the end-effector to go through. Note that we are starting from the new start state above. The initial pose (start state) does not need to be added to the waypoint list but adding it can help with visualizations。 This solves the "Trajectory message contains waypoints that are not strictly increasing in time" issue for me, using the Python MoveIt interface (which is definitely undocumented!).

3,机械臂出现保护性停止卡顿问题（未解决）我试着修改了安全配置这一块，避免保护性停止，但是效果并不好。

程序下载地址：

https://github.com/harrycomeon/UR5-Drawing

文件结构

aubo_robot 遨博机械臂驱动 industrial_core 遨博机械臂驱动依赖 draw_core 绘图部分核心代码

arm_controller.py 机械臂控制核心
gcode_draw.py gcode解析与绘制
gcode_excute gcode指令与机械臂控制的中间接口文件
svg_convert svg转gcode
py_svg2gcode svg转Gcode核心代码库
- lib/shapes.py svg形状类型解析
- lib/cubicsuperpath.py 曲线插值

参考网址：

https://github.com/mywisdomfly/drawing_manipulator

https://zhuanlan.zhihu.com/p/63216771

ROS

原创文章作者：光头明明。如若转载，请注明出处：古月居 https://www.guyuehome.com/27468

打赏 0

上一篇：Kinect V2 相机标定

下一篇：fatal error: Eigen/Geometry: 没有那个文件或目录

UR5-Drawing

光头明明

本系统在 Ubuntu 16.04 + ros kinetic下测试通过 ，机械臂控制采用moveit

整体方案：

具体原理：

1，图片转换成svg格式

2，解析svg图像

3，转为gcode

4，对gcode代码进行解析，进行逆运动学规划，生成路点

5，笛卡尔路径规划

具体实现：

1，图片转换成svg格式

1-1，去除图片背景

1-2，图片转svg格式

2，解析svg图像，转为gcode

3，查看gcode代码

4,解析gcode指令

5,笛卡尔路径规划

6,执行

最后实验：

1,首先在模拟环境下：

2,实际环境中，进行的实验：

解决错误：

程序下载地址：

文件结构

参考网址：

为你推荐

RTOS学习笔记--任务管理

C#上位机开发——基于Arduino的工控器设计

Kaggle从google drive下载文件的方法

教你用Python快速开发3D点云数据处理上位机（1）

第五讲：相机与图像

人工智能绘画的算法和原理简述

评论（6）

关于作者

光头明明

23

0

0

2

利用ur5_ros_control实现基于速度的位置控制UR5机械臂

UR5-Drawing

解决 ros 安装sudo rosdep init 出现 ERROR: cannot download default sources list from: https://raw.githubuse

相关推荐

ROS CAN总线设备接入（一）Linux动态库的显式调用

【Python】Scipy 简介，signal 模块介绍

精选傅里叶级数、傅里叶变换 (FT)

C语言——数据类型 【32位/64位】下的字节大小

精选推荐一个嵌入式静态代码扫描工具！

【LVGL学习笔记】（二） 基础概念

热门泡泡

30积分 失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

给作者打赏

忘记密码

修改头像

添加你感兴趣的标签

举报类型（必选）

举报详情（选填）

本系统在 Ubuntu 16.04 + ros kinetic下测试通过，机械臂控制采用moveit

C语言——数据类型【32位/64位】下的字节大小

【LVGL学习笔记】（二）基础概念

30积分失眠，聊聊自己搞ROS的心得体会吧

5积分想买能用ROS2的开发套件。或者开发板