二维逆运动学 – 代码 - IPS99技术分享

标签：length0 length2 代码 float 运动学 Mathf 二维 position length1

介绍

在本系列的前一部分中，我们讨论了具有两个自由度的机械臂的反向运动学问题；如下图所示。

在这种情况下，机械臂的长度 $c$ 和 $一个$ 通常是已知的。如果我们必须到达的点是 $C$ ，那么配置就变成了一个三角形，其中所有边都是已知的。

然后，我们推导出了角度和 $一个$ $B$ 的方程，它控制着机械臂关节的旋转：

(1) $\begin{equation*} A = \underset{\alpha}{\underbrace{\cos^{-1}{\left（\frac{b^2+c^2-a^2}{2bc}\right）}}} + \underset{A'}{\underbrace{\tan^{-1}{\left（\frac{C_Y-A_Y}{C_X-A_X}\right）}}} \end{equation*}$

(2) $\begin{equation*} B = \pi - \underset{\beta}{\underbrace{\cos^{-1}{\left（\frac{a^2 + c^2 -b^2}{2ac}\right）}}} \end{equation*}$

乍一看，它们可能看起来相当吓人;另一方面，从上图来看，它们的几何解释应该非常直观。

创建机械臂

实施此解决方案的第一步是创建机械臂。“关节”的概念不是Unity附带的。但是，可以利用引擎提供的育儿系统来创建与机械臂完全相同的组件层次结构。

这个想法是使用

游戏对象

对于每个关节，因此旋转其变换将导致连接到它的臂也旋转。将第二个关节与第一个关节联系起来将导致它们旋转，如第一张图所示。

生成的层次结构将变为：

根
- 关节 A
  - 骨 A
  - 关节 B
    - 骨B
    - 手

然后，我们可以将脚本添加到名为SimpleIK，这将负责旋转关节以达到所需目标。

 1 using System.Collections;
 2 using UnityEngine;
 3 
 4 namespace AlanZucconi.IK
 5 {
 6     public class SimpleIK : MonoBehaviour
 7     {
 8         [Header("Joints")]
 9         public Transform Joint0;
10         public Transform Joint1;
11         public Transform Hand;
12 
13         [Header("Target")]
14         public Transform Target;
15 
16         ...
17     }
18 }

在本教程的前一部分中推导的方程需要知道前两块骨头（分别称为 $c$ 和 $一个$ ）的长度。由于骨骼的长度不应该改变，因此可以在

开始功能。然而，这要求手臂在游戏开始时处于良好的配置。

1 private length0;
2 private length1;
3 
4 void Start ()
5 {
6     length0 = Vector2.Distance(Joint0.position, Joint1.position);
7     length1 = Vector2.Distance(Joint1.position, Hand.position  );
8 }

旋转关节

在显示代码的最终版本之前，让我们从一个简化的版本开始。如果我们将等式（1）和（2）直接转换为代码，我们最终会得到这样的结果：

 1 void Update ()
 2 {
 3     // Distance from Joint0 to Target
 4     float length2 = Vector2.Distance(Joint0.position, Target.position);
 5 
 6     // Inner angle alpha
 7     float cosAngle0 = ((length2 * length2) + (length0 * length0) - (length1 * length1)) / (2 * length2 * length0);
 8     float angle0 = Mathf.Acos(cosAngle0) * Mathf.Rad2Deg;
 9 
10     // Inner angle beta
11     float cosAngle1 = ((length1 * length1) + (length0 * length0) - (length2 * length2)) / (2 * length1 * length0);
12     float angle1 = Mathf.Acos(cosAngle1) * Mathf.Rad2Deg;
13 
14     // Angle from Joint0 and Target
15     Vector2 diff = Target.position - Joint0.position;
16     float atan = Mathf.Atan2(diff.y, diff.x) * Mathf.Rad2Deg;
17 
18     // So they work in Unity reference frame
19     float jointAngle0 = atan - angle0;    // Angle A
20     float jointAngle1 = 180f - angle1;    // Angle B
21 
22     ...
23 }

数学函数 $cos^{-1}$ 和 $tan^{-1}$ 在 Unity 中称为 Mathf.Acos和Mathf.Atan2，此外，最终角度也转换为度数Mathf.Rad2Deg，由于Transform组件接受度数，而不是弧度。

瞄准无法实现的目标

虽然上面的代码似乎有效，但存在失败的情况。如果无法访问目标，会发生什么情况？目前的实施没有考虑到这一点，导致了不良行为。

一种常见的解决方案是将手臂完全伸展到目标方向。这种行为与我们试图模拟的伸展运动是一致的。

下面的代码通过检查与根部的距离是否大于手臂的总长度来检测目标是否遥不可及。

 1 void Update ()
 2 {
 3     float jointAngle0;
 4     float jointAngle1;
 5 
 6     float length2 = Vector2.Distance(Joint0.position, Target.position);
 7 
 8     // Angle from Joint0 and Target
 9     Vector2 diff = Target.position - Joint0.position;
10     float atan = Mathf.Atan2(diff.y, diff.x) * Mathf.Rad2Deg;
11 
12     // Is the target reachable?
13     // If not, we stretch as far as possible
14     if (length0 + length1 < length2)
15     {
16         jointAngle0 = atan;
17         jointAngle1 = 0f;
18     }
19     else
20     {
21         float cosAngle0 = ((length2 * length2) + (length0 * length0) - (length1 * length1)) / (2 * length2 * length0);
22         float angle0 = Mathf.Acos(cosAngle0) * Mathf.Rad2Deg;
23 
24         float cosAngle1 = ((length1 * length1) + (length0 * length0) - (length2 * length2)) / (2 * length1 * length0);
25         float angle1 = Mathf.Acos(cosAngle1) * Mathf.Rad2Deg;
26 
27         // So they work in Unity reference frame
28         jointAngle0 = atan - angle0;
29         jointAngle1 = 180f - angle1;
30     }
31 
32     ...
33 }

旋转关节

现在剩下的就是旋转关节。这可以通过访问关节的Transform组件的localEulerAngles属性，不过很遗憾，无法直接更改角度，因此需要复制、编辑和替换矢量。

1 Vector3 Euler0 = Joint0.transform.localEulerAngles;
2 Euler0.z = jointAngle0;
3 Joint0.transform.localEulerAngles = Euler0;
4 
5 Vector3 Euler1 = Joint1.transform.localEulerAngles;
6 Euler1.z = jointAngle1;
7 Joint1.transform.localEulerAngles = Euler1;