CRANE-X7 Cで実装する「実践ロボット制御」 技術情報・開発日誌

Cで実装する「実践ロボット制御」[3] 〜3自由度ロボットの順運動学と逆運動学〜

Cで実装する「実践ロボット制御」[3] 〜3自由度ロボットの順運動学と逆運動学〜 CRANE-X7

 

こんにちはinukaiです!

Cで実装する「実践ロボット制御」“の連載第3回目です!
前回は、2自由度ロボットの順運動学と逆運動学について学び、プログラムの例を紹介しました。
今回は、3次元に拡張して3自由度ロボットの順運動学と逆運動学を学んでいきましょう!

垂直3自由度ロボットの運動学

それでは、早速CRANE-X7を垂直3自由度ロボットと見立てて、運動学について考えていきます。

垂直3自由度ロボットの順運動学

前回の「CRANE-X7の軸数を減らして取り扱う方法」に従って、CRANE-X7を垂直3自由度ロボットとして見立てます。「実践ロボット制御」(以降、教科書)の内容を復習しながら、順運動学を計算していきます。
下の図のように、原点をCRANE-X7の1軸目と2軸目の直交する点に、アームの正面の方向に\( X \)軸、垂直方向に\( Z \)軸をとり、Y軸は右手系となるように作業座標系をとります。CRANE-X7の1軸目の角度を\( \theta_1 \)、1軸目の角度を\( \theta_2 \)、4軸目の角度を\( \theta_3 \)、手先位置を\( \begin{bmatrix}
p_{x} \\
p_{y} \\
p_{z}
\end{bmatrix} \)として計算してみます。

原点をCRANE-X7の1軸目と2軸目の直交する点に、アームの正面の方向にX軸、垂直方向にZ軸をとり、Y軸は右手系となるように作業座標系をとる

もう少し、関節角度と手先位置の関係を図式的に見てみます。関節角度と手先位置の関係図の左は、\( l_2 \)と\( l_3 \)の二つの辺を内包する平面を切り取って見たものです。仮にこの平面の水平方向を\( X_1 \)軸、垂直方向を\( Z_1 \)軸とすると、\( \theta_2 \)、\( \theta_3 \)と、この平面上の手先位置\( \begin{bmatrix}
p_{x1} \\
p_{z1}
\end{bmatrix} \)の関係は、前回の平面2自由度ロボットの順運動学とみることができるため、\begin{align}
\begin{bmatrix}
p_{x1} \\
p_{z1}
\end{bmatrix}
=
\begin{bmatrix}
l_{2}C_{2} + l_{3}C_{23}\\
l_{2}S_{2} + l_{3}S_{23}
\end{bmatrix}
\end{align}
となります。次に、図の右のように元の作業座標系を鉛直上方向からみると、
\begin{align}
\begin{bmatrix}
p_{x} \\
p_{y}
\end{bmatrix}
=
\begin{bmatrix}
p_{x1}C_{1}\\
p_{z1}S_{1}
\end{bmatrix}
\end{align}
となることがわかります。これらの関係から、垂直3自由度ロボットの順運動学は、
\begin{align}
p_{x} &= C_{1} ( l_{2}C_{2} + l_{3}C_{23} )\\
p_{y} &= S_{1} (l_{2}S_{2} + l_{3}S_{23} )\\
p_{z} &= l_{2}S_{2} + l_{3}S_{23}
\end{align}
教科書の式(1.13)~(1.15)と同様に求めることができます。

垂直3自由度ロボットの逆運動学

先ほどの式を、手先位置から関節角度を求める式に変形して逆運動学問題を解いていきます。
まず、\( \theta_3 \)から求めていきます。前回同様、教科書と別の方法で余弦定理を用いて求めてみます。長さ\( l_2 \)、\( l_3 \)のリンクと、原点と手先位置を結んだ3つの辺からなる三角形を考えます。これを図示すると下図のようになります。

 

長さl2、l3のリンクと、原点と手先位置を結んだ3つの辺からなる三角形

