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

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

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

環境の定義

衝突プリミティブを使用してシンプルな環境を作成できます。たとえば、ワークスペースにロボットがあり、その目的は円形の照明器具をよけながらあるテーブルから別のテーブルに物体を移動することだとします。これらの物体は 2 つのボックスと 1 つの球体としてモデル化できます。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};

衝突配列内を反復する補助関数を使用して、環境を可視化します。

exampleHelperVisualizeCollisionEnvironment(worldCollisionArray);

マニピュレーター ロボットの追加

環境の原点に Kinova マニピュレーターを追加します。提供されているロボット モデルを読み込みます。障害物を可視化して、同じ Figure にロボットを表示します。

robot = loadrobot("kinovaGen3","DataFormat","column","Gravity",[0 0 -9.81]);
ax = exampleHelperVisualizeCollisionEnvironment(worldCollisionArray);
show(robot,homeConfiguration(robot),"Parent",ax);

マニピュレーターを衝突オブジェクトの配列としてモデル化

rigidBodyTreeオブジェクトから衝突オブジェクトの配列を作成します。この方法では、各rigidBodyオブジェクトの最初のビジュアルからメッシュを抽出する、例の補助関数 exampleHelperManipCollisionsFromVisuals を使用します。他の方法の概要については、マニピュレーターの衝突チェック用の衝突オブジェクトの作成を参照してください。

% Generate an array of collision objects from the visuals of the associated tree
collisionArray = exampleHelperManipCollisionsFromVisuals(robot);

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

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

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

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

%Initialize outputs
isCollision = false(length(q),1); % Check whether each pose is in collision
selfCollisionPairIdx = cell(length(q),1); % Provide the bodies that are in collision
worldCollisionPairIdx = cell(length(q),1); % Provide the bodies that are in collision

for i = 1:length(q)
    [isCollision(i),selfCollisionPairIdx{i},worldCollisionPairIdx{i}] = exampleHelperManipCheckCollisions(robot,collisionArray,worldCollisionArray,q(:,i),false);
end
isTrajectoryInCollision = any(isCollision)

isTrajectoryInCollision = logical
   1

問題のあるケースの検査

衝突を検査すると、衝突が 2 回起きています。これらのコンフィギュレーションを可視化してさらに詳しく調査します。

problemIdx1 = find(isCollision,1);
problemIdx2 = find(isCollision,1,"last");

% Identify the problem rigid bodies
problemBodies1 = [selfCollisionPairIdx{problemIdx1} worldCollisionPairIdx{problemIdx1}*[1 0]'];
problemBodies2 = [selfCollisionPairIdx{problemIdx2} worldCollisionPairIdx{problemIdx2}*[1 0]'];

% Visualize the environment
ax = exampleHelperVisualizeCollisionEnvironment(worldCollisionArray);

% Add the robots & highlight the problem bodies
show(robot,q(:,problemIdx1),"Parent",ax,"PreservePlot",false);
exampleHelperHighlightCollisionBodies(robot,problemBodies1,ax);
show(robot,q(:,problemIdx2),"Parent"',ax);
exampleHelperHighlightCollisionBodies(robot,problemBodies2,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(:,problemIdx1));
intermediateConfig2 = ik("EndEffector_Link",intermediatePose2,weights,q(:,problemIdx2));

% 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
isCollision = false(length(q),1); % Check whether each pose is in collision
collisionPairIdx = cell(length(q),1); % Provide the bodies that are in collision
for i = 1:length(q)
    [isCollision(i),collisionPairIdx{i}] = exampleHelperManipCheckCollisions(robot,collisionArray,worldCollisionArray,q(:,i),false);
end
isTrajectoryInCollision = any(isCollision)

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

×

例を開く

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

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