Main Content

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

マニピュレーターの衝突チェック用の衝突オブジェクトの作成

この例では、マニピュレーターの衝突チェック用に衝突オブジェクトを作成する 3 つの異なる方法を説明します。自己衝突または環境衝突の検知をチェックする詳細な例については、以下の例を試してください。

マニピュレーターの衝突チェックの概要

関数 checkCollisions は、任意のアプリケーションまたは特定のメッシュ構成に対して汎用です。これは次のことを意味します。

  • 衝突をチェックするには、各衝突オブジェクトをプリミティブ型またはメッシュとして表現し、姿勢を指定する必要があります。

  • マニピュレーターの場合、剛体ツリーから姿勢を取得できますが、衝突オブジェクトに姿勢を割り当てなければなりません。

この例では、衝突オブジェクトとその姿勢を格納する cell 配列を作成するために、このプロセスを自動化する 2 つの例の補助関数を示します。次のオプションがあります。

  • 剛体衝突配列。1 行 2 列の cell 配列であり、各 cell にはベクトル [base robot.Bodies]i 番目のボディに対応する衝突プリミティブと、その衝突オブジェクトの原点を関連する剛体ジョイントに関係付ける変換が含まれます。

  • ワールド衝突配列。ワールド衝突オブジェクトの cell 配列。

剛体衝突配列を初期化するには、次の 3 つの方法が推奨されます。

  1. 既知のディレクトリからインポートしたメッシュを使用する。

  2. rigidBodyTree オブジェクトからメッシュを抽出する。

  3. 衝突プリミティブを使用してジオメトリを表現する。

方法 1: 既知のディレクトリ内のメッシュを使用

多くのロボットでは、付属の衝突メッシュが、Unified Robot Definition Format (URDF) ファイルに指定されています。

IIWA ロボットには一連の衝突メッシュが付属しており、これらは可視化メッシュを簡略化したバージョンです。importrobot を呼び出して、URDF ファイルから rigidBodyTree オブジェクトを生成します。コンフィギュレーション用の出力形式を "column" に設定します。

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

衝突メッシュを使用した衝突チェック ツールの作成

剛体ツリーを作成したら、剛体ツリーの各リンクについて一連の衝突メッシュを生成します。このために、rigidBodyTree オブジェクト内の n 個のボディについて n+1 の cell 配列を生成します。追加の要素にはベースとなる衝突オブジェクトが格納されます。

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

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

% Create a rigid body tree using the collision mesh path to associate rigid
% bodies with collision mesh STL files
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)
        % Assumes the first Visuals element is the correct one.
        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
        collisionArrayFromMesh{i,1} = collisionMesh(stlData.Points);
        
        % Transform is always identity
        collisionArrayFromMesh{i,2} = eye(4);
    end
end

指定されたコンフィギュレーションでの自己衝突のチェック

与えられた衝突メッシュの配列について、用意されている関数 exampleHelperManipCheckCollisions はすべてのボディを反復して衝突をチェックし、衝突するペアのインデックスを返します。

config = [0 -pi/4 pi 0.9*pi 0 -pi/2 0]';
[isCollision, selfCollisionPairIdx] = exampleHelperManipCheckCollisions(iiwaCollision, collisionArrayFromMesh, {}, config, true);
disp(isCollision)
   1

ロボット コンフィギュレーションを可視化し、exampleHelperHighlightCollisionBodies を使用して、衝突するボディを強調表示します。

show(iiwa, config);
exampleHelperHighlightCollisionBodies(iiwaCollision, selfCollisionPairIdx, gca);

方法 2: 剛体ツリーからメッシュを抽出

ビジュアル メッシュおよび衝突メッシュの組み合わせをもつ剛体ツリーがある場合、ツリー内の各剛体の Visuals プロパティからメッシュを作成します。exampleHelperManipCollisionsFromVisuals クラスの createCollisionArray を使用します。補助関数には、ビジュアルが複数あるときに最初のビジュアルを使用するという前提があります。特定のボディにビジュアルがない場合、対応する cell 配列は空のままになります。