余弦定理から、
\begin{align}
p_{x}^2 + p_{y}^2 + p_{z}^2 = l_2^2 + l_3^2 – 2 l_2 l_3 \cos( \pi – \theta_3)
\end{align}
\begin{align}
p_{x}^2 + p_{y}^2 + p_{z}^2 = l_2^2 + l_3^2 + 2 l_2 l_3 C_3
\end{align}
ここから、
\begin{align}
C_3 = \cos^{-1} \frac{p_x^2 + p_y^2 + p_z^2 – l_2^2 – l_3^2}{2l_2 l_3}
\end{align}
教科書の式(1.17)と同様に求めることができます。この式が解を持つ条件は、\( -1 \leq C_3  \leq 1 \) より、
\begin{align}
(l_2 – l_3 )^2 \leq p_x^2 +p_y^2 +p_z^2 \leq (l_2 + l_3)^2
\end{align}
となります。\( \theta_3 \)の解については、平面2自由度の例題と同様2つありますが、実際にはCRANE-X7の4軸目の可動範囲はリンク情報より-2.74~0 rad(-157°~0°)であることを考慮すると、今回も\( \theta_3 \)が負の解を使用することになります。

あとは、\( \theta_1 \)と\( \theta_2 \)について式変形するだけです。本記事では導出は省略して結果だけ記載いたします。詳解は教科書をご参照ください。

\begin{align}
\theta_1 = \mathrm{atan2} (\pm p_y , \pm p_x )
\end{align}
(教科書の式(1.19)より引用)、以下複合同順です。ただし、\( p_x^2 +p_y^2 = 0 \)のとき、\( \theta_1 \)は任意。

\begin{align}
\theta_2 = \mathrm{atan2} ( \mp l_3 S_3 \sqrt{ p_x^2 +p_y^2 } + ( l_2 + l_3 C_3 ) p_z, \pm( l_2 + l_3 C_3) \sqrt{ p_x^2 +p_y^2 } + l_3 S_3 P_z)
\end{align}
(教科書の式(1.20)より引用)

よって、\( p_x^2 +p_y^2 \neq 0 \)のとき、\( \theta_3 \)が2通り、\( \theta_1 \)の複合で2通りの合計4通りの解があり、\( p_x^2 +p_y^2 = 0 \)のとき\( \theta_1 \)は任意となります。

ここで、\( \theta_1 \)の解の符号をどう決めればいいか、ここで整理しておきましょう。\( \theta_1 \)の符号は、その導出過程の下記の式の符号で決まります。
\begin{align}
l_2 C_2 + l_3 C_{23} = \pm \sqrt{p_x^2 +p_y^2}
\end{align}
(教科書の式(1.18)より引用)
\( l_2 C_2 + l_3 C_{23} \)の符号により、解がどうなるかを図式的に詳しく見てみます。
\( l_2 \)と\( l_3 \)の二つの辺を内包する平面を切り取って見てみると、下図のようになります。

l2とl3の二つの辺を内包する平面

これを3次元的に見てみると下図のようになります。

3次元で見た図

4通りのうちどの解を選択するかは、今回の例では可動範囲を絞って決めてみます。先ほど、\( \theta_3 \)の解についてはCRANE-X7の可動範囲から負の解しかとることができないとしました。よって、図の下の二つとなります。また、CRANE-X7の1軸目の可動範囲はリンク情報より-2.74~2.74 rad(-157°~157°)より、\( l_2 C_2 + l_3 C_{23} \leq 0 \)の解(図右下)は、\( \theta_1 \)が\( \pi \)の姿勢をとることができないため、アームの正面を向くことができないことがわかります(一方で手首を考えたときに、手首を上に向けたいときなどは右下の姿勢が有利と考えることもできます)。今回は、図左下の解を使用することとします。よって、
\begin{align}
\theta_1 &= \mathrm{atan2} ( p_y ,  p_x ) \\
\theta_2 &= \mathrm{atan2} ( – l_3 S_3 \sqrt{ p_x^2 +p_y^2 } + ( l_2 + l_3 C_3 ) p_z, ( l_2 + l_3 C_3) \sqrt{ p_x^2 +p_y^2 } + l_3 S_3 P_z)
\end{align}
複合同順。ただし、\( p_x^2 +p_y^2 = 0 \)のとき、\( \theta_1 \)は任意(実際に使う値は、プログラムの解説で後述します。atan2の仕様を利用します)。

順運動学と逆運動学のプログラムを作成する

それでは、前回の続きからプログラムしていきましょう!

下準備

