python——VREP数字孪生四轴机械臂联合仿真（持续更新中...）

• 前言
• NO1.打通python-vrep的通讯
• • 1.1 通讯框架设计
  • • （1）python侧的设置：
    • （2）vrep侧的设置：
  • 1.2两侧调通的基本通讯测试源码
  • • （1）python侧通讯源码
    • （2）vrep侧lua源码

本篇利用前期对机械臂的运动控制（https://blog.csdn.net/kanbide/article/details/119819623?spm=1001.2014.3001.5501）python编程基础上，构建一个四轴机械臂仿真系统，系统使用vrep环境及内置的四轴机械臂模型与python进行通讯实现。实现的框架如下：

不了解什么是vrep的读者请自行学习，如有必要介绍请留言。vrep的仿真环境直观如下图：

本篇使用的模型文件是UARM四轴机械臂。为了实现python能够控制这个机械臂，我们利用vrep的信号通讯机制“simxReadStringStream”等API接口实现信息的传递。笔者探索了一种比较好用的机制，python与vrep字符串通讯机制框架：

如上图所述，python和vrep机械臂的通讯主要是通过两个环境的字符串通讯实现的，类似串口通讯，我们可以借鉴G代码的通讯规约来实现两者的通讯，这样的好处是python侧可以无缝移植到串口通讯，实现实物机械臂的控制（关于实物机械臂将在本篇完结后发布）。

规划实现两个初级功能，一个是监盘操作机械臂的左右上下运动，二是实现一个moveto函数，实现基于x，y，z坐标的控制。核心moveto函数算法流程是：首先根据前面的基础，将x，y,z坐标进行转换，转换成三个轴的旋转运动（第四轴由于在吸头端，不是抓手，本篇不予考虑）。然后，将三个轴的转角规整为G代码的字符串，最后通过verp提供的字符串通讯API接口发送。

vrep侧，主要是在UARM的lua脚本中对sysCall_actuation()和coroutineMain()两个函数进行扩展。在sysCall_actuation()函数中，完成对python侧请求字符串的解析，并更新响应的电机状态；在coroutineMain()函数中，根据最新的状态执行模型响应的动作。
基础代码如下：

"""
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')

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

未完待续…