简单机械臂逻辑

　　最近有些需求, 要做机械臂的表现, 用动画来做可以解燃眉之急, 不过长远来看还是需要通过逻辑来做的...

　　其实有一些软件已经有求解器, 可以计算出相关数据, 不过需要在外部计算或者导入相关DLL, 需要长期调试, 只能简单找一些逻辑先应付着.

　　找到个 CyclicCoordinateDescent 类似于IK的计算方法, 其实就是贪心算法, 在这基础上修改了一些逻辑, 就成了.

　　首先是给机械臂的可旋转节点添加相关信息, 包括可旋转范围设定, 旋转轴设定, 旋转速度限制设定等.

　　然后通过反向计算旋转角, 就能找到合适的旋转了.

　　这里旋转的计算因为限制了旋转轴, 所以逻辑上首先获取机械臂[旋转点]到机械臂[锚点]的向量, 然后获取机械臂[旋转点]到[目标点]的向量, 然后将它们投影到旋转点的本地坐标系里的旋转平面上, 这样就得出当前点需要旋转到目标向量的角度.

using System.Collections;
using System.Collections.Generic;
using UnityEngine;

namespace CCD
{
    public enum RotateAxis
    {
        Local_X = 0,
        Local_Y,
        Local_Z,
        EndPoint,
    }

    public class CCD_Controller : MonoBehaviour
    {
        [SerializeField]
        public Transform Target;

        [SerializeField]
        public bool work = true;

        [SerializeField]
        public List<CCD_Root> Joints = new List<CCD_Root>();

        [SerializeField]
        [Header("运行时自动读取节点")]
        public bool autoLoadRoots = true;

        [SerializeField]
        [Header("渐进参数值")]
        [UnityEngine.Range(2, 5)]
        public float approachValue = 2f;

        [SerializeField]
        [Header("贪心测试")]
        public bool greedCheck = true;

        const float Epsilon = 0.001f;

        private void Awake()
        {
            if(autoLoadRoots)
            {
                Joints.Clear();
                Joints.AddRange(GetComponentsInChildren<CCD_Root>());
            }
        }

        private void Update()
        {
            if(false == work)
            {
                return;
            }
　　　　　　　// .......

        }


        [Header("编辑器变量")]
        public bool ShowGizmos = true;
        public float GizmosSize = 1f;

        private void OnDrawGizmos()
        {
            if(ShowGizmos)
            {
                var c = Gizmos.color;
                if(Joints != null && Joints.Count > 1)
                {
                    var endPoint = Joints[Joints.Count - 1];
                    if(endPoint)
                    {
                        Gizmos.color = Color.cyan;
                        Gizmos.DrawSphere(endPoint.transform.position, GizmosSize);

                        for(int i = Joints.Count - 2; i >= 0; i--)
                        {
                            var currentPoint = Joints[i];
                            if(currentPoint)
                            {
                                Gizmos.color = Color.gray;
                                Gizmos.DrawSphere(currentPoint.transform.position, GizmosSize);
                                Gizmos.color = Color.green;
                                Gizmos.DrawLine(endPoint.transform.position, currentPoint.transform.position);
                                Gizmos.color = Color.blue;
                                Gizmos.DrawLine(Target.position, currentPoint.transform.position);
                            }
                        }
                    }
                }
                if(Target)
                {
                    Gizmos.color = Color.red;
                    Gizmos.DrawSphere(Target.position, GizmosSize);
                }
                Gizmos.color = c;
            }
        }
    }
}

  

　　这里有几个地方有问题, 这里用机械臂的夹子作为[锚点]:

　　1. 贪心测试 greedCheck, 旋转某个节点之后, 机械臂的锚点是否离原来的节点更远了, 其实这种情况是正常的, 因为机械臂的臂长不能改变, 必定需要曲线救国, 不管是正确的求解还是错误的求解, 都会有这种情况发生, 如果进行贪心测试, 就在每次旋转过后决定是否要还原旋转前的姿态即可.

　　2. 当前的旋转角度的获取返回的角度不一定在原始的范围区间, 比如-90度可能返回的270度630度等等, 需要把范围限定回去.

　　3. approachValue 这个作为一个旋转判定, 在某个节点的旋转时, 有可能原始旋转角度非常小, 可是投影到本地旋转平面的时候变得非常大, 如下:

      

　　第一张就是原始旋转角, 并不大, 可是如果将两个向量投影到本地坐标平面上的话, 可能就成为图2中的样子, 角度变得很大...所以做一个判断, 当转换后的角度比原始角度还大了 approachValue 倍的时候, 判断这个节点的旋转并不是一个很好的选择, 直接跳过它. 而实际如果没有这个判定, 在旋转中心点(上图的球)离两个旋转目标点的投影位置很近的时候, 机械臂角度发生突变, 如果限制了旋转速度, 它就会一直转个不停...

 

 　　最终效果: