MATLAB 向けに RRT プランナーと Stateflow を使用したピックアンドプレースのワークフロー

この例では次を使用します。

この例では、Kinova® Gen3 などのロボットマニピュレーター用にエンドツーエンドのピックアンドプレースワークフローを設定する方法を説明します。

この例で実装されるピックアンドプレースワークフローは、さまざまなシナリオ、プランナー、シミュレーションプラットフォーム、オブジェクト検出オプションに適合させることができます。ここで示す例では、Rapidly-Exploring Random Tree (RRT) アルゴリズムを使用して計画を行い、MATLAB® でロボットをシミュレートします。その他の用途については、次を参照してください。

概要

この例では、Kinova Gen3 マニピュレーターを使用して、検出されたオブジェクトを並べ替え、棚に配置します。この例では、2 つのツールボックスのツールを使用します。

"Robotics System Toolbox™" を使用して、マニピュレーターのモデル化、シミュレーションおよび可視化を行い、さらに RRT を使用してマニピュレーターが追従するための衝突のないパスを計画します。
Stateflow® を使用して、例にある高水準のタスクをスケジュールし、タスクからタスクへとステップ実行します。

この例では、パス計画に RRT アルゴリズムを使用します。RRT プランナーについて詳しく説明するもう 1 つの例については、Pick and Place Using RRT for Manipulatorsを参照してください。

Stateflow チャート

この例では Stateflow チャートを使用して例のタスクをスケジュールします。チャートを開いて内容を確認し、チャートの実行中の状態遷移を把握します。

edit exampleHelperFlowChartPickPlaceRRT.sfx

このチャートは、マニピュレーターが目標物 (パーツ) とどのように相互作用するかを決定します。これは、基本的な初期化手順と、それに続く 2 つの主要セクションで構成されます。

パーツの識別とその配置位置の判別
ピックアンドプレースワークフローの実行

ロボットと環境の初期化

まず、チャートによって、Kinova Gen3 マニピュレーター、仕分けされる 3 つのパーツ、仕分けに使用される棚、および青の障害物で構成される環境が作成されます。次に、ロボットがホーム位置に移動します。

パーツの識別とその配置位置の判別

識別段階の最初のステップでは、パーツを検出しなければなりません。関数 exampleCommandDetectParts は、目標物の姿勢を直接指定します。このクラスを、センサーや目標物に基づく独自の物体検出アルゴリズムに置き換えます。

次に、パーツを分類しなければなりません。関数 exampleCommandClassifyParts は、パーツを 2 つのタイプに分類して、配置する位置 (上または下の棚) を判別します。ここでも、この関数を任意のパーツ分類メソッドに置き換えることができます。

ピックアンドプレースワークフローの実行

パーツが識別されてその移動先が割り当てられたら、マニピュレーターは、すべてのパーツで繰り返し、パーツを適切なテーブルに移動しなければなりません。

目標物のピックアップ

ピッキング段階では、次の図に示すようにロボットが目標物まで移動し、目標物をつかみ上げて安全な位置に移動します。

関数 exampleCommandComputeGraspPose は、つかむ姿勢を計算します。このクラスは各パーツについてタスク空間でのつかむ姿勢を計算します。パーツに接近してアームを伸ばす中間ステップも、その目標物を基準にして定義されます。

このロボットは、シミュレートされた空気圧グリッパーを使用して目標物をつかみ上げます。グリッパーがアクティブになると、exampleCommandActivateGripper はオブジェクト関数addCollisionを使用して、ロボットの表現 rigidBodyTree の上にパーツの衝突メッシュを追加します。この目標物はその間、衝突検出に含まれます。その後、ロボットは他のパーツから離れた後退位置に移動します。

目標物の配置

次に、ロボットは目標物を適切な棚の上に置きます。

ピッキングのワークフローと同様に、目的とする既知の配置位置を基準として配置の接近位置と後退位置が計算されます。exampleCommandActivateGripper を使用してグリッパーが非アクティブにされ、clearCollisionを使用してロボットからパーツが離れます。特定のタイプのパーツが配置されるたびに、このオブジェクトタイプの配置姿勢が更新され、同じタイプの次のパーツが配置済みパーツの横に配置されるようになります。

マニピュレーターを指定姿勢に移動

タスク実行のほとんどは、さまざまな指定姿勢の間でロボットを移動する命令で構成されます。関数 exampleHelperMoveToTaskConfig は、manipulatorRRTオブジェクトを使用して RRT プランナーを定義します。これは、シーン内の指定された衝突オブジェクトとの衝突を回避することで、初期ジョイントコンフィギュレーションから目的のジョイントコンフィギュレーションへのパスを計画します。結果のパスは、最初に短縮され、次に目的の検証距離で内挿されます。軌跡を生成するには、関数trapveltrajを使用して、台形プロファイルに従う内挿された各ウェイポイントにタイムステップを割り当てます。最後に、時間が関連付けられたウェイポイントが目的のサンプルレート (0.1 秒ごと) に内挿されます。生成された軌跡により、ロボットは、目標物に近づいたときまたは目標物を配置するときに、動作の開始時と終了時に確実にゆっくりと移動できるようになります。

