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

この例では、軌跡の実行時にマニピュレーターの自己衝突をチェックする方法を説明します。衝突メッシュは、ロボット モデルの URDF で定義された <collision> タグを使用して読み込まれます。衝突データを読み込む他の方法の詳細については、マニピュレーターの衝突チェック用の衝突データの取得を参照してください。

ロボット表現の作成

KUKA® IIWA-14 シリアル マニピュレーターの URDF ファイルをrigidBodyTreeモデルとしてインポートします。URDF はロボットの剛体の衝突メッシュ ファイルをキャプチャします。剛体に衝突オブジェクトを個別に追加する場合は、関数addCollisionを使用できます。提供されたロボット モデルに衝突オブジェクトを追加して読み込む場合は、関数loadrobotを参照してください。

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

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

開始および終了のコンフィギュレーションをジョイント位置のセットとして指定します。これらの位置は衝突しないようにする必要があります。

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]';

これらの 2 つのコンフィギュレーションを 3 秒以内に接続するジョイント空間の軌跡を求めます。

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

この出力軌跡に自己衝突が含まれないことを検証するには、出力サンプルを反復し、関数checkCollisionを使用して衝突する点の有無を確認します。

軌跡 q の反復中に、この軌跡のすべてのコンフィギュレーションで関数 checkCollision を呼び出します。網羅的チェックをオンにすると、最初の衝突が検出された後も衝突のチェックが続行されます。

変数 isConfigInCollision は、各コンフィギュレーションの衝突ステータスを追跡します。sepDistForConfig は、ロボットのボディ間の分離距離を追跡します。衝突ごとにボディ インデックスのペアが変数 configCollisionPairs に格納されます。隣接するボディはジョイントの接続により常に接触しているため、チェックされません。

isConfigInCollision = false(100,1);
configCollisionPairs = cell(100,1);
sepDistForConfig = zeros(iiwa.NumBodies+1,iiwa.NumBodies+1,100);
for i = 1:length(q)
    [isColliding, sepDist] = checkCollision(iiwa,q(:,i),'Exhaustive','on','SkippedSelfCollisions','parent');
    isConfigInCollision(i) = isColliding;
    sepDistForConfig(:,:,i) = sepDist;
end

衝突しているボディのインデックスを特定するには、sepDistForConfig のどのエントリが NaN であるかを調べます。septDist は対称行列なので、同じ値が反転したインデックスによりインデックスが返されます。unique を使用して、リストを簡略化します。

for i = 1:length(q)
    sepDist = sepDistForConfig(:,:,i);
    [body1Idx,body2Idx] = find(isnan(sepDist));

    collidingPairs = unique(sort([body1Idx,body2Idx],2));
    configCollisionPairs{i} = collidingPairs;
end

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

any(isConfigInCollision)

ans = logical
   1

firstCollisionIdx = find(isConfigInCollision,1);

% Visualize the first configuration that is in collision.
figure;
show(iiwa,q(:,firstCollisionIdx));
exampleHelperHighlightCollisionBodies(iiwa,configCollisionPairs{firstCollisionIdx}+1,gca);

衝突のない軌跡の生成

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

新しい軌跡を生成し、衝突を再びチェックします。

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

isConfigInCollision = false(100,1);
configCollisionPairs = cell(100,1);
for i = 1:length(q)
    isColliding = checkCollision(iiwa,q(:,i),'Exhaustive','on','SkippedSelfCollisions','parent');
    isConfigInCollision(i) = isColliding;
end

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

any(isConfigInCollision)

ans = logical
   0