Posted 2024-08-01Updated 2024-09-28Notea minute read (About 192 words)

Robotic Arm Digital Twin(DT)

机械臂 DT 需要的功能：

有和现实机械臂相似的模型外观
有碰撞箱
有基本的关节旋转控制脚本
接受路径规划单元传来的控制数据

模型外观

三个模型资产，分别从

导入。

碰撞箱

主要由 capsule collider 构成。整个 DT 的 layer 属于 RobotArm

关节控制脚本

挂载在 robotarm 物体上，用于控制各个关节旋转的角度

其中 RotateDir 为一次性配置，Joint 会自动检测进行配置

控制时只需要更改 Rotate 的值即可（runtime）

Posted 2024-08-01Updated 2024-09-28Note3 minutes read (About 425 words)

IK Path Planning

IK Solver

用于将关键点数据转化为关节动作（各关节旋转角度）

希望机械臂的末端始终竖直。方便夹取。

Unity IK: https://github.com/2000222/Robotic-Arm-IK-in-Unity

自定义自由度，非常好用，基于关节梯度迭代的方法

由这个 IK 算法负责机械臂前三个 joint 的旋转，确保 j4 的 anchor（红点）处于物体后上方的特定位置。

j4 位置的计算方法：

public Vector3 GetPositionForJ4(Vector3 pos)
    {
        Vector3 forward = pos - Joints[0].transform.position;
        forward.y = 0;
        float angle = Mathf.Acos(0.1148f / forward.magnitude);
        Vector3 left = Quaternion.AngleAxis(-angle * 180f/Mathf.PI, Vector3.up) * forward;
        left = left.normalized * 0.1148f;
        Vector3 offset = forward - left;
        offset = offset.normalized * 0.1175f;
        return pos + Vector3.up * 0.2446f - offset;
    }

之后由向量计算得出 j4 应该旋转的角度，以确保末端竖直。

j5 不旋转，j6 的旋转角度由物体朝向决定。

Router

Route

一条路径由若干个关键点构成，在构建路径时只需输入若干 turning point，就会自行生成路径中的关键点。

public class KeyPoint
{
    public Vector3 pos = Vector3.zero;
    public bool grab = false;
    public bool wait = false;
    public KeyPoint(Vector3 pos, bool grab = false, bool wait = false)
    {
        this.pos = pos;
        this.grab = grab;
        this.wait = wait;
    }
}

public class Route
{
    public List<KeyPoint> routes = new List<KeyPoint>();
    public Route(List<KeyPoint> turningPoints)
    {
        if (turningPoints.Count < 2)
        {
            return;
        }
        for (int i = 1; i < turningPoints.Count; i++)
        {
            KeyPoint start = turningPoints[i-1];
            KeyPoint end = turningPoints[i];
            float dist = (start.pos - end.pos).magnitude;
            int sigment = (int)(dist * 20f);
            for (int j = 0; j <= sigment; j++)
            {
                routes.Add(new KeyPoint(start.pos + ((float)j) / sigment * (end.pos - start.pos), start.grab));
            }
        }
    }
}

IK 只需要获取关键点的坐标信息就可以生成各个关节旋转的数据。

1	InverseKinematics(GetPositionForJ4(position));