V-rep机器人仿真软件使用的学习笔记-续

前言

这篇博客主要是记录自己学习和使用机器人仿真软件V-rep过程中的一些知识点和踩的坑。是之前这篇V-rep机器人仿真软件使用的学习笔记的续集，新增2020年使用V-rep的笔记，因为是做项目过程遇到问题解决问题的记录，内容依旧比较零散且没有展开详细讲，更基础的V-rep知识请参看之前的博客V-rep机器人仿真软件使用的学习笔记。下面进入正文。

正文

1.child script中thread script中如果在while循环中添加sim.switchThread()，则该thread script执行的时间间隔和仿真间隔保持一致（如果仿真间隔为real-time间隔，则与real-time间隔保持一致），否则该script将以尽可能快的速度进行执行。参考：https://www.cnblogs.com/21207-iHome/p/6767839.html

2.关节电机在力/力矩模式下且电机和控制循环均使能时的三种控制方式：

自定义控制方式：使用Joint callback functions关节回调函数，在回调函数中实现关节电机的闭环控制
位置控制（PID闭环），此时力矩是恒定的
弹簧阻尼模式：通过力/扭矩调制，该关节将起到弹簧-阻尼系统的作用，此时速度是恒定的

3.关节空间的轨迹规划和位置控制方法

使用RML库实现轨迹规划和控制：输入多个目标关节位置，速度参数等，进行运动，阻塞方式执行。RML官方网站：http://www.reflexxes.ws/products/overview-and-download.html。
需要自己实现位置闭环，利用sim.setJointTargetVelocity作为速度内环，实现位置速度双环控制，并对多个关节进行控制实现机械臂位置控制，此时关节电机需要电机使能，但控制回路不使能。
利用关节电机的位置控制方式，利用sim.setJointTargetPosition输入关节目标位置，V-rep已实现PID位置闭环控制，然后对多个关节进行控制实现机械臂位置控制

4.V-rep中读写文件至本地的方法，参考：http://forum.coppeliarobotics.com/viewtopic.php?f=9&t=571&p=7467&hilit=file+write+in+a+file#p7467, 在脚本中使用Lua的文件读写命令实现，写文件默认路径是V-rep可执行文件所在的路径。

写文件

name = "toto.txt" -- 此时文件位于V-rep可执行文件所在目录
-- name = "C:/Sam/A-level/Simulation_Environment/4CONTROL_ALGORITHM/toto.txt"  --此时文件位于指定目录
file = io.open(name, "w")
file:write("Hello World")
file:close()

读文件

name = "toto.txt"
file = io.open(name, "r")
print(file:read())
file:close()

注意：在windows系统下文件路径分隔符需要使用左斜杠"/",如果用右斜杠会报错"Lua runtime error: [string “CHILD SCRIPT Cuboid”]:28: attempt to index global ‘file’ (a nil value)".

5.单个关节实现位置闭环，内环为速度环的一种实现（增量式PID）

function pid_init(current_p)
    pid = {}
    pid["target_position"] = 0  -- target postion
    pid["actual_position"] = current_p --actual position
    pid["error"] = 0 --??  
    pid["error_next"] = 0 --previous err 
    pid["error_last"] = 0 --previous previous err
    pid["kp"] = 3.5   --kp ki kd parematers   note: rad: kp=0.1    reg: pid = 3.5 0.3 0.3
    pid["ki"] = 0.3
    pid["kd"] = 0.3
    factor = 180.0/3.14159
    increment_position = 0 --incremnet
end

function pid_realise(target_p, current_p)  --implement pid, rarget is deg, current is rad
    pid["target_position"] = target_p  --set target position 
    pid["actual_position"] = current_p  -- current position
    pid["error"] = pid["target_position"] - pid["actual_position"]
    
     
    increment_position =increment_position + pid["kp"]*(pid["error"]-pid["error_next"])+pid["ki"]*pid["error"]+pid["kd"]*(pid["error"]-2*pid["error_next"]+pid["error_last"])  --??????  
    
    
    pid["error_last"] = pid["error_next"]  --next loop
    pid["error_next"] = pid["error"] 

    if (increment_position > 175)   --limited the increment
    then
        increment_position = 175
    end
    if (increment_position < -175)
    then
        increment_position = -175
    end

    print(increment_position, pid["error"], pid["target_position"], pid["actual_position"])
    return increment_position
end
 


