%% Pick-and-Place Workflow in Gazebo Using Point-Cloud Processing and RRT Path Planning
% Setup an end-to-end pick and place workflow for a robotic manipulator like the KINOVA® Gen3.
% The pick-and-place workflow implemented in this example can be adapted to different scenarios, planners, simulation platforms, and object detection options. The example shown here uses RRT for planning and simulates the robot in Gazebo using the Robot Operating System (ROS). For other pick-and-place workflows, see:
% Pick-and-Place Workflow using Stateflow for MATLAB
% Pick and Place Workflow Using RRT Planner and Stateflow for MATLAB
% Pick-and-Place Workflow in Gazebo using ROS
%
%% Overview
% This example identifies and recycles objects into two bins using a KINOVA Gen3 manipulator. The example uses tools from five toolboxes:
% Robotics System Toolbox™ is used to model and simulate the manipulator.
% ROS Toolbox™ is used for connecting MATLAB to Gazebo.
% Image Processing Toolbox™ and Computer Vision Toolbox™ are used for object detection using point cloud processing and simulated depth camera in Gazebo.
% This example builds on key concepts from the following related examples:
% Pick and Place Using RRT for Manipulators
% Get Started with Gazebo and a Simulated TurtleBot (ROS Toolbox)
% 3-D Point Cloud Registration and Stitching (Computer Vision Toolbox)
%
%% Robot Simulation and Control in Gazebo
% Start a ROS-based simulator for a KINOVA Gen3 robot and configure the MATLAB® connection with the robot simulator.
% This example uses a virtual machine (VM) available for download. See the Get Started with Gazebo and a Simulated TurtleBot (ROS Toolbox) example.
% Start the Ubuntu® virtual machine desktop.
% In the Ubuntu desktop, click the Gazebo Recycling World - Depth Sensing icon to start the Gazebo world built for this example.
% Specify the IP address and port number of the ROS master in Gazebo so that MATLAB® can communicate with the robot simulator. For this example, the ROS master in Gazebo uses the IP address of 172.21.72.160 displayed on the Desktop. Adjust the rosIP variable based on your VM.
% Start the ROS 1 network using rosinit.

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

rosshutdown;

rosinit(rosIP,11311); % Initialize ROS connection

% After initializing the Gazebo world by click the icon, the VM loads a KINOVA Gen3 Robot arm on a table with one recycling bin on each side. To simulate and control the robot arm in Gazebo, the VM contains the ros_kortex ROS package, which are provided by KINOVA.
% The packages use ros_control to control the joints to desired joint positions. For additional details on using the VM, refer to Get Started with Gazebo and a Simulated TurtleBot.
%% Pick-and-Place Tasks
% The Pick-and-Place workflow is implemented in MATLAB and consists of basic initialization steps, followed by two main sections:
% Identify Parts and Determine Where to Place Them
% Execute Pick-and-Place Workflow
%
% For an implementation that uses Stateflow to schedule the tasks, see Pick and Place Workflow using Stateflow for MATLAB.
%% Scanning the environment to build planning scene for RRT path planner
% Before starting the pick-and-place job, the robot goes through a set of tasks to identify the planning scene in the exampleCommandBuildWorld function and detects the objects to pick using the exampleCommandDetectParts function.
%
% First, the robot moves to predefined scanning poses one by one and captures a set of point clouds of the scene using an onboard depth sensor. At each of the scanning poses, the current camera pose is retrieved by reading the corresponding ROS transformation using rostf and getTransform. The scanning poses are visualized below:
%
% Once the robot has visited all the scanning poses, the captured point clouds are transformed from camera to world frame using pctransform and merged to a single point cloud using pcmerge. The final point cloud is segmented based on Euclidean distance using pcsegdist. The resulting point cloud segments are then encoded as collision meshes (see collisionMesh) to be easily identified as obstacles during RRT path planning. The process from point cloud to collision meshes is shown one mesh at at a time below.
%
%
%% Opening and closing the gripper
% The command for activating the gripper, exampleCommandActivateGripper, sends an action request to open and close the gripper implemented in Gazebo. For example, to send a request to open the gripper, the following code is used.
% [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);
%% Moving the manipulator to a specified pose
% Most of the task execution consists of instructing the robot to move between different specified poses. The exampleHelperMoveToTaskConfig function defines an RRT planner using the manipulatorRRT object, which plans paths from an initial to a desired joint configuration by avoiding collisions with specified collision objects in the scene. The resulting path is first shortened and then interpolated at a desired validation distance. To generate a trajectory, the trapveltraj function is used to assign time steps to each of the interpolated waypoints following a trapezoidal profile. Finally, the waypoints with their associated times are interpolated to a desired sample rate (every 0.1 seconds). The generated trajectories ensure that the robot moves slowly at the start and the end of the motion when it is approaching or placing an object.
%
% The planned paths are visualized in MATLAB along with the planning scene.
%
% This workflow is examined in detail in the Pick and Place Workflow Using RRT Planner and Stateflow for MATLAB example. For more information about the RRT planner, see Pick and Place Using RRT for Manipulators. For simpler trajectories where the paths are known to be obstacle-free, trajectories could be executed using trajectory generation tools and simulated using the manipulator motion models. See Plan and Execute Task- and Joint-Space Trajectories Using a KINOVA Gen3 Manipulator.
%% Joint Trajectory Controller in ROS
% After generating a joint trajectory for the robot to follow, the exampleCommandMoveToTaskConfig function samples the trajectory at the desired sample rate, packages it into joint-trajectory ROS messages and sends an action request to the joint-trajectory controller implemented in the KINOVA ROS package.
%% Detecting and classifying objects in the scene
% The functions exampleCommandDetectParts and exampleCommandClassifyParts use the simulated end-effector depth camera feed from the robot to detect the recyclable parts. Since a complete point cloud of the scene is available from the Build Environment step, the iterative closest point (ICP) registration algorithm implemented in pcregistericp identifies which of the segmented point clouds match the geometries of objects that should be picked.
%
%% Start the Pick-and-Place Workflow
% This simulation uses a KINOVA Gen3 manipulator with a gripper attached.

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

%% Initialize the Pick-and-Place Application
% Set the initial robot configuration and name of the end-effector body.

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

% Initialize the coordinator by giving the robot model, initial configuration, and end-effector name.

coordinator = exampleHelperCoordinatorPickPlaceROSGazeboScene(robot,initialRobotJConfig, endEffectorFrame);

% Specify pick-and-place coordinator properties.

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

%% Run the Pick-and-Place Application Step by Step

% Task 1: Build world
exampleCommandBuildWorldROSGazeboScene(coordinator);
% Task 2: Move to home position
exampleCommandMoveToTaskConfigROSGazeboScene(coordinator,coordinator.HomeRobotTaskConfig);
% Task 3: Detect objects in the scene to pick
exampleCommandDetectPartsROSGazeboScene(coordinator);

% 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);    
end
% Shut down ros when the pick-and-place application is done
rosshutdown;

%% Visualize the Pick-and-Place Action in Gazebo
% The Gazebo world shows the robot in the working area as it moves parts to the recycling bins. The robot continues working until all parts have been placed.
%
%
%
%
%  Copyright 2021 The MathWorks, Inc.
%
%
%