SCARA机器人随机点位生成可视化

1.利用lua脚本随机生成XYZC的关节角度，再把关节逆解为笛卡尔坐标，保存为CSV数据。

2.利用python脚本读取lua生成的CSV表格进行绘图，到达仿真机器人运行的随机点在XY平面的分布。

3.设置生成100万和10万和1万个随机点的效果图：500万时脚本几经奔溃，数据量太大，到达黑洞视界边缘，

一个lua数组变量或python列表能存放数以万计的数据也是很强大了，不要过分弄那么大数据量哦。

红色：为随机点坐标

黄色：为机器人底座中心(x=0,y=0)及机器人不可到达的工作区域

绿色：为机器人可工作区域

因机器人各关节存在软限位，左侧区域是有部分无法到达的。

4.实现代码如下：

#!/usr/bin/env python3.6.5
# -*- coding: utf-8 -*-

from matplotlib.patches import Circle #matplotlib绘制圆形库
import matplotlib.pyplot as plt
from lupa import LuaRuntime
import lupa,time,csv #lupa v1.8
#下面两行代码专门用来解决中文显示问题
from pylab import *  
mpl.rcParams['font.sans-serif'] = ['Microsoft YaHei']

lua = LuaRuntime()
SCARA_Kinematics_Module = lua.require('SCARA_Kinematics_Py_Lua') #加载SCARA_Kinematics_Py_Lua.lua
end = "#" #Python代码块结束符

#-------------------------------------
#Lua代码
LuaCode = """
    function (TransNum)
        --********************* sub1 end *************************--
        --此处预留lua函数
        --******************* sub2 function **********************--
        --********************* sub2 end *************************--
        --此处预留lua函数
        --******************* sub3 function **********************--
        --此处预留lua函数
        --********************* sub3 end *************************--

        --lua入口--
	local T1 = tonumber(os.date("%S",os.time())) --开始时秒数
	--local file1 = assert(io.open("C:/Users/Administrator/Desktop/RandomJointPosData.csv","w")) --创建文件
	local file2 = assert(io.open("C:/Users/Administrator/Desktop/RandomCartPosData.csv","w")) --创建文件
	local ArmX,ArmY = 200,200                --X和Y臂臂长
	local XLimit,YLimit,CLimit = 130,130,180 --XYC关节软限位
	local ZLimit1,ZLimit2 = -140,0           --Z关节软限位
	--获取参数文件中的对应值
	--local XLimit,YLimit,CLimit = rwparam("Dsp_SoftLimitP|0"), rwparam("Dsp_SoftLimitP|1"), rwparam("Dsp_SoftLimitP|3")
	--local ZLimit1,ZLimit2 = rwparam("Dsp_SoftLimitP|2"), rwparam("Dsp_SoftLimitN|2")
	local ToolLen = 1                        --C轴末端工具杆长度
	local DZxLen,DZyLen = 300/2,270/2        --安装座长度和宽度
	local DesktopZ = -60                     --机器人Z原点到安装座的距离
	local UnworkRadius = 130.2               --机器人不可工作半径
	local J1Limit,J2Limit,J3Limit,J4Limit    --关节软限位
	local xpos,ypos,zpos,cpos,Hand           --笛卡尔坐标
	local H = nil                            --手系
	local count = 1                          --计数随机点个数
	local countOK = 0                        --计数随机点完成标志
	local TransNum = TransNum                --训练随机点坐标个数
	local JointPos,CartPos = {},{}           --保存的关节和笛卡尔坐标
        --local WhereFlag = {}                   --随机点在哪个区域的标志
	for i = 1,TransNum do                    --初始化
		--JointPos[i] = {0, 0, 0, 0}
		CartPos[i] = {0, 0, 0, 0, 0}
                --WhereFlag[i] = nil
	end
	
	math.randomseed(tostring(os.time()):reverse():sub(1,7)) --随机种子
	
	while true do
		while true do
			--产生整型关节限位数据
			J1Limit = math.random(-XLimit,XLimit)  --J1轴关节 负正 限位
			J2Limit = math.random(-YLimit,YLimit)  --J2轴关节 负正 限位
			J3Limit = math.random(ZLimit1,ZLimit2) --J3轴关节 负正 限位
			J4Limit = math.random(-CLimit,CLimit)  --J4轴关节 负正 限位
			
			--产生浮点型关节限位数据(random函数的参数只能是整数,实际上需要浮点数)
			J1Limit = J1Limit - J1Limit * math.random()
			J2Limit = J2Limit - J2Limit * math.random()
			J3Limit = J3Limit - J3Limit * math.random()
			J4Limit = J4Limit - J4Limit * math.random()
			
			--解算为笛卡尔坐标系
			xpos,ypos,cpos,Hand = SCARA_Forward_Kinematics(J1Limit,J2Limit,J4Limit,ArmX,ArmY)
			--剔除不需要的坐标
			--先判断Z高度,再选择可以生成在安装座之内还是安装座之外
			zpos = J3Limit
			if zpos > DesktopZ then --在安装座之内
				local Distance = math.sqrt((xpos-0)^2 + (ypos-0)^2) --机器人圆心到当前点的距离
				if Distance <= UnworkRadius + ToolLen then --距离小于工作半径+工具长度
					break --剔除
				end
				--WhereFlag[count] = 0 --在安装座之内的标志
			else --在安装座之外
				if xpos <= DZxLen+ToolLen and xpos >= -DZxLen-ToolLen and ypos <= DZyLen+ToolLen and ypos >= -DZyLen-ToolLen then
					break --剔除
				end
				--WhereFlag[count] = 1 --在安装座之外的标志
			end
			
			--解算为关节坐标
			if Hand == -1 then H = 0 elseif Hand == 1 then H = 1 end
			local jointX,jointY,jointC = SCARA_Inverse_Kinematics(xpos,ypos,cpos,ArmX,ArmY,H)
			--nil为解算错误
			if jointX == nil or jointY == nil or jointC == nil then break end
			--判断关节坐标是否超限
			if jointX>=XLimit or jointX <=-XLimit or jointY>=YLimit or jointY<=-YLimit or jointC>=CLimit or jointC<=-CLimit then
				break --剔除
			end
			if jointY < 0 then H = 0 else H = 1 end --手系(0:L 1:R)
                        
			--file1:write(jointX,",",jointY,",",J3Limit,"\\n") --写/保存关节数据到CSV
			file2:write(xpos,",",ypos,",",J3Limit,"\\n")     --写/保存笛卡尔数据到CSV
			
			--保存点位到数组中
			--JointPos[count][1] = jointX
			--JointPos[count][2] = jointY
			--JointPos[count][3] = J3Limit
			--JointPos[count][4] = jointC
			CartPos[count][1] = xpos
			CartPos[count][2] = ypos
			CartPos[count][3] = J3Limit
			CartPos[count][4] = cpos
			CartPos[count][5] = H
			
			count = count + 1
			if count == TransNum + 1 then --判断训练随机点个数是否达到设定值
				countOK = 1
				break
			end
		end
		if countOK == 1 then break end
	end
	--file1:close()
	file2:close()
	
	local T2 = tonumber(os.date("%S",os.time()))
	print("耗时：",T2 - T1," s") --结束时秒数
	--
	print("Trans pos end")
        --return JointPos, CartPos
        return TransNum
    end
"""
#-------------------------------------

