目录
- 前言
- NO1.打通python-vrep的通讯
- 1.1 通讯框架设计
- （1）python侧的设置：
- （2）vrep侧的设置：
- 1.2两侧调通的基本通讯测试源码
- （1）python侧通讯源码
- （2）vrep侧lua源码
- NO2.手动控制uarm的三个轴
- 2.1 控制指令的解析程序
- 2.2 实现关节的手动控制
- 2.3 实现坐标逆解控制
前言
本篇利用前期对机械臂的运动控制（https://blog.csdn/kanbide/article/details/119819623?spm=1001.2014.3001.5501）python编程基础上，构建一个四轴机械臂仿真系统，系统使用vrep环境及内置的四轴机械臂模型与python进行通讯实现。实现的框架如下：
NO1.打通python-vrep的通讯
1.1 通讯框架设计
不了解什么是vrep的读者请自行学习，如有必要介绍请留言。vrep的仿真环境直观如下图：
本篇使用的模型文件是UARM四轴机械臂。为了实现python能够控制这个机械臂，我们利用vrep的信号通讯机制“simxReadStringStream”等API接口实现信息的传递。笔者探索了一种比较好用的机制，python与vrep字符串通讯机制框架：
如上图所述，python和vrep机械臂的通讯主要是通过两个环境的字符串通讯实现的，类似串口通讯，我们可以借鉴G代码的通讯规约来实现两者的通讯，这样的好处是python侧可以无缝移植到串口通讯，实现实物机械臂的控制（关于实物机械臂将在本篇完结后发布）。
（1）python侧的设置：
规划实现两个初级功能，一个是监盘操作机械臂的左右上下运动，二是实现一个moveto函数，实现基于x，y，z坐标的控制。核心moveto函数算法流程是：首先根据前面的基础，将x，y,z坐标进行转换，转换成三个轴的旋转运动（第四轴由于在吸头端，不是抓手，本篇不予考虑）。然后，将三个轴的转角规整为G代码的字符串，最后通过verp提供的字符串通讯API接口发送。
（2）vrep侧的设置：
vrep侧，主要是在UARM的lua脚本中对sysCall_actuation()和coroutineMain()两个函数进行扩展。在sysCall_actuation()函数中，完成对python侧请求字符串的解析，并更新响应的电机状态；在coroutineMain()函数中，根据最新的状态执行模型响应的动作。
基础代码如下：
1.2两侧调通的基本通讯测试源码
（1）python侧通讯源码
"""
try:
import sim
except:
print ('--------------------------------------------------------------')
print ('"sim.py" could not be imported. This means very probably that')
print ('either "sim.py" or the remoteApi library could not be found.')
print ('Make sure both are in the same folder as this file,')
print ('or appropriately adjust the file "sim.py"')
print ('--------------------------------------------------------------')
print ('')
"""
import time
import numpy as np
import math
RAD2EDG = 180 / math.pi # 常数，弧度转度数
print ('Program started')
simxFinish(-1) # just in case, close all opened connections
#sim.simxFinish(-1) # just in case, close all opened connections
clientID=simxStart('127.0.0.1',19999,True,True,5000,5) # Connect to CoppeliaSim sim.
'''Initialization phase:'''
err,signal=simxReadStringStream(clientID,"toClient",simx_opmode_streaming)
if clientID!=-1:
print ('Connected to remote API server')
# Now try to retrieve data in a blocking fashion (i.e. a service call):
res,objs=simxGetObjects(clientID,sim_handle_all,simx_opmode_blocking)#sim.
if res==simx_return_ok:#sim.
print ('Number of objects in the scene: ',len(objs))
else:
print ('Remote API function call returned with error code: ',res)
time.sleep(2)
jointNum=4
jointName="uarm_auxJoint"
# 然后读取Base和Joint的句柄
jointHandle = np.zeros((jointNum,), dtype=np.int) # 注意是整型
for i in range(jointNum):
_, returnHandle = simxGetObjectHandle(clientID, jointName + str(i+1), simx_opmode_blocking)
jointHandle[i] = returnHandle
_, baseHandle = simxGetObjectHandle(clientID, "uarm_base", simx_opmode_blocking)
print('Handles available!')
# 然后首次读取关节的初始值，以streaming的形式
jointConfig = np.zeros((jointNum,))
for i in range(jointNum):
_, jpos = simxGetJointPosition(clientID, jointHandle[i], simx_opmode_streaming)
jointConfig[i] = jpos
# Now retrieve streaming data (i.e. in a non-blocking fashion):
startTime=time.time()
simxGetIntegerParameter(clientID,sim_intparam_mouse_x,simx_opmode_streaming) # sim. Initialize streaming
err,signal=simxReadStringStream(clientID,"toClient",simx_opmode_buffer)
#simxClearStringStream(clientID,"toClient",simx_opmode_buffer)
receive=str(signal, "utf8")
print("toClient:",receive)
if (err==simx_return_ok):
simxWriteStringStream(clientID,"fromClient",b'start',simx_opmode_oneshot)
for i in range(100):
#while time.time()-startTime < 2:
# 读取当前的状态值，之后都用buffer形式读取
"""
for i in range(jointNum):
_, jpos = simxGetJointPosition(clientID, jointHandle[i], simx_opmode_buffer)
print("joint %d"%i,round(jpos , 2))
jointConfig[i] = jpos
"""
'''while we are connected:'''
err,signal=simxReadStringStream(clientID,"toClient",simx_opmode_buffer)
#simxClearStringStream(clientID,"toClient",simx_opmode_buffer)
receive=str(signal, "utf8")
print("toClient:",receive)
if (err==simx_return_ok):
simxWriteStringStream(clientID,"fromClient",b'hello',simx_opmode_oneshot)
#Data produced by the child script was retrieved! Send it back to the child script:'''
time.sleep(0.5)
for i in range(jointNum):
_, jpos = simxGetJointPosition(clientID, jointHandle[i], simx_opmode_buffer)
print("joint2 %d"%i,round(jpos , 2))
jointConfig[i] = jpos
time.sleep(0.5)
returnCode,data=simxGetIntegerParameter(clientID,sim_intparam_mouse_x,simx_opmode_buffer) # Try to retrieve the streamed data
if returnCode==simx_return_ok: # After initialization of streaming, it will take a few ms before the first value arrives, so check the return code
pass
#print ('Mouse position x: ',data) # Mouse position x is actualized when the cursor is over CoppeliaSim's window
time.sleep(0.5)
# Now send some data to CoppeliaSim in a non-blocking fashion:
simxAddStatusbarMessage(clientID,'Hello CoppeliaSim!',simx_opmode_oneshot)
# Before closing the connection to CoppeliaSim, make sure that the last command sent out had time to arrive. You can guarantee this with (for example):
simxGetPingTime(clientID)
# Now close the connection to CoppeliaSim:
simxFinish(clientID)
else:
print ('Failed connecting to remote API server')
print ('Program ended')
（2）vrep侧lua源码
simRemoteApi.start(19999)
function sysCall_init()
corout=coroutine.create(coroutineMain)
i=0
lastReceived=-1
end
function sysCall_actuation()
if coroutine.status(corout)~='dead' then
local ok,errorMsg=coroutine.resume(corout)
if errorMsg then
error(debug.traceback(corout,errorMsg),2)
end
end
-- First send a stream of integers that count up:
dat=sim.getStringSignal('toClient')
if not dat then
dat='hello'
end
--dat=dat..sim.packInt32Table({i})
i=i+1
-- Here receive the integer stream in return and check if each number is correct:
dat=sim.getStringSignal('fromClient')
if dat then
sim.clearStringSignal('fromClient')
if dat=='hello' then
print('receive:hello')
sim.setStringSignal('toClient','hello')
end
if dat=='start' then
print('receive:start')
sim.setStringSignal('toClient','start')
order='start'
end
end
end
function initialize(maxVel,maxAccel,maxJerk,maxTorque)
motorHandles={-1,-1,-1,-1}
for i=1,4,1 do
motorHandles[i]=sim.getObjectHandle('uarm_motor'..i)
sim.setJointMaxForce(motorHandles[i],maxTorque)
sim.setObjectFloatParam(motorHandles[i],sim.jointfloatparam_upper_limit,maxVel)
end
maxVelVect={maxVel,maxVel,maxVel,maxVel}
maxAccelVect={maxAccel,maxAccel,maxAccel,maxAccel}
maxJerkVect={maxJerk,maxJerk,maxJerk,maxJerk}
end
function movCallback(config,vel,accel,handles)
for i=1,#handles,1 do
sim.setJointTargetPosition(handles[i],config[i])
end
end
function moveToPosition(newMotorPositions,synchronous,maxVel,maxAccel,maxJerk)
local _maxVelVect={}
local _maxAccelVect={}
local _maxJerkVect={}
if not maxVel then
maxVel=maxVelVect[1]
end
if not maxAccel then
maxAccel=maxAccelVect[1]
end
if not maxJerk then
maxJerk=maxJerkVect[1]
end
for i=1,4,1 do
_maxVelVect[i]=math.max(1*math.pi/180,math.min(maxVel,maxVelVect[i]))
_maxAccelVect[i]=math.max(1*math.pi/180,math.min(maxAccel,maxAccelVect[i]))
_maxJerkVect[i]=math.max(1*math.pi/180,math.min(maxJerk,maxJerkVect[i]))
end
local op=sim.rml_no_sync+sim.rml_keep_target_vel
if synchronous then
op=-1
end
local currentConf={}
for i=1,#motorHandles,1 do
currentConf[i]=sim.getJointPosition(motorHandles[i])
end
sim.moveToConfig(op,currentConf,nil,nil,_maxVelVect,_maxAccelVect,_maxJerkVect,newMotorPositions,nil,movCallback,motorHandles)
end
function enableSuctionCup(enable)
if enable then
sim.writeCustomDataBlock(gripperHandle,'activity','on')
else
sim.writeCustomDataBlock(gripperHandle,'activity','off')
end
end
function action1()
enableSuctionCup(false)
-- Synchronous operation of the individual joints:
moveToPosition({180*math.pi/180,59*math.pi/180,84*math.pi/180,180*math.pi/180},true)
moveToPosition({180*math.pi/180,52.5*math.pi/180,84*math.pi/180,180*math.pi/180},true)
enableSuctionCup(true)
moveToPosition({180*math.pi/180,59*math.pi/180,84*math.pi/180,180*math.pi/180},true)
moveToPosition({90*math.pi/180,104*math.pi/180,60*math.pi/180,90*math.pi/180},true)
-- Asynchronous operation of the individual joints:
moveToPosition({180*math.pi/180,59*math.pi/180,84*math.pi/180,180*math.pi/180},false)
moveToPosition({180*math.pi/180,52.5*math.pi/180,84*math.pi/180,180*math.pi/180},true)
enableSuctionCup(false)
moveToPosition({180*math.pi/180,59*math.pi/180,84*math.pi/180,180*math.pi/180},true)
moveToPosition({90*math.pi/180,104*math.pi/180,60*math.pi/180,90*math.pi/180},false)
end
function coroutineMain()
modelBase=sim.getObjectHandle(sim.handle_self)
gripperHandle=sim.getObjectHandle('uarmVacuumGripper')
pickupPart=sim.getObjectHandle('uarm_pickupPart')
sim.setObjectParent(pickupPart,-1,true)
local maxVelocity=45*math.pi/180 -- rad/s
local maxAcceleration=40*math.pi/180 -- rad/s^2
local maxJerk=80*math.pi/180 -- rad/s^3
local maxTorque=10 -- kg*m^2/s^2
initialize(maxVelocity,maxAcceleration,maxJerk,maxTorque)
while true do
if order=='start' then
action1()
order=''
end
end
end
NO2.手动控制uarm的三个轴
本节所要达到的效果是我们在python侧，发送G代码指令，如““G90 G0 X90 Y10 Z10 \r””，UARM侧接受到指令后，提取指令中的角度信息，并解析成三个轴的角度目标值，最后根据得到的角度改变机械臂的状态。
2.1 控制指令的解析程序
上一节我们已经实现了发送字符串的机制，本小节主要是在uarm侧加入解析字符串的机制就可以，代码如下：
if string.sub(dat,1,6)=='G90 G0' then
local order = split(dat,' ')
local J1=tonumber(string.sub(order[3],2,string.len(order[3])))
local J2=tonumber(string.sub(order[4],2,string.len(order[4])))
local J3=tonumber(string.sub(order[5],2,string.len(order[5])))
print('receive G90 G0:',J1,J2,J3)
end
然后在python侧发送字符串（“G90 G0 X90 Y10 Z10 \r”）进行试验，可以得到如下结果，说明已经解析成功了：
2.2 实现关节的手动控制
这一节我们来实现一个uarm的关节角度手动操控器，如下图效果：
如上图所示，先实现一个状态及操控面板，第一个框显示的是uarm反馈回来的各个关节的角度值，以及吸头的状态，第二个框是控制组件，通过调节滑块实现机械臂的运动控制。
整个操作的机制是将滑块的数值通过通讯字符串发送给vrep侧，vrep侧进行解析后动作。
部分代码如下，源码将在完结后是上传或者通过关注公众号届时发放：
控制界面侧的部分代码如下：
状态框的显示更新程序：
for i in range(3):
_, jpos = sim.simxGetJointPosition(clientID, jointHandle[i], sim.simx_opmode_blocking)
window['-J'+str(i+1)+'-'].Update(round(jpos/np.pi*180,2))
window['TJ'+str(i+1)].Update(str(round(jpos/np.pi*180,2)))
err,signal=sim.simxReadStringStream(clientID,"toClient",sim.simx_opmode_buffer)
receive=str(signal, "utf8")
print("toClient:",receive+'\n')
if receive=='G999 G1 on':
#values['on']=1
print("on now")
window['on'].Update(True)
if receive=='G999 G0 off':
window['off'].Update(True)
print("off now")
控制框的的核心程序：
if event == 'send':
J1V=str(int(values['-J1c-']))
J2V=str(int(values['-J2c-']))
J3V=str(int(values['-J3c-']))
print("send angles:",J1V,J2V,J2V)
sendangles='G90 G0 X'+J1V+' Y'+J2V+' Z'+J3V+' \r'
sim.simxWriteStringStream(clientID,"fromClient",str.encode(sendangles),sim.simx_opmode_oneshot)
if values['-on-'] and oldstate==0:
oldstate=1
sendangles='G999 G1 \r'
sim.simxWriteStringStream(clientID,"fromClient",str.encode(sendangles),sim.simx_opmode_oneshot)
if values['-off-'] and oldstate==1:
oldstate=0
sendangles='G999 G0 \r'
sim.simxWriteStringStream(clientID,"fromClient",str.encode(sendangles),sim.simx_opmode_oneshot)
如上图所示，通过一系列手动调节，把目标物体给拿了起来。
在操作过程中发现，关节角度不能设置得太过了，因为手臂的机械结构决定它是有范围的，太过会使得手臂卡住或者断裂（当然仿真无所谓，可以重启），接下来我们将对关节的控制范围进行设定，并且实现前后，上下的操控实现。
2.3 实现坐标逆解控制
我们已经在本文的姊妹篇中，介绍了uarm机械臂的逆解算法，并实现了python的逆解算法代码实现。这一节就可以引入该算法，实现机械臂基于末端目标坐标（x,y,z）的逆解控制。
上图在坐标逆解操作的面板里面，我们可以通过调节x，y，z的坐标值，点击fire，通过通信规约发送经过逆解算法得出的（J1，J2,J3）的角度，在vrep侧机械臂就运动到了我们所期望的目标坐标！
可以看出，经过笔者的多次试验调试，算法达到了比较小的精度，需要目标坐标经过坐标逆解发送控制角度到vrep机械臂，执行后返回末端位置，（x，y，z）数值基本是一致的，想了解中间调节的细节的网友，可以留言交流。
PS**关注公众号届时发放源码。
下篇预告：
《在VREP环境中，UARM与摄像头联动，实现基于视觉识别的自动抓取》
更多推荐
python机器人编程——VREP数字孪生四轴机械臂联合仿真
发布评论