概要

この例では,KINOVA Gen3マニピュレーターを使用して,検出されたオブジェクトを並べ替え,ベンチに配置します。4例ではつのツールボックスからのツールを使用します。

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

Stateflowチャート

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

编辑exampleHelperFlowChartPickPlace.sfx

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

ロボットと環境の初期化

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

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

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

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

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

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

目標物のピックアップ

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

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

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

目標物の配置

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

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

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

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

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

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

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

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

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

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

负载(“exampleHelperKINOVAGen3GripperColl.mat”）；

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

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

currentRobotJConfig = homeConfiguration(机器人);协调员= exampleHelperCoordinatorPickPlace (currentRobotJConfig的机器人“爪”）；

さまざまな種類の目標物を配置するために,ホームコンフィギュレーションと2つの姿勢を指定します。

协调员。HomeRobotTaskConfig = trvec2tform([0.4, 0, 0.6])*axang2tform([0 1 0 pi]); / /点击这里协调员。PlacingPose{1} = trvec2tform([0.23 0.62 0.33])*axang2tform([0 1 0 pi]);协调员。PlacingPose{2} = trvec2tform([0.23 -0.62 0.33])*axang2tform([0 1 0 pi]);

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

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

协调员。流程图= exampleHelperFlowChartPickPlace (“协调员”协调员);

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

回答= questdlg (“你想现在就开始找工作吗?”，．..“开始工作”，“是的”，“不”，“不”）；开关回答情况下“是的”%触发事件以启动状态流程图中的选择和放置coordinator.FlowChart.startPickPlace;情况下“不”%结束挑选和放置coordinator.FlowChart.endPickPlace;删除(coordinator.FlowChart);删除(协调);结束

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

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

% coordinator.FlowChart.endPickPlace;%删除(coordinator.FlowChart);%删除(协调);

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

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

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

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

参考文献

舒尔曼等。带有顺序凸优化和凸碰撞检查的运动规划国际机器人研究杂志33.9(2014): 1251 - 1270。