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

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

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

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

ロボットの読み込み

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

sawyer = loadrobot("rethinkSawyer",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");
title("Path Segments Plotted on L-Membrane")
hold on
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 with title Path Segments Plotted on L-Membrane contains 7 objects of type surface, line.

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

figure
show(sawyer);
hold on
title("Path Segments Plotted in Robot Workspace")
for i=1:numel(pathSegments)
    segment = pathSegments{i};
    plot3(segment(:,1),segment(:,2),segment(:,3),"x-",LineWidth=2);
end

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

逆運動学ソルバーの作成

読み込んだ 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 つの主要部分があります。

Trajectory Generation セクションは、ジョイント空間パスセグメントの行列 jointPathSegmentMatrix を受け取り、シミュレーションのタイムステップごとに MATLAB Function ブロックを使用して、セグメントをジョイント空間の一連の離散化ウェイポイント (ジョイントコンフィギュレーション) に変換します。Polynomial Trajectory ブロックは、一連のジョイントコンフィギュレーションを、ジョイント空間の平滑化された B スプライン軌跡 (時間単位) に変換します。
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 on
plot3(xPositionsEE,yPositionsEE,zPositionsEE)
grid on
title(["Smoothed End-Effector Trajectories","Plotted on L-Membrane Surface"])
hold off

Figure contains an axes object. The axes object with title Smoothed End-Effector Trajectories Plotted on L-Membrane Surface 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");
title("Sawyer Tracing L-Membrane")
% 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

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