[Lib库 2]CoppeliaSim机械臂抓取的Simulin

2020-06-17 本文已影响0人 monkey61

前言

机械臂抓取和摆放物体是常见的应用场景，在上一节机械臂的案例中（B站视频）我们实现了逆运动学控制之后，便可进行一些任务功能的实现。在这个过程中，我们试试在Simulink控制CoppeliaSim，跳过建立机械臂的运动学模型，我们可以做到什么地步。录制B站视频的时候一行一行敲代码真是非常耗费时间的，大家也很难从中间找到重点，而且也不方便后期查询和检索，还是通过文本来辅助，视频让大家了解主要功能，文字了解一些细节，然后代码和模型就留给大家自己去啃了……（不过放心，有不懂的可以联系我，淘宝或微信都可以）。
所有CoppeliaSim的Simulink的模型都可以在购买CoppeliaLib库后免费获得（购买链接），最新版本请联系淘宝客服。

整体效果

整个执行过程可以看下面这张图：

机械臂抓取和摆放的过程

CoppeliaSim模型

首先我们用一张图描述一下整个模型：

Dummy点的使用

我们使用了几个dummy来进行笛卡尔空间的辅助定位，控制机械臂的时候主要参考这几个中间辅助点就可以了

relaseDummy是用来标记物体的释放点，也就是每隔一段时间，会从传送带上方释放一个物体块；
GraspPoint是表示抓取点的位置，也就是说物体在传送带上停留的位置，机械臂不管物体的姿态和形状，都是从这个点开始抓取。（这里其实是有一个很强的假设条件的，就是物体肯定处于机械臂可以抓取的状态，所以物体中途如果遇到扰动的话，抓取可能会失败）；
RelasePoint是表示释放点的基准位置，后面机械臂的真实释放点是通过基准位置加偏移位置共同确定的。

IKGroupToSimulink

这个模块在B站的视频机械臂控制中已经介绍过了，这里再赘述一下。
首先要设置CoppeliaSim的IK计算模块，设置过程如下图：

然后依次设置UR5的每个关节的，注意关节要设置为Torque/force Mode，不要设置为Inverse kinematics mode，因为我们这合理要通过Simulink来控制机械臂的关节位置。

接下来就是重点了，IKGroupToSimulink的配置，首先如果从modelLib库中将其拖入到模型环境中，会弹出如下的对话框提示你接下来应该怎么操作。

根据提示我们需要修改改模块的三部分代码。

第一个为IK group的名称，1号箭头所指的地方
第二个为关节数量，UR5位6关节机械臂，因此设置为6
第三个为关节的名称，UR5的命名格式为UR5_joint+编号的格式，因此第三部分为前面去掉编号的字符串。

最后Simulink中模块的配置如下，含义和刚才的类似。

传送带代码

传送带代码主要怎加了读取测距传感器的功能和添加物体的功能。如果测距传感器检测到障碍物，那么传送带停止运动；每隔20秒向传送带添加一个物体。两个功能代码比较简单，大家自己参考。

function sysCall_init()
         
    pathHandle=sim.getObjectHandle("ConveyorBeltPath")
    forwarder=sim.getObjectHandle('ConveyorBelt_forwarder')
    sensor=sim.getObjectHandle('conveyorBelt_sensor')
    sim.setPathTargetNominalVelocity(pathHandle,0) -- for backward compatibility
    
    sampleTime = sim.getSimulationTimeStep() --获取系统运行周期
    
    ChangeCount = 20/sampleTime     --20秒释放一个物体，获取执行周期的次数
    count = ChangeCount
    relaseDummy = sim.getObjectHandle('relaseDummy')
    count = 0
    cuboidHandle = sim.getObjectHandle('Cuboid')

end

function sysCall_cleanup() 
 
end 

function sysCall_actuation() 

    count = count + 1
    if count >=ChangeCount then
        count  =0   --重新置零
        local hndShapes_table = sim.copyPasteObjects({cuboidHandle},0)
        hndShape = hndShapes_table[1]
        sim.setObjectPosition(hndShape,relaseDummy,{0,0,0})
        
    end

    beltVelocity=0.08

    if sim.readProximitySensor(sensor)>0 then
        --如果检测到物体，传送带停止运行
        beltVelocity=0
    end

    local dt=sim.getSimulationTimeStep()
    local p=sim.getPathPosition(pathHandle)
    sim.setPathPosition(pathHandle,p+beltVelocity*dt)
    --这后面的不用看，不用改
    -- Here we "fake" the transportation pads with a single static rectangle that we dynamically reset
    -- at each simulation pass (while not forgetting to set its initial velocity vector) :
    
    relativeLinearVelocity={beltVelocity,0,0}
    -- Reset the dynamic rectangle from the simulation (it will be removed and added again)
    sim.resetDynamicObject(forwarder)
    -- Compute the absolute velocity vector:
    m=sim.getObjectMatrix(forwarder,-1)
    m[4]=0 -- Make sure the translation component is discarded
    m[8]=0 -- Make sure the translation component is discarded
    m[12]=0 -- Make sure the translation component is discarded
    absoluteLinearVelocity=sim.multiplyVector(m,relativeLinearVelocity)
    -- Now set the initial velocity of the dynamic rectangle:
    sim.setObjectFloatParameter(forwarder,sim.shapefloatparam_init_velocity_x,absoluteLinearVelocity[1])
    sim.setObjectFloatParameter(forwarder,sim.shapefloatparam_init_velocity_y,absoluteLinearVelocity[2])
    sim.setObjectFloatParameter(forwarder,sim.shapefloatparam_init_velocity_z,absoluteLinearVelocity[3])
