点群処理および RRT パスプランニングを使用した Gazebo でのピックアンドプレースワークフロー

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

Kinova® Gen3 などのロボットマニピュレーター用にエンドツーエンドのピックアンドプレースワークフローを設定します。

この例で実装されるピックアンドプレースワークフローは、さまざまなシナリオ、プランナー、シミュレーションプラットフォーム、オブジェクト検出オプションに適合させることができます。ここで示す例では、プランニングに RRT を使用し、Robot Operating System (ROS) を使用して Gazebo でロボットをシミュレートします。他のピックアンドプレースワークフローについては、以下をご覧ください。

概要

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

Robotics System Toolbox™ を使用して、マニピュレーターをモデル化し、シミュレートします。
"ROS Toolbox" を使用して、MATLAB® を Gazebo に接続します。
"Image Processing Toolbox™" および "Computer Vision Toolbox™" は、Gazebo で点群処理およびシミュレートされた深度カメラを使用したオブジェクト検出に使用されます。

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

Pick and Place Using RRT for Manipulators
Gazebo およびシミュレートされた TurtleBot の入門 (ROS Toolbox) (ROS Toolbox)
3 次元点群のレジストレーションと繋ぎ合わせ (Computer Vision Toolbox) (Computer Vision Toolbox)

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

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

この例では、こちらからダウンロード可能な、ROS Melodic が含まれるバーチャルマシン (VM) を使用します。

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

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

rosshutdown;

rosinit(rosIP,11311); % Initialize ROS connection

Initializing global node /matlab_global_node_36341 with NodeURI http://192.168.17.1:59807/ and MasterURI http://192.168.17.130:11311.

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

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

ピックアンドプレースタスク

ピックアンドプレースワークフローは MATLAB で実装され、基本的な初期化手順と、それに続く 2 つの主要セクションで構成されます。

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

Stateflow を使用してタスクをスケジュールする実装については、MATLAB 向けに Stateflow を使用したピックアンドプレースのワークフローを参照してください。

RRT パスプランナーのプランニングシーンを作成するための環境のスキャン

ピックアンドプレースジョブを開始する前に、ロボットは一連のタスクを実行して関数 exampleCommandBuildWorld でプランニングシーンを特定し、関数 exampleCommandDetectParts を使用してピックするオブジェクトを検出します。

まず、ロボットは事前定義されたスキャン姿勢に 1 つずつ移動し、オンボードの深度センサーを使用してシーンの一連の点群を取得します。スキャン姿勢ごとに、rostf (ROS Toolbox)およびgetTransform (ROS Toolbox)を使用して対応する ROS 変換を読み取って、現在のカメラの姿勢が取得されます。以下にスキャン姿勢を可視化します。

ロボットがすべてのスキャン姿勢をとったら、pctransform (Computer Vision Toolbox)を使用して取得された点群がカメラ座標系からワールド座標系に変換され、pcmerge (Computer Vision Toolbox)を使用して単一の点群にマージされます。最終の点群は、pcsegdist (Computer Vision Toolbox)を使用してユークリッド距離に基づいてセグメント化されます。結果の点群セグメントは、RRT パスプランニング時に障害物として容易に特定できるように、衝突メッシュ (collisionMeshを参照) として符号化されます。一度に 1 つのメッシュずつ、点群から衝突メッシュへのプロセスを以下に示します。

グリッパーの開閉

グリッパーをアクティブにするためのコマンド exampleCommandActivateGripper は、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);

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

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

計画されたパスは、MATLAB でプランニングシーンとともに可視化されます。

このワークフローは、MATLAB 向けに RRT プランナーと Stateflow を使用したピックアンドプレースのワークフローの例で詳細に説明しています。RRT プランナーの詳細については、Pick and Place Using RRT for Manipulatorsを参照してください。単純な軌跡でパスに障害物のないことがわかっている場合には、軌跡生成ツールを使用して軌跡を実行し、マニピュレーター運動モデルを使用して軌跡をシミュレートすることができます。Kinova Gen3 マニピュレーターを使用したタスク空間およびジョイント空間の軌跡の計画と実行を参照してください。

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

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

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

関数 exampleCommandDetectParts および exampleCommandClassifyParts は、ロボットからのシミュレートされたエンドエフェクタ深度カメラフィードを使用し、リサイクル可能なパーツを検出します。シーンの完全な点群は "環境の作成" の手順で使用可能になるため、pcregistericp (Computer Vision Toolbox)で実装されている反復最近接点 (ICP) レジストレーションアルゴリズムにより、ピックする必要があるオブジェクトのジオメトリに一致するセグメント化された点群を特定します。

