本稿では，OpenRCF ver2.3 の Robot.cs 内に重力補償を実装します．具体的には，各関節に質量を設定すると，それらの重みによって各関節に生じるトルクを計算できるようにします．計算で求めた自重トルクと，実際のサーボから得られたトルクの差を活用することで，重力補償ができるようになります．

OpenRCFは下記のサイトからダウンロードできます．
https://mase.openrcf.site/

Robot.cs内のKinematicsクラスとPairクラスに，いくつかの変数と関数を追加します．以下に新たに追加するコードを示します．

Robot.cs

public class Kinematics
{
    public BlockVector tauSelf;
    private BlockVector mg;
    private static float g = 9.80665f;
    private BlockMatrix Y;        

    public Kinematics(uint[] DOF)
    {
        tauSelf = new BlockVector(DOF);     
        Y = new BlockMatrix(DOF, DOF);
        mg = new BlockVector(DOF);     
    }

    private void UpdateRegressorMatrix()
    {           
        for (uint m = 0; m < Chain[0].DOF; m++)
        {
            for (uint j = 0; j <= m; j++)
            {
                if (Chain[0].Pair[j].IsRevolute || Chain[0].Pair[j].IsParallelA)
                {
                    Y[0, 0][j, m] = -Chain[0].Pair[j].axis.TimesCross(Chain[0].Pair[m + 1].p.Minus(Chain[0].Pair[j].p.Get))[2];
                }
                else if (Chain[0].Pair[j].IsPrismatic)
                {
                    Y[0, 0][j, m] = -Chain[0].Pair[j].axis[2];
                }
                else
                {
                    Y[0, 0][j, m] = 0;
                }
            }
        }

        for (uint i = 1; i < Chain.Length; i++)
        {
            for (uint m = 0; m < Chain[i].DOF; m++)
            {
                for (uint j = 0; j < Chain[0].DOF; j++)
                {
                    if (Chain[0].Pair[j].IsRevolute || Chain[0].Pair[j].IsParallelA)
                    {
                        Y[0, i][j, m] = -Chain[0].Pair[j].axis.TimesCross(Chain[i].Pair[m + 1].p.Minus(Chain[0].Pair[j].p.Get))[2];
                    }
                    else if (Chain[0].Pair[j].IsPrismatic)
                    {
                        Y[0, i][j, m] = -Chain[0].Pair[j].axis[2];
                    }
                    else
                    {
                        Y[0, i][j, m] = 0;
                    }
                }

                for (uint j = 0; j <= m; j++)
                {
                    if (Chain[i].Pair[j].IsRevolute || Chain[i].Pair[j].IsParallelA)
                    {
                        Y[i, i][j, m] = -Chain[i].Pair[j].axis.TimesCross(Chain[i].Pair[m + 1].p.Minus(Chain[i].Pair[j].p.Get))[2];
                    }
                    else if (Chain[i].Pair[j].IsPrismatic)
                    {
                        Y[i, i][j, m] = -Chain[i].Pair[j].axis[2];
                    }
                    else
                    {
                        Y[i, i][j, m] = 0;
                    }
                }
            }
        }
    }

    public void CalcTauSelf()
    {
        UpdateRegressorMatrix();

        for(uint i = 0; i < Chain.Length; i++)
        {
            for (uint j = 0; j < Chain[i].DOF; j++)
            {
                mg[i][j] = g * Chain[i].Pair[j].mass;
            }
        }

        tauSelf.Set = Y.Times(mg.Get);
    }

}


public class Pair
{        
    public float mass = 0.2f;
}

ここで計算しているリグレッサ行列 $\boldsymbol{Y(q)}$ は，重力項のみが考慮されています．また，実際にはリンクと関節の両方に質量がありますが，ここではリンクの質量もまとめて関節にあると見なして計算を行っています．質量の単位は kg ，トルクの単位は Nm です．

MainWindow.xaml.cs内での使用例を以下に示します．ロボットアームを作成するコードは略記してるので，ロボットの作成方法については公式の説明書をご覧ください．

MainWindow.xaml.cs

private void Button1_Click(object sender, RoutedEventArgs e)
{
    Robot[0, 0].mass = 0.3f;
    Robot[0, 1].mass = 0.3f;
    Robot[0, 2].mass = 0.3f;

    Robot.Kinematics.CalcTauSelf();
    Robot.Kinematics.tauSelf.ConsoleWrite();
}

上記のプログラムでは，ボタン1を押すと関節に質量が設定され，それらの重みによって生じる自重トルク（tauSelf）が計算されます．

これを7軸のロボットアームに対して実行すると，以下のような結果が得られます．