end

Simulink模型

Simulink模型的总体图如下：

位置的没有太多需要说明的地方，这里都是获取和设置的绝对位置，也就是相对于世界坐标系的位置。

姿态的获取和设定

姿态的话需要进行一下重点说明。我这里为了案例演示，姿态的获取采用四元数，姿态的设置采用了欧拉角，两种表示各有优缺点。

四元数的优点就是使用方便，它可以准确的表示出空间的姿态，并且也方便姿态之间的插值，但是缺点是优点抽象，很难和实际的物理量对应起来（这里又要BB两句了，很多同学非要知道它的物理意义才能用，而对数学概念不接受，你就知道它是一个姿态的表示，能拿来用就可以了，说实话我对四元数的物理意义到底是啥也不懂(￣ェ￣;)，你要是非要物理意义就转换成欧拉角，多好理解）
欧拉角的有点是非常的形象和直观，而且还可以根据你坐标系的建立方式选择不同的旋转方式，所以就会出现那么多种欧拉角的格式了；然而缺点就是欧拉角奇异性的问题，导致姿态直接插值会有问题。

因此，我们使用两种方式，获取姿态采用四元数，方便后面姿态进行插值；设置姿态就无所谓了，可以选择四元素或者是欧拉角，这里为了演示就选择欧拉角。两个的使用注意事项我在模块说明中都已经写清楚了，大家可以看一下。主要区别就是四元数的顺序和matlab中的不一样，欧拉角的默认旋转顺序不一样，所以做转换的时候一定要明确指定欧拉角的旋转顺序（不想麻烦的话设置的时候也直接用四元数，就不用考虑欧拉角的旋转顺序了）。

RG2手抓控制

手抓控制需要使用到simulink和CoppeliaSim通信的一个重要功能，就是发送float数组，其实这个场景非常的常见，如果你内嵌脚本比较熟悉的话，你可以用内嵌脚本直接把数据打包好，然后发送给simulink，然后将来真是的机器人就把通信模块换成数据总线，就就可以将控制器应用在实际机器人上了。
simulink模块的控制逻辑是输入0或1表示打开和闭合手爪，然后生成第一个量，就是目标速度，从而操作手爪的打开和闭合。具体还是自己看模块的解释和手爪的代码吧。

Stateflow

Stateflow的流程动态是这样的

回顾一下整个执行过程

机械臂抓取和摆放的过程
看到这里其实心细的同学可能已近将Stateflow的流程和机械臂的运动流程对应起来了。实际上就是这样，Stateflow的图就是控制状态转移图，机械臂每一阶段的运动都对应一个状态，多个状态的组合完成整体的功能。

这里还需要说明的一点就是有的同学可能会说“我感觉这个过程不复杂啊，为啥用了这么多状态，是不是简单问题复杂化了”，这样的问题我最开始学习Stateflow的时候也时常有，因为习惯了文本编程语言的思维方式，用文本语言我们一直希望通过封装增加程序可读性，各种使用类或者是把某一块功能单独做成一个函数文件等等，所有这些操作都是通过隐藏足够多的细节来提升程序的可读性，所以到最后的状态机可能就是几十行代码，看起来比较简洁，但是这样的问题是有看到的细节不够多。使用Stateflow也可以进行功能模块的组装，但是我个人风格就是一张蓝图绘到底，二话不说需要状态就直接堆，缩小了就是整个状态流图，放大了就可以看到状态的细节，多方便；反之对于文本编程的话，这样写肯定除了你谁都看不懂了，这就是Simulink的优势。而且对于一个成熟产品，状态机一般都至少十几个或者几十个或者上百个或者上千个或者……没有上限，所以！！！状态不是越少越好，代码不是越短越好，不要纠结1+1=2用matlab计算是不是有点掉价，搞工程的，要有总体概念，不要老是反省自己我是不是用牛刀杀鸡了，我们干的就是用牛刀杀鸡的活！！！（这段话个人情绪有点重，但实在是肺腑之言，实际大部分的情况，就是你连个铅笔刀都没有，问题怎么都解决不了，哈哈，所以快速用牛刀解决好解决的问题，然后留下时间找你的铅笔刀吧）。

跳过上面的一堆废话后，我们再去看一下这个Stateflow模型，过程划分的比较明显，具体过程还是去看B站视频吧，毕竟展示流程还是视屏更加的方便。这里我们以第一个状态讲一下。

第一个状态的主要功能就是使用getPath函数进行UR5_ikTarget的起始位置和姿态到目标位置和姿态之间的插值。然后用一个计数器依次读取轨迹的数据。这个模块的中文注释非常详细，我这里就不再赘述了，大家直接看模块的说明就可以。stateflow真是没得讲，看看就明白什么回事，极大降低沟通成品，如果再有好的命名规范，完全可以做到代码即注释的效果。（不过你要是阅读过程中有什么疑问，可以淘宝或微信联系我）。
最后还是依次放一下各个状态的功能演示。