Main Content

このページの翻訳は最新ではありません。ここをクリックして、英語の最新版を参照してください。

衝突メッシュを使用したマニピュレーターの自己衝突のチェック

この例では、URDF ソース フォルダーにある衝突メッシュを使用してマニピュレーターの自己衝突をチェックする方法を説明します。以下の関連する例では、衝突メッシュを定義する他の方法と、環境衝突をチェックする方法を説明しています。

KUKA IIWA ロボット表現の作成

KUKA® IIWA-14 シリアル マニピュレーター用のrigidBodyTreeオブジェクトを作成します。

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

剛体ツリー用の衝突メッシュの作成

剛体ツリーの各リンクについて、衝突メッシュの配列を生成します。これらのリンクは、ベースおよび剛体ツリー内の n 個のボディ用として n+1 行の cell 配列に保存されます。各行は、衝突オブジェクト、および関連する剛体ツリーの原点とそのオブジェクトとの関係を示す変換の 2 つの要素をもちます。

collisionArray = cell(iiwa.NumBodies+1,2);

この配列を埋める方法はいくつかあります。マニピュレーターの衝突チェック用の衝突オブジェクトの作成で、剛体ツリーのリンクについて衝突オブジェクトを生成する 3 つの異なる方法を説明しています。IIWA マニピュレーターは固有の衝突メッシュを URDF ディレクトリに用意しているため、この例ではメッシュ パスにある情報から衝突メッシュを作成します。

rigidBodyTree オブジェクトを作成して、衝突メッシュを含むフォルダーのパスを指定します。次の構文は、剛体オブジェクトと STL メッシュ ファイルをそれらの指定名に基づいて関連付けています。

collisionMeshPath = fullfile(matlabroot,'toolbox','robotics',...
        'robotexamples','robotmanip','data','iiwa_description',...
        'meshes','iiwa14','collision');
iiwaCollision = importrobot('iiwa14.urdf','MeshPath',collisionMeshPath);
iiwaCollision.DataFormat = 'column';

ソース ディレクトリからの各剛体について、衝突メッシュの定義を作成します。

  • STL を読み取って各 STL ファイルを面と頂点に変換する: stldata = stlread(file.STL)

  • 頂点から collisionMesh を作成する: mesh = collisionMesh(stldata.Points);

  • メッシュを cell 配列 collisionArray の (i+1) 番目の要素に入れることにより、メッシュを i 番目のボディに割り当てる。ベース メッシュは cell 配列の最初の要素を占有します。

% For each body, read the corresponding STL file
robotBodies = [{iiwaCollision.Base} iiwaCollision.Bodies];
for i = 1:numel(robotBodies)
    if ~isempty(robotBodies{i}.Visuals)
        % As a simplifying assumption, assume that the first visual is the 
        % only associated collision mesh.
        visualDetails = robotBodies{i}.Visuals{1};
        
        % Extract the part of the visual that actually specifies the STL name
        visualParts = strsplit(visualDetails,':');
        stlFileName = visualParts{2};
        
        % Read the STL file
        stlData = stlread(fullfile(collisionMeshPath,stlFileName));
        
        % Create a collisionMesh object from the vertices
        collisionArray{i,1} = collisionMesh(stlData.Points);
        
        % For imported meshes, the origin of the imported mesh and the
        % rigid body origin are the same
        collisionArray{i,2} = eye(4);
    end
end

軌跡の生成と衝突のチェック

開始と終了のコンフィギュレーションを指定して、これらを接続するジョイント空間の軌跡を求めます。

% Define collision-free start & goal configurations
startConfig = [0 -pi/4 pi 3*pi/2 0 -pi/2 pi/8]';
goalConfig = [0 -pi/4 pi 3*pi/4 0 -pi/2 pi/8]';

% Define a trapezoidal velocity trajectory between the two configurations
q = trapveltraj([startConfig goalConfig],100,'EndTime',3);

この出力軌跡に自己衝突が含まれないことを検証するには、出力サンプルを反復して衝突する点の有無を確認します。衝突チェックの実行に使用される例の補助関数は、2 つの for ループを使用して各ボディをその他すべてのボディに対して検証します。ここでは次の具体的な仮定が考慮されます。

  • ボディはジョイント位置で隣のボディと常に接触しているため、隣のボディに対するチェックは行われません。ジョイントの範囲は、そのすぐ隣にあるボディとの衝突を防ぐように設定されると仮定します。

  • すべてのボディは、他の各ボディに対して 1 回のみチェックされます (つまり、ボディ 1 についてボディ 5 との衝突がチェックされますが、その逆は行われません)。

ロボットに自己衝突と、衝突のペアの m 行 2 列の行列 collisionPairIdx がある場合、例はフラグ isInCollision を true として出力します。この行列の要素は、関連する衝突メッシュの cell 配列 collisionArray 内のボディに衝突をマッピングするインデックスです。

isConfigInCollision = false(100,1);
configCollisionPairs = cell(100,1);
for i = 1:length(q)
    [isConfigInCollision(i),configCollisionPairs{i}] = exampleHelperManipCheckSelfCollisions(iiwaCollision,collisionArray,q(:,i),false);
end

計画された軌跡が一連の衝突を通ります。最初の衝突が起きるときのコンフィギュレーションを可視化し、ボディを強調表示します。

any(isConfigInCollision)
ans = logical
   1

firstCollisionIdx = find(isConfigInCollision,1);

% Visualize the first configuration that causes problems
figure;
show(iiwaCollision,q(:,firstCollisionIdx));
exampleHelperHighlightCollisionBodies(iiwaCollision,configCollisionPairs{firstCollisionIdx},gca);

衝突のない軌跡の生成

開始のコンフィギュレーションでは、この最初の衝突が実際に発生します。ジョイント位置がその範囲を超えて指定されているためです。wrapToPi を呼び出して、ジョイントの開始位置を制限します。

新しい軌跡を生成し、衝突を再びチェックします。軌跡の部分を可視化するには、show 関数呼び出しのコメントを解除します。

newStartConfig = wrapToPi(startConfig);
q = trapveltraj([newStartConfig goalConfig],100,'EndTime',3);

isConfigInCollision = false(100,1);
configCollisionPairs = cell(100,1);
for i = 1:length(q)
    [isConfigInCollision(i),configCollisionPairs{i}] = exampleHelperManipCheckSelfCollisions(iiwaCollision,collisionArray,q(:,i),false);
%     if rem(i,10) == 0
%         show(iiwaCollision,q(:,i));
%         drawnow
%     end
end

軌跡全体をチェックした結果、衝突は見つかりませんでした。

any(isConfigInCollision)
ans = logical
   0