まず、robotics_from_scratch/内に”ch03″というディレクトリとその中に”build”ディレクトリを作ります。そしたら、先週の”ch02/build”内のMakefileを先程作ったディレクトリにコピーします。また、先週作成した、自作の運動学ライブラリ、”myCX7_KDL_library.h”、”myCX7_KDL_library.c”を”ch02″から”ch03″にコピーしましょう。ターミナルでの一連の作業例は下記です。

$ cd ~/robotics_from_scratch
$ mkdir -p ch03/build
$ cp ch02/build/Makefile ch03/build/Makefile
$ cp ch02/myCX7_KDL_library.h ch03/myCX7_KDL_library.h
$ cp ch02/bmyCX7_KDL_library.c ch03/myCX7_KDL_library.c

これで先週の続きから学習できます。
新規に実施する方は、先週のセットアップに従って環境構築でクローンした状態で「ch03」が存在していると思います。”ch03″の中身を一度削除して、本記事に従って一緒に作成していきましょう!

オリジナル運動学のライブラリへの3自由度運動学の追加

自作ライブラリのヘッダーファイルの編集

それでは、自作ライブラリのヘッダファイル”myCX7_KDL_library.h”に、下記を追記します。

//途中省略//
//// Functions related to kinematics ////
int forwardKinematics2Dof(VECTOR_3D *, double *);
int inverseKinematics2Dof(VECTOR_3D, double *);
int forwardKinematics3Dof(VECTOR_3D *, double *);
int inverseKinematics3Dof(VECTOR_3D, double *);
#endif

上記のように、今回作成する3自由度の順運動学と逆運動学を解く関数の定義を、前回の関数の定義の下に追加します。

自作ライブラリのソースファイルの編集

次に、実際にmyCX7_KDL_library.cを編集して、関数の中身を作っていきます。

#include <stdio.h>
#include <math.h>
#include "../common/arm_parameter.h"
#include "myCX7_KDL_library.h"
//途中省略//
int forwardKinematics3Dof(VECTOR_3D *p, double *theta)
{

    double L2 = link_parameter_3dof[1].length;
    double L3 = link_parameter_3dof[2].length;

    double S1 = sin(theta[0]);
    double C1 = cos(theta[0]);
    double S2 = sin(theta[1]);
    double C2 = cos(theta[1]);
    double S23 = sin(theta[1] + theta[3]);
    double C23 = cos(theta[1] + theta[3]);

    p->x = C1 * (L2 * C2 + L3 * C23);
    p->y = S1 * (L2 * C2 + L3 * C23);
    p->z = L2 * S2 + L3 * S23;

    return 0;
}

int inverseKinematics3Dof(VECTOR_3D p, double *theta)
{

    double L2 = link_parameter_3dof[1].length;
    double L3 = link_parameter_3dof[2].length;

    double S2, C2, S3, C3;

    //手先位置の値が可動範囲の外であればエラー値を返す
    if (((pow(L2 - L3, 2)) > (p.x * p.x + p.y * p.y + p.z * p.z)) || ((p.x * p.x + p.y * p.y + p.z * p.z) > (pow(L2 + L3, 2))))
    {
        printf("Target position is out of range.\n");
        return 1;
    }

    //1軸目theta[0]と2軸目theta[1],4軸目theta[3]以外は0 radで固定
    for (int i = 0; i < JOINT_NUM; i++)
    {
        theta[i] = 0;
    }
    //1軸目の角度
    theta[0] = atan2(p.y, p.x);
    //4軸目の角度
    C3 = (p.x * p.x + p.y * p.y + p.z * p.z - L2 * L2 - L3 * L3) / (2 * L2 * L3);
    theta[3] = -acos(C3); //acosは0:πの範囲で計算、CRANE-X7において、θ3の可動はマイナス方向のみ
    //2軸目の角度
    S3 = sin(theta[3]);
    C2 = (L2 + L3 * C3) * sqrt(p.x * p.x + p.y * p.y) + (L3 * S3) * (p.z);
    S2 = -(L3 * S3) * sqrt(p.x * p.x + p.y * p.y) + (L2 + L3 * C3) * (p.z);
    theta[1] = atan2(S2, C2);

    //得られた関節角度が可動範囲外であればエラーを返す
    for (int i = 0; i < JOINT_NUM; i++)
    {
        if (((joint_range[i].min) > (theta[i])) || ((theta[i]) > (joint_range[i].max)))
        {
            printf("Theta[ %d] is out of range.\n", i);
            return 1;
        }
    }
    return 0;
}

