OpenRCFでCRANE-X7を動かす方法

本稿では，OpenRCFでCRANE-X7を動かす方法を解説します．
OpenRCF自体は下記のサイトからダウンロードできます．
https://mase.openrcf.site/

まず，MainWindow.Xaml.csに次のコードを追加します．

MainWindow.Xaml.cs

Robot Robot = new Robot(7);

void Setup()
{           
    Robot[0, 0].lInit.Set = new float[3] { 0, 0, 0.050f };
    Robot[0, 0].axisInit.SetUnitVectorZ();
    Robot[0, 0].JointRange = new float[2] { -0.8f * PI, 0.8f * PI };
    Robot[0, 1].lInit.Set = new float[3] { 0, 0, 0.055f };
    Robot[0, 1].axisInit.SetUnitVectorY();
    Robot[0, 1].JointRange = new float[2] { -PI, PI };
    Robot[0, 1].q = -0.1f * PI;
    Robot[0, 2].lInit.Set = new float[3] { 0, 0, 0.100f };
    Robot[0, 2].axisInit.SetUnitVectorZ();
    Robot[0, 2].JointRange = new float[2] { -0.8f * PI, 0.8f * PI };
    Robot[0, 3].lInit.Set = new float[3] { 0, 0, 0.150f };
    Robot[0, 3].axisInit.SetUnitVectorY();
    Robot[0, 3].JointRange = new float[2] { 0, 0.8f * PI };
    Robot[0, 3].q = 0.65f * PI;
    Robot[0, 4].lInit.Set = new float[3] { 0, 0, 0.125f };
    Robot[0, 4].axisInit.SetUnitVectorZ();
    Robot[0, 4].JointRange = new float[2] { -0.75f * PI, 0.75f * PI };
    Robot[0, 5].lInit.Set = new float[3] { 0, 0, 0.125f };
    Robot[0, 5].axisInit.SetUnitVectorY();
    Robot[0, 5].JointRange = new float[2] { -0.5f * PI, 0.5f * PI };
    Robot[0, 5].q = 0.45f * PI;
    Robot[0, 6].lInit.Set = new float[3] { 0, 0, 0.043f };
    Robot[0, 6].axisInit.SetUnitVectorZ();
    Robot[0, 6].JointRange = new float[2] { -0.9f * PI, 0.9f * PI };
    Robot[0, 7].lInit.Set = new float[3] { 0, 0, 0.060f };

    Robot.Kinematics.ForwardKinematics();
    Robot.Kinematics.SetJointLinkRadiusAuto();
    Robot.Kinematics.ResetTargets();
}


void Draw()
{
    Robot.Draw();
}

SerialDevice.Dynamixel Dynamixel = new SerialDevice.Dynamixel();
byte[] id = new byte[8] { 2, 3, 4, 5, 6, 7, 8, 9 };

void Button1_Click(object sender, RoutedEventArgs e)
{
    Dynamixel.SetBaudRate57600();
    Dynamixel.PortOpen("COM5");
}

void Button2_Click(object sender, RoutedEventArgs e)
{
    Dynamixel.RequestPositionReply(id);      
    int[] intAngles = Dynamixel.Position(id);
            
    Robot[0, 0].q = 2 * PI * intAngles[0] / 4096 - PI;
    Robot[0, 1].q = -(2 * PI * intAngles[1] / 4096 - PI);
    Robot[0, 2].q = 2 * PI * intAngles[2] / 4096 - PI;
    Robot[0, 3].q = -(2 * PI * intAngles[3] / 4096 - PI);
    Robot[0, 4].q = 2 * PI * intAngles[4] / 4096 - PI;
    Robot[0, 5].q = -(2 * PI * intAngles[5] / 4096 - PI);
    Robot[0, 6].q = 2 * PI * intAngles[6] / 4096 - PI;
            
    Robot.Kinematics.ForwardKinematics();
}

実行すると，下図のようにロボットアームが生成されます．寸法は実際のCRANE-X7と同じになっています．

ボタン1を押すとCOMポートが開きます．もし開かない場合は，COMの番号やボーレート，非常停止ボタンが押されていないか，電源が供給されているかを確認してみてください．
ボタン2を押すと，CRANE-X7の各関節角度がシミュレータ内のロボットに反映されます．関数Dynamixel.RequestPositionReply(id)を実行した時点では，まだ関節角度を受信していない点にご注意ください（ボタン2を最初に押したときは全ての関節角度が0となる）．一定の時間(1ms未満)が経過すると，サーボから関節角度の返信がありますので，関数Dynamixel.Position(id)で取得できる関節角度が更新されます（ボタン2は２回押す必要あり）．グリッパーの角度は，intAngles[7]に格納されています．

逆運動学を計算する方法や，各サーボに角度指令値を与える方法については，公式の説明書に記載されています．