倍福TwinCAT(贝福Beckhoff应用教程13.2 TwinCAT控制松下伺服 CS说明

虚拟仿真上，要注意仿真只是为了可视化，可以看到数据的变动是否和实际一致，所以Robot2D才是主要因素，虚拟仿真采集机器人的关节位置或者TCP位置来显示而已，为了测试一些别的算法，我们还可以在虚拟仿真的环境中放入临时的点位，然后显示一个球体或者一个坐标系，以此来判断是否算的准确（OPENGL无非rotate和translate到目标位置，然后draw即可）

仿真上还要注意，坐标系方向是自己定义的，比如我认为底座的正中心就是世界坐标系的原点。每加载一个模块，可以认为坐标系会产生一次平移和旋转，因此即便是直线模组也建议采用DH法构建从Base到Tool的坐标系变换，这对于后续的处理会比较简单。在本例中大关节映射了机器人Y坐标，小关节映射了机器人X坐标，当然Z方面只是一个画笔（也可以认为是一个不动的坐标）。每一个一起运动的部分可以认为是一个整体，Solidworks用STL格式输出即可指定坐标系（比如我认为对于底座部分如下，底座的原点就是底盘的正中心，对于大模组X）

X模组让末端产生Z=15的偏移值

Y模组让末端产生Z=81的偏移值

Z模组让末端产生X=-11，Y=-85，Z=-44的偏移值

工具端的画笔模块让末端产生X=10，Y=10，Z=0的偏移值

总的来说整个模组机构让末端产生X=-1，Y=-75，Z=52的偏移值

也就是说当J0和J1都回到零点的时候，整个TCP末端相对于基座的偏移为X=-1，Y=-75，Z=52mm），因此当两个关节都是0度的时候，TCP的XY坐标不是0（这同样也需要在OPENGL中手工给出偏移具体数值

从静态算法来分析，所有机器人都至少可以从关节位置更新TCP位置和从TCP位置更新关节位置两种方法（get_forward_kin()和get_inverse_kin()），当然在本例子中比较简单，因为一个关节控制了一个自由度，互相不干涉（不管是否添加Z轴），正向分析，电机转动通过丝杠传递到末端，一个电机角度就对应了一个X或者Y的值（正比例关系），反之也是如此，实际上在串联转动关节的机器人中，正解必然存在且唯一，逆解不一定存在且不一定唯一（0,1或者多个），但是如论简单还是复杂还是应该按照统一的写法。

从动态算法来分析，所有机器人都至少可以点动关节或点动TCP（movej()和mvoel()），当然在本例中为了简单演示只做了MVOEJ和MOVEL的一个特例（JOG单轴，实际上MOVEJ和MVOEL是可以多个轴或者多个方向的，给定的速度，加速度重新计算到每一个轴上，让每一个轴都一起开始一起停止，不管哪种方式最终都是要算到关节位置的修改这一步，因为电机只认识关节的旋转位置。）停止的过程也只是简单的直接终止线程（实际上为了保证运动的平滑和稳定，需要从当前速度开始慢慢减速到零stopj()和stopl()）

在本例中，上位和下位是分开的（C#的上位优势是可以做的比较好看好用，TwinCAT的下位优势是可以跟具体硬件交互实现具体功能）。有很多功能既可以在上位完成也可以在下位完成，比如运动过程中的正负限位设置，速度过大保护等等（一般涉及具体硬件的应该在下位实现，因为上位的实时性不够，如果到了限位没有反应过来就超了，而下位的PLC程序可以确保高速和可靠。而和应用相关的应该在上位实现，因为上位做起来更快更方便，而且下位缺少很多高级语言才有的方法，函数）。在最后的例子中，将会演示上位和下位相结合的情况，上位负责显示和跟用户交互，下位负责执行和反馈实际硬件状态。