ピックアンドプレースワークフローの開始

このシミュレーションでは、グリッパーが接続された KINOVA Gen3 マニピュレーターを使用します。

load('exampleHelperKINOVAGen3GripperGazeboRRTScene.mat'); 
rng(0)

ピックアンドプレースアプリケーションの開始

ロボットの初期コンフィギュレーションおよびエンドエフェクタボディの名前を設定します。

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

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

coordinator = exampleHelperCoordinatorPickPlaceROSGazeboScene(robot,initialRobotJConfig, endEffectorFrame);

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

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]);

ピックアンドプレースアプリケーションの段階的な実行

% Task 1: Build world
exampleCommandBuildWorldROSGazeboScene(coordinator);

Moving to scanning pose 1
Searching for other config...
Now planning...
Waiting until robot reaches the desired configuration
Capturing point cloud 1
Getting camera pose 1
Moving to scanning pose 2
Now planning...
Waiting until robot reaches the desired configuration
Capturing point cloud 2
Getting camera pose 2
Moving to scanning pose 3
Searching for other config...
Now planning...
Waiting until robot reaches the desired configuration
Capturing point cloud 3
Getting camera pose 3
Moving to scanning pose 4
Now planning...
Waiting until robot reaches the desired configuration
Capturing point cloud 4
Getting camera pose 4
Moving to scanning pose 5
Now planning...
Waiting until robot reaches the desired configuration
Capturing point cloud 5
Getting camera pose 5

% Task 2: Move to home position
exampleCommandMoveToTaskConfigROSGazeboScene(coordinator,coordinator.HomeRobotTaskConfig);

Now planning...
Waiting until robot reaches the desired configuration

% Task 3: Detect objects in the scene to pick
exampleCommandDetectPartsROSGazeboScene(coordinator);

Bottle detected...
Can detected...

% Task 4: Select next part to pick
remainingParts = exampleCommandPickingLogicROSGazeboScene(coordinator);

while remainingParts==true
    % Task 5: [PICKING] Compute grasp pose
    exampleCommandComputeGraspPoseROSGazeboScene(coordinator);
    
    % Task 6: [PICKING] Move to picking pose
    exampleCommandMoveToTaskConfigROSGazeboScene(coordinator, coordinator.GraspPose);
    
    % Task 7: [PICKING] Activate gripper
    exampleCommandActivateGripperROSGazeboScene(coordinator,'on');
    
    % Part has been picked
    
    % Task 8: [PLACING] Move to placing pose
    exampleCommandMoveToTaskConfigROSGazeboScene(coordinator, ...
    coordinator.PlacingPose{coordinator.DetectedParts{coordinator.NextPart}.placingBelt});
    
    % Task 9: [PLACING] Deactivate gripper
    exampleCommandActivateGripperROSGazeboScene(coordinator,'off');
    
    % Part has been placed
    
    % Select next part to pick
    remainingParts = exampleCommandPickingLogicROSGazeboScene(coordinator);    

    % Move to home position
    exampleCommandMoveToTaskConfigROSGazeboScene(coordinator,coordinator.HomeRobotTaskConfig);
end

Now planning...
Waiting until robot reaches the desired configuration

Gripper closed...

Now planning...
Waiting until robot reaches the desired configuration

Gripper open...

Now planning...
Waiting until robot reaches the desired configuration

Now planning...
Waiting until robot reaches the desired configuration

Gripper closed...

Now planning...
Waiting until robot reaches the desired configuration

Gripper open...

Now planning...
Waiting until robot reaches the desired configuration

% Shut down ros when the pick-and-place application is done
rosshutdown;

Shutting down global node /matlab_global_node_36341 with NodeURI http://192.168.17.1:59807/ and MasterURI http://192.168.17.130:11311.

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

Gazebo ワールドは、作業領域でロボットがパーツをごみ箱に移動する様子を示します。すべてのパーツが配置されるまで、ロボットは動作を続けます。

点群処理および RRT パス プランニングを使用した Gazebo でのピックアンドプレース ワークフロー

概要

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

ピックアンドプレース タスク

RRT パス プランナーのプランニング シーンを作成するための環境のスキャン