unity3d实现机械臂建模来抓取物体和放下
from https://www.zhihu.com/question/511100488
作者：Randy
链接：https://www.zhihu.com/question/511100488/answer/2308338736
来源：知乎
著作权归作者所有。商业转载请联系作者获得授权，非商业转载请注明出处。
这里在2022/01/12回答的基础上，又花了一天时间把demo的功能完善了一下，来详细讲解一下制作中的各种细节要点先来看看最终效果Unity机械臂03/20 更新，评论区已经有两人提到了关节角度限制/碰撞的问题了，这里在01/26的基础上，增加了关节的角度限制的功能，以避免碰撞和一些奇异的姿态（详见第4节）同时发现节点设计中，对于第一个关节的初始旋向布置有误，已更新解决方案（详见第2.1节的补充）Unity 机械臂 - 角度限制1.计算动画与反向运动学这个问题最核心的地方，是如何根据末端机械手的目标位置（要抓取的目标物体），反向求解出机械臂父链节点的姿态，并实现机械臂的动画效果在动画模式上区别于传统的角色关键帧动画(Key-Frame Animation)，这里就需要我们使用计算动画(Caculate Animation)的模式因为机械臂要抓取的目标点可能是空间内的任意位置，无法通过关键帧动画进行预制作+实时采样输出，而是需要我们实时的根据所需的目标点来生成机械臂的动画在运动模式上，机械臂的各个节点呈父子关系嵌套，会形成FK(Front Kinematics)正向运动效应，父节点的运动会影响子节点相对世界的运动(父--->子)，但在动画解算中，我们却是要根据末端子节点的位置，来解算父链节点的姿态(子--->父)，也就是所谓的IK(Inverse Kinematics)逆向运动学求解链状体的逆向运动学是一个很复杂的问题，因为往往是有无穷多解的，很难给出一个线性的解析式（请想象一下你拿着一条锁链的两端，锁链并没有因为你拿着两端就被完全固定，它中间的链节仍可以随意振动）这里用到了一种比较主流的，基于迭代的求解方法，被称作CCD(Cyclic Coordinate Descent)循环坐标下降法它的思路如下：如果最终我们找到了某一个符合要求的姿态，那么一定满足，所有父链节点与末端节点的连线均指向目标点<img src="https://picx.zhimg.com/50/v2-7541530e15086d87d5ddae5585e65652_720w.jpg?source=1940ef5c" data-caption="" data-size="normal" data-rawwidth="866" data-rawheight="480" data-default-watermark-src="https://pic1.zhimg.com/50/v2-2a113e95f81967711f943f1ab5e639a7_720w.jpg?source=1940ef5c" class="origin_image zh-lightbox-thumb" width="866" data-original="https://picx.zhimg.com/v2-7541530e15086d87d5ddae5585e65652_r.jpg?source=1940ef5c"/>每一次迭代，我们从父节点到子节点进行一次遍历，每个节点要旋转的角度，等于该节点指向末端的向量，与该节点指向目标点的夹角<img src="https://picx.zhimg.com/50/v2-8056b861b472049c19ed7685dbef53e2_720w.jpg?source=1940ef5c" data-caption="" data-size="normal" data-rawwidth="846" data-rawheight="488" data-default-watermark-src="https://picx.zhimg.com/50/v2-ad8d343ccaaa67a9fecfd9abe33051c5_720w.jpg?source=1940ef5c" class="origin_image zh-lightbox-thumb" width="846" data-original="https://pica.zhimg.com/v2-8056b861b472049c19ed7685dbef53e2_r.jpg?source=1940ef5c"/>每个节点旋转完成后，都保证它与末端节点的连线能够指向目标点但每个节点旋转过后，会破坏其它节点的指向性不过在多次迭代之后，依托于初始状态的限制，以及我们遍历节点的顺序的限制，整个链状体的姿态会逐渐收敛向一个唯一的最终结果上2.制作中的各种细节---2.1 节点的设计评论区已经有人在问机械臂节点的问题了这里没有用骨骼蒙皮，因为只是简单做个实列，所以直接将网格物体嵌套为父子链注意末端要创建一个虚拟物体作为IK匹配的定位点/指示节点当然这样的话一个很大的缺点就是drawcall不友好，因为每个分离出来的网格都是一个单独的物体，都需要一个drawcall进行渲染如果是正经的工程的话，可以考虑Dynamic Batching动态合批，或者如果机械臂网格很复杂，顶点比较多，也可以考虑用骨骼蒙皮的方式来做<img src="https://pic1.zhimg.com/50/v2-3448633e902ee4044e6327fde2cdb68f_720w.jpg?source=1940ef5c" data-size="normal" data-rawwidth="1217" data-rawheight="790" data-default-watermark-src="https://picx.zhimg.com/50/v2-1671b9f678e0c720c3284623746f0fb3_720w.jpg?source=1940ef5c" class="origin_image zh-lightbox-thumb" width="1217" data-original="https://pica.zhimg.com/v2-3448633e902ee4044e6327fde2cdb68f_r.jpg?source=1940ef5c"/>父子关系如图所示，注意夹子末端闭合位置要创建一个虚拟物体，作为末端定位节点在Unity中我们原地复制一份物体，删除掉网格，只保留骨骼节点和末端的Point（左右两个夹子可以去掉），从而获得一份骨骼节点CCD对这一套骨骼节点进行解算，得到最终的结果位置，再通过Update将机械臂插值匹配到骨骼节点上，形成计算动画的效果末端的那个Point节点，会作为IK位置匹配的定位节点（我们最终是要将它的位置与目标位置匹配上），在CCD计算中会依照它的位置和其它骨骼来计算指向向量在IK中，链状体最末端的节点是不转动的，只作为匹配的定位节点，因为它自己的指向向量是算不出来的<img src="https://picx.zhimg.com/50/v2-f756cbd57ca53909eddd54be9894075a_720w.jpg?source=1940ef5c" data-caption="" data-size="normal" data-rawwidth="1920" data-rawheight="1008" data-default-watermark-src="https://pica.zhimg.com/50/v2-633c6c5a436444a0bf26a79a0c9537ce_720w.jpg?source=1940ef5c" class="origin_image zh-lightbox-thumb" width="1920" data-original="https://picx.zhimg.com/v2-f756cbd57ca53909eddd54be9894075a_r.jpg?source=1940ef5c"/>更多有关IK的一些概念，可以看看这篇文章前面的部分Randy：学习笔记---3dMax动画系统（机械、角色动画篇）58 赞同 · 9 评论文章要注意一下模型的物理尺寸控制，和轴心的调整问题<img src="https://pic1.zhimg.com/50/v2-3937cec28ba4daf9d8a95c3837be875e_720w.jpg?source=1940ef5c" data-size="normal" data-rawwidth="1347" data-rawheight="796" data-default-watermark-src="https://pic1.zhimg.com/50/v2-4072dcf0e4af90d6ac509a6b412807f2_720w.jpg?source=1940ef5c" class="origin_image zh-lightbox-thumb" width="1347" data-original="https://picx.zhimg.com/v2-3937cec28ba4daf9d8a95c3837be875e_r.jpg?source=1940ef5c"/>最高父物体承台，Y+冲上，Z+冲前，匹配Unity世界中的习惯<img src="https://picx.zhimg.com/50/v2-42b970f59935d73ce458110b03055b8c_720w.jpg?source=1940ef5c" data-size="normal" data-rawwidth="1205" data-rawheight="801" data-default-watermark-src="https://picx.zhimg.com/50/v2-65f5d43ed91011e46153f069e5212bc7_720w.jpg?source=1940ef5c" class="origin_image zh-lightbox-thumb" width="1205" data-original="https://pic1.zhimg.com/v2-42b970f59935d73ce458110b03055b8c_r.jpg?source=1940ef5c"/>机械臂节点，以Y轴作为转轴，Unity采用Heading(Y)--Pitch(X)--Bank(Z)，所以用最外层Y轴不容易出错，注意调整轴心的位置到旋转中心处，并且所有节点Y+应该冲向同一侧我们在解算CCD的时候，是需要计算转轴方向的如图下图所示，因为所有臂节点Y轴都冲向同一侧，所以代码里才可以统一叉积的向量顺序<img src="https://pic1.zhimg.com/50/v2-98e20d99b49e390e2b68bcd44ed58269_720w.jpg?source=1940ef5c" data-size="normal" data-rawwidth="1360" data-rawheight="560" data-default-watermark-src="https://picx.zhimg.com/50/v2-37ccad86d9ba66c3542d585c64629a7a_720w.jpg?source=1940ef5c" class="origin_image zh-lightbox-thumb" width="1360" data-original="https://pica.zhimg.com/v2-98e20d99b49e390e2b68bcd44ed58269_r.jpg?source=1940ef5c"/>Unity是左手系要用左手螺旋，Y轴正向旋转按左手螺旋，等效为 从 节点指向末端的向量 到 节点指向目标的向量所以代码里面算转轴的时候，向量的顺序是这样的//bonetopoint 骨骼节点指向末端的向量
//bonetotar 骨骼节点指向目标的向量
axis = Vector3.Cross(bonetopoint, bonetotar).normalized; //！！！！ 注意这里的向量顺序
更多有关3dsMax导出模型与Unity对接的问题，可以参见下面的文章Randy：当3dMax遇上Unity3d---模型导入的前后你需要注意的地方285 赞同 · 39 评论文章03/20 更新，发现了之前关于节点设计没做好的一点我们需要在承台和第一节机械臂关节之间插入一个节点，该节点的位置旋向，对齐到第一节机械臂节点<img src="https://pic1.zhimg.com/50/v2-0db644649502a30198e7fe387edc51f8_720w.jpg?source=1940ef5c" data-caption="" data-size="normal" data-rawwidth="1627" data-rawheight="790" data-default-watermark-src="https://picx.zhimg.com/50/v2-ee5cd0a347617ff69bea236022d3d5f3_720w.jpg?source=1940ef5c" class="origin_image zh-lightbox-thumb" width="1627" data-original="https://picx.zhimg.com/v2-0db644649502a30198e7fe387edc51f8_r.jpg?source=1940ef5c"/>这个节点的插入是为了保证第一节关节的转轴也是Y轴因为节点的轴心初始姿态，位置上由节点自行定义，旋向上是继承自上一级父节点的本质上是由于 Transform 的 vSRT 矩阵（行左乘顺序），按照空间考虑 T 是 R 的父变换，因此节点轴心位置由自己定义，但进行自己的R旋转时，初始旋向是来自父坐标系的这个问题，更详细的解释不妨参见下面这两篇文章Randy：矩阵变换的本质 --- 欧拉角与矩阵与万向锁问题112 赞同 · 19 评论文章3dmax做钟表参数链接后转秒针分针不动是为什么？1 赞同 · 6 评论回答<img src="https://picx.zhimg.com/50/v2-581ea110d225a183b0633322861aa986_720w.jpg?source=1940ef5c" data-caption="" data-size="normal" data-rawwidth="1420" data-rawheight="737" data-default-watermark-src="https://pic1.zhimg.com/50/v2-f85dff6c1d958d7c13f936941ba6b5df_720w.jpg?source=1940ef5c" class="origin_image zh-lightbox-thumb" width="1420" data-original="https://pica.zhimg.com/v2-581ea110d225a183b0633322861aa986_r.jpg?source=1940ef5c"/>承台的Y轴，为了适配Unity世界是冲上的，如果承台直接作为第一节机械臂的父物体，那么第一节机械臂的转轴其实是X轴而非Y轴<img src="https://picx.zhimg.com/50/v2-a22d7f08095508654de2a8b13ef7d00b_720w.jpg?source=1940ef5c" data-caption="" data-size="normal" data-rawwidth="1541" data-rawheight="884" data-default-watermark-src="https://picx.zhimg.com/50/v2-b1416804c6f27a2e25e14e2e7bc84f75_720w.jpg?source=1940ef5c" class="origin_image zh-lightbox-thumb" width="1541" data-original="https://pica.zhimg.com/v2-a22d7f08095508654de2a8b13ef7d00b_r.jpg?source=1940ef5c"/>并且由于这个更改，在代码中Awake阶段初始化时，需要先递进一次，跳过FPoint节点private void Awake()
{
arms = new List<Transform>();
bones = new List<Transform>();
baseRotation = new List<Quaternion>();
armRotation = new List<Quaternion>();
animator = gameObject.GetComponent<Animator>();
Transform pre = gameObject.transform.GetChild(0);//先往下走1，跳开FPoint
Transform preb = pbone.transform.GetChild(0);
//遍历录入机械臂节点，骨骼节点
}
---2.2 夹取的效果------2.2.1 夹住物体的效果我们希望末端的两个夹子能够根据不同的网格碰撞体，夹紧到边缘的位置，我们采用了关键帧动画+碰撞检测+动画暂停的思路这里3dsMax（DCC）中，在夹子的末端创建两个Box<img src="https://pic1.zhimg.com/50/v2-28dc12fb57a11039b8e3908499875c30_720w.jpg?source=1940ef5c" data-size="normal" data-rawwidth="1602" data-rawheight="801" data-default-watermark-src="https://pic1.zhimg.com/50/v2-8ee349bbbcbfd441bacc3c4ec8880a86_720w.jpg?source=1940ef5c" class="origin_image zh-lightbox-thumb" width="1602" data-original="https://pic1.zhimg.com/v2-28dc12fb57a11039b8e3908499875c30_r.jpg?source=1940ef5c"/>在3dsMax中创建两个Box，作为夹子的子物体，调整其位置Unity中通过这两个Box来展开碰撞体<img src="https://picx.zhimg.com/50/v2-2f7f80d859113c3ac3a8a842c9c6174b_720w.jpg?source=1940ef5c" data-size="normal" data-rawwidth="1920" data-rawheight="666" data-default-watermark-src="https://picx.zhimg.com/50/v2-06d7b24be1886268d8e7be2d3a0fc049_720w.jpg?source=1940ef5c" class="origin_image zh-lightbox-thumb" width="1920" data-original="https://picx.zhimg.com/v2-2f7f80d859113c3ac3a8a842c9c6174b_r.jpg?source=1940ef5c"/>Unity中添加BoxCollider，自动适配，勾选trigger只用于监测碰撞，删除掉Mesh网格之后在Unity中承台节点上添加一个静态刚体，用于监测子节点collider的碰撞（发生碰撞必须要有刚体才行）我们会使用Unity中的onTriggerStay碰撞方法，触发条件是两物体均有collider，任意一方有Rigidbody，任意一方勾选Trigger，本质上是物理系统中，由Rigidbody发起碰撞检测，去找tigger collider这里不用onTriggerEnter是为了规避bug，如果用onTriggerEnter可能机械臂旋转完成之前，碰撞监测开启之前，就已经Enter了，而Stay会一直触发，所以不用担心时序问题<img src="https://picx.zhimg.com/50/v2-807b6565e47d3e60ff8504a5172f89f7_720w.jpg?source=1940ef5c" data-caption="" data-size="normal" data-rawwidth="1920" data-rawheight="967" data-default-watermark-src="https://picx.zhimg.com/50/v2-fa61509ec859d06b079afbb67d039266_720w.jpg?source=1940ef5c" class="origin_image zh-lightbox-thumb" width="1920" data-original="https://picx.zhimg.com/v2-807b6565e47d3e60ff8504a5172f89f7_r.jpg?source=1940ef5c"/>在动画方面3dsMax中通过关键帧动画制作夹子夹紧的动画效果，动画关键帧布局如图这里只有夹子制作了关键帧动画，机械臂中要算IK的节点是没有关键帧的，所以不用担心Unity中Animator动画和我们Update中的计算动画控制冲突<img src="https://pica.zhimg.com/50/v2-b48ce8741d7786a3c6585da5f3e257f3_720w.jpg?source=1940ef5c" data-caption="" data-size="normal" data-rawwidth="1679" data-rawheight="916" data-default-watermark-src="https://picx.zhimg.com/50/v2-7a960d2e070f362e1cbb968808b99c9c_720w.jpg?source=1940ef5c" class="origin_image zh-lightbox-thumb" width="1679" data-original="https://picx.zhimg.com/v2-b48ce8741d7786a3c6585da5f3e257f3_r.jpg?source=1940ef5c"/> 使用Unity的Mecanim动画系统，状态机配置如下图所示我们会给夹子制作一个静态的默认状态(Base)，一个静态的呈最大张开的状态(Open)，默认状态和张开状态，两个静止姿态通过动画过渡混合生成张开的效果<img src="https://picx.zhimg.com/50/v2-43992e4c9b5322d8874f676ac3c1a029_720w.jpg?source=1940ef5c" data-caption="" data-size="normal" data-rawwidth="1829" data-rawheight="1008" data-default-watermark-src="https://pic1.zhimg.com/50/v2-704a7e905fd2c52e151d774286087ddb_720w.jpg?source=1940ef5c" class="origin_image zh-lightbox-thumb" width="1829" data-original="https://pic1.zhimg.com/v2-43992e4c9b5322d8874f676ac3c1a029_r.jpg?source=1940ef5c"/>我们会制作一个动态的从最大张开夹到最紧的动画State(Close)夹紧动画执行时我们会开启碰撞监测，当onTriggerStay发现碰撞到了目标物体时，会让状态过渡进一个空State，这个State会取消勾选Write Defalut，从而进入这个State就会产生动画暂停的效果<img src="https://pic1.zhimg.com/50/v2-646207f1066448aeaba360bbfb0b2506_720w.jpg?source=1940ef5c" data-caption="" data-size="normal" data-rawwidth="812" data-rawheight="638" data-default-watermark-src="https://pic1.zhimg.com/50/v2-865e6b72e71f94df373f839fa0643c87_720w.jpg?source=1940ef5c" class="origin_image zh-lightbox-thumb" width="812" data-original="https://picx.zhimg.com/v2-646207f1066448aeaba360bbfb0b2506_r.jpg?source=1940ef5c"/>private void OnTriggerStay(Collider other) //碰撞到夹取目标物体
{
if (!isCatch&&onPhyCheck&&other.CompareTag("obj")&&catchTar.Cld == other) {
isCatch = true;
onPhyCheck = false;
animator.SetTrigger(stopHash); //触发Tigger，让状态机进入空状态，形成暂停效果
armState = ArmState.stati;
catchTar.Rbdy.isKinematic = true;//刚体静态化
}
}从而无论我们夹什么物体，通过关键帧动画+碰撞检测+动画暂停的方法，夹子就会在夹紧到物体碰撞网格的边缘时就会停下来<img src="https://picx.zhimg.com/50/v2-b2854ccb24304b73f631e30e4d4c8cc5_720w.jpg?source=1940ef5c" data-size="normal" data-rawwidth="1166" data-rawheight="670" data-default-watermark-src="https://pic1.zhimg.com/50/v2-16d613b42bcaae78fdcf501666894f91_720w.jpg?source=1940ef5c" class="origin_image zh-lightbox-thumb" width="1166" data-original="https://pica.zhimg.com/v2-b2854ccb24304b73f631e30e4d4c8cc5_r.jpg?source=1940ef5c"/>夹紧圆柱<img src="https://picx.zhimg.com/50/v2-a1eef5bfb4edb55d200651b8096c56f7_720w.jpg?source=1940ef5c" data-size="normal" data-rawwidth="661" data-rawheight="473" data-default-watermark-src="https://picx.zhimg.com/50/v2-13c8ea2c8904ddc11d7edf5229d2d166_720w.jpg?source=1940ef5c" class="origin_image zh-lightbox-thumb" width="661" data-original="https://picx.zhimg.com/v2-a1eef5bfb4edb55d200651b8096c56f7_r.jpg?source=1940ef5c"/>夹紧立方体更多有关Unity Mecanim动画系统的讲解，不妨参阅下面这篇文章关于Write Default的应用，可以看看文末的附录Randy：学习笔记 --- Unity动画系统347 赞同 · 48 评论文章------2.2.2 夹取物体的匹配问题这里并没有使用将夹取物体置为末端节点子物体，再释放的方式因此实际应用后发现，由于机械臂的旋转问题，可能最后物体在抓放的过程中被旋转到了奇怪的角度，我们其实希望物体能平拿平放所以最后选择了在Update中计算，实现在放置物体时，物体绕承台旋转，同时匹配到抓取位点的方法{ //正在旋转匹配
lerp = nowTime / rotateTime;
gameObject.transform.rotation = Quaternion.Slerp(armRotation[0], pbone.rotation, lerp);
for (int i = 1; i <= depth; ++i)
arms[i - 1].localRotation = Quaternion.Slerp(armRotation[i], bones[i - 1].localRotation, lerp);
//抓取目标的位置跟随匹配
if (isCatch && catchTar) {
catchTar.transform.RotateAround(transform.position, roVec, Time.deltaTime*cproAngle);
catchTar.transform.position = catchPoint.position + delVec;
}
}
当然另一种思路是，在CCD计算中做文章我们可以把最后的这个圆球关节作为末端点来计算CCD，每次抓取时，根据LookAt向量的反向，计算匹配位置，之后将夹子转平去夹取/放置，从而实现物体的平拿平放<img src="https://picx.zhimg.com/50/v2-03d9063779dca8e21ff43f460c8e2fc8_720w.jpg?source=1940ef5c" data-caption="" data-size="normal" data-rawwidth="796" data-rawheight="521" data-default-watermark-src="https://picx.zhimg.com/50/v2-c55e97eed3156005c579d0e2448c4fba_720w.jpg?source=1940ef5c" class="origin_image zh-lightbox-thumb" width="796" data-original="https://pic1.zhimg.com/v2-03d9063779dca8e21ff43f460c8e2fc8_r.jpg?source=1940ef5c"/>------2.2.3 中心偏移与放置悬浮Unity中创建的Box Cylinder 它们的轴心位于其网格的中心所以夹取的时候，就夹物体所在的位置就好<img src="https://pica.zhimg.com/50/v2-fcf3ab8bc8ea6a0873033b058ba1c71b_720w.jpg?source=1940ef5c" data-caption="" data-size="normal" data-rawwidth="615" data-rawheight="424" data-default-watermark-src="https://pic1.zhimg.com/50/v2-f667977352aeeb3c8b20e8e2333d9094_720w.jpg?source=1940ef5c" class="origin_image zh-lightbox-thumb" width="615" data-original="https://pic1.zhimg.com/v2-fcf3ab8bc8ea6a0873033b058ba1c71b_r.jpg?source=1940ef5c"/>但并不是所有物体轴心都在网格中心处，一般我们通过DCC建模出来的模型，为了方便在Unity场景中进行放置，都会将模型轴心位置至于底部，此时要夹取的Center位置就和物体的所在的位置有一个偏移量<img src="https://pica.zhimg.com/50/v2-9f4b67a6aea644180be141b5d59895b6_720w.jpg?source=1940ef5c" data-caption="" data-size="normal" data-rawwidth="1285" data-rawheight="999" data-default-watermark-src="https://picx.zhimg.com/50/v2-668ce9f58fc79c473159e52eb345cd45_720w.jpg?source=1940ef5c" class="origin_image zh-lightbox-thumb" width="1285" data-original="https://pic1.zhimg.com/v2-9f4b67a6aea644180be141b5d59895b6_r.jpg?source=1940ef5c"/>放置物体的时候，物体需要被放置位置和射线击中的地板位置也会有一个差量，或者说up悬浮高度Unity中的Box就应该放置在0.5高度处，如果网格不规则，那么放置悬浮高度最好是其中心到边缘的最大距离为此我们就需要通过一个脚本来统一的设置/获取被夹取物体的中心偏移，和放置悬浮高度using System.Collections;
using System.Collections.Generic;
using UnityEngine;
[RequireComponent(typeof(Collider), typeof(Rigidbody))]
public class catchObj : MonoBehaviour
{
Collider cld;
Rigidbody rbdy;
[SerializeField]
float catchOverHeight = 0f;//相对于轴心的悬浮中心高度
[SerializeField]
float putOverHeight = 0f;//相对放置位置需要悬浮的高度
public Collider Cld {
get {
return cld;
}
}
public Rigidbody Rbdy {
get {
return rbdy;
}
}
public Vector3 CatchCenterPoint { //获取物体被夹取的中心位点 世界坐标
get {
return transform.position + catchOverHeight * transform.up; //注意要用物体本地坐标的up方向来算
}
}
public Vector3 PutOverVec { //物体中心相对放置位点应当悬浮的高度向量
get {
return Vector3.up * putOverHeight;
}
}
private void Awake()
{
cld = gameObject.GetComponent<Collider>();
rbdy = gameObject.GetComponent<Rigidbody>();
}
}
------2.2.4 夹取失败毕竟机械臂长度有限，可能完全伸展也无法go到物体，这里状态的过渡设计上就要考虑夹取不到的状况我们可以在夹取动画最后面的位置，当夹子已经完全合上的时候，通过一个关键帧事件来触发一个检测方法<img src="https://picx.zhimg.com/50/v2-44277400402ca4edac93aa29dacc2a9e_720w.jpg?source=1940ef5c" data-caption="" data-size="normal" data-rawwidth="346" data-rawheight="662" data-default-watermark-src="https://pic1.zhimg.com/50/v2-04c6ca9862351c449d7a849b6d46219f_720w.jpg?source=1940ef5c" class="content_image" width="346"/>如果通过flag判断，如果发现此时还未夹到物体，那么判断夹取失败，将机械臂复位 public void catchCheck() //Close执行到最后 是否夹取到
{
if (onPhyCheck && !isCatch) { //夹取失败，将机械臂复位
isCatch = false;
catchTar = null;
onPhyCheck = false;
animator.SetTrigger(resetHash);
armState = ArmState.stati;
ccdReset();
}
}
状态机加了这么一条夹取失败的过渡，返回默认状态<img src="https://picx.zhimg.com/50/v2-35e25a758b35e39e68295145817f0dc3_720w.jpg?source=1940ef5c" data-caption="" data-size="normal" data-rawwidth="946" data-rawheight="422" data-default-watermark-src="https://picx.zhimg.com/50/v2-2c59f694ae6269c14fe819039390f20c_720w.jpg?source=1940ef5c" class="origin_image zh-lightbox-thumb" width="946" data-original="https://pic1.zhimg.com/v2-35e25a758b35e39e68295145817f0dc3_r.jpg?source=1940ef5c"/>3.CCD的姿态问题我们可能在一些位置发现CCD解算出了奇奇怪怪的姿态，例如下面就是本例中真实遇到的状况<img src="https://picx.zhimg.com/50/v2-f9806031c5a3d2cb4e3ce956e3e2d89d_720w.jpg?source=1940ef5c" data-size="normal" data-rawwidth="638" data-rawheight="543" data-default-watermark-src="https://picx.zhimg.com/50/v2-2904329f580bc2e8a1666b9051fc0324_720w.jpg?source=1940ef5c" class="origin_image zh-lightbox-thumb" width="638" data-original="https://picx.zhimg.com/v2-f9806031c5a3d2cb4e3ce956e3e2d89d_r.jpg?source=1940ef5c"/>在夹取近处物体时，出现奇怪的姿态，鬼手掏...要知道CCD受初始姿态的导向是强的，最终链状体能在无穷多解中收敛向一个唯一确定的姿态，很大程度上取决于一开始的位置对于出现这种奇异姿态，我们可以通过调整初始姿态来解决，注意机械臂和骨骼两套节点要一起调整<img src="https://picx.zhimg.com/50/v2-2198648f72aeb2e68a0f60bd1f9b14d5_720w.jpg?source=1940ef5c" data-size="normal" data-rawwidth="1920" data-rawheight="696" data-default-watermark-src="https://pic1.zhimg.com/50/v2-0476b930739feadc3969bff52736f465_720w.jpg?source=1940ef5c" class="origin_image zh-lightbox-thumb" width="1920" data-original="https://picx.zhimg.com/v2-2198648f72aeb2e68a0f60bd1f9b14d5_r.jpg?source=1940ef5c"/>转动初始姿态，保证夹取近点时姿态正常<img src="https://picx.zhimg.com/50/v2-1b768d1548803f5b82830fa5179528d3_720w.jpg?source=1940ef5c" data-size="normal" data-rawwidth="1920" data-rawheight="677" data-default-watermark-src="https://picx.zhimg.com/50/v2-d97d4b31f0c43cae69baf3c4e7323b99_720w.jpg?source=1940ef5c" class="origin_image zh-lightbox-thumb" width="1920" data-original="https://pica.zhimg.com/v2-1b768d1548803f5b82830fa5179528d3_r.jpg?source=1940ef5c"/>然后可以转动某一个节点，再把机械臂转上去<img src="https://picx.zhimg.com/50/v2-5550d80913a643a2d7045212f1dd856c_720w.jpg?source=1940ef5c" data-size="normal" data-rawwidth="715" data-rawheight="533" data-default-watermark-src="https://picx.zhimg.com/50/v2-5a6b9e74081e84adf962dca4373a6f1d_720w.jpg?source=1940ef5c" class="origin_image zh-lightbox-thumb" width="715" data-original="https://pic1.zhimg.com/v2-5550d80913a643a2d7045212f1dd856c_r.jpg?source=1940ef5c"/>ok问题解决另一种避免奇异姿态的方法，是通过关节限制（详见下一节↓）关节限制能很好的避免奇异姿态，但也需要更深的迭代次数支持4.关节的限制---4.1 关节的角度限制评论区已经有两人提到了，有关关节的角度限制，碰撞避免相关的问题了这里其实碰撞问题没必要使用传统的物理检测，可以转化为关节角度的限制问题<img src="https://picx.zhimg.com/50/v2-11493349ba9b27d9ad3420e9002688cd_720w.jpg?source=1940ef5c" data-size="normal" data-rawwidth="719" data-rawheight="498" data-default-watermark-src="https://picx.zhimg.com/50/v2-63f3a14d8da463ac80ddb63839a7ad5d_720w.jpg?source=1940ef5c" class="origin_image zh-lightbox-thumb" width="719" data-original="https://picx.zhimg.com/v2-11493349ba9b27d9ad3420e9002688cd_r.jpg?source=1940ef5c"/>确实一些特定位置的匹配，第一节机械臂可能碰撞进了承台里，我们并不希望出现这种情况，因此可以对关节的旋转角度进行一定的限制来避免碰撞我们可以在Unity中测一下，第一节关节不碰到底部承台，所允许的 / 能够接受的，旋转角度范围事实上我们可以对每个关节都测一下旋转角度的限制，关节的限制会产生一个强导向作用，用来规避奇异姿态的出现<img src="https://picx.zhimg.com/50/v2-82dbf91dc6d9e9a29ee6ccc1ab8083a1_720w.jpg?source=1940ef5c" data-size="normal" data-rawwidth="1920" data-rawheight="681" data-default-watermark-src="https://picx.zhimg.com/50/v2-64d184871e4808ff73e80182a1c1b84a_720w.jpg?source=1940ef5c" class="origin_image zh-lightbox-thumb" width="1920" data-original="https://pica.zhimg.com/v2-82dbf91dc6d9e9a29ee6ccc1ab8083a1_r.jpg?source=1940ef5c"/>转一转，量一量在代码中解算CCD时，我们对相应的关节，在计算完成后，夹一个Clamp就好，非常简单CCD关节会在限制下尽可能的尝试够到目标位点，节点被clamp后匹配的不足会由其它节点自动补齐//CCD解算骨骼节点
for (int i = 0; i < ccdDepth; ++i) { //迭代次数
for (int j = 0; j < depth; ++j) { //依次计算每根骨骼的旋量
//计算bones[j] 的匹配旋转
//节点旋向限制
//第一时间对Y轴旋转夹一个Clamp
if (j == 0) {
bones[0].localEulerAngles = new Vector3(0, Mathf.Clamp(bones[0].localEulerAngles.y, -60.0f, 130.0f), 0);
//这里如果节点没错的话，那么 x z local旋转肯定是0，不用 bones[0].localEulerAngles.x/.z 这样写
}
else if (j == 1 || j == 2) {
bones[j].localEulerAngles = new Vector3(0, Mathf.Clamp(bones[j].localEulerAngles.y, 0, 120.0f), 0);
}
else if (j == 3) {
bones[3].localEulerAngles = new Vector3(0, Mathf.Clamp(bones[3].localEulerAngles.y, -20.0f, 20.0f), 0);
}
}
}不过上面这种if - else if的写法好像有点...丑，我们可以换一种更好的写法创建一个 class pointRoClamp ，用于记录每个关节的旋向限制信息，使用一个序列化的List，形成一张表单也可以结合 ScriptableObject 将限制信息形成资源配置文件 [System.Serializable]
class pointRoClamp //关节旋向限制
{
public bool onClamp;//是否启用限制
public float maxro;//最大 最小旋量
public float minro;
}
[SerializeField]
List<pointRoClamp> roclamps;//关节旋向限制表单
之后我们就可以在Inspector中通过序列化的方式配置每个关节的旋转了<img src="https://picx.zhimg.com/50/v2-7a61e3688c3db9f96ad50ab99773b14d_720w.jpg?source=1940ef5c" data-caption="" data-size="normal" data-rawwidth="352" data-rawheight="836" data-default-watermark-src="https://pic1.zhimg.com/50/v2-5cd376e8d23701cb7e21d96997985d30_720w.jpg?source=1940ef5c" class="content_image" width="352"/>相应的代码里通过表单来进行限制 //CCD解算骨骼节点
for (int i = 0; i < ccdDepth; ++i) { //迭代次数
for (int j = 0; j < depth; ++j) { //依次计算每根骨骼的旋量
//计算bones[j] 的匹配旋转
//根据clamps表单进行关节限制
if (roclamps.Count > j && roclamps[j].onClamp) {
bones[j].localEulerAngles = new Vector3(0, Mathf.Clamp(bones[j].localEulerAngles.y, roclamps[j].minro, roclamps[j].maxro), 0);
}
}
}
当然其实最好还是做成无限制的模式（通过更好的建模，使用时的一些限制），并通过初始姿态和节点计算顺序的导向，来避免奇异的姿态产生越是限制，就越需要更多的迭代次数，才能填平不足但好处是，通过限制，可以很好的规避奇异姿态这里在限制后，我们就需要增大CCD的迭代次数，以免因次数不够，错过一些本来能够到的位置<img src="https://pica.zhimg.com/50/v2-94979d380eaae3cca0098e2d16940c69_720w.jpg?source=1940ef5c" data-size="normal" data-rawwidth="594" data-rawheight="427" data-default-watermark-src="https://pic1.zhimg.com/50/v2-3b110eb1f54606a5987237039f303445_720w.jpg?source=1940ef5c" class="origin_image zh-lightbox-thumb" width="594" data-original="https://pica.zhimg.com/v2-94979d380eaae3cca0098e2d16940c69_r.jpg?source=1940ef5c"/>其实后面甚至加到了25层，30层才满意...---4.2 关节的选用限制关节限制其实能玩出很多花样，旋转只是最基础的一种这里考虑一些过近位置的放置，由于位置已经近过了第一节关节，第一节关节先行旋转匹配后，导致其它关节没有操作空间我们可以通过判断距离的方式，在过近的放置点出现时，不启用第一节关节的旋转（保持默认姿态/或者可以放置到一个更好的姿态/或者启用另一套角度限制）<img src="https://picx.zhimg.com/50/v2-8fc4a30bec47d752df32ca92ab62c2ac_720w.jpg?source=1940ef5c" data-caption="" data-size="normal" data-rawwidth="846" data-rawheight="626" data-default-watermark-src="https://pica.zhimg.com/50/v2-af06d34f5fcf6d491974aab5809ac751_720w.jpg?source=1940ef5c" class="origin_image zh-lightbox-thumb" width="846" data-original="https://picx.zhimg.com/v2-8fc4a30bec47d752df32ca92ab62c2ac_r.jpg?source=1940ef5c"/>这里就简单的在放置点过近时，取消第一节关节的旋转//CCD解算骨骼节点
for (int i = 0; i < ccdDepth; ++i) { //迭代次数
for (int j = 0; j < depth; ++j) { //依次计算每根骨骼的旋量
//节点选择限制
if (Vector3.Distance(p, gameObject.transform.position) < 2.0f && j == 0) { //如果目标点位置过近，CCD计算跳过第一个节点
continue;
}
//计算bones[j] 的匹配旋转
////节点旋向限制
if (roclamps.Count > j && roclamps[j].onClamp) { //根据clamps表单进行关节限制
bones[j].localEulerAngles = new Vector3(0, Mathf.Clamp(bones[j].localEulerAngles.y, roclamps[j].minro, roclamps[j].maxro), 0);
}
}---4.3 放置的失败这里夹取的失败我们在 2.2.4 节通过帧事件的触发判断解决了（夹取动画末尾判断能否夹到）然而关节限制又引出了放置失败的问题，这里夹取的失败是旋转匹配之后通过帧事件判断（伸过去夹一下，夹不到就失败复位）而放置的失败，需要我们进行预先判断（不能拿过去了，才发现放不到！！！）因此ccd迭代解算完成后，需要进行一个判断[SerializeField]
Transform boneEndPoint;//骨骼末端位置 - 用于放置物体时，判断是否能放到目标位点上
///ccdgo 解算末尾
if (IsCatch && Vector3.Distance(p, boneEndPoint.position) > 0.1f) {//受Clamp影像，放置目标时，预先判断能否放到指定位置
//如果放不到 Reset机械臂
catchTar.Rbdy.isKinematic = false;//刚体恢复
catchTar.Cld.enabled = true;
catchTar = null;
isCatch = false;
animator.SetTrigger(resetHash);//爪子恢复默认状态
armState = ArmState.stati;
ccdReset();//重置机械臂姿态
Debug.Log("无法完成放置！！！");
}
else {//正常开启Update旋转的初始化
armState = ArmState.rotate;
nowTime = 0;
armRotation[0] = gameObject.transform.rotation;//记录承台旋量
for (int i = 1; i <= depth; ++i)
armRotation[i] = arms[i - 1].localRotation;
}5.贴一下代码最后贴一下控制机械臂的代码给大家参考吧03/20 增加了关节角度限制的版本，最终代码如下：using System.Collections;
using System.Collections.Generic;
using UnityEngine;
public class CCDCon : MonoBehaviour //机械臂控制脚本
{
//CCD控制
[SerializeField]
Transform pbone;//骨骼承台节点
[SerializeField]
int depth = 0;//节点深度 - 脚本所在的物体是承台，往下depth数量的节点为机械臂
[SerializeField]
int ccdDepth = 5;//CCD迭代次数
[SerializeField]
float rotateTime = 3.0f;//旋转匹配时间
float nowTime = 0;//当前时间
List<Transform> bones;//骨骼节点
List<Transform> arms;//臂节点
Transform bonePoint;//骨骼末端点
List<Quaternion> baseRotation;//初始旋量
List<Quaternion> armRotation;//机械臂节点的初始旋量
//状态相关 - 夹取相关
public enum ArmState
{
rotate, //正在旋转
stati //已静止的 可以被操作的
}
ArmState armState = ArmState.stati;
public ArmState AState {
get {
return armState;
}
}
bool onPhyCheck = false;//用于开启物理碰撞检测
bool isCatch = false;//是否夹取到了目标 --- 用于在Update位置匹配完成后指示下一步操作
catchObj catchTar = null;//夹取目标
public bool IsCatch {
get {
return isCatch;
}
}
[SerializeField]
Transform catchPoint; //末端位点 - 用于夹取物体后移动时的位置匹配
[SerializeField]
Transform boneEndPoint;//骨骼末端位置 - 用于放置物体时，判断是否能放到目标位点上
Vector3 delVec;//与末端位点的差量
Vector3 roVec;//转轴向量
float cproAngle;//每秒需要旋转的角度
[System.Serializable]
class pointRoClamp
{
public bool onClamp;
public float maxro;
public float minro;
}
[SerializeField]
List<pointRoClamp> roclamps;
//动画相关
Animator animator;
int openHash = Animator.StringToHash("open");
int closeHash = Animator.StringToHash("close");
int stopHash = Animator.StringToHash("stop");
int resetHash = Animator.StringToHash("reset");
private void Awake()
{
arms = new List<Transform>();
bones = new List<Transform>();
baseRotation = new List<Quaternion>();
armRotation = new List<Quaternion>();
animator = gameObject.GetComponent<Animator>();
Transform pre = gameObject.transform.GetChild(0);//往下走1，跳开FPoint
Transform preb = pbone.transform.GetChild(0);
catchTar = null;
armRotation.Add(gameObject.transform.rotation);
for (int i = 0; i < depth; ++i) {
pre = pre.GetChild(0);
preb = preb.GetChild(0);
arms.Add(pre);
bones.Add(preb);
baseRotation.Add(preb.transform.localRotation);
armRotation.Add(pre.transform.localRotation);
}
bonePoint = preb.GetChild(0);//骨骼末端点
}
Vector3 bonetopoint;//骨骼指向末端点向量
Vector3 bonetotar;//骨骼指向目标点向量
Vector3 vec;
float angle;//旋转角度
Vector3 axis;//转轴
public void ccdgo(Vector3 p, catchObj tar = null) //解算CCD尝试够到世界坐标p位置
{
if (armState == ArmState.rotate)//如果正在旋转应忽略
return;
if (isCatch == (tar != null)) //未抓住物体必须有目标，已抓住物体必须无目标进行放置
return;
//骨骼承台对准
pbone.LookAt(new Vector3(p.x, pbone.transform.position.y, p.z), Vector3.up);
//
if (catchTar == null) { //夹取物体
catchTar = tar;//设置夹取目标
animator.SetTrigger(openHash);//打开爪子
}
else { //放置物体
catchTar.Cld.enabled = false;//开始移动夹取物体时，关闭碰撞
p += catchTar.PutOverVec;//增加悬浮高度
//计算位置&旋转匹配数据
delVec = catchTar.transform.position - catchPoint.position;
cproAngle = Vector3.Angle(transform.forward, pbone.forward) / rotateTime;
roVec = Vector3.Cross(transform.forward, pbone.forward).normalized;
}
//骨骼节点初始化旋转
for (int i = 0; i < depth; ++i)
bones[i].transform.localRotation = baseRotation[i];
//
//CCD解算骨骼节点
for (int i = 0; i < ccdDepth; ++i) { //迭代次数
for (int j = 0; j < depth; ++j) { //依次计算每根骨骼的旋量
//节点选择
if (Vector3.Distance(p, gameObject.transform.position) < 2.0f && j == 0) { //如果目标点位置过近，CCD计算跳过第一个节点
continue;
}
vec = bones[j].InverseTransformPoint(bonePoint.position);//转入本地坐标
bonetopoint = new Vector3(vec.x, 0, vec.z).normalized;//Y轴强制归0，化为平面内旋转
vec = bones[j].InverseTransformPoint(p);//转入本地坐标
bonetotar = new Vector3(vec.x, 0, vec.z).normalized;//Y轴强制归0，化为平面内旋转
angle = Vector3.Angle(bonetopoint, bonetotar);
axis = Vector3.Cross(bonetopoint, bonetotar).normalized;
bones[j].Rotate(axis, angle);
////节点旋向限制
//if (j == 0) {
// //第一时间对Y轴旋转夹一个Clamp
// bones[0].localEulerAngles = new Vector3(0, Mathf.Clamp(bones[0].localEulerAngles.y, -60.0f, 130.0f), 0);
// //这里如果节点没错的话，那么 x z local旋转肯定是0，不用 bones[0].localEulerAngles.x/.z 这样写
//}
//else if (j == 1 || j == 2) {
// bones[j].localEulerAngles = new Vector3(0, Mathf.Clamp(bones[j].localEulerAngles.y, 0, 120.0f), 0);
//}
//else if (j == 3) {
// bones[3].localEulerAngles = new Vector3(0, Mathf.Clamp(bones[3].localEulerAngles.y, -20.0f, 20.0f), 0);
//}
if (roclamps.Count > j && roclamps[j].onClamp) { //根据clamps表单进行关节限制
bones[j].localEulerAngles = new Vector3(0, Mathf.Clamp(bones[j].localEulerAngles.y, roclamps[j].minro, roclamps[j].maxro), 0);
}
}
}
//操作机械臂节点匹配
//机械臂直接匹配
//gameObject.transform.rotation = pbone.rotation;
//for (int i = 0; i < depth; ++i)
// arms[i].rotation = bones[i].rotation;
//设置状态 Update 插值匹配
if (IsCatch && Vector3.Distance(p, boneEndPoint.position) > 0.1f) {//受Clamp影像，放置目标时，预先判断能否放到指定位置
//如果放不到 Reset机械臂
catchTar.Rbdy.isKinematic = false;//刚体恢复
catchTar.Cld.enabled = true;
catchTar = null;
isCatch = false;
animator.SetTrigger(resetHash);//爪子恢复默认状态
armState = ArmState.stati;
ccdReset();//重置机械臂姿态
Debug.Log("无法完成放置！！！");
}
else {//正常开启Update旋转的初始化
armState = ArmState.rotate;
nowTime = 0;
armRotation[0] = gameObject.transform.rotation;//记录承台旋量
for (int i = 1; i <= depth; ++i)
armRotation[i] = arms[i - 1].localRotation;
}
}
public void ccdReset() //复位
{
if (armState == ArmState.rotate) return;//如果正在旋转应忽略
//对准承台
pbone.rotation = gameObject.transform.rotation;
//骨骼复位
for (int i = 0; i < depth; ++i)
bones[i].localRotation = baseRotation[i];
//设置状态 Update 插值匹配
armState = ArmState.rotate;
nowTime = 0;
armRotation[0] = gameObject.transform.rotation;//记录承台旋量
for (int i = 1; i <= depth; ++i)
armRotation[i] = arms[i - 1].localRotation;
}
float lerp;
private void Update()
{
if (!onPhyCheck&&armState == ArmState.rotate) {//正在旋转匹配的
nowTime += Time.deltaTime;
if (nowTime >= rotateTime) { //匹配已经完成
//完成匹配 对齐姿态
gameObject.transform.rotation = pbone.rotation;
for (int i = 0; i < depth; ++i)
arms[i].localRotation = bones[i].localRotation;
//根据 isCatch 进行下一步操作
if (catchTar) { //有夹取目标需要进行下一步操作
if (isCatch) { //应该释放夹取目标
//catchTar.parent = null;//释放目标
catchTar.Rbdy.isKinematic = false;//刚体恢复
catchTar.Cld.enabled = true;
catchTar = null;
isCatch = false;
animator.SetTrigger(resetHash);//爪子恢复默认状态
armState = ArmState.stati;
ccdReset();//重置机械臂姿态
}
else {//应该完成夹取动作
//夹取动画&物理检测
animator.SetTrigger(closeHash);//执行合抓动画
onPhyCheck = true;//开启物理检测
}
}else
armState = ArmState.stati;//不需要下一步 重置状态为静止
}
else { //正在旋转匹配
lerp = nowTime / rotateTime;
gameObject.transform.rotation = Quaternion.Slerp(armRotation[0], pbone.rotation, lerp);
for (int i = 1; i <= depth; ++i)
arms[i - 1].localRotation = Quaternion.Slerp(armRotation[i], bones[i - 1].localRotation, lerp);
//抓取目标的位置跟随匹配
if (isCatch && catchTar) {
catchTar.transform.RotateAround(transform.position, roVec, Time.deltaTime*cproAngle);
catchTar.transform.position = catchPoint.position + delVec;
}
}
}
}
private void OnTriggerStay(Collider other) //碰撞到夹取目标物体
{
if (!isCatch&&onPhyCheck&&other.CompareTag("obj")&&catchTar.Cld == other) {
isCatch = true;
onPhyCheck = false;
animator.SetTrigger(stopHash);
armState = ArmState.stati;
catchTar.Rbdy.isKinematic = true;//刚体静态化
}
}
public void catchCheck() //Close执行到最后 是否夹取到
{
if (onPhyCheck && !isCatch) { //抓取失败
isCatch = false;
catchTar = null;
onPhyCheck = false;
animator.SetTrigger(resetHash);
armState = ArmState.stati;
ccdReset();
Debug.Log("夹取失败！！！");
}
}
}
实现机械臂，在核心上只要有 IK 计算动画的思想就行
留言
張貼留言