目录
一. 前言
二. 设置一个定时器
三. 添加机械臂事件
四. 机械臂控制函数OnArmCtrl
五. 定义上面的3个机械臂移动方法
六. 机械臂各关节转动控制
七. 场景实现
八. 完整代码
一. 前言
上一篇使用了DoTween插件，并且改写了事件的相关参数，本篇将实现在三自由度逆向解算3的基础上，使用发送事件的方式，实现机械臂的自动抓取、放置、回零，所有动作都通过逆向解算来完成。但在测试过程中也产生了不少问题，比如机械臂抖动，节奏不协调等，通过调试之后解决了部分问题，后期会继续测试，也大神请批评指正。
以下是机械臂控制代码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;
}
}
![[QT编程系列-1]：C++图形用户界面编程，QT框架快速入门培训 - 0- 总述](https://img-blog.csdnimg.cn/a1cce0d0c8554a60bf3494ca2239a0bd.png)