こちらのコードについて説明していきます。基本的に前回の2自由度の運動学の関数と同じような仕様になっています。

順運動学を計算する関数

それでは、11〜29行目の順運動学を計算する関数を見ていきます。
関数は下記のような定義とします。

  • int forwardKinematics3Dof(VECTOR_3D p, double *theta)
    • 引数:手先位置の格納先のVECTOR_3D型のポインタ *p 、および関節角度の配列のポインタ *theta
    • 戻り値:予約

引数の*pは、手先位置の計算結果を格納するVECTOR 3D型の構造体変数のポインタです。 theta は各関節角度の配列ポインタです。theta には、CRANE-X7 の第 1 関節から第 7 関節までとグリッパーの角度が順番に格納された配列変数のポインタが与えられるとします。
戻り値については、エラー処理などに使用するため予約 としていますので、必要に応じて使用してください。逆運動学問題を 解く関数を実装する場合などでは、エラー処理を実装するために戻り値を使用し ます。これは計算結果が可動範囲外の値となった場合に、その結果をロボットに指令すると予想しない動きをする可能性があるためです。一方、今回 のような順運動学問題を解くうえでは、その計算結果は直接ロボットに与える値ではないため、エラー処理を実装しません。

次に、各行の説明です。
14、15行目では、垂直3自由度ロボットのリンク長L2、L3を格納する変数を定義し、値を初期化しています。(link_parameter_3dof[1].lengthの読み込みについては、前回のパラメータの初期化関数を参照ください。)
17〜22行目は、sin や cos の値を格納する変数を定義して、値を計算しています。垂直3 自由度ロボットの θ1 は CRANE-X7 の1軸目(対応する変数:theta[0])、θ2 は CRANE-X7 の2軸目 (対応する変数:theta[1])、 θ3は CRANE-X7 の4軸目(対応する変数:theta[3])が対応しています。
24〜26行目が、実際に順運動学を解く式で、3行で済みます。これは教科書の式(1.13)~(1.15)そのものですね。変数名と数式中の記号を見やすいように置き換えていますが、ここは好みの問題なので変数の値の置き換えをしなくても問題ありません。

逆運動学を計算する関数

次に、31〜72行目の逆運動学を計算する関数を見ていきます。
関数は下記のような定義とします。

  • int inverseKinematics3Dof(VECTOR_3D p, double *theta)
    • 引数:手先位置の構造体変数 p 、および関節角度を格納する配列変数のポインタ *theta
    • 戻り値:可動範囲内であれば正常値 0、可動範囲外であればエラー値 1 を返す

手先の座標は、VECTOR 3D 型の構造体の引数として関数に渡します。各関節角度の計算結果は、配列変数のポインタを用いて引き渡しをします。この関数は、 CRANE-X7 が垂直3自由度ロボットとして動作するように、手先の x、y、z座標である p.x、p.y、p.y から、CRANE-X7 の1軸目の角度theta[0](垂直3自由度ロボットの θ1)、2軸目の角度 theta[1](平垂直3自由度ロボットの θ2)と4軸目の角度 theta[3](垂直3自由度ロボットの θ3)を計算します。その他の関節角度は 0 rad とします。
戻り値は、指定した座標、あるいは計算された関節角度が可動範囲内かどうかを判定した結果を返します。逆運動学問題を解く関数を実装する場合などでは、計算結果が可動範囲外の値となった場合にその結果をロボットに指令すると、ロボッ トが予想しない動きなどをする可能性があります。そのため、エラー処理を実装する必要があります。例えば、この関数をメイン関数から呼び出すときに戻り値でエラーを検出したらロボットを止める、プログラムを停止する、手先位置を変更して再度計算するなどの処理をすればよいです。

