Main Content

このページの内容は最新ではありません。最新版の英語を参照するには、ここをクリックします。

MATLAB および Simulink でのマニピュレーターによる形状のトレース

この例では、空間内に事前定義された 3 次元形状をトレースする方法を説明します。滑らかで明確なパスに従うことは、溶接、製造、検査などの多くのロボティクス アプリケーションで役立ちます。3 次元の軌跡は MATLAB® の膜をトレースするためのタスク空間で解決され、Rethink Robotics® の Sawyer ロボットを使用して実行されます。目的は、一連のウェイポイントに基づいてロボットのエンド エフェクタが従う滑らかなパスを生成することです。

MATLAB および Simulink でのマニピュレーターによる形状のトレースの例では、密に離散化された一連のセグメントを生成する方法を説明します。これらの一連のセグメントを逆運動学ソルバーに渡し、反復解法を使用して解を求めることができます。ただし、この例では、計算量を削減する代替方法を提供します。この例では、パス セグメントを少数の離散点に分割し、平滑化関数を使用してウェイポイントの間に内挿します。この方法により、より滑らかな軌跡が生成され、実行時の効率が改善されます。

ロボットの読み込み

この例では、Rethink Robotics® の Sawyer ロボットを使用します。剛体ダイナミクスを指定する URDF ファイルをインポートします。列ベクトルを使用してロボット コンフィギュレーションを定義するように DataFormat を設定します。Simulink® は列ベクトルを使用します。タスク空間の範囲は、実測データに基づいて定義します。

sawyer = importrobot('sawyer.urdf');
sawyer.DataFormat = 'column';
taskSpaceLimits = [0.5 0.75; -0.125 0.125; 0.25 0.5];
numJoints = 8; % Number of joints in robot

一連のタスク空間ウェイポイントの生成

この例の目的は、MATLAB® の膜ロゴをトレースする一連のパス セグメントを取得することです。膜の表面とパス セグメントは、補助関数 generateMembranePaths を使用して cell 配列として生成されます。表面上に重なるパスを可視化するには、surf を使用して表面をプロットし、パス セグメントの cell 配列を反復することによってパス セグメントをプロットします。numSamples を増やすと、表面上をより細かくサンプリングできます。

numSamples = 7;
[pathSegments, surface] = generateMembranePaths(numSamples, taskSpaceLimits);

% Visualize the output
figure
surf(surface{:},'FaceAlpha',0.3,'EdgeColor','none');
hold all
for i=1:numel(pathSegments)
    segment = pathSegments{i};
    plot3(segment(:,1),segment(:,2),segment(:,3),'x-','LineWidth', 2);
end
hold off

Figure contains an axes object. The axes object contains 7 objects of type surface, line.

ロボットが確実に出力をトレースできるようにするには、ロボット ワークスペース内で形状を可視化します。sawyer ロボットを表示し、同じ Figure 内に線分をプロットします。

figure
show(sawyer);
hold all

for i=1:numel(pathSegments)
    segment = pathSegments{i};
    plot3(segment(:,1),segment(:,2),segment(:,3),'x-','LineWidth',2);
end

view(135,20)
axis([-1 1 -.5 .5 -1 .75])
hold off

Figure contains an axes object. The axes object with xlabel X, ylabel Y contains 59 objects of type patch, line. These objects represent base, controller_box, pedestal_feet, pedestal, right_arm_base_link, right_l0, head, screen, head_camera, right_l1, right_l2, right_l3, right_l4, right_arm_itb, right_l5, right_hand_camera, right_l6, right_hand, right_wrist, right_torso_itb, torso, pedestal_mesh, right_arm_base_link_mesh, right_l0_mesh, head_mesh, screen_mesh, right_l1_mesh, right_l2_mesh, right_l3_mesh, right_l4_mesh, right_l5_mesh, right_l6_mesh, torso_mesh.

逆運動学ソルバーの作成

読み込んだ sawyer 剛体ツリーを使用して、逆運動学 (IK) を作成します。これは当初、初期推定としてホーム コンフィギュレーションを使用し、一様な重みのセットを指定して構成されます。初期推定をホーム コンフィギュレーションに設定し、姿勢の許容誤差を一様な重みで設定します。IK ソルバーのエンドエフェクタは、ロボットの 'right_hand' ボディです。

s=rng(0); % Capture the current seed and seed the rng to 0.
ik = inverseKinematics('RigidBodyTree', sawyer);
ik.SolverParameters.AllowRandomRestarts = false;
initialGuess = sawyer.homeConfiguration;
weights = [1 1 1 1 1 1];
eeName = 'right_hand';

逆運動学を使用したタスク空間ウェイポイントからジョイント空間への変換

逆運動学ソルバーを使用して、一連のジョイント空間ウェイポイントを生成します。これにより、生成された pathSegments の各点でロボットのジョイント コンフィギュレーションが得られます。各ジョイント空間セグメントが行列 jointPathSegmentMatrix にまとめられ、これが Simulink モデルに入力として渡されます。