#python画图
def DrawFig(f):
    #return_joint, return_cart = f
    return_TransNum = f
    font = [
            {'family': 'FangSong',    #字体类型：FangSong-仿宋
             'color':  'red',        #字体颜色：darkred-暗红
             'weight': 'normal',      #字体宽度：
             'size': 20,} ,           #字体大小：
            #-----------------------------------------------
            {'family': 'FangSong',    #字体类型：FangSong-仿宋
             'color':  'blue',         #字体颜色：darkred-暗红
             'weight': 'normal',      #字体宽度：
             'size': 10,} ,           #字体大小：        
            ]
    
    plt.close()  #clf() # 清图  cla() # 清坐标轴 close() # 关窗口
    fig = plt.figure()
    ax = fig.add_subplot(111)

    posNum = return_TransNum #随机点数
    X, Y, Z = [[] for ii in range(posNum)], [[] for jj in range(posNum)], [[] for Kk in range(posNum)] #初始化列表
    i = 0
    for line in open("RandomCartPosData.CSV"): #读取csv文件(一行一行读)
        x,y,z = line.split(",") #解析分隔符
        x,y,z = x.strip(' \t\r\n'),y.strip(' \t\r\n'),z.strip(' \t\r\n') #提取数据
        X[i].append(float(x))
        Y[i].append(float(y))
        #Z[i].append(float(z))
        i += 1
    end

    J1ArmLen,J2ArmLen = 200,200 #机器人J1和J2臂长
    ArmLen = J1ArmLen + J2ArmLen  #机器人臂长
    UnWorkArea = 130.2 #机器人不可工作区域半径

    #机器人工作半径
    cir1 = Circle(xy = (0.0, 0.0), radius = ArmLen, facecolor = 'green', alpha = 0.5) #颜色深度：0.1 ～ 1 = 0 ～ 255
    ax.add_patch(cir1) #画大圆

    #机器人不可工作区域半径
    cir2 = Circle(xy = (0.0, 0.0), radius = UnWorkArea, facecolor = 'yellow', alpha = 1)
    ax.add_patch(cir2) #画小圆
    
    #标签(size:文字大小 rotation：旋转角度 color：文字颜色 ha：文字在X轴的位置 va：文字在Y轴的位置)
    plt.text(0, 0, '不可运动区域',
             size=10, rotation=0, color='r', ha="center", va="center",
             )#bbox=dict(boxstyle="round", ec=(1., 0.5, 0.5), fc=(1., 0.8, 0.8)))#文字外围加边框
    
    plt.xlabel('-------------> X+ 方向', fontdict=font[0])
    plt.ylabel('-------------> Y+ 方向', fontdict=font[0])
    
    #随机位置散点图
    #for color, mark, i, j in [('r', 'o', range(len(X)),range(len(Y)))]:
    for color, mark, i, j in [('r', 'o', range(posNum),range(posNum))]:
        ax.plot(X, Y, 'ro',markersize = 1)
    end
    
    plt.axis('scaled')
    #ax.set_xlim(-4, 4)
    #ax.set_ylim(-4, 4)
    plt.axis('equal') #比例
    plt.grid(True) #添加网格
    plt.savefig('RandomPos.png')
    plt.show()
end

#调用Lua生成数据并保存到CSV文件
def Call_Lua():
    Start_time = time.time() #计时
    LuaFun = lua.eval(LuaCode) #处理数据(调用Lua脚本进行CSV文件处理)
    End_time = time.time()
    print("Process time is：", End_time - Start_time, " s")
    
    TransNum = 200 * 10000 #训练随机点数
    DrawFig(LuaFun(TransNum)) #绘图
end

Call_Lua()

 

5.lua正解和逆解函数，代码就不放了，可以去网上查资料。

需要注意的是，Python的lupa库在调用lua中的math.atan2(y,x)需要改成math.atan(y,x).

其次是lua代码中的"\n"需要改成"\\n"，否则运行将出错。