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

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

概要

この例では、KINOVA Gen3 マニピュレーターを使用して、検出された物体を棚に仕分けします。例では 4 つのツールボックスからのツールを使用します。

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

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

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

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

Stateflow チャート

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

edit exampleHelperFlowChartPickPlace.sfx

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

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

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

ロボットと環境の初期化

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

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

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

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

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

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

目標物のピックアップ

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

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

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

目標物の配置

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

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

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

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

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

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

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

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

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

このシミュレーションでは、Robotiq グリッパーをもつ KINOVA Gen3 マニピュレーターを使用します。

load('exampleHelperKINOVAGen3RobotiqGripper.mat'); 

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

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

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

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

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

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

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

coordinator.FlowChart = exampleHelperFlowChartPickPlace('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++ を使用します。可視化によって作業領域内でロボットがパーツを移動する様子が表示されます。ロボットは、環境 (青の円柱) 内で障害物を回避し、分類に基づいて目標物を上下いずれかの棚に配置します。すべてのパーツが配置されるまで、ロボットは動作を続けます。

参考文献

[1] Schulman, J., et al. "Motion planning with sequential convex optimization and convex collision checking." The International Journal of Robotics Research 33.9 (2014): 1251-1270.