Main Content

点群処理および 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 で点群処理およびシミュレートされた深度カメラを使用したオブジェクト検出に使用されます。

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

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 = '172.21.72.160'; % IP address of ROS-enabled machine  

rosshutdown;

rosinit(rosIP,11311); % Initialize ROS connection
Initializing global node /matlab_global_node_06101 with NodeURI http://192.168.71.1:37621/ and MasterURI http://192.168.71.128: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);
Warning: MATLAB has disabled some advanced graphics rendering features by switching to software OpenGL. For more information, click <a href="matlab:opengl('problems')">here</a>.

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

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);
     1
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...
     2
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_06101 with NodeURI http://192.168.71.1:37621/ and MasterURI http://192.168.71.128:11311.

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

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

Copyright 2021 The MathWorks, Inc.