RRT プランナーについて詳しく説明するもう 1 つの例については、Pick and Place Using RRT for Manipulatorsを参照してください。

単純な軌跡でパスに障害物のないことがわかっている場合には、軌跡生成ツールを使用して軌跡を実行し、マニピュレーター運動モデルを使用して軌跡をシミュレートすることができます。Kinova Gen3 マニピュレーターを使用したタスク空間およびジョイント空間の軌跡の計画と実行の例を参照してください。

Stateflow チャートでのタスクスケジューリング

この例では、Stateflow チャートを使用して MATLAB® におけるワークフローを指示します。Stateflow チャートを作成する方法の詳細については、MATLAB オブジェクトとして実行する Stateflow チャートの作成 (Stateflow)を参照してください。

Stateflow チャートは、コマンド関数を使用して、MATLAB でのタスクの実行を指示します。コマンドの実行が終了すると、"入力イベント" を送信してチャートを起動し、タスク実行の次のステップに進みます。スタンドアロンのチャートの実行 (Stateflow)を参照してください。

シミュレーションの実行と可視化

このシミュレーションでは、Robotiq グリッパーをもつ KINOVA Gen3 マニピュレーターを使用します。.mat ファイルから rigidBodyTree オブジェクトとしてロボットモデルを読み込みます。

load('exampleHelperKINOVAGen3GripperCollRRT.mat');

ピックアンドプレースコーディネーターの初期化

ロボットの初期コンフィギュレーションを設定します。ロボットモデル、初期コンフィギュレーションおよびエンドエフェクタ名を指定して、ロボット制御を処理するコーディネーターを作成します。

currentRobotJConfig = homeConfiguration(robot);
coordinator = exampleHelperCoordinatorPickPlaceRRT(robot,currentRobotJConfig, "gripper");

2 種類の目標物を配置するために、ホームコンフィギュレーションと 2 つの姿勢を指定します。最初の姿勢は、タイプ 1 のすべてのパーツが配置される中央の棚に対応し、2 番目の姿勢は、タイプ 2 のパーツが配置される上の棚に対応します。新しいパーツが正常に配置されるたびに、Stateflow チャートで配置姿勢が更新されます。異なるタイプのパーツは、可視化の際に異なる色で識別されます。

coordinator.HomeRobotTaskConfig = trvec2tform([0.4, 0, 0.5])*axang2tform([0 1 0 pi]);
coordinator.PlacingPose{1} = trvec2tform([[-0.15 0.52 0.46]])*axang2tform([1 0 0 -pi/2]);
coordinator.PlacingPose{2} = trvec2tform([[-0.15 0.52 0.63]])*axang2tform([1 0 0 -pi/2]);

シミュレーションの実行と可視化

コーディネーターを Stateflow チャートに接続します。開始された Stateflow チャートは、目標物の検出、つまみ上げ、および正しいステージング領域への配置という各ステートを連続的に経由させます。

coordinator.FlowChart = exampleHelperFlowChartPickPlaceRRT('coordinator', coordinator);

ダイアログを使用して、ピックアンドプレースタスクの実行を開始します。ダイアログで [はい] をクリックしてシミュレーションを開始します。

answer = questdlg('Do you want to start the pick-and-place job now?', ...
         'Start job','Yes','No', 'No');

switch answer
    case 'Yes'
        % Trigger event to start Pick and Place in the Stateflow Chart
        coordinator.FlowChart.startPickPlace;        
    case 'No'
        % End Pick and Place
        coordinator.FlowChart.endPickPlace;        
        delete(coordinator.FlowChart);
        delete(coordinator);
end

ピックアンドプレースタスクの終了

新しい目標物の検出に 3 回失敗すると、Stateflow チャートは自動的に実行を終了します。ピックアンドプレースタスクを途中で終了するには、以下のコード行をコメント解除して実行するか、コマンドウィンドウで Ctrl + C を押します。

% coordinator.FlowChart.endPickPlace;        
% delete(coordinator.FlowChart);
% delete(coordinator);

シミュレーションステートの観察

実行中、Stateflow チャートには各時点におけるアクティブステートが青く強調表示されます。これは、ロボットの動作とそのタイミングを追跡する上で役立ちます。サブシステムをクリックしていくと、動作中のステートの詳細を確認できます。

ピックアンドプレースアクションの可視化

この例では、ロボットの可視化にinteractiveRigidBodyTreeオブジェクトを使用します。可視化によって作業領域内でロボットがパーツを移動する様子が表示されます。ロボットは、環境 (青の円柱) 内で障害物を回避し、分類に基づいて目標物を上下いずれかの棚に配置します。すべてのパーツが配置されるまで、ロボットは動作を続けます。