Simulink における逆運動学を用いたエンドエフェクタの軌跡のトレース

この例では次を使用します。

Simulink® を使用して、剛体ロボットモデルを使った逆運動学の計算を行います。ロボットのエンドエフェクタの軌跡を定義し、点をループして、この軌跡をトレースするロボットコンフィギュレーションを求めます。

URDF (Unified Robot Description Format) ファイルから、RigidBodyTree オブジェクトとしてロボットモデルをインポートします。

robot = importrobot('iiwa14.urdf');
robot.DataFormat = 'column';

ロボットを表示します。

ax = show(robot);

ロボットの軌跡を指定します。これらの "xyz" 座標は、ロボットの前に N 字型の形状を描画します。

x = 0.5*zeros(1,4)+0.25;
y = 0.25*[-1 -1 1 1];
z = 0.25*[-1 1 -1 1] + 0.75;

hold on
plot3(x,y,z,'--r','LineWidth',2,'Parent',ax)
hold off

逆運動学を実行するモデルを開きます。MATLAB® で定義されている "xyz" 座標は同次変換に変換され、望ましい Pose として入力されます。出力される逆運動学の解は、次の解の初期推定としてフィードバックされます。この初期推定は、エンドエフェクタの姿勢を追跡し滑らかなコンフィギュレーションを生成するのに役立ちます。

コールバックボタンを押すと、定義したばかりのロボットモデルと軌跡を再生成できます。

close
open_system('sm_ik_trajectory_model.slx')

シミュレーションを実行します。モデルは、エンドエフェクタの指定された軌跡を追従するロボットコンフィギュレーション (configs) を生成します。

sim('sm_ik_trajectory_model.slx')

ロボットコンフィギュレーションをループして、タイムステップごとにロボットを表示します。エンドエフェクタの位置を xyz に保存します。

figure('Visible','on');
tformIndex = 1;
for i = 1:10:numel(configs.Data)/7
    currConfig = configs.Data(:,1,i);
    show(robot,currConfig);
    drawnow

    xyz(tformIndex,:) = tform2trvec(getTransform(robot,currConfig,'iiwa_link_ee'));
    tformIndex = tformIndex + 1;
end

エンドエフェクタの最終的な軌跡を黒のラインで描画します。図では、当初定義された N 字型の形状 (赤の点線) をエンドエフェクタがトレースしていることが示されています。

figure('Visible','on')
show(robot,configs.Data(:,1,end));

hold on
plot3(xyz(:,1),xyz(:,2),xyz(:,3),'-k','LineWidth',3);
plot3(x,y,z,'--r','LineWidth',3)
hold off

Simulink における逆運動学を用いたエンドエフェクタの軌跡のトレース

参考

オブジェクト

ブロック

関連するトピック