collisionArrayFromVisuals = exampleHelperManipCollisionsFromVisuals(iiwa);

config = [0 -pi/4 pi 0.9*pi 0 -pi/2 0]';
[isCollision, selfCollisionPairIdx] = exampleHelperManipCheckCollisions(iiwa, collisionArrayFromVisuals, {}, config, true);
disp(isCollision)
   1

ロボットを可視化して、衝突するオブジェクトを強調表示します。

show(iiwa,config);
exampleHelperHighlightCollisionBodies(iiwa, selfCollisionPairIdx, gca);

方法 3: 衝突プリミティブを定義

簡略化されたジオメトリを使用する衝突プリミティブを作成します。寸法を手動で指定します。この方法は一般に、高い忠実度のメッシュ定義を使用するよりも精度が低くなります。

この例では、高さと半径をもつ collisionCylinder オブジェクトを使用します。各剛体のサイズを配列として指定します。

dimensionArray = [...
    .1 0; ... % Base
    .15, 0.1575; ... % iiwa_link_0
    .12, 0.3; ... % iiwa_link_1
    .13, 0.3; ... % iiwa_link_2
    .1, 0.3; ... % iiwa_link_3
    .1, 0.25; ... % iiwa_link_4
    .1, 0.2155; ... % iiwa_link_5
    .08, 0.17; ... % iiwa_link_6
    .05, 0.06; ... % iiwa_link_7
    .01, 0; ... % iiwa_link_ee_kuka
    .01, 0;]; % iiwa_link_ee

指定した寸法から衝突配列を作成します。collisionCylinder オブジェクトを作成し、そのジオメトリに基づいてそれぞれの間の変換を指定します。

primitiveCollisionArray = { ...
    [] eye(4); ... %Base (world)
    collisionCylinder(dimensionArray(2,1), dimensionArray(2,2)) trvec2tform([0 0 dimensionArray(2,2)/2]); ... % iiwa_link_0
    collisionCylinder(dimensionArray(3,1), dimensionArray(3,2)) trvec2tform([0 0 dimensionArray(3,2)/2]); ... % iiwa_link_1
    collisionCylinder(dimensionArray(4,1), dimensionArray(4,2)) axang2tform([1 0 0 -pi/2])*trvec2tform([0 0 dimensionArray(4,2)/2]); ... % iiwa_link_2
    collisionCylinder(dimensionArray(5,1), dimensionArray(5,2)) trvec2tform([0 .025 dimensionArray(5,2)/2]); ... % iiwa_link_3
    collisionCylinder(dimensionArray(6,1), dimensionArray(6,2)) axang2tform([1 0 0 -pi/2])*trvec2tform([0 -.02 dimensionArray(6,2)/2]); ... % iiwa_link_4
    collisionCylinder(dimensionArray(7,1), dimensionArray(7,2)) trvec2tform([0 0 dimensionArray(7,2)/2]); ... % iiwa_link_5
    collisionCylinder(dimensionArray(8,1), dimensionArray(8,2)) axang2tform([1 0 0 -pi/2]); ... % iiwa_link_6
    collisionCylinder(dimensionArray(9,1), dimensionArray(9,2)) trvec2tform([0 0 dimensionArray(9,2)/2]); ... % iiwa_link_7
    [] eye(4); ... % iiwa_link_ee_kuka
    [] eye(4); ... % iiwa_link_ee
    };

衝突をチェックします。これらの衝突の精度が低いため、このインスタンスでは衝突チェックによって多くの衝突が返されます。

config = [0 -pi/4 pi 0.9*pi 0 -pi/2 0]';
show(iiwa, config);
[isCollision, selfCollisionPairIdx] = exampleHelperManipCheckCollisions(iiwa, primitiveCollisionArray, {}, config, true);
exampleHelperHighlightCollisionBodies(iiwa, selfCollisionPairIdx, gca);

衝突プリミティブを可視化して、精度の低いことを確認します。

exampleHelperShowCollisionTree(iiwa, primitiveCollisionArray, config);
title('Collision Array from Collision Primitives')