MATLABでURDFを参照して順運動学計を計算したい【学習#4プログラミング】
ロボットアームは、複数の関節軸をもち、人の腕のような動きをするロボットです。複数の関節軸の回転角度により、最終的なロボットアームの先端の位置と姿勢が決まるため、軸の数が多くなるほど、入力した関節軸の角度からロボットの動作を想像することは困難になります。
順運動学は、関節軸の角度からロボットアームの手先の位置と姿勢を算出する計算です。２つの記事に分けて順運動学計算を行うプログラムの作成についてまとめました。
今回は、MATLABのToolboxを使用した計算方法をまとめます。次回は、Toolboxを使用せずに計算する関数を実装します。
追記: Toolboxを使用せずに計算
1. 順運動学の計算が「簡単な場合」と「難しい場合」
順運動学計算について簡単に紹介します。
【簡単な場合】
関節軸のが２個の場合を考えます。以下の２つのサイトに２リンクロボットアームのモデル化方法が載っています。
上記のサイトのように、２つの軸が同一平面上にある場合は中高生でも算出できるほどの計算で手先の位置を算出することができます。
【難しい場合】
工場の自動化に用いられるようなロボットアーム（gifのような構造）は、関節軸を６個以上もち、軸が３次元空間に配置されるため、上記のような単純なモデル化により順運動学を計算することはできません。
順運動学の計算のためには４×４行列の積によって座標変換を繰り返し行うことで計算します。詳細については書籍や以下のYouTubeの動画で勉強することをお勧めします。
順運動学の計算は行列で記述できるため、プログラミングと相性が良いです。そのため、さまざまな順運動学や逆運動学（手先の位置と姿勢から関節軸の角度を算出）用のソルバーが存在します。
その１つがMATLABのRobotics System Toolboxです。以降はToolboxを使った順運動学の計算をまとめます。
2. Toolboxを使用した順運動学計算の実装
メリットとデメリット
まず、Toolboxを使うメリットとデメリットについてです。
【メリット】
プログラミングで特定の方式に従ってロボットアームの構造を定義することで、順運動学だけでなく、自作が困難な逆運動学や衝突検出、シミュレーションによる可視化が容易にできます。
【デメリット】
追加で使用料を払う必要があります。商用利用や企業が利用する場合はけっこうかかる？ また、計算アルゴリズムの内容を勉強しなくても使えてしまうので、ただ使うだけでは知識が習得しにくく、ロボットの動作計画に関する論文などを読む際に少し困ってしますこともあります。
個人的には、デメリットを考えても、初めてロボットを触る人や専門の人以外にとっては比較的容易にシミュレーションができるというメリットは大きいと思います。特に、MATLABはToolboxを使うことで自分が用意したロボットで計算ができる点は非常に便利です！
自分が用意したロボットを定義する方法
Robotics System Toolboxでさまざまな計算をするためにはロボットを定義する必要があります。Toolboxでロボットで定義する方法は大きく３つあります。上から順に、設定が簡単な順です。
① loadrobot 関数を使用する
MATLABが事前に用意しているデータを読み込んで使用します。Toolboxを練習したい場合や、このモデルの中に自分が使いたいロボットがある場合は、この方法で十分です。
② importrobot 関数を使用する
独自のロボット モデルを Unified Robot Description Format (URDF) ファイル、XML Macros (Xacro) ファイル、Simulation Description Format (SDF) ファイルを使用する場合、つまり①のようにMATLABで用意されているロボットの中に使用したいロボットが無い場合はこちらの方法となります。
③ 剛体ツリーモデルを１から設定する
rigidBodyTree は Robotics System Toolbox における剛体とジョイントの表現で、ロボットをツリーモデルとして定義します。ロボットの軸やリンクの構造のパラメータ、メッシュモデルによってロボットを定義します。②ではURDFファイルなどをインポートすることで、③の作業の手間を無くしています。
以上の３つがありますが、基本的に②の方法がオススメです。
「自分が用意したロボット=自分で設計したロボット」であるため、①のモデルは使用できず、③はかなり面倒になります。②はCADデータなどから、URDFファイルを生成する手間はありますが、③と比較すると圧倒的に楽です。
したがって、今回は②の方法で実際にロボットを設定して順運動学の計算をしてみます。
importrobot 関数を使用してロボットを定義して順運動学を計算する
【① URDFファイルを用意する】
今回は自分で作成せずにフリーで使えるURDFファイルを利用しました。下記の記事で紹介されてたgitからダウンロードできます。「iiwa7 KUKA」のURDFを使用しました。
【② 作業用フォルダを作成してURDFファイル、MATLABファイルを置く】
gitからダウンロードした「iiwa7 KUKA」のフォルダ、プログラムを書くMATLABのファイルを置く。今回は「test_urdf.mlx」という名前で作成した。
【③ URDFファイルをMATLABに読み込み、順運動学を計算する】
最初にURDFファイルを読み込み、ロボットに取り付けるツール先端位置を定義します。その後、関節軸角度を設定して、順運動学を計算します。以下がそのコードです。
clear
%URDFモデルで記述されたロボットの読込み
robot = importrobot('iiwa7.urdf','DataFormat','column')
show(robot)
% ツールの取付け
body9 = rigidBody('tool');
jnt9 = rigidBodyJoint('jnt8','fixed');
tform9 = [0 0 0.208 0];
setFixedTransform(jnt9,tform9,"mdh");
body9.Joint = jnt9;
addBody(robot,body9,'iiwa_link_ee');
% 関節軸角度の下限上限
disp('逆運動学計算時のジョイント制限の確認')
for i = 1:numel(robot.Bodies)
joint = robot.Bodies{i}.Joint; % ジョイントオブジェクトの取得
if ~strcmp(joint.Type, 'fixed') % 固定ジョイントを除外
fprintf('Joint Name: %s\n', joint.Name); % ジョイント名の表示
disp('Position Limits:'); % 制限のラベルを表示
disp(rad2deg(joint.PositionLimits)); % ジョイントの制限を表示
fprintf('\n');
end
end
% 角度を入力して順運動学を計算
desiredConfig = deg2rad([60 50 40 -50 60 -50 50]'); % 7つの関節の角度を設定
%show(robot, desiredConfig);
endEffectorName = 'tool'; % ツールの名前を指定
endEffectorTransform = getTransform(robot, desiredConfig, endEffectorName)%手先の位置と姿勢を取得各処理について簡単に説明します。
%URDFモデルで記述されたロボットの読込み
robot = importrobot('iiwa7.urdf','DataFormat','column')
show(robot)この部分は、importrobot 関数を使用してURDFファイルを読み込みます。ここで、変数 robot は rigidBodyTree 構造にもとづいてロボットが定義されます。show 関数で読み込んだロボットを３次元で表示できます。
% ツールの取付け
body9 = rigidBody('tool');
jnt9 = rigidBodyJoint('jnt8','fixed');
tform9 = [0 0 0.200 0];
setFixedTransform(jnt9,tform9,"mdh");
body9.Joint = jnt9;
addBody(robot,body9,'iiwa_link_ee');この部分は、ツールに応じてツール先端位置をDHパラメータにもとづいて定義します。ここでDHパラメータとは、ロボットをプログラミング上で定義するために一般的に使われる表現方法です。
DHパラメータの表現は、文献やソフトウェアによって修正DHパラメータと修正でないDHパラメータがあるため、どちらが採用されているかには気を付ける必要があります。setFixedTransform 関数では 'mdh' と指定することで修正DHパラメータで使うことができます。
ロボットは、rigidBodyTree 構造の記述に従って定義されます。robot を開くと図のようになっており、Bodies にはロボットのリンクごとの情報を確認できます。
Bodiesを開くと、図のようになっています。ツール取付けのコードを実行しない場合（URDFを読み込んだだけ）、１～８番目までが定義されています。追加したツールは９番目に格納されます。
８番目を開くと図のようになっており、リンクの名前やジョイントの種類の定義などが設定されていることを確認できます。
addBody(robot,body9,'iiwa_link_ee');ここでコードに戻ると、addBody 関数でツールのボディを追加するときに、ツールをどのボディに追加するかを定義するために、ロボットの先端の部分である８番目のボディの名前 'iiwa_link_ee' を定義しています。
tform9 = [0 0 0.200 0];この部分は、ツール座標系のZ軸方向に200 mmオフセットした位置をツール先端位置と定義しています。Z軸方向に0 mmの場合と比較すると、画像のような違いがあります。
% 関節軸角度の下限上限
disp('逆運動学計算時のジョイント制限の確認')
for i = 1:numel(robot.Bodies)
joint = robot.Bodies{i}.Joint; % ジョイントオブジェクトの取得
if ~strcmp(joint.Type, 'fixed') % 固定ジョイントを除外
fprintf('Joint Name: %s\n', joint.Name); % ジョイント名の表示
disp(rad2deg(joint.PositionLimits)); % ジョイントの制限を表示
fprintf('\n');
end
endこの部分は、URDFで定義されている関節軸角度の回転角度の下限と上限を表示させる部分です。rigidBodyTree 構造で定義することで、このように定義したロボットの詳細を確認することができます。
% 角度を入力して順運動学を計算
desiredConfig = deg2rad([60 50 40 -50 60 -50 50]'); % 7つの関節の角度を設定
show(robot, desiredConfig);
endEffectorName = 'tool'; % ツールの名前を指定
endEffectorTransform = getTransform(robot, desiredConfig, endEffectorName)%手先の位置と姿勢を取得順運動学を計算する部分です。まず、desiredConfigに軸角度構成を設定します。今回のロボットは７軸なので７変数あります。
show(robot, desiredConfig)desiredConfigのときのロボットの状態を表示します。サンプルコードの場合は以下のようになります。
endEffectorName = 'tool'; % ツールの名前を指定
endEffectorTransform = getTransform(robot, desiredConfig, endEffectorName)%手先の位置と姿勢を取得getTransform 関数はdesiredConfigのときのツール位置と姿勢の値を取得します。このときに得られる値は、endEffectorName に対応する座標系（関節軸の位置やツール付近などDHパラメータで定義される各座標系）の同次変換行列となります。今回はツール先端の位置と姿勢なので endEffectorName に 'tool' としています。実際に得られる値は以下のようになりました。
以上がURDFを読み込んで、順運動学を計算する基本的な流れです。順運動学のアルゴリズムをあまり知らなくても、値を得ることができますが、座標系の考え方やDHパラメータの定義を知らないと使いこなすことは難しいと思います。
3. さいごに
今回はMATLABの Robotics System Toolbox を使用して、URDFファイルを読み込んで順運動学の計算をしました。
URDFファイルさえあれば簡単に計算ができて便利ですが、実はちょっとした問題もあります。最適化計算などの反復計算の中に今回の順運動学計算を使って複数の関節軸構成のときの手先の位置と姿勢を算出したときの計算時間は以下のようになります。
対して、自分で実装した順運動学の関数の計算時間は以下のようになります。
結果からわかるように、Toolbox のほうは計算時間が10倍ほど長いです。もし、計算時間を少しでも短くしたい場合は自作したほうが良いということになります。（Robotics System Toolbox で getTransform 関数、もしくは別の関数で複数の関節軸角度構成のときにforループで毎回呼び出さずに計算できる方法があれば知りたいです。）
次回は、Robotics System Toolbox を使わずに計算する関数を作成します。実際に書くことで座標系の考え方やDHパラメータの理解も深まると思います。
以上！！！！
【前の記事】
【次の記事】