Unityでロボットアームの順運動学

Unityでロボットアームを作り、順運動学の計算をしてみました（最終的には逆運動学をやりたいので、その前段階として）。Matrix4x4クラスで各関節の同次変換行列を作り、チェーンルールを利用してシンプルに計算することができました。

ロボットアームの構造

なるべく簡単なモデルで試したかったので、自由度は3としました。リンクは入れ子構造にすることで、各軸のローカル座標系の回転に合わせてその先のリンクが動きます。

初期姿勢は、各軸のローカル座標系がワールド座標系と平行となるようにしました。

C#スクリプト

各軸の角度をpublicで宣言して、UnityのInspectorビューから入力できるようにしました。処理の流れは、

1．入力された角度で各軸のTransformを更新し、アームを動かす。
2．入力された角度で順運動学の計算を行う。
3．Transformの値と順運動学の計算結果を表示する（Transformと計算結果が一致すれば成功）。

using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class RobotControllerScript : MonoBehaviour {
    //ロボット制御
    public Transform axis1_tf, axis2_tf, axis3_tf;      //各軸のTransform
    public float axis1_angle, axis2_angle, axis3_angle; //各軸の角度（スクリプトの入力）
    private Quaternion axis1_rot, axis2_rot, axis3_rot; //Transform更新用のクオータニオン

    //順運動学
    private Matrix4x4 T01, T12, T23;    //同次変換行列
    private Vector4 hand_pos_world;     //アーム先端のワールド座標(順運動学で算出する)
    private Vector4 hand_pos_local;     //最後の関節の座標系での、アーム先端のローカル座標

    //UI
    public UnityEngine.UI.Text transform_label;
    public UnityEngine.UI.Text forward_K_label;
    public Transform hand_transform;

    void Start () {
        //同次変換行列を初期化
        T01 = Matrix4x4.identity;
        T12 = Matrix4x4.identity;
        T23 = Matrix4x4.identity;

        //最後の関節の座標系での、アーム先端のローカル座標を設定（これは変化しない）
        hand_pos_local = new Vector4(0f, 3f, 0f, 1f);
    }

    void FixedUpdate () {
        //////ロボット制御//////
        
        //スクリプトに入力された関節角度をロボットに反映
        axis1_rot = Quaternion.AngleAxis(axis1_angle, Vector3.up);
        axis2_rot = Quaternion.AngleAxis(axis2_angle, Vector3.right);
        axis3_rot = Quaternion.AngleAxis(axis3_angle, Vector3.right);

        axis1_tf.localRotation = axis1_rot;
        axis2_tf.localRotation = axis2_rot;
        axis3_tf.localRotation = axis3_rot;

        //////順運動学//////

        //スクリプトに入力された関節角度で、同次変換行列を更新
        T01.SetTRS(new Vector3(0f, 1f, 0f),
                    Quaternion.AngleAxis(axis1_angle, Vector3.up),
                    new Vector3(1f, 1f, 1f));

        T12.SetTRS(new Vector3(0f, 1f, 0f),
                    Quaternion.AngleAxis(axis2_angle, Vector3.right),
                    new Vector3(1f, 1f, 1f));

        T23.SetTRS(new Vector3(0f, 3f, 0f),
                    Quaternion.AngleAxis(axis3_angle, Vector3.right),
                    new Vector3(1f, 1f, 1f));

        //順運動学の計算
        hand_pos_world = T01 * T12 * T23 * hand_pos_local;

        //UIに表示
        transform_label.text = "Transform X:" + hand_transform.position.x.ToString("F2") +
                        " Y:" + hand_transform.position.y.ToString("F2") +
                        " Z:" + hand_transform.position.z.ToString("F2");

        forward_K_label.text = "Forward K X:" + hand_pos_world.x.ToString("F2") +
                        " Y:" + hand_pos_world.y.ToString("F2") +
                        " Z:" + hand_pos_world.z.ToString("F2");

    }
}

  

実行結果

表示の意味は、
・Transform：アーム先端のTransform.positionを表示
・Forward K：順運動学の計算結果を表示

両者は一致しており、順運動学の計算を正しく行えていることがわかります。