% Initialize the output matrix
jointPathSegmentMatrix = zeros(length(pathSegments),numJoints,numSamples);

% Define the orientation so that the end effector is oriented down
sawyerOrientation = axang2rotm([0 1 0 pi]);

% Compute IK at each waypoint along each segment
for i = 1:length(pathSegments)
    currentTaskSpaceSegment = pathSegments{i};
    currentJointSegment = zeros(numJoints, length(currentTaskSpaceSegment));
    for j = 1:length(currentTaskSpaceSegment)
        pose = [sawyerOrientation currentTaskSpaceSegment(j,:)'; 0 0 0 1];
        currentJointSegment(:,j) = ik(eeName,pose,weights,initialGuess);
        initialGuess = currentJointSegment(:,j);       
    end
    
    jointPathSegmentMatrix(i, :, :) = (currentJointSegment);
end

Simulink モデルの読み込み

shapeTracingSawyer モデルを使用して軌跡を実行し、ロボットの運動学モデルでシミュレートします。

open_system("shapeTracingSawyer.slx")

この Simulink モデルには次の 2 つの主要部分があります。

  1. Trajectory Generation セクションは、ジョイント空間パス セグメントの行列 jointPathSegmentMatrix を受け取り、シミュレーションのタイム ステップごとに MATLAB Function ブロックを使用して、セグメントをジョイント空間の一連の離散化ウェイポイント (ジョイント コンフィギュレーション) に変換します。Polynomial Trajectory ブロックは、一連のジョイント コンフィギュレーションを、ジョイント空間の平滑化された B スプライン軌跡 (時間単位) に変換します。

  2. Robot Kinematics Simulation セクションは、平滑化された軌跡からジョイント空間のウェイポイントを受け入れて、結果として得られるロボットのエンドエフェクタ位置を計算します。

軌跡の生成

ロボット運動学のシミュレーション

Simulink でのジョイント空間の軌跡の実行

モデルをシミュレートして軌跡の生成を実行します。

sim("shapeTracingSawyer.slx")

軌跡の生成結果の表示

このモデルは、ロボットのジョイント コンフィギュレーションとエンドエフェクタ位置を、それぞれの平滑化されたパスの軌跡に沿って出力します。MATLAB プロット ツールで簡単に処理するために、データの形状を変更します。

% End effector positions
xPositionsEE = reshape(eePosData.Data(1,:,:),1,size(eePosData.Data,3));
yPositionsEE = reshape(eePosData.Data(2,:,:),1,size(eePosData.Data,3));
zPositionsEE = reshape(eePosData.Data(3,:,:),1,size(eePosData.Data,3));

% Extract joint-space results
jointConfigurationData = reshape(jointPosData.Data, numJoints, size(eePosData.Data,3));

新しいエンドエフェクタ位置を、元の膜の表面上にプロットします。

figure
surf(surface{:},'FaceAlpha',0.3,'EdgeColor','none');
hold all
plot3(xPositionsEE,yPositionsEE,zPositionsEE)
grid on
hold off

Figure contains an axes object. The axes object contains 2 objects of type surface, line.

ロボットの動作の可視化

前述の可視化に加えて、Sawyer ロボット モデルを使用してトレース動作を再作成できます。jointConfigurationData のジョイント コンフィギュレーションを反復し、show を使用してロボットを可視化して、3 次元空間でのエンドエフェクタ位置を継続的にプロットします。

% For faster visualization, only display every few steps
vizStep = 5;

% Initialize a new figure window
figure
set(gcf,'Visible','on');

% Iterate through all joint configurations and end-effectort positions
for i = 1:vizStep:length(xPositionsEE)
    show(sawyer, jointConfigurationData(:,i),'Frames','off','PreservePlot',false);
    hold on
    plot3(xPositionsEE(1:i),yPositionsEE(1:i),zPositionsEE(1:i),'b','LineWidth',3)

    view(135,20)
    axis([-1 1 -.5 .5 -1 .75])

    drawnow
end
hold off

Figure contains an axes object. The axes object with xlabel X, ylabel Y contains 253 objects of type line, patch. These objects represent base, controller_box, pedestal_feet, pedestal, right_arm_base_link, right_l0, head, screen, head_camera, right_l1, right_l2, right_l3, right_l4, right_arm_itb, right_l5, right_hand_camera, right_l6, right_hand, right_wrist, right_torso_itb, torso, pedestal_mesh, right_arm_base_link_mesh, right_l0_mesh, head_mesh, screen_mesh, right_l1_mesh, right_l2_mesh, right_l3_mesh, right_l4_mesh, right_l5_mesh, right_l6_mesh, torso_mesh.

rng(s); % restore the global state of the random number generator