次に、各行の説明です。
34、35行目では、平面2自由度ロボットのリンク長L2、L3を格納する変数を定義し、値を初期化しています。順運動学の時と同様に、リンク長の変数の定義と、リンク長を変数に代入しています。
37 行目は、θ2、θ3を求めるために使用するS2、C2、S3、C3の変数定義です。
40〜44行目は、逆運動学を解く関数に与えられた手先位置が、逆運動学の解(θ3の解の求まる範囲)が存在する範囲であるか判別しています。解が存在しない範囲である場合、エラーメッセージを表示、エラー値を返して関数を終了します。
47〜50行目は、CRANE-X7の各関節の値を0で初期化しています。
52〜60行目が、逆運動学の計算になります。
52行目は教科書の式(1.19)を用いてθ1を計算してCRANE-X7の1軸目の角度を格納する配列要素theta[0]に代入しています。\( p_x^2 + p_y^2 = 0 \)のときは解が任意の値となりますが、今回はmathライブラリのatan2の仕様をそのまま使用して、このときのθ1は0とします。
52、53行目は教科書の式(1.17)を用いてθ3を計算し、CRANE-X7の4軸目の角度を格納する配列要素theta[3]に代入しています。また、可動範囲から値が負の解を選んでいます。
58〜60行目で教科書の式(1.20)を用いてθ1の解を計算し、CRANE-X7の2軸目の角度を格納する配列要素theta[1]に代入しています。
63〜70行目は、解いた逆運動学の解が、CRANE-X7の各関節の可動範囲の中にあるか確認しています。(joint_range[i]の読み込みについては、前回のパラメータの初期化関数を参照ください。)可動範囲外であれば、エラーメッセージを表示、エラー値を返して関数を終了します。
71行目、可動範囲内の関節角度の値として逆運動学の解が求まれば、戻り値に0を返して関数を終了します。

以上で、垂直3自由度の順運動学および逆運動学を求める関数を実装できたので、確認していきましょう。

動かす前に数値で確認をする

それでは、「ch03」内に「main.c」というファイルを作成して、下記の内容を書いてみましょう。
こちらは、最低限関数の用法などを確認するコードに留めています。

 
#include <stdio.h>
#include "../common/arm_parameter.h"
#include "myCX7_KDL_library.h"

int main()
{
  double theta1[JOINT_NUM] = {0.244979, 1.664740, 0.0, -2.071451, 0.0, 0.0, 0.0, 0.0};
  double theta2[JOINT_NUM] = {0};

  VECTOR_3D pos1 = {0};
  VECTOR_3D pos2 = {0.15, 0.15, 0.15};

  initParam();

  forwardKinematics3Dof(&pos1, theta1);
  inverseKinematics3Dof(pos2, theta2);

  printf("Forward kinematics result [θ1 θ2 θ3]:[%lf %lf %lf] -> [x y z]:[%lf %lf %lf]\n", theta1[0], theta1[1], theta1[3], pos1.x, pos1.y, pos1.z);
  printf("Inverse kinematics result [x y z]:[%lf %lf %lf] -> [θ1 θ2 θ3]:[%lf %lf %lf]\n", pos2.x, pos2.y, pos2.z, theta2[0], theta2[1], theta2[3]);

  return 0;
}

こちらについて簡単に説明します。
7行目に順運動学を解く関数に与える関節角度を予め用意した配列変数、8行目に逆運動学の結果の関節角度を格納する配列変数を定義しています。
10行目に順運動学を解いた結果を格納する手先位置を格納する構造体、11行目に逆運動学を解く関数に与える手先位置を予め用意した構造体を定義しています。
位置の単位は[m]、角度の単位は[rad]です。
13行目で、自作ライブラリ”myCX7_KDL_library”で使用するアームの物理パラメータを初期化します。
15、16行目で順運動学と逆運動学をそれぞれ計算します。
18、19行目で計算結果を表示しています。
それでは実行していきましょう。

コンパイルおよび実行すると下記のように、順運動学と逆運動学の計算結果が出力されます。

$ cd ~/robotics_from_scratch/ch03/build
$ make
$ ../bin/crane_x7_test
Forward kinematics result [θ1 θ2 θ3]:[0.244979 1.664740 -2.071451] ->; [x y z]:[0.200000 0.050000 0.150000]
Inverse kinematics result [x y z]:[0.150000 0.150000 0.150000] ->; [θ1 θ2 θ3]:[0.785398 1.639875 -2.048792]

実際に数値を確認すると、順運動学について教科書の式(1.13)~(1.15)、逆運動学については教科書の式(1.17)、(1.19)および(1.20)を満たしていることが確認できます。上記プログラムでは、順運動学・逆運動学それぞれ1点ずつしか確認していませんが、垂直3自由度の問題は3変数の連立方程式であったため、それぞれ最低限3点以上値を確認して関数の実装に問題がないか確認しておきましょう!また、可動範囲外ではエラーが返ってくるかどうかも確認してみましょう。
結果を確認する際には、scanfなどで値を入力したら結果を見れるようにするなど工夫してみてください。

