unity 机械臂控制（二）——计算定点定位 抓取物体

unity 机械臂控制（二）

先看实现效果

工程文件下载地址：点击链接

需要解决的问题

1. 机械臂的基本控制
2. 抓取判断
3. 机械臂姿态的自动调整

一、机械臂的基本控制

可以参见上一篇文章《unity 机械臂控制（一）》地址

二、抓取的判断

关于碰撞检测

这里开始的思路是想通过Unity 的Bounds包围盒，做碰撞检测的，但是须要做一个实时判断的方法，所以后面选用了OnTriggerEnter检测，这样就需要给机械爪和抓取物体添加碰撞。

注意：碰撞体的isTrigger需要勾选

using UnityEngine;

public class GrabObjcet : MonoBehaviour
{
    Bounds my_Bounds;
    void OnTriggerEnter(Collider collision)
    {
        if (!JxbControler.Instance.Ishold)
        {
        //这里把抓取物体的tag设置成Player，避免误抓。
            if (collision.gameObject.tag=="Player")
            {
                ClawControl.Instance.HoldObjct(collision.gameObject);
            }
        }
    }
}

关于机械爪控制

机械爪的控制很简单，它有三个状态，开、关和制动，还有两种情况分别是得到物体和放下物体。

using UnityEngine;

public class ClawControl : MonoBehaviour
{
    public static ClawControl _instance;
    public static ClawControl Instance
    {
        get
        {
            if (null == _instance)
            {
                _instance = FindObjectOfType(typeof(ClawControl)) as ClawControl;
            }
            return _instance;
        }
    }
    public Transform Claw1; // 爪1
    public Transform Claw2;// 爪2
    public float Speed;//开合速度
   
    GameObject my_holdObjet;//抓取物体
    float minAngle = -25;//机械爪最小角度
    float maxAngle = 25;//机械爪最大角度
    bool isMove = false;
    enum ClawState { open, close, stop }
    ClawState my_clawState;
    float my_AngleSpeed = 0;
    private ClawState My_clawState
    { get { return my_clawState; }
        set {
            my_clawState = value;
            if (My_clawState== ClawState.open)
            {
                isMove = true;
                if (Speed < 0)
                {
                    Speed *= -1;
                }
            }
            else if (My_clawState == ClawState.close)
            {
                isMove = true;
                if (Speed > 0)
                {
                    Speed *= -1;
                }
            }
            else
            {
                isMove = false;
            }
        }
    }
    private void Update()
    {
        if (isMove)
        {
            if (my_AngleSpeed > maxAngle)
            {
                my_AngleSpeed = maxAngle;
                isMove = false;
            }
            else if (my_AngleSpeed < minAngle)
            {
                my_AngleSpeed = minAngle;
                Debug.Log("没有抓到");
                OpenClaw();
                JxbControler.Instance.ControlerUpMove();
            }
            else
            {
                my_AngleSpeed += Time.deltaTime * Speed;
            }
            Claw1.localRotation = Quaternion.AngleAxis(my_AngleSpeed, Vector3.forward);
            Claw2.localRotation = Quaternion.AngleAxis(-my_AngleSpeed, Vector3.forward);
        }
   //机械爪测试
        //if (Input.GetKeyDown(KeyCode.B))
        //{
        //    My_clawState = ClawState.open;
        //}
        //if (Input.GetKeyDown(KeyCode.C))
        //{
        //    My_clawState = ClawState.close;
        //}
        //if (Input.GetKeyDown(KeyCode.D))
        //{
        //    My_clawState = ClawState.stop;
        //}
    }
    /// 

    /// 打开机械爪
    /// 
    public void  OpenClaw()
    {
        My_clawState = ClawState.open;
    }
    /// 

    /// 闭合机械爪
    /// 
    public void CloseClaw()
    {
        My_clawState = ClawState.close;
    }
    /// 

    /// 制动机械爪
    /// 
    public void StopClaw()
    {
        My_clawState = ClawState.stop;

    }
    /// 

    /// 得到物体
    /// 
    /// 
    public void HoldObjct(GameObject Object)
    {
        StopClaw();
        my_holdObjet = Object;
        Object.transform.SetParent(transform);
        JxbControler.Instance.Ishold = true;
        JxbControler.Instance.ControlerUpMove();
    }
    /// 

    /// 放下物体
    /// 
    public void GiveUpObjct()
    {
        OpenClaw();
        my_holdObjet.transform.SetParent(null);
        JxbControler.Instance.Ishold = false;
        JxbControler.Instance.ControlerUpMove();
    }
}

