Gazebo での ROS を使用したピックアンドプレースワークフロー

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

ライブスクリプトを開く

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

概要

この例では、Kinova Gen3 マニピュレーターを使用して、目標物を識別して 2 つのビンにリサイクルします。この例では、5 つのツールボックスのツールを使用します。

Robotics System Toolbox™ を使用して、マニピュレーターをモデル化し、シミュレートします。
Stateflow® を使用して、例にある高水準のタスクをスケジュールし、タスクからタスクへとステップ実行します。
"ROS Toolbox" を使用して、MATLAB® を Gazebo に接続します。
Computer Vision Toolbox™ および Deep Learning Toolbox™ を使用して、Gazebo 内のシミュレートされたカメラを使用して目標物を検出します。

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

Kinova Gen3 マニピュレーターを使用したタスク空間およびジョイント空間の軌跡の計画と実行では、エンドエフェクタの初期姿勢から目的の姿勢に移動するジョイント軌跡を内挿して生成し、シミュレートする方法を説明しています。
MATLAB 向けに Stateflow を使用したピックアンドプレースのワークフロー
Computer Vision Toolbox の例: 車両検出のための YOLO v2 ネットワークの学習 (Computer Vision Toolbox)
ROS Toolbox の例: Gazebo およびシミュレートされた TurtleBot の入門 (ROS Toolbox)

Gazebo でのロボットのシミュレーションと制御

Kinova Gen3 ロボットの ROS ベースのシミュレーターを起動して、MATLAB® とロボットシミュレーターとの接続を設定します。

この例では、こちらからダウンロード可能な、ROS Melodic が含まれるバーチャルマシン (VM) を使用します。これまでに使用経験がない場合は、Gazebo およびシミュレートされた TurtleBot の入門 (ROS Toolbox)を参照してください。

VM 設定で、[VM]、[Settings]、[Hardware]、[Display] に移動し、[Accelerate 3D graphics] を無効にします。
Ubuntu® バーチャルマシンデスクトップを起動します。
Ubuntu デスクトップで、[Gazebo Recycling World] アイコンをクリックして、この例のために作成された Gazebo ワールドを起動します。[再生] ボタンをクリックして Gazebo ワールドを実行します。
Gazebo で ROS マスターの IP アドレスとポート番号を指定して、MATLAB® がロボットシミュレーターと通信できるようにします。この例では、Gazebo 内の ROS マスターは、デスクトップに表示されている IP アドレス 192.168.203.131 を使用します。使用している VM に基づいて、変数 rosIP を調整します。
rosinit を使用して、ROS 1 ネットワークを起動します。

rosIP = '192.168.203.131';   % IP address of ROS-enabled machine  

rosinit(rosIP,11311); % Initialize ROS connection

The value of the ROS_IP environment variable, 192.168.31.1, will be used to set the advertised address for the ROS node.
Initializing global node /matlab_global_node_36570 with NodeURI http://192.168.31.1:51073/

Gazebo ワールドの実行が開始されると、VM は、各側に 1 つずつごみ箱があるテーブル上に Kinova Gen3 ロボットアームを読み込みます。Gazebo でロボットアームをシミュレートし制御するために、VM には、Kinova が提供する ros_kortex ROS パッケージが含まれています。

パッケージは ros_control を使用して、目的のジョイント位置までジョイントを制御します。VM の使用方法の詳細については、Gazebo およびシミュレートされた TurtleBot の入門 (ROS Toolbox)を参照してください。

Stateflow チャート

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

edit exampleHelperFlowChartPickPlaceROSGazebo.sfx

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

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

ピックアンドプレースのステップに関する高水準の説明については、MATLAB 向けに Stateflow を使用したピックアンドプレースのワークフローを参照してください。

グリッパーの開閉

グリッパーをアクティブにするためのコマンド exampleCommandActivateGripperROSGazebo は、Gazebo に実装されているグリッパーを開閉するためのアクション要求を送信します。グリッパーを開く要求を送信するには、次のコードを使用します。メモ: ここに示すコード例は、コマンドが何を行うかを説明しているだけです。実行しないでください。

[gripAct,gripGoal] = rosactionclient('/my_gen3/custom_gripper_controller/gripper_cmd');
gripperCommand = rosmessage('control_msgs/GripperCommand');
gripperCommand.Position = 0.0;  
gripGoal.Command = gripperCommand;
sendGoal(gripAct,gripGoal);

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

コマンド関数 exampleCommandMoveToTaskConfigROSGazebo は、指定された姿勢にマニピュレーターを移動するために使用されます。

計画

