UnityVR--机械臂场景11-简单流水线应用3

一. 前言

二. 设置一个定时器

三. 添加机械臂事件

一. 前言

上一篇使用了DoTween插件，并且改写了事件的相关参数，本篇将实现在三自由度逆向解算3的基础上，使用发送事件的方式，实现机械臂的自动抓取、放置、回零，所有动作都通过逆向解算来完成。但在测试过程中也产生了不少问题，比如机械臂抖动，节奏不协调等，通过调试之后解决了部分问题，后期会继续测试，也大神请批评指正。

扫描二维码关注公众号，回复： 16758223 查看本文章

以下是机械臂控制代码Robot_MoveEvent.cs，代码比较长，分段解释。

二. 设置一个定时器

由于流水线涉及到很多部分需要相互通信，时常会有不协调的情况，因此先设定一个定时器，调节一下等待的时间：

  if (timer < waittime)
      timer += Time.deltaTime;
  else
  {
     //做一些事情
     timer = 0;
     return;
  }

三. 添加机械臂事件

在Start（）中定义一个机械臂事件，监听者为本类，回调OnArmCtrl函数：

EventManager.Instance.AddEvent(EventType.OnArmCtrl, this, OnArmCtrl);

四. 机械臂控制函数OnArmCtrl

在这个函数中，根据传递的数据data不同，判断机械臂执行什么状态的动作，现在定义了回零、取工件和放置工件3个动作。

public void OnArmCtrl(EventDataBase data)
{//接收事件传输数据
    EventDataArm eventdata = data as EventDataArm;
    EArmMoveType armtype = eventdata.armType;
    Transform armTarget = eventdata.target;
    EClawType clawType = eventdata.clawType;
    float waittime = eventdata.waitTime;

    if (armtype == EArmMoveType.Home) //回零时发送手爪打开事件
        ArmHome(new EventDataClaw { clawType=EClawType.Open,waitTime=0});

    //if (armtype == EArmMoveType.Up) ArmUp(upPlace);  //移动到安全高度
    if (armtype == EArmMoveType.Get)
    {//去抓取物体时发送手爪关闭事件
      ArmDown(armTarget,new EventDataClaw { clawType=EClawType.Close,waitTime=waittime});
    }
    if (armtype == EArmMoveType.Put)
    {//启用定时器，推迟打开手爪的时间
      if (timer < waittime)
          timer += Time.deltaTime;
      else
      {
        ArmPutObject(putPlace,new EventDataClaw {clawType=EClawType.Open,waitTime=0.5f});
        timer = 0;
        return;
      }
    }
}

五. 定义上面的3个机械臂移动方法

其中使用的逆解函数IKCaculator()和之前三自由度逆向解算3中一样，抄一下。如果在项目中经常需要使用到IK函数，也可以将之写到一个static的工具类中。

    public void ArmDown(Transform target,EventDataBase data)
    {//机械臂做抓取动作，target为hand的目标位置
        armDown = IKCaculator(target, armDown);
        armDown[3] = 90;    //手爪根部T_Axis_6再旋转90度
        MoveJoints(armDown, 0,data) ;
    }
    public void ArmHome(EventDataBase data)
    {//机械臂回零，存储位置的数组在一开始就已经定义
        MoveJoints(armHome, 0,data);
    }

    public void ArmUp(Transform upPlace,EventDataBase data)
    {//机械臂抬起位置
        armUp = IKCaculator(upPlace, armUp);
        armUp[3] = -90;
        MoveJoints(armUp, 0,data);
    }

    public void ArmPutObject(Transform place, EventDataBase data)
    {//机械臂放置物体,根据放置物体的位置逆向求解
        armPut = IKCaculator(place, armPut);
        armPut[3] = -90;
        MoveJoints(armPut, 0,data);
    }

六. 机械臂各关节转动控制

上一篇中的测试脚本Test.cs的MoveJoints函数，添加一个回调事件，也就是在所有关节旋转到位时，发送一个手爪事件，让手爪张开或闭合

    public void MoveJoints(float[] angle, int i,EventDataBase data)
    {//遍历所有活动关节
        joints[i].SetAngle(angle[i], () =>
        {
            if (i == joints.Length-1)
            {
                EventManager.Instance.SendEvent(EventType.OnClawCtrl, data);
                return ;
            }
            i++;
            MoveJoints(angle, i,data);
        });
    }

七. 场景实现

将这个机械臂控制代码挂在机械臂的根节点上，设置相应的旋转节点，还需要设置一个放置工件的PutPlace，可以在场景中定义一个空节点表示。如下图：

八. 完整代码

代码有点长，本来想搞个下载的链接，无奈不知道怎么弄，就简单粗暴全部贴上吧。

using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
//作用：ABB机械臂加上机械爪，使用事件出发抓取动作
//Base：S_Axis_1
//Arm0：L_Axis_2
//Arm1：U_Axis_3
//Hand:T_Axis_6
//putPlace：放置位置
public class Robot_MoveEvent : MonoBehaviour
{
    public Transform Base, Arm0, Arm1, Hand,putPlace;
    public Robot_JointCtrl[] joints;  //数组存储需要控制的各个关节
    float[] armHome = { 0, 0, 0, 0,0 };  //机械臂零位
    float[] armUp= { 0, 0, 0, 0,0 };     //设置一个机械臂抬起的位置
    float[] armDown = { 0, 0, 0, 0, 0, 1 };  //数组记录机械臂下降坐标
    float[] armPut = { 0, 0, 0, 0, -1, 0 };   //数组记录机械臂放置物体的坐标
    private float L1, L2;  //从Base开始计算的手臂长
    private float sita_L1, sita_L2;//L1、L2初始位置的偏移角
    private Vector3 V_L1, V_L2;    //从Arm0开始计算的手臂长
    private float timer = 0;