三、机械臂姿态的自动调整

这里解决的问题是，如何通过终点位置计算机械臂的姿态。

这里可以拆分成两个问题，水平面的旋转和垂直面角度的变化。

水平面的旋转

看过《unity 机械臂控制（一）》就知道，在设计机械臂旋转的时候，分为了水平旋转也就是Y的角度变化，旋转轴是向上的（Vector3.up），水平面的旋转只会控制机械爪自身的一个旋转。这里只需要得到旋转点到终点的向量就可以解决。

机械臂控制部分代码

            //旋转点到终点的向量
            Vector3 vector = pos - transform.GetChild(0).position;
            //角度赋值
            place0[1] = Quaternion.LookRotation(vector).eulerAngles.y;
            place1[1] = place0[1];

垂直面的旋转

垂直面的旋转比较复杂如图

A→B→C是机械臂的三个支点，C点是机械爪的位置，也是终点。AB和BC是机械臂的臂长，和A点和C点的坐标都是已知的。我们要求的就是B的坐标、a角、b角。

已知通过余弦公式 求出角b等于
$$b=arccos\left(\frac{A{B}^2+A{C}^2-B{C}^2}{2\times AB\times AC}\right)$$
角a同理。（180 - ΔABC ）

    //余弦公式
    float GetAngle(float across, float side1, float side2)
    {
        float angle = Mathf.Acos(((side1 * side1) + (side2 * side2) - (across * across)) / (2 * side2 * side1)) * Mathf.Rad2Deg;
        return angle;
    }

机械爪的偏移问题

因为终点其实并不是实际终点，还有一个机械爪的偏移量在里面，所以还要减去机械爪的臂长。机械爪也有一个旋转量，理论上可以选择垂直于抓取物和平行于抓取物，本文选择的是平行于抓取物。

机械爪抓取保持水平的方法

这里通过计算出的拐点坐标和实际的终点坐标，得到拐点到终点的向量，用该向量和水平面求夹角，从而求出机械爪的角度，这的角度计算用到了上图中，B点的坐标和C点的坐标。即旋转角度等于向量BC和水平面的夹角。这里有个注意点，向量有可能是朝向上和朝向下的，朝上时角度应该反向。

机械臂控制完整代码

using System;
using UnityEngine;
public enum RotateType
{
    X,
    Y,
    Z
}
public class JxbControler : MonoBehaviour
{
    public static JxbControler _instance;
    public static JxbControler Instance
    {
        get
        {
            if (null == _instance)
            {
                _instance = FindObjectOfType(typeof(JxbControler)) as JxbControler;
            }
            return _instance;
        }
    }
    public bool Ishold;
    private void Start()
    {
        Ishold = false;
    }



    public float arm1Long;
    public float arm2Long;
    public Vector3 offset;

    public JxbPoint[] JxbPoints;
    //旋转的数据
    //下参考坐标
    float[] place0 = { 0, 0, 120, -26, 0, 0, 0 };
    //起参考坐标
    float[] place1 = { 0, 0, 0, 0, 0, -30, 0 };

    /// 

    /// 移动机械臂
    /// 
    /// 
    /// 
    /// 下抓是返回方法
    void MoveJxb(float[] data, int i,Action action=null)
    {
        JxbPoints[i].SetAngle(data[i], () =>
        {
            i++;
            if (i >= JxbPoints.Length)
            {
               
                if (action!= null)
                {
                    action();
                }
            }
            else
            {
                if (action == null)
                {
                    MoveJxb(data, i);
                }
                else
                {
                    MoveJxb(data, i, action);
                }
            }
          
        });
    }
 /// 

 /// 机械臂升起
 /// 
    public void ControlerUpMove()
    {
        MoveJxb(place1, 0);
    }
    /// 