パス計画は、trapveltrajおよびtransformtrajを使用して、初期のタスクコンフィギュレーションから目的のタスクコンフィギュレーションへの、シンプルなタスク空間の軌跡を生成します。軌跡の計画と実行の詳細については、Kinova Gen3 マニピュレーターを使用したタスク空間およびジョイント空間の軌跡の計画と実行を参照してください。

ROS のジョイント軌跡コントローラー

ロボットが追従するジョイント軌跡の生成後、exampleCommandMoveToTaskConfigROSGazebo は、目的のサンプルレートで軌跡をサンプリングし、ジョイント軌跡 ROS メッセージにパッケージ化して、Kinova ROS パッケージに実装されているジョイント軌跡コントローラーにアクション要求を送信します。

シーンでの目標物の検出と分類

関数 exampleCommandDetectPartsROSGazebo および exampleCommandClassifyPartsROSGazebo は、ロボットからのシミュレートされたエンドエフェクタカメラフィードを使用し、事前学習済みの深層学習モデルを適用してリサイクル可能なパーツを検出します。モデルは、カメラフレームを入力として取得し、目標物の 2 次元位置 (ピクセル位置) と、必要なリサイクルのタイプ (青または緑のビン) を出力します。イメージフレームの 2 次元位置は、ロボットの base 座標系にマップされます。

深層学習モデルの学習: Gazebo イメージの取得とラベル付け

検出モデルの学習には、Gazebo ワールド内のシミュレートされた環境で取得された一連のイメージが使用されました。これらのイメージでは、2 つのクラスの目標物 (ボトルと缶) がテーブルの異なる位置に配置されていました。イメージは、ロボットに搭載されているシミュレートされたカメラから取得されます。このカメラは水平方向および垂直方向の平面に沿って移動し、多くの異なるカメラ視点から目標物のイメージを取得します。

イメージにはその後、イメージラベラー (Computer Vision Toolbox)アプリを使用してラベルが付けられ、YOLO v2 検出モデルの学習データセットが作成されます。trainYOLOv2ObjectDetector (Computer Vision Toolbox)がモデルに学習させます。MATLAB で YOLO v2 ネットワークに学習させる方法については、車両検出のための YOLO v2 ネットワークの学習 (Computer Vision Toolbox)を参照してください。

ロボットがホームポジションにあるときに、搭載カメラによって取得された単一のイメージに関するオンライン推定用として学習済みモデルが展開されます。関数detect (Computer Vision Toolbox)は、検出された目標物の境界ボックスのイメージ位置とクラスを返し、その後それを使用して、目標物の最上部中央の位置が求められます。シンプルなカメラ投影手法を使用し、目標物の高さが既知であると仮定して、目標物の位置がワールドに投影され、最終的に目標物をピックするための基準位置として使用されます。境界ボックスに関連付けられているクラスにより、どのビンに目標物を配置するかが決まります。

ピックアンドプレースタスクの開始

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

load('exampleHelperKINOVAGen3GripperROSGazebo.mat');

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

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

initialRobotJConfig =  [3.5797   -0.6562   -1.2507   -0.7008    0.7303   -2.0500   -1.9053];
endEffectorFrame = "gripper";

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

coordinator = exampleHelperCoordinatorPickPlaceROSGazebo(robot,initialRobotJConfig, endEffectorFrame);

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

coordinator.HomeRobotTaskConfig = getTransform(robot, initialRobotJConfig, endEffectorFrame); 
coordinator.PlacingPose{1} = trvec2tform([[0.2 0.55 0.26]])*axang2tform([0 0 1 pi/2])*axang2tform([0 1 0 pi]);
coordinator.PlacingPose{2} = trvec2tform([[0.2 -0.55 0.26]])*axang2tform([0 0 1 pi/2])*axang2tform([0 1 0 pi]);

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

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

coordinator.FlowChart = exampleHelperFlowChartPickPlaceROSGazebo('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'
        coordinator.FlowChart.endPickPlace;
        delete(coordinator.FlowChart)
        delete(coordinator);
end

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

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

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

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

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

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

Gazebo ワールドは、作業領域でロボットがパーツをごみ箱に移動する様子を示します。すべてのパーツが配置されるまで、ロボットは動作を続けます。検出ステップで 4 回にわたってパーツが見つからない場合、Stateflow チャートは終了します。

if strcmp(answer,'Yes')
    while  coordinator.NumDetectionRuns <  4
        % Wait for no parts to be detected.
    end
end

例を終了したら、ROS ネットワークをシャットダウンします。

rosshutdown

Shutting down global node /matlab_global_node_36570 with NodeURI http://192.168.31.1:51073/

Gazebo での ROS を使用したピックアンドプレース ワークフロー

概要