Unityを使った磁気浮上シミュレーション

Unityで、磁気浮上を模擬するプログラムを作成しました。

1. 要求事項

[1]　磁気浮上アクチュエータを２個とする。
[2]　磁気浮上アクチュエータは浮上体の下面につける。
[3]　ある位置に停止させる（定位置制御）

2. 実施事項

[1]　磁気浮上アクチュエータを子オブジェクトとする。
[2]　浮上体は親オブジェクトとして、磁気浮上アクチュエータである子オブジェクトをFixed Jointで固定する。

3. 具体的な実施事項

・親オブジェクトに追加するスクリプトから、子オブジェクトを読み出し、子オブジェクトに重力に反する力を発生させる。
・定位置制御として、位置P速度PI制御及び重力補償を加える。

動画が埋め込められないので、状況は分かりにくいですが、スクリプトのみを添付しますので、
参考になればと思います。

4. スクリプト

4.1 コード

using UnityEngine;

public class Carrier_Control : MonoBehaviour
{
    public Rigidbody carrier;
    private double f1;
    private double f2;
    private double posY_old1 = 0;
    private double posY_old2 = 0;
    private double posY_old = 0;
    private double Kp = 20;
    private double Kv = 20;
    private double Ts = 1;
    private double Ki = 0.02;
    private double M = 10;
    private double G = 9.8;
    private double Mg;
    private double velY_old = 0;
    private double velY = 0;
    private double refV = 0;
    private double refV_old = 0;

    // Start is called before the first frame update
    void Start()
    {
        Mg = M/2 * G;
    }

    // Update is called once per frame
    void FixedUpdate()
    {
        Transform myTransform = this.transform;

        Vector3 pos = myTransform.position;

        Transform mag1Transform = transform.GetChild(0).gameObject.GetComponent<Transform>();
        Transform mag2Transform = transform.GetChild(1).gameObject.GetComponent<Transform>();

        Vector3 magPos1 = mag1Transform.position;
        Vector3 magPos2 = mag2Transform.position;

        velY = (pos.y - posY_old)/Ts;
        refV = (0.45 - pos.y) * Kp;          
        f1 =  (refV-velY_old) * Kv + (refV + refV_old - velY_old) * Ki * Kv;
        f2 =  (refV-velY_old) * Kv + (refV + refV_old - velY_old) * Ki * Kv;

        Vector3 force1 = new Vector3(0.0f, (float)(f1*M/2+Mg), 0.0f);
        Vector3 force2 = new Vector3(0.0f, (float)(f2*M/2+Mg), 0.0f);

        Rigidbody mag1 = transform.GetChild(0).gameObject.GetComponent<Rigidbody>();
        Rigidbody mag2 = transform.GetChild(1).gameObject.GetComponent<Rigidbody>();

        mag1.AddForce(force1);
        mag2.AddForce(force2);

        posY_old1 = magPos1.y;
        posY_old2 = magPos2.y;
        posY_old = pos.y;
        velY_old = velY;
        refV_old = refV - velY_old;
    }
}

4.2 コードのポイント

Qiitaの参考記事にもありますが、Unityの基本はgameObjectを利用することであり、Transformクラスをコンポーネントとして取得し、さらに、位置情報を取得する点。

        Transform mag1Transform = transform.GetChild(0).gameObject.GetComponent<Transform>();
        Transform mag2Transform = transform.GetChild(1).gameObject.GetComponent<Transform>();

        Vector3 magPos1 = mag1Transform.position;
        Vector3 magPos2 = mag2Transform.position;

Rigidbodyクラスをコンポーネントとして取得し、子オブジェクトに力を発生させる点。

        Rigidbody mag1 = transform.GetChild(0).gameObject.GetComponent<Rigidbody>();
        Rigidbody mag2 = transform.GetChild(1).gameObject.GetComponent<Rigidbody>();

        mag1.AddForce(force1);
        mag2.AddForce(force2);

5. 感想

　Unityをうまく使いこなすには、GameObjectの理解が重要ですね。参考記事1
　ちなみに、データの逐次計算部分は、FixedUpdate()を利用してください。
　Update()では浮上させることができませんでした。