    /// 机械臂下抓
    /// 
    /// 真实终点坐标
    public void ControlerDownMove(Vector3 pos)
    {
        
        if (GetGetArmAngle(pos))
        {
            //旋转点到终点的向量
            Vector3 vector = pos - transform.GetChild(0).position;
            //角度赋值
            place0[1] = Quaternion.LookRotation(vector).eulerAngles.y;
            place1[1] = place0[1];
            MoveJxb(place0, 0, () => {
                if (Ishold)//是否机械爪上有物体
                {
                    ClawControl.Instance.GiveUpObjct();
                }
                else
                {
                    ClawControl.Instance.CloseClaw();
                }
            });
        } 
 
    }
    bool GetGetArmAngle(Vector3 pos)
    {
        //起点坐标
        Vector3 originPoint = transform.GetChild(0).GetChild(0).GetChild(0).position;
        //真实终点向量
        Vector3 realEndVec = (originPoint - pos);
        //机械爪的偏移向量 7是机械爪臂长
        Vector3 offet = new Vector3(realEndVec.x, 0, realEndVec.z).normalized * 7;
        //终点向量
        Vector3 endVec = (pos- originPoint) + offet;
        //点击点的坐标等于pos
        Vector3 clickPoint = endVec + originPoint - offet;
        //平面投影
        Vector3 projection = new Vector3(endVec.x, 0, endVec.z);
        //终点向量和平面投影的夹角
        float targrtAngle = Vector3.Angle(endVec, projection);
        //中间拐点坐标
        Vector3 midPoint = retrunVector(originPoint, endVec, arm1Long, arm2Long);

        if (endVec.magnitude< arm2Long+ arm1Long)
        {
            Debug.Log("够得着");
            place0[2] =90- GetAngle(arm2Long, arm1Long, endVec.magnitude) - targrtAngle;
          
            place0[3] = 180- GetAngle(endVec.magnitude, arm2Long, arm1Long);
          
            place0[5] = HorizontalAngles(midPoint -(endVec +originPoint)  );
            return true;
        }
        else
        {
            Debug.Log("够不着");
            return false;
        }

    }
    //余弦公式
    float GetAngle(float across, float side1, float side2)
    {
        float angle = Mathf.Acos(((side1 * side1) + (side2 * side2) - (across * across)) / (2 * side2 * side1)) * Mathf.Rad2Deg;
        return angle;
    }
    /// 

    /// 求中心拐点位置
    /// 
    /// 
    /// 
    /// 
    /// 
    /// 
    Vector3 retrunVector(Vector3 self, Vector3 target, float arm1Long, float arm2Long)
    {
        float arm3Long = target.magnitude;
        if (arm3Long < arm1Long + arm2Long)
        {
            Vector3 projection = new Vector3(target.x, 0, target.z);
            float targrtAngle = Vector3.Angle(target, projection);
            float selfAngle = targrtAngle + GetAngle(arm2Long, arm1Long, arm3Long);
            //这个夹角有可能是大与90度的，要分情况处理。
              float y = 0;
            if (selfAngle<90)
            {
                 y = Mathf.Tan(selfAngle * Mathf.Deg2Rad) * projection.magnitude;
                return new Vector3(target.x, y, target.z).normalized * arm1Long + self;
            }
            else if (selfAngle > 90)
            {
                 y = Mathf.Tan((180-selfAngle) * Mathf.Deg2Rad) * projection.magnitude;
                return new Vector3(-target.x, y, -target.z).normalized * arm1Long + self;
            }
            else
            {
                 y =  projection.magnitude;
                return new Vector3(0, y, 0).normalized * arm1Long + self;
            }

        }
        else
        {
            return Vector3.zero;
        }
    }
    /// 

    /// 求向量水平夹角
    /// 
    /// 
    /// 
    float HorizontalAngles(Vector3 vector)
    {
        Debug.Log(vector);
        Vector3 horizontalVector = new Vector3(vector.x, 0, vector.z);
        float angles = Vector3.Angle(vector, horizontalVector);
        Debug.Log(angles);
        if (vector.y > 0)
        {
            return -angles;
        }
        else
        {
            return angles;
        }

    }
}

真对《unity 机械臂控制（一）》的问题修改

在轴旋转的时候可能出现抖动情况，重新调整了扩展方法

using UnityEngine;
using DG.Tweening;
using System;

public static class TransformExtention
{
    public static void RotateX(this Transform transform, float x, float duration, Action action)
    {
        Vector3 my_EulerAngles = transform.eulerAngles;

       
        transform.DOLocalRotateQuaternion(Quaternion.AngleAxis(x, Vector3.right), duration).OnComplete(() => action());
    
      
    }
    public static void RotateY(this Transform transform, float y, float duration, Action action)
    {
        Vector3 my_EulerAngles = transform.eulerAngles;
    
            transform.DOLocalRotateQuaternion(Quaternion.AngleAxis(y, Vector3.up), duration).OnComplete(() => action());
   
    }
    public static void RotateZ(this Transform transform, float z, float duration, Action action)
    {
        Vector3 my_EulerAngles = transform.eulerAngles;

        transform.DOLocalRotateQuaternion(Quaternion.AngleAxis(z, Vector3.forward), duration).OnComplete(() => action());

    }
}

---