CRANE-X7を動かして確認する

CRANE-X7を動かすメイン関数

それでは、”ch03″内の”main.c”を”main1.c”などに変更しておいて、新しい「main.c」を作成して下記の内容を書いてみましょう。
コマンド操作の場合の例は下記です。

$ cd ~/robotics_from_scratch/ch03
$ mv main.c main1.c
$ touch main.c
#include <stdio.h>
#include <unistd.h>
#include "../common/crane_x7_comm.h"
#include "myCX7_KDL_library.h"

int main()
{
  uint8_t operating_mode[JOINT_NUM] = {POSITION_CONTROL_MODE, POSITION_CONTROL_MODE, POSITION_CONTROL_MODE, POSITION_CONTROL_MODE, POSITION_CONTROL_MODE, POSITION_CONTROL_MODE, POSITION_CONTROL_MODE, POSITION_CONTROL_MODE};
  VECTOR_3D target_pos = {0};                 //目標位置格納用の変数
  VECTOR_3D target_pos1 = {0.15, 0.15, 0.15}; //目標位置1
  VECTOR_3D target_pos2 = {0.35, 0.15, 0.15}; //目標位置2
  VECTOR_3D target_pos3 = {0.15, 0.15, 0.35}; //目標位置3
  VECTOR_3D target_pos4 = {0., 0., 0.35};     //目標位置4
  VECTOR_3D present_pos = {0};                //現在位置格納用の変数

  double target_theta[JOINT_NUM] = {0};    //目標角度格納用の変数
  double present_theta[JOINT_NUM] = {0};   //現在角度を格納する変数
  double present_angvel[JOINT_NUM] = {0};  //現在速度を格納する変数
  double present_current[JOINT_NUM] = {0}; //現在トルクを格納する変数

  int state = 0; //目標位置を切り替えるための変数
  int cnt = 0;   //ループのカウント

  printf("Press any key to start (or press q to quit)\n");
  if (getchar() == ('q'))
    return 0;

  // 運動学ライブラリの初期化
  initParam();

  // サーボ関連の設定の初期化
  if (initilizeCranex7(operating_mode))
  {
    return 1;
  }
  // CRANE-X7のトルクON
  setCranex7TorqueEnable(TORQUE_ENABLE);

  target_pos = target_pos1;

  // main関数のループ
  while (cnt < 9)
  { //10回繰り返したらプログラムを終了する
    cnt++;

    if (inverseKinematics3Dof(target_pos, target_theta))
    {
      break;
    }
    setCranex7Angle(target_theta);

    sleep(3);

    getCranex7JointState(present_theta, present_angvel, present_current);
    forwardKinematics3Dof(&present_pos, present_theta);
    printf("Target position [x y z]:[%lf %lf %lf]\n", target_pos.x, target_pos.y, target_pos.z);
    printf("Present position [x y z]:[%lf %lf %lf]\n", present_pos.x, present_pos.y, present_pos.z);

    //target_angleを変更
    if (state == 0)
    {
      state = 1;
      target_pos = target_pos2;
    }
    else if (state == 1)
    {
      state = 2;
      target_pos = target_pos3;
    }
    else if (state == 2)
    {
      state = 3;
      target_pos = target_pos4;
    }
    else
    {
      state = 0;
      target_pos = target_pos1;
    }
  } //end main while

  brakeCranex7Joint(); //CRANE X7をブレーキにして終了
  closeCranex7Port();  //シリアルポートを閉じる
  return 0;
}

