unity 机械臂控制（二）
先看实现效果
工程文件下载地址：点击链接
需要解决的问题
- 机械臂的基本控制
- 抓取判断
- 机械臂姿态的自动调整
一、机械臂的基本控制
可以参见上一篇文章《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;
//}
}
/// <summary>
/// 打开机械爪
/// </summary>
public void OpenClaw()
{
My_clawState = ClawState.open;
}
/// <summary>
/// 闭合机械爪
/// </summary>
public void CloseClaw()
{
My_clawState = ClawState.close;
}
/// <summary>
/// 制动机械爪
/// </summary>
public void StopClaw()
{
My_clawState = ClawState.stop;
}
/// <summary>
/// 得到物体
/// </summary>
/// <param name="Object"></param>
public void HoldObjct(GameObject Object)
{
StopClaw();
my_holdObjet = Object;
Object.transform.SetParent(transform);
JxbControler.Instance.Ishold = true;
JxbControler.Instance.ControlerUpMove();
}
/// <summary>
/// 放下物体
/// </summary>
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 = a r c c o s ( A B 2 + A C 2 − B C 2 2 × A B × A C ) b=arccos\Big(\frac{AB^2+AC^2-BC^2}{2\times AB \times AC} \Big) b=arccos(2×AB×ACAB2+AC2−BC2)
角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 };
/// <summary>
/// 移动机械臂
/// </summary>
/// <param name="data"></param>
/// <param name="i"></param>
/// <param name="action">下抓是返回方法</param>
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);
}
}
});
}
/// <summary>
/// 机械臂升起
/// </summary>
public void ControlerUpMove()
{
MoveJxb(place1, 0);
}
/// <summary>
/// 机械臂下抓
/// </summary>
/// <param name="pos">真实终点坐标</param>
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;
}
/// <summary>
/// 求中心拐点位置
/// </summary>
/// <param name="self"></param>
/// <param name="target"></param>
/// <param name="arm1Long"></param>
/// <param name="arm2Long"></param>
/// <returns></returns>
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;
}
}
/// <summary>
/// 求向量水平夹角
/// </summary>
/// <param name="vector"></param>
/// <returns></returns>
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());
}
}