首先感谢实验课的助教工作，然我看到了学生们的热情以及大家的智慧。这是我在其中绘图机器人实验的基础上进行复现完成的一次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代码
通过以下软件,可以进行查看
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.pygcode解析与绘制gcode_excutegcode指令与机械臂控制的中间接口文件svg_convertsvg转gcodepy_svg2gcodesvg转Gcode核心代码库lib/shapes.pysvg形状类型解析lib/cubicsuperpath.py曲线插值
参考网址：
https://github.com/mywisdomfly/drawing_manipulator