2行目は、sleep関数を使用できるようにするためにunistdをインクルードします。
3行目は、先週に説明した、”common”内に用意されているCRANE-X7と通信するための関数を使用できるようにしています。
4行目は、先程作成した自作のライブラリを読み込んでいます。
それではメイン関数の説明に入ります。
8行目は、CRANE-X7の動作モードを指定するための配列定義です。initilizeCranex7()関数に与える引数です。今回はモータを位置制御モードで使用するため、配列の中身はすべてPOSITION_CONTROL_MODEとします。(POSITION_CONTROL_MODEの値は”crane_x7_comm.h”内に定義されています。)
9〜22行目は、変数の定義です。これらは使用するタイミングで説明します。
24〜26行目は、プログラムを実行後にいきなりロボットが動き出さないようにキー入力を待ちます。qを入力すると終了し、それ以外の入力でプログラムを開始します。
29行目は、自作のライブラリ用のパラメータの初期化です。
32〜35行目は、CRANE-X7との通信を初期化します。引数に8行目で定義した配列を渡して、動作モードを位置制御にします。
37行目は、CRANE-X7のトルクがONになり、位置制御を開始します。CRANE-X7の関節が脱力した状態から制御がかかった状態になります。
39行目は、手先位置の指令値を初期化します。
42〜80行目が、CRANE-X7が実際に動作するプログラムです。順を追ってみていきます。
このループは9回繰り返されます。繰り返すごとに44行目で、cntをインクリメントします。
46〜49行目で、逆運動学を解いて、手先位置の指令値を関節角度の指令値に変換しています。もし逆運動学が解けない場合はwhile文を抜けてプログラムを終了します。
50行目で、逆運動学を解いた結果の関節角度指令値をCRANE-X7に送ります。
52行目は、CRANE-X7が司令した角度に動くまで待つために3秒間sleepして待ちます。
54行目は、指令した角度に動いたあとに、CRANE-X7の角度や速度、トルクといった値を取得しています。今回は角度の値だけその後に用います。
55行目で、現在角度から順運動学を計算して手先位置を計算します。
55〜56行目で、指令した手先位置と実際の手先位置にどの程度差があるかを画面に出力して確認します。
60〜79行目は、手先位置の指令値をループの繰り返し回数ごとに変化させています。
82行目は、動作が終わったら動作を終了する前にCRANE-X7をブレーキモードにします。これをすることでゆっくりとCRANE-X7が脱力します。
84行目で、CRANE-X7との通信ポートを閉じて終了します。

CRANE-X7を実際に動かして確認する

それでは、コンパイルして実行しましょう。コンパイル方法は先週と同様、”ch03/build/”ディレクトリに移動してmakeコマンドを実行します。
../bin/crane_x7_testで実行できます。
動かした動画は下記になります。このように4点を順番に手先位置が移動すれば成功です!

【Cで実装する「実践ロボット制御」】CRANE-X7を垂直3自由度ロボットとして動かす

下記に、コンパイルと実行時に必要なコマンドと、実行時にターミナルに表示される例を示します。
前回の実行結果と同様、ほぼ指定した位置に動くことができていますが、サーボのゲインがデフォルトの値だと1〜2cm程度絶対位置はずれます(特に重力方向)

$ ~/robotics_from_scratch/ch03/build
$ make
$ ../bin/crane_x7_test
//途中省略//
Target position [x y z]:[0.150000 0.150000 0.150000]
Present position [x y z]:[0.146863 0.147767 0.137844]

おまけ(手首姿勢のヒント)

前回同様、簡単に手首の姿勢を決めて動かしてみます。
例えば、垂直3自由度の逆運動学の解に6軸目の角度を指定するように、下記のような行を追加してあげれば、写真のように手首を常に下に向けるようなことができます。また、7軸目も下記の例のように、1軸目の角度θ1に応じて動くようにすれば、写真のように手首が同じ姿勢となります。このようにすれば、ピックアンドプレイスなども作れるかと思います。手の開閉はtheta[7]に値を入れてあげればできます。

#define PI (3.141592)
//途中省略//
theta[5] = -PI / 2 - theta[1] - theta[3];
theta[6] = theta[0];

手首を常に下に向ける手首を常に下に向ける

まとめ

今回は、垂直3自由度ロボットの順運動学および逆運動学をCRANE-X7を題材に解いてみました。また実際に動かすプログラムを作成しました。今回のプログラムも本ブログ連載のgithubリポジトリにアップロードされていますので、躓いたらそちらを見てみてください。

前回動かした動画や、今回の動画から、手先が目標位置に動くときに振動していたり、各関節がばらばらの時刻で目標角度に達しているのが見て取れます。次回は、これらの振動や各関節の同時発着を解決するために、目標軌道の生成をしてCRANE-X7を動かしてみます。
ではまた次回!

タイトルとURLをコピーしました