マニピュレーターの環境衝突のチェック
制約付きワークスペース内に衝突のない軌跡を生成します。この例では、衝突の軌跡を検査して軌跡を手動で設計する方法を説明します。代わりの方法として、Pick and Place Using RRT for Manipulatorsに示すようにモーション プランナーを使用できます。
衝突環境の定義
衝突プリミティブを使用して単純な環境を作成します。この例では、ワークスペースにあるロボットが、あるテーブルから別のテーブルに物体を移動するシーンを作成します。また、このロボットはワークスペースの上にある円形の照明器具をよけなければなりません。テーブルを 2 つのボックスおよび球面としてモデル化し、ワールド内での姿勢を指定します。collisionMesh
オブジェクトを使用して、さらに複雑な環境を作成できます。
% Create two platforms platform1 = collisionBox(0.5,0.5,0.25); platform1.Pose = trvec2tform([-0.5 0.4 0.2]); platform2 = collisionBox(0.5,0.5,0.25); platform2.Pose = trvec2tform([0.5 0.2 0.2]); % Add a light fixture, modeled as a sphere lightFixture = collisionSphere(0.1); lightFixture.Pose = trvec2tform([.2 0 1]); % Store in a cell array for collision-checking worldCollisionArray = {platform1 platform2 lightFixture};
衝突配列内を反復する補助関数を使用して、環境を可視化します。
ax = exampleHelperVisualizeCollisionEnvironment(worldCollisionArray);
環境へのロボットの追加
関数 loadrobot
を使用して、Kinova マニピュレーター モデルを rigidBodyTree
オブジェクトとして読み込みます。
robot = loadrobot("kinovaGen3","DataFormat","column","Gravity",[0 0 -9.81]);
衝突オブジェクトと同じ座標軸を使用して、環境内でロボットを表示します。ロボットのベースはワールドの原点に固定されます。
show(robot,homeConfiguration(robot),"Parent",ax);
軌跡の生成と衝突のチェック
プラットフォーム 1 の上に開始ジョイント コンフィギュレーションを、プラットフォーム 2 の上に終了ジョイント コンフィギュレーションを定義します。
startConfig = [3.8906 1.1924 0.0000 0.0000 0.0001 1.9454 0.7491]';
endConfig = [-0.9240 1.2703 1.9865 1.2394 1.7457 -2.0500 0.4222]';
% Show initial and final positions
show(robot,startConfig);
show(robot,endConfig);
台形速度プロファイルを使用して、ホーム位置から開始位置まで、さらに終了位置までの滑らかな軌跡を生成します。
q = trapveltraj([homeConfiguration(robot),startConfig,endConfig],200,"EndTime",2);
関数 checkCollision
を使用して、環境内の障害物との衝突を確認します。名前と値の引数 IgnoreSelfCollision
と Exhaustive
を有効にします。ロボット モデルのジョイント制限により大部分の自己衝突が防止されるため、自己衝突は無視されます。この関数は、網羅的なチェックを行って確実にすべての分離距離を計算し、最初の衝突の検出後も衝突を探し続けます。
出力 sepDist
には、ロボット ボディとワールド衝突オブジェクトの間の距離が行列として格納されます。各行は特定のワールド衝突オブジェクトに対応します。各列はロボット ボディに対応します。NaN
値は衝突を表します。衝突のインデックスを cell 配列として格納します。
% Initialize outputs inCollision = false(length(q), 1); % Check whether each pose is in collision worldCollisionPairIdx = cell(length(q),1); % Provide the bodies that are in collision for i = 1:length(q) [inCollision(i),sepDist] = checkCollision(robot,q(:,i),worldCollisionArray,"IgnoreSelfCollision","on","Exhaustive","on","SkippedSelfCollisions","parent"); [bodyIdx,worldCollisionObjIdx] = find(isnan(sepDist)); % Find collision pairs worldCollidingPairs = [bodyIdx,worldCollisionObjIdx]; worldCollisionPairIdx{i} = worldCollidingPairs; end isTrajectoryInCollision = any(inCollision)
isTrajectoryInCollision = logical
1
検出された衝突の確認
最後のステップから 2 件の衝突が検出されています。これらのコンフィギュレーションを可視化してさらに詳しく調査します。関数 exampleHelperHighlightCollisionBodies
を使用して、インデックスに基づきボディを強調表示します。球面とテーブルで衝突が発生していることがわかります。
collidingIdx1 = find(inCollision,1); collidingIdx2 = find(inCollision,1,"last"); % Identify the colliding rigid bodies. collidingBodies1 = worldCollisionPairIdx{collidingIdx1}*[1 0]'; collidingBodies2 = worldCollisionPairIdx{collidingIdx2}*[1 0]'; % Visualize the environment. ax = exampleHelperVisualizeCollisionEnvironment(worldCollisionArray); % Add the robotconfigurations & highlight the colliding bodies. show(robot,q(:,collidingIdx1),"Parent",ax,"PreservePlot",false); exampleHelperHighlightCollisionBodies(robot,collidingBodies1 + 1,ax); show(robot,q(:,collidingIdx2),"Parent"',ax); exampleHelperHighlightCollisionBodies(robot,collidingBodies2 + 1,ax);
衝突のない軌跡の生成
これらの衝突を避けるには、ロボットが確実に障害物をよけて動作するように中継のジョイント コンフィギュレーションを追加します。
intermediateConfig1 = [-0.3644 -1.8365 0.0430 1.6606 0.2889 0.9386 0.1271]'; intermediateConfig2 =[-1.8895 0.6716 2.2225 1.4608 2.5503 -2.0500 0.1536]'; % Show the new intermediate poses ax = exampleHelperVisualizeCollisionEnvironment(worldCollisionArray); show(robot,intermediateConfig1,"Parent",ax,"PreservePlot",false); show(robot,intermediateConfig2,"Parent",ax);
新しい軌跡を生成します。
[q,qd,qdd,t] = trapveltraj([homeConfiguration(robot),intermediateConfig1,startConfig,intermediateConfig2,endConfig],200,"EndTime",2);
衝突のないことを確認します。
%Initialize outputs inCollision = false(length(q),1); % Check whether each pose is in collision for i = 1:length(q) inCollision(i) = checkCollision(robot,q(:,i),worldCollisionArray,"IgnoreSelfCollision","on","SkippedSelfCollisions","parent"); end isTrajectoryInCollision = any(inCollision)
isTrajectoryInCollision = logical
0
生成された軌跡の可視化
結果をアニメーション化します。
% Plot the environment ax2 = exampleHelperVisualizeCollisionEnvironment(worldCollisionArray); % Visualize the robot in its home configuration show(robot,startConfig,"Parent",ax2); % Update the axis size axis equal % Loop through the other positions for i = 1:length(q) show(robot,q(:,i),"Parent",ax2,"PreservePlot",false); % Update the figure drawnow end
時間の経過に伴うジョイント位置をプロットします。
figure plot(t,q) xlabel("Time") ylabel("Joint Position")