マニピュレーターの環境衝突のチェック

制約付きワークスペース内に衝突のない軌跡を生成します。

衝突環境の定義

衝突プリミティブを使用して単純な環境を作成します。この例では、ワークスペースにあるロボットが、あるテーブルから別のテーブルに物体を移動するシーンを作成します。また、このロボットはワークスペースの上にある円形の照明器具をよけなければなりません。テーブルを 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);

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

変換行列乗算により結合された位置ベクトルと方向ベクトルを使用して、開始姿勢と終了姿勢を定義します。

startPose = trvec2tform([-0.5,0.5,0.4])*axang2tform([1 0 0 pi]);
endPose = trvec2tform([0.5,0.2,0.4])*axang2tform([1 0 0 pi]);

inverseKinematicsを使用して、目的の姿勢に基づいてジョイント位置を求めます。手動の検査によりコンフィギュレーションが有効であることを確認します。

% Use a fixed random seed to ensure repeatable results
rng(0);
ik = inverseKinematics("RigidBodyTree",robot);
weights = ones(1,6);
startConfig = ik("EndEffector_Link",startPose,weights,robot.homeConfiguration);
endConfig = ik("EndEffector_Link",endPose,weights,robot.homeConfiguration);

% 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");
    
    [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);

衝突のない軌跡の生成

これらの衝突を避けるには、ロボットが確実に障害物をよけて動作するように中継のウェイポイントを追加します。

intermediatePose1 = trvec2tform([-.3 -.2 .6])*axang2tform([0 1 0 -pi/4]); % Out and around the sphere
intermediatePose2 = trvec2tform([0.2,0.2,0.6])*axang2tform([1 0 0 pi]); % Come in from above

intermediateConfig1 = ik("EndEffector_Link",intermediatePose1,weights,q(:,collidingIdx1));
intermediateConfig2 = ik("EndEffector_Link",intermediatePose2,weights,q(:,collidingIdx2));

% 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");
end
isTrajectoryInCollision = any(inCollision)

isTrajectoryInCollision = logical
   1

生成された軌跡の可視化

結果をアニメーション化します。

% 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")

×

例を開く

この例の変更されたバージョンがあります。編集された方の例を開きますか?

いいえ、変更されたバージョンを上書きします。 はい