function sysCall_threadmain()
    -- Put some initialization code here
    position = sim.getJointPosition(18)   -- 18 is the handle of the controled motor
    pid_init(position)

    -- Put your main loop here, e.g.:
    --
    while sim.getSimulationState()~=sim.simulation_advancing_abouttostop do
    --     local p=sim.getObjectPosition(objHandle,-1)
    --     p[1]=p[1]+0.001
    --     sim.setObjectPosition(objHandle,-1,p)
         --time_step = sim.getSimulationTimeStep()
         time_step = sim.getSimulationTime()
         position = sim.getJointPosition(18)
         increment = pid_realise(90, position*factor)

         --print('Test')
         print(time_step, increment)--position*factor)
         sim.setJointTargetVelocity(18, increment/factor)
         --name = "C:/Sam/A-level/Simulation_Environment/4CONTROL_ALGORITHM/toto.txt"
         --file = io.open(name, "a")
         --file:write("Hello World \n")
         --file:write(increment)
         --file:write("\n")
         --file:close()
         sim.switchThread() -- resume in next simulation step
    end
end

5.机械臂逆解的两种方法
V-rep支持伪逆法和数值法两种机械臂逆解方法，通常伪逆法速度快，但是遇到奇异问题可能解不存在，数值法速度慢但是基本能保证解的存在。。可以同时设置这两种解法，V-rep会默认先使用伪逆法，当伪逆法无解时，会再调用数值法求解，对于DLS逆解方法需要设置Edit conditional parameters，在Perform if…条件栏中进行配置：如果undamped方法失败，则执行damped方法。可以参考官方robots–>non-mobile–>7 Dof manipulator.ttm场景模型中的设置方法以及博客：https://www.cnblogs.com/21207-iHome/p/7420733.html。

6.机械臂逆运动学模式的使用
在逆运动学模式下，拖动target，关节将自动求逆解并转动至对应位置。这种模式的设置参考官方robots–>non-mobile–>7 Dof manipulator.ttm场景模型中的设置方法，主要有以下几个要点：

机械臂关节处于Inverse kinematics mode，如果机械臂均设置为动力学使能，则Hybrid operation也要使能
必须设置两组逆运动学解算对，一组使用Pseudo inverse（伪逆解）方法，一组使用DLS（数值解）方法

7.URDF文件不支持对并行结构的机器人结构（包含闭式运动链结构）进行描述，https://answers.ros.org/question/9050/using-ros-for-a-delta-robot/?answer=13094?answer=13094#post-id-13094

8.V-rep中将模型保存至本地
选中模型的根节点，然后在菜单栏中选择File --> Save model as… 将其保存。
注意： 如果Save model as...选项为灰色，说明所选中的模型没有被设置为根节点，这时可以通过设置模型属性Scene Object Properties-->common-->Object is model base即可，还在在Object is model base中Edit model properties编辑模型的相关属性和说明。

9.外部控制器时间步设置与同步的问题
外部力矩控制：https://blog.csdn.net/huangdianye/article/details/80628932

10.让转动关节成为固定关节（不发生转动）的设置方法
可以设置关节的最小位置和转动范围均为0，则关节相当于不发生转动。

11.逆运动学解算模块的细节知识点
位于官方文档的Calculation modules–>Inverse kinematics–>Basics on IK groups and IK elements位置。

连杆：如果关节不处于IK模式，则被认为是连杆
存在多个IK group时，如果各个group的连杆运动之间存在干涉，则先求解对其他group有影响的group。
如果IK chains包含共享关节的结构，则将对应的tip-target包含至一个IK group中：

12.并联机构的正逆运动学

13.双臂紧约束协作得到路径的几种尝试

两个机械臂都工作在IK mode，首先将两臂的吸盘吸至板材，然后同时选中两臂的IK_Target进行移动和旋转，由于两臂工作在IK mode，两臂将跟随IK_Target进行运动，这时就可以得到两臂的协作路径，这种方法相当于是人工示教得到双臂协作路径，两臂同时进行路径规划。
将两臂分为主从臂，主臂正常进行路径规划和运动，从臂的IK_Target设置在主臂上，这样当主臂运动时，从臂就可以跟随主臂运动。参考官方场景robotCollaboration.ttt的实现方法，利用主从思想和协作约束进行协作路径规划。
如图所示将一个机械臂的IK_Target附着在另一个机械臂末端（主臂）上，且与所附着机械臂（主臂）的IK_Target保持协作约束关系，此时主臂运动时，从臂将跟随运动。
将两臂和板材看成闭式运动链，构成配置空间，然后进行运动规划。这种应该是最合理的方法，但是实现难度较大。

