Overview

This example identifies and recycles objects into two bins using a KINOVA Gen3 manipulator. The example uses tools from five toolboxes:

This example builds on key concepts from the following related examples:

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) containing ROS Melodic available for downloadhere. If you have never used it before, see从露台和模拟TurtleBot开始(ROS Toolbox).

rosIP ='192.168.203.131';% IP address of ROS-enabled machinerosinit(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/

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 theros_kortexROS package, which are provided by KINOVA.

The packages useros_controlto control the joints to desired joint positions. For additional details on using the VM, refer to从露台和模拟TurtleBot开始(ROS Toolbox)

Stateflow Chart

This example uses a Stateflow chart to schedule tasks in the example. Open the chart to examine the contents and follow state transitions during chart execution.

editexampleHelperFlowChartPickPlaceROSGazebo.sfx

The chart dictates how the manipulator interacts with the objects, or parts. It consists of basic initialization steps, followed by two main sections:

For a high-level description of the pick-and-place steps, seePick-and-Place Workflow Using Stateflow for MATLAB.

Opening and closing the gripper

The command for activating the gripper,exampleCommandActivateGripperROSGazebo, sends an action request to open and close the gripper implemented in Gazebo. To send a request to open the gripper, the following code is used.Note:The example code shown just illustrates what the command does. Do not run.

[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

ThecommandMoveToTaskConfigcommand function is used to move the manipulator to specified poses.

Planning

The path planning generates simple task-space trajectories from an initial to a desired task configuration usingtrapveltrajandtransformtraj. For more details on planning and executing trajectories, seePlan and Execute Task- and Joint-Space Trajectories Using KINOVA Gen3 Manipulator.

Joint Trajectory Controller in ROS

After generating a joint trajectory for the robot to follow,commandMoveToTaskConfigsamples 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 functionscommandDetectPartsandcommandClassifyPartsuse the simulated end-effector camera feed from the robot and apply a pretrained deep-learning model to detect the recyclable parts. The model takes a camera frame as input and outputs the 2D location of the object (pixel position) and the type of recycling it requires (blue or green bin). The 2D location on the image frame is mapped to the robot base frame.

Deep-Learning Model Training: Acquiring and Labeling Gazebo Images

The detection model was trained using a set of images acquired in the simulated environment within the Gazebo world with the two classes of objects (bottle, can) placed on different locations of the table. The images are acquired from the simulated camera on-board the robot, which is moved along the horizontal and vertical planes to get images of the objects from many different camera perspectives.

The images are then labeled using theImage Labeler(Computer Vision Toolbox)app, creating the training dataset for the YOLO v2 detection model.trainYOLOv2ObjectDetector(Computer Vision Toolbox)trains the model. To see how to train a YOLO v2 network in MATLAB, seeTrain YOLO v2 Network for Vehicle Detection(Computer Vision Toolbox).

The trained model is deployed for online inference on the single image acquired by the on-board camera when the robot is in the home position. Thedetect(Computer Vision Toolbox)function returns the image position of the bounding boxes of the detected objects, along with their classes, that is then used to find the position of the center of the top part of the objects. Using a simple camera projection approach, assuming the height of the objects is known, the object position is projected into the world and finally used as reference position for picking the object. The class associated with the bounding boxed decides which bin to place the object.

Start the Pick-and-Place Task

This simulation uses a KINOVA Gen3 manipulator with aRobotiqgripper attached. Load the robot model from a.matfile as arigidBodyTreeobject.

load('exampleHelperKINOVAGen3GripperROSGazebo.mat');

在itialize the Pick and Place Coordinator

Set the initial robot configuration. Create the coordinator, which handles the robot control, by giving the robot model, initial configuration, and end-effector name.

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

在itialize the coordinator by giving the robot model, initial configuration, and end-effector name.

coordinator = exampleHelperCoordinatorPickPlaceROSGazebo(robot,initialRobotJConfig, endEffectorFrame);

Specify the home configuration and two poses for placing objects.

协调员。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 and Visualize the Simulation

Connect the coordinator to the Stateflow Chart. Once started, the Stateflow chart is responsible for continuously going through the states of detecting objects, picking them up and placing them in the correct staging area.

协调员。流程图= exampleHelperFlowChartPickPlaceROSGazebo('coordinator', coordinator);

Use a dialog to start the pick-and-place task execution. ClickYesin the dialog to begin the simulation.

answer = questdlg('Do you want to start the pick-and-place job now?',...'Start job','Yes','No','No');switchanswercase'Yes'% Trigger event to start Pick and Place in the Stateflow Chart协调员。FlowChart.startPickPlace;case'No'协调员。FlowChart.endPickPlace; delete(coordinator.FlowChart) delete(coordinator);end

Ending the pick-and-place task

The Stateflow chart will finish executing automatically after 3 failed attempts to detect new objects. To end the pick-and-place task prematurely, uncomment and execute the following lines of code or press Ctrl+C in the command window.

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

Observe the Simulation States

During execution, the active states at each point in time are highlighted in blue in the Stateflow chart. This helps keeping track of what the robot does and when. You can click through the subsystems to see the details of the state in action.

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. When the detection step doesn't find any more parts four times, the Stateflow chart exits.

ifstrcmp(answer,'Yes')while协调员。NumDetectionRuns < 4% Wait for no parts to be detected.endend

Shutdown the ROS network after finishing the example.

rosshutdown

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

版权2020年MathWorks,在c.