V-rep远程API控制之Python系列（1）：位置控制模式

本节我们主要介绍如何使用远程API（Python）的方式来控制机械臂的运动，主要介绍位置控制模式。

在V-rep外部控制中，我们通常称远程API端为Client端，称V-REP为Server端，下面的内容分别从Server端和Client端介绍如何配置，最终完成联合仿真。

1. 建模

该部分不属于本节的重点，故我们直接从左侧[Model Brower]->[robots]->[non-mobile]里面选择Jaco arm.ttm，拖入到一个new scene里面，保存。

2. V-rep配置

2.1 关节配置

在[Scene hiearchy]中打开Object（也就是Jaco），你会发现里面是一个链式结构，有Base，Joint，Link等，今天我们主要配置Joint。

双击Jaco_joint*前面的图标，会跳出第一个弹窗（Scene Object Properties），需要配置以下内容：
（1）关节角范围[Pos.min, Pos.min + Pos.range]：这么写我想已经很清楚了，比方说你的关节角转动范围为[-90deg, 90deg]，那么你需要在Pos.min后面填写-90，在Pos.range里面填写180；（deg是提醒你，这里的单位是°，不是rad）
（2）关节角的初值Pos：Jaco里面默认所有的关节，初始角都是180°，你可以自己尝试改变角度，看看Jaco的构型如何变化；
（3）Mode：Joint的控制模式有四种，今天需要用到的是Torque / force mode；
然后进入Dynamic Properity：点击最小面的按钮，进入第二个弹窗Joint Dynamic Properties，需要配置如下内容：
（4）Motor enabled：选中，使能电机；
（5）Control loop enabled：选中，进入默认的控制；
（6）Target Position：目标位置，设置为关节初始值，因为外部控制，我们会进行更改的；
（7）Position control （PID）：这是默认控制的一种形式，我们今天采用这种，小伙伴们在完成本节内容，可以自行过来调节PID的参数，看看不同的参数，所对应的响应如何。（多说一句：现在很多厂家的机械臂都不开放力矩环，对应的控制就是位置控制，适合初学者适应实际机械臂控制。）
（8）其他部分暂时不需要动，注意配置所有的关节！！！！

2.2 V-rep端通信配置

外部控制，我们通常采用Non-threaded child script，所以我们需要配置这个子脚本。
我们点击Object（Jaco）后面的一个“脚本”图标，会发现它的名字是 Thread child script，我们delete掉，然后add 一个associated child script（Non-threaded）

打开该脚本，会发现里面有四个子函数

function sysCall_init()
function sysCall_actuation()
function sysCall_sensing()
function sysCall_cleanup()

根据名字，我们不难猜测，分别是初始化、执行器、传感器、清除等操作的配置。为了给一些不熟悉Lua语言的朋友更好的学习，我们仅在初始化函数里面配置通信，其他的部分均在远程API里面完成。

我们来介绍一下这个函数：simRemoteApi.start，方便以后大家也学会去自己理解Manual。

功能描述：在指定端口上启动临时远程API服务器服务。当从模拟脚本启动时，服务将在模拟完成时自动结束
输入：
portNumber:安装服务器服务的端口。20000以上端口优先。为了使用共享内存，可以指定负端口号，而不是套接字通信。
maxPacketSize:套接字发送包的最大大小。确保将值保持在1300，除非客户端有不同的设置。
debug:如果为真，窗口将显示该端口上的数据流量。
preEnableTrigger:如果为真，将对来自客户机的同步触发信号预启用服务器服务。
输出：
num result:-1如果操作不成功。在未来的版本中，可能会有一个更差异化的回报价值

[有道词典翻译，服务于头疼英语的朋友。]大致可以理解一下，这个函数通常输入一些端口等配置，来实现远程API与V-rep的临时通信（V-rep运行时才能通信！！！切记，划重点，要考）

so，怎么用呢，直接把下面一行复制粘贴到function sysCall_init()里面就好了，有人说那你上面说这么多干嘛，哈哈，就是逗你玩的！！

repeat until (simRemoteApi.start(19999,1300,false,true)~=-1)

以上完成了V-rep端的配置，下面介绍如何配置Python端。

3. API端配置

3.1 Python的配置

笔者用的是Python 3.6，编译器是PyCharm，如果想知道怎么安装Python和PyCharm，出门右拐，看笔者软件安装部分的博客。

3.2 函数库的导入