14.IK Mode和FK Mode机械臂的控制
在IK Mode下从空间位置解算得到关节位置，如果机械臂处于IK Mode，则移动IK_Target时机械臂会自动跟随，在IK mode下给定关节位置，机械臂是不会运动的。
在FK Mode时给定机械臂关节角，机械臂移动至对应的位置。
在程序中常使用如下代码实现以上两种状态的切换：

setIkMode=function()
    sim.setExplicitHandling(ikGroup,0) -- that enables implicit IK handling
    sim.switchThread()
end

setFkMode=function()
    sim.setExplicitHandling(ikGroup,1) -- that disables implicit IK handling
    sim.switchThread()
    sim.setObjectParent(ikTarget,ikTip,true)
    sim.setObjectPosition(ikTarget,ikTip,{0,0,0})
    sim.setObjectPosition(ikTarget,ikTip,{0,0,0})
end

15.MATLAB Simscape Multibody中导入URDF文件
官方文档：https://ww2.mathworks.cn/help/physmod/sm/ug/urdf-import.html
URDF中的部分标签Simscape Multibody支持，有些属性标签Simscape Multibody并不支持（材料属性，碰撞属性，关节限位属性等），对于不支持的属性标签，导入时会自动忽略。模型的mesh只支持stl格式，其他格式虽然无法导入，但只会导致机械臂外形无法显示出来，不影响机械臂模型的动力学仿真。

导入abb-4600机械臂模型的过程记录（官方文档）：
切换至URDF工作路径，然后使用smimport命令进行导入：
smimport('./abb_irb4600_support/urdf/test_abb_4600.urdf')
导入后在simulink界面使用simulation->update diagram更新后可以在Simscape explorer里看到机械臂模型，如果机械臂显示不正常，可能是mesh导入不正常，手动修改simulink中机械臂每个link子模块的visual的stl路径之后再次simulation->update diagram就可以正常显示，或者通过编程自动设置stl文件路径也可以。
ps: 此urdf文件中没有包含inertial属性，默认全为零，所以动力学模型无法正常使用，下面尝试使用meshlab软件结合此机械臂官网datasheet中整机质量参数，来计算inertial参数。

16.机械臂的惯性参数获取问题
① 从Solidworks导出成URDF文件时，可以导出inertial属性，包含惯性参数
② 使用V-rep软件获取惯性参数
③ 可以使用软件Meshlab进行计算，参考ROS官网文档的说明：http://wiki.ros.org/abb_irb1200_support 。大概思路是：官方的机械臂datasheet只提供了整个机械臂的重量，假设机械臂密度均匀恒定，则每个连杆的体积决定其重量，在meshlab中可以得到机械臂各个连杆的体积，进一步得到惯性参数。
PS： 可以去机械臂的官网下载机械臂的CAD文件，比如：https://new.abb.com/products/robotics/zh/industrial-robots/irb-4600/irb-4600-cad

惯性参数获取问题，参见另一篇博客机械臂惯性参数的获取方法。

MATLAB联合V-rep进行机械臂力矩控制仿真
切记： V-rep力矩控制时必须将仿真时间步设置为5ms与物理引擎同步，默认是50ms，参考官方文档说明。
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-vzDsSbTP-1592535376092)(./pics_vrep/pics1_32.png)]
参考实现：

V-rep关节类型与控制方式和代码实现：coppeliarobotics.com/helpFiles/en/jointDescription.htm#controller
V-rep力矩控制实现：https://forum.coppeliarobotics.com/viewtopic.php?t=25
https://blog.csdn.net/huangdianye/article/details/80628932
共享内存插件：https://github.com/santdiego/SASTools
V-rep共享内存的使用：https://forum.coppeliarobotics.com/viewtopic.php?t=4157
需要将仿真周期设置为5ms，与物理引擎的时间步相同。
MATLAB SIMULINK实时仿真插件：https://www.mathworks.com/matlabcentral/fileexchange/29107-real-time-pacer-for-simulink
V-rep同步模式介绍：https://www.coppeliarobotics.com/helpFiles/en/remoteApiModusOperandi.htm#synchronous