    void Start()
    {
        L1 = Vector3.Distance(Arm0.position, Arm1.position);
        L2 = Vector3.Distance(Arm1.position, Hand.position);
        V_L1 = Arm1.position - Arm0.position;
        V_L2 = Arm1.position - Hand.position;
        sita_L1 = Vector3.Angle(V_L1, new Vector3(0, V_L1.y, V_L1.z));  //L1相对于yz平面的初始偏移角
        sita_L2 = Vector3.Angle(V_L2, new Vector3(0, V_L2.y, V_L2.z));//L2相对于yz平面的初始偏移角
        EventManager.Instance.AddEvent(EventType.OnArmCtrl, this, OnArmCtrl);
    }
    public void OnArmCtrl(EventDataBase data)
    {
        EventDataArm eventdata = data as EventDataArm;
        EArmMoveType armtype = eventdata.armType;
        Transform armTarget = eventdata.target;
        EClawType clawType = eventdata.clawType;
        float waittime = eventdata.waitTime;

        if (armtype == EArmMoveType.Home) //回零时发送手爪打开事件
            ArmHome(new EventDataClaw { clawType=EClawType.Open,waitTime=0});
        //if (armtype == EArmMoveType.Up) ArmUp(upPlace);  //设置移动到安全高度
        if (armtype == EArmMoveType.Get)
        {
            ArmDown(armTarget,new EventDataClaw { clawType=EClawType.Close,waitTime=waittime});
        }
        if (armtype == EArmMoveType.Put)
        {
            Debug.Log("准备调用放置方法");
            if (timer < waittime)
                timer += Time.deltaTime;
            else
        {
        ArmPutObject(putPlace,new EventDataClaw {clawType=EClawType.Open,waitTime=0.5f});
        timer = 0;
        return;
        }
        }
    }
    public void ArmDown(Transform target,EventDataBase data)
    {//机械臂做抓取动作，target为hand的目标位置
        armDown = IKCaculator(target, armDown);
        armDown[3] = 90;    //手爪根部T_Axis_6再旋转90度
        MoveJoints(armDown, 0,data) ;
    }
    public void ArmHome(EventDataBase data)
    {//机械臂回零
        Debug.LogError("开始回零");
        MoveJoints(armHome, 0,data);
    }

    public void ArmUp(Transform upPlace,EventDataBase data)
    {//机械臂抬起位置
        armUp = IKCaculator(upPlace, armUp);
        armUp[3] = -90;
        MoveJoints(armUp, 0,data);
    }

    public void ArmPutObject(Transform place, EventDataBase data)
    {//机械臂放置物体,根据放置物体的位置逆向求解
        Debug.Log("放置物体");
        armPut = IKCaculator(place, armPut);
        armPut[3] = -90;
        MoveJoints(armPut, 0,data);
    }

    public void MoveJoints(float[] angle, int i,EventDataBase data)
    {//遍历所有活动关节
        joints[i].SetAngle(angle[i], () =>
        {
            if (i == joints.Length-1)
            {
                EventManager.Instance.SendEvent(EventType.OnClawCtrl, data);
                return ;
            }
            i++;
            MoveJoints(angle, i,data);
        });
    }

    //适合ABB400的IK计算
    float[] IKCaculator(Transform target, float[] Scara)
    {
        float sita_1, sita_2, sita_3, sita_T, X, Y, Z, W, A, x_plus, y_plus, z_plus;
        //计算目标Target和第一个关节Base距离的三个分量X、Y、Z
        X = target.position.x - Base.transform.position.x;
        Y = target.position.y - Base.transform.position.y;
        Z = target.position.z - Base.transform.position.z;
        //计算目标Target和Arm0距离的三个分量x_plus、y_plus、z_plus
        x_plus = target.position.x - Arm0.position.x;
        y_plus = target.position.y - Arm0.position.y;
        z_plus = target.position.z - Arm0.position.z;
        //计算W和A，在图中为W'和A'
        W = Mathf.Sqrt(square(x_plus) + square(z_plus));
        A = Mathf.Sqrt(square(W) + square(y_plus));  //A的长度=根号（W平方+Y平方）
        sita_T = Mathf.Acos(W / A);  //辅助角T
        sita_1 = Mathf.Acos((square(L1) + square(W) + square(y_plus) - square(L2)) / (2 * L1 * A)) + sita_T;
        sita_2 = Mathf.Acos((square(W) + square(y_plus) - square(L1) - square(L2)) / (2 * L1 * L2));
        sita_3 = Mathf.Atan2(X, Z);

        sita_1 *= Mathf.Rad2Deg;
        sita_2 *= Mathf.Rad2Deg;
        sita_3 *= Mathf.Rad2Deg;

        Scara[0] = sita_3-90;    //ABB机械臂的误差排除
        Scara[1] =-(90- sita_1+sita_L1);
        Scara[2] = -(sita_2-sita_L2);

        return Scara;
    }
    static float square(float f)
    {
        return f * f;
    }
}