最新のリリースでは、このページがまだ翻訳されていません。 このページの最新版は英語でご覧になれます。

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

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

概要

この例では KINOVA Gen3 マニピュレーターを使用して、物体を識別し 2 台のテーブルに仕分けします。例では 4 つのツールボックスからのツールを使用します。

  • Robotics System Toolbox™ を使用して、マニピュレーターのモデル化、シミュレーションおよび可視化、さらに衝突チェックを行います。

  • Model Predictive Control Toolbox™ Optimization Toolbox™ を使用して、マニピュレーターが追従するための最適化された衝突のない軌跡を生成します。

  • Stateflow® を使用して、例にある高水準のタスクをスケジュールし、タスクからタスクへとステップ実行します。

この例は、2 つの関連する例の主要概念に基づいています。

Stateflow チャート

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

edit exampleHelperFlowChartPickPlace.sfx

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

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

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

ロボットと環境の初期化

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

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

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

次に、パーツを分類しなければなりません。commandClassifyParts クラスはパーツを 2 タイプに分類して、配置位置 (左または右のテーブル) を判別します。ここでも、このクラスを任意のパーツ分類方法に置き換えることができます。

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

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

目標物のピックアップ

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

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

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

目標物の配置

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

ピッキングのワークフローと同様に、目的とする既知の配置位置を基準として配置の接近位置と後退位置が計算されます。CommandActivateGripper を使用してグリッパーが非アクティブにされ、ロボットからパーツが離れます。

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

タスク実行のほとんどは、さまざまな指定姿勢の間でロボットを移動する命令で構成されます。関数 exampleHelperPlanExecuteTrajectoryPickPlace は、非線形モデル予測コントローラーを使用してソルバーを定義します (Nonlinear MPC (Model Predictive Control Toolbox)を参照)。このコントローラーは、nlmpcmovecheckCollisionを使用して実行可能な衝突のない最適化された基準軌跡を計算します。マニピュレーターの環境衝突のチェックに類似の方法を使用して、マニピュレーターと環境の衝突チェックが計算されます。次に、補助関数がjointSpaceMotionModelオブジェクトを使用して基準軌跡を追従しながら、計算トルク制御下でマニピュレーターの動作をシミュレートし、可視化を更新します。Stateflow チャートから CommandMoveToTaskConfig により補助関数が呼び出され、これにより正しい入力が定義されます。

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

コマンド ディスパッチャーを使用した Stateflow チャートでのタスク スケジューリング

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

Stateflow チャートは、exampleHelperDispatcherPickPlace によってディスパッチされるコマンド クラスを使用して、MATLAB でのタスクの実行を指示します。コマンド シーケンスをディスパッチすることにより、チャート内に関数呼び出しの大きなスタックを作成せずに済みます。コマンドの実行が終了すると、ディスパッチャーは "入力イベント" を送信してチャートを起動し、タスク実行の次のステップに進みます。スタンドアロンのチャートの実行 (Stateflow)を参照してください。

コマンドの実行

ディスパッチャーは、コマンド クラスへのハンドルのキューをプロパティとして保持します。Stateflow チャート exampleHelperFlowChartPickPlace は、チャート内に定義されているステート遷移に従って、ActivateGripperDetectParts などのコマンドをこのキューに追加します。ディスパッチャーは、実行中に一定の間隔でキューに新しいコマンドがないかをチェックし、実行するために新しいコマンドをディスパッチします。

コマンド定義

コマンドは、ピックアンドプレース ワークフローを完了するためにロボットが実行しなければならない明確な動作を記述する継承クラスです。

カスタム コマンドを追加するには、提供されているコマンドをテンプレートとして使用します。コマンドには、少なくとも以下が必要です。

  • ハンドル クラス exampleHelperCommandPickPlace から継承する。

  • ディスパッチャーからのデータを使用し、データをディスパッチャーに返して、必要な機能を実装する doit() メソッドを含める (たとえば、パーツの検出が点群分類に依存する場合、ディスパッチャーは点群を保持し、CommandDetectParts コマンドの doit() メソッドが点群を解析してパーツの位置を判別する)。

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

このシミュレーションでは KINOVA Gen3 マニピュレーターを使用します。loadrobot を使用してロボットを読み込みます。行ベクトルのロボット コンフィギュレーションを取得するために、データ形式を 'row' として指定します。

robot = loadrobot('kinovaGen3','DataFormat','row');

ピック アンド プレースのコマンド ディスパッチャーの初期化

ロボットの初期コンフィギュレーションを設定します。ロボット モデル、初期コンフィギュレーションおよびエンドエフェクタに名前を付けてディスパッチャーを作成します。

currentRobotJConfig = homeConfiguration(robot);
dispatcher = exampleHelperDispatcherPickPlace(robot,currentRobotJConfig, "EndEffector_Link");

ピックアンドプレースのディスパッチャーのプロパティを指定します。

dispatcher.homeRobotTaskConfig = trvec2tform([0.4, 0, 0.6])*axang2tform([0 1 0 pi]);
dispatcher.placingPose{1} = trvec2tform([[0.31 0.62 0.36]])*axang2tform([0 1 0 pi]);
dispatcher.placingPose{2} = trvec2tform([[0.31 -0.62 0.36]])*axang2tform([0 1 0 pi]);

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

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

dispatcher.flowChart = exampleHelperFlowChartPickPlace('dispatcher', dispatcher); 

ダイアログを使用して、ピックアンドプレース タスクの実行を開始します。検出実行が固定の回数に達するまで、コマンド ディスパッチャーの実行を続けます。実行が完了したら、ディスパッチャー オブジェクトを削除します。

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

switch answer
    case 'Yes'
        while dispatcher.numDetectionRuns < 4
             dispatcher.run;
        end
        delete(dispatcher);
    case 'No'
        delete(dispatcher);
end

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

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

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

可視化によって作業領域内でロボットがパーツを移動する様子が表示されます。ロボットは環境内の障害物 (青のボール) を避けて、目標物をその分類に基づいて配置します。すべてのパーツが配置されるまで、ロボットは動作を続けます。

Copyright 2019 The MathWorks, Inc.