简单来说，需要配置配置几个文件，就像pip install numpy那样，安装一些外接的库，在Python里面添加接口相关文件，这些文件可以在Vrep的安装文件夹中找到，包括：

vrep.py
vrepConst.py
remoteApi.dll(win) remoteApi.dylib(mac) remoteApi.so(linux)

Vrep安装文件夹->programming->remoteApiBindings->
(1)->python->python
(1)->lib->lib->(32/64 Bit)

将这些文件复制到PyCharm定义的Project文件夹中，这样也省事，如图所示。

4. Python程序编写

4.1 仿真的步骤

1. Setting（设置引入一些库函数等、参数等）
2. Init（关闭其他连接、通信检测）
3. Configuration（配置句柄，设置同步模式等）
4. Simulation（设置仿真参数）
5. CoreControl （在仿真大循环里面写规划与控制）

Step1. Setting

from __future__ import division
import numpy as np
import math
import vrep

RAD2EDG = 180 / math.pi   # 常数，弧度转度数
tstep = 0.005             # 定义仿真步长
# 配置关节信息
jointNum = 6
baseName = 'Jaco'
jointName = 'Jaco_joint'

Step2. Init

print('Program started')
# 关闭潜在的连接
vrep.simxFinish(-1)
# 每隔0.2s检测一次，直到连接上V-rep
while True:
    clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
    if clientID > -1:
        break
    else:
        time.sleep(0.2)
        print("Failed connecting to remote API server!")
print("Connection success!")

Step3. Configuration

# 设置仿真步长，为了保持API端与V-rep端相同步长
vrep.simxSetFloatingParameter(clientID, vrep.sim_floatparam_simulation_time_step, tstep, vrep.simx_opmode_oneshot)
# 然后打开同步模式
vrep.simxSynchronous(clientID, True) 
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)

# 然后读取Base和Joint的句柄
jointHandle = np.zeros((jointNum,), dtype=np.int) # 注意是整型
for i in range(jointNum):
    _, returnHandle = vrep.simxGetObjectHandle(clientID, jointName + str(i+1), vrep.simx_opmode_blocking)
    jointHandle[i] = returnHandle

_, baseHandle = vrep.simxGetObjectHandle(clientID, baseName, vrep.simx_opmode_blocking)

print('Handles available!')

# 然后首次读取关节的初始值，以streaming的形式
jointConfig = np.zeros((jointNum,))
for i in range(jointNum):
     _, jpos = vrep.simxGetJointPosition(clientID, jointHandle[i], vrep.simx_opmode_streaming)
     jointConfig[i] = jpos

Step4：Simulation

lastCmdTime=vrep.simxGetLastCmdTime(clientID)  # 记录当前时间
vrep.simxSynchronousTrigger(clientID)  # 让仿真走一步
# 开始仿真
while vrep.simxGetConnectionId(clientID) != -1：
    currCmdTime=vrep.simxGetLastCmdTime(clientID)  # 记录当前时间
    dt = currCmdTime - lastCmdTime # 记录时间间隔，用于控制
    # ***
    # ***
    # ***
    lastCmdTime=currCmdTime    # 记录当前时间
    vrep.simxSynchronousTrigger(clientID)  # 进行下一步
    vrep.simxGetPingTime(clientID)    # 使得该仿真步走完

注：在***处插入的CoreControl部分的代码，也可以自己写

Step5. CoreControl

        # 读取当前的状态值，之后都用buffer形式读取
        for i in range(jointNum):
            _, jpos = vrep.simxGetJointPosition(clientID, jointHandle[i], vrep.simx_opmode_buffer)
            print(round(jpos * RAD2DEG, 2))
            jointConfig[i] = jpos

        # 控制命令需要同时方式，故暂停通信，用于存储所有控制命令一起发送
        vrep.simxPauseCommunication(clientID, True)
        for i in range(jointNum):
            vrep.simxSetJointTargetPosition(clientID, jointHandle[i], 120/RAD2DEG, vrep.simx_opmode_oneshot)
        vrep.simxPauseCommunication(clientID, False)

好了，编写结束；注：笔者控制所有关节的期望位置为120°，当然大家可以尝试更复杂的控制命令，按照上面的模板。

5. 仿真

先run一下V-rep端的仿真，然后run Python端的代码，可以发现，所有关节都转动到120°的位置。

祝大家学习顺利！

Reference

[1]. V-rep user manual
[2]. https://blog.csdn.net/huangdianye/article/details/80628932