双臂机器人抓取仿真
https://www.mathworks.com/help/robotics/examples/model-and-control-a-manipulator-arm-with-simscape.html
MATLAB实时仿真
方法一：使用这个模块实现与实际时间的同步：https://www.mathworks.com/matlabcentral/fileexchange/29107-real-time-pacer-for-simulink
方法二：使用官方提供的实时模块Real-time sync，不过在使用这个模块时需要安装Real-Time Kernel，然后重启电脑使Real-Time Kernel生效。
方法二使用官方例程时：open_system(fullfile(matlabroot,'toolbox','sldrt','sldrtexamples','sldrtex_vdp'));，出现如下错误：Error reported by S-function 'sldrtsync' in 'sldrtex_vdp/Real-Time Synchronization': Synchronization to real-time kernel has failed.
该问题在网上找到的解决办法：https://www.mathworks.com/matlabcentral/answers/434279-why-does-simulink-real-time-run-very-slow-in-matlab-r2018b-on-windows-and-a-real-time-synchronizati，但是尝试之后仍没有解决，甚至导致电脑蓝屏，暂且放弃方法二实现实时仿真。
MATLAB 物理建模与仿真

MATLAB SIMSCAPE物理模型建立与仿真：通用型物理模型建模
MATLAB Simscape Multibody：针对机械臂等机器人进行物理模型建模仿真
Simscape Multibody Link：导入CAD软件3D模型至MATLAB进行物理建模
MATLAB Simscape Multibody常用模块作用：
Rigid Transform：座标系之间的位姿变换，同时存在平移和旋转变换时，应该先平移再旋转
Brick Solid：刚性立方体模型
Mechanism Configuration block：可以指定整个系统的重力

初级例程：

高级例程：

MATLAB Simulink Data Inspector出现空白，无法显示图像的问题
使用Simulink.sdi.clearPreferences解决了上述问题。google查到的解决办法使用sdi.Repository.clearRepositoryFile没有任何效果。另附Simulink Data Inspector官方教程。
MATLAB Robotics System Toolbox的使用
常用函数文档：
- 正运动学：getTransform
- 逆运动学：inverseKinematics，用法可以根据工具箱中源码文档确定，不同版本的MATLAB用法不同。
MATLAB Simulink生成C代码
参考博客：
- 初级教程
- 官方讲座1
- 官方讲座2：在1小时20分30秒时讲解了如何将生成的代码集成到自己的项目中。
- 官方文档
6自由度机械臂动力学模型的推导和符号运算：https://github.com/aseleit/robotic_manipulator_dynamics
C联合V-rep仿真：机械臂力矩控制
1. V-rep端需要使用simRemoteApi.start(19999,1300,false,true)打开远程api接口，第四项参数很重要，当为true时才能开启同步模式。此处的portNumber不使用19997是因为端口19997是默认端口，工作在continuous remote API server service下，无法更改。
2. 在Qtcreator中创建控制器C++程序，并添加MATLAB生成的控制器C代码。如图所示，由于主函数为CPP文件，所以添加的C函数和C头文件都要放在extern "C"中，否则就会出现undefine reference to xx的编译错误。
V-rep启动多个RemoteAPI Server，从而可以在一个Client中使用多个thread连接V-rep的方法
当外部client程序一方面需要对V-rep中的机器人进行控制，另一方面需要定时获取机器人状态，这时候就需要多个thread和V-rep进行通讯，就产生了标题的需求。首先官方文档说V-rep是支持多个外部client同时连接的，关键点在于要在v-rep端开启不同port的server服务：

v-rep端开启不同port的server服务主要有以下两种方法：
这里我使用第一种方法，直接修改V-rep根目录下的remoteApiConnections.txt文件，修改后的文件如下：
可以看到该文件注释部分也介绍了如何开启多个port，这里开启了19997和19998两个port，这个文件修改完成后启动V-rep，将完成对应port的server的开启，这样外部client程序就可以同时使用两个thread与V-rep进行通讯了。
在V-rep命令行中使用simRemoteApi.status()命令可以查看server端口是否开启成功：
可以看到19997和19998端口都已经成功开启。
V-rep中通过D-H参数自动生成机器人模型
D-H参考：
- D-H参数生成三维模型： https://wenku.baidu.com/view/aaff395a52ea551811a68730.html
- D-H参数介绍： https://blog.csdn.net/pengjc2001/article/details/70156333
- abb4600标准型D-H参数： https://www.doc88.com/p-0478665844349.html
- abb4600改进型D-H参数： https://bulletin.incas.ro/files/apostolescu_savu_ion-guta_ionita__vol_11_iss_4.pdf