ROS2 pick and place for UR5 using Moveit2

This repo provides ROS2 packages for initiating a pick-and-place service for the UR5 Robotic Manipulator combining a Robotiq85 2 DoF gripper.

It is part of the following project: AR pick and place with UR5

Prerequisites

The following Linux version as well as the ROS distribution is required:

• Ubuntu 20.04 / ROS 2 Galactic Geochelone

Packages in the Repository:

1. moveit_ur5_msgs - custom message package containing one srv message for moveit_ur5_interface.
2. moveit_ur5_interface - a service/client interface to control the UR5 robot using moveit2.
3. pnp_msgs - custom message package containing three action messages for pnp action.
4. pnp_actionlib - implementation of a pnp action service for the UR5 robot.
5. robotiq_description - description files for the robotiq gripper: meshes, URDF/XACRO files, etc.
6. ur5_gripper_description - description files for the UR5 combined with the gripper.
7. ur5_gripper_moveit_config - MoveIt configuration for UR5 with the gripper.

Installation

• First you'll need to clone this repo into the src folder of your workspace

  cd <COLCON_WS>/src
  git clone https://github.com/JuoTungChen/ROS2_pick_and_place_UR5.git
  

• To control the UR5, the official UR5 driver needs to be installed.
  You can install it by running the following command:

  vcs import src < galactic.repos
  

• To control the gripper, the official gripper driver needs to be installed.
  You can follow this link to setup the driver for the gripper: gripper driver installation guide

Moveit UR5 msgs

This package provides a custom service type to request trajectory planning from Moveit2.

The message moveit_ur5_msgs/srv/PlanRequest has the following format:

geometry_msgs/Pose target 

--- 

string result 

Moveit UR5 Interface

This package provides a service/client interface between the official Universal Robots ROS2 Driver and the Moveit2 API. The package takes position commands through the custom service moveit_ur5_msgs described above and generates a trajectory for the robot using Moveit2. It then uses the UR5 ROS2 driver to control the robot and follow the trajectory.

Launch Files

The package provides the following launch files:

1. ros2 launch moveit_ur5_interface moveit_ur5_interface_sim.launch
   This launch file is used when you don't have the actual UR5 and want to see it in simulation. It starts the UR5 driver with fake hardware controller and the Moveit2 server. It launches the UR5 Moveit configuration with the gripper attached to the end effector and opens Rviz. You can call services or run a client node that publishes the target pose to move the robot to anywhere you want.

2. ros2 launch moveit_ur5_interface moveit_ur5_interface.launch
   This launch file is used when you want to actually control a real UR5. It starts up the UR5 driver and the Moveit2 server with custom settings. It launches the UR5 Moveit configuration with the gripper attached to the end effector and opens Rviz.
   To connect to a real UR5, you can follow this instruction: Setting up a UR robot for ur_robot_driver
   After finishing setting up the robot, you can run the launch file, and press the play button on the pendant.

Executables

The package provides the following executables:

1. moveit_server_node.cpp

   ros2 run moveit_ur5_interface moveit_srv_node
   

   This node initiates a server that communicates with the move group node when receiving a planning request.

2. moveit_client_node.cpp

   ros2 run moveit_ur5_interface moveit_cli_node
   

   This is an example client node that sends a planning request to the moveit_server_node.cpp. It constructs and sends a request to the server by calling the following function (overloaded with two variants):

   call(const double &x, const double &y, const double &z, const double &q_x, const double &q_y, const double &q_z, const double &q_w);
   
   call(const geometry_msgs::msg::Pose &target_pose);
   

3. moveit_interface.cpp

   ros2 run moveit_ur5_interface moveit_interface
   

   This is an simple example of using the Moveit C++ API to control the UR5. It uses the move group to plan and execute a path for the UR5. It does the same thing as moveit_server but without the ability to initiate a planning request from the client.

Usage

1. To send a planning request from the command line, use the following commands:

   ros2 launch moveit_ur5_interface moveit_ur5_interface.launch 
   
   ros2 service call /moveit_ur5/target_pose moveit_ur5_msgs/srv/PlanRequest "{target:{position: {x: -0.3, y: -0.4, z: 0.6}, orientation: {x: -1.0, y: 0.0, z: 0.0, w: 0.0}}}"
   

2. To send a plan request from the client node, modify the target within the moveit_client_node.cpp file and use the following commands:

   ros2 launch moveit_ur5_interface moveit_ur5_interface.launch 
   
   ros2 run moveit_ur5_interface moveit_cli_node
   

NOTE: After launching the moveit_ur5_interface.launch, be sure to close the joint_state_publisher_gui before executing the motion of the robot. (The joint_state_publisher_gui is there just to avoid move_group complaining about missing joint state for the gripper.)

PNP msgs

This package provides a custom action message type to request a pick-and-place action for the robot.

There are three action messages in this package:

1. Robot.action:

   a. Goal: A geometry_msgs/Pose indicating the target pose of the robot

   b. Result: A string that specifies a success or failure of the robot action.

   c. Feedback: A floating point value between 0 and 1 that represents the progress of the robot’s motion (0 at the beginning and 1 when completed).

2. Gripper.action:

   a. Goal: A string that indicates if the gripper must open or close.

   b. Result: A string that specifies a success or failure of the gripper action.

   c. Feedback: A floating point value between 0 and 1 that represents the progress of the gripper’s motion (0 at the beginning and 1 when completed).

3. PickAndPlace.action:

   a. Goal: There are two entries. One corresponds to a target pose where the robot must pick the object (a geometry_msgs/Pose that can be passed to Robot.action). The other corresponds to a target pose where the robot must place the object (also a geometry_msgs/Pose).

   b. Result: A string that specifies a success or failure of the pnp action.

   c. Feedback: A string that indicates if 1. the robot is moving to pick the object 2. closing the gripper 3. moving to place the object
   4. opening the gripper.

PNP actionlib

This package provides a action for initiating a pick-and-place task for the UR5 manipulator.

Launch Files

The package provides the following launch files:

1. ros2 launch pnp_actionlib ur5_pnp_actionlib_sim.launch
   This launch file is used when you don't have the actual UR5 and want to see it in simulation. It launches the moveit_ur5_interface_sim.launch and starts the pnp server that waits for action calls to initiate a pnp action service.

2. ros2 launch pnp_actionlib ur5_pnp_actionlib.launch
   This launch file is used when you want to actually control a real UR5. It launches the moveit_ur5_interface.launch and starts the pnp server that waits for action calls to initiate a pnp action service.
   To connect to a real UR5, you can follow this instruction: Setting up a UR robot for ur_robot_driver
   After finishing setting up the robot, you can run the launch file, and press the play button on the pendant.

Executables

The package provides the following executables:

1. pnp_server.cpp

   ros2 run pnp_actinlib server
   

   This node initiates a action server waits for action calls to initiate a pnp action service.

2. pnp_client.cpp

   ros2 run pnp_actinlib client
   

   This is an example client node that sends a action request to the pnp_server.cpp. It constructs and sends a request to the server by calling the following function:

   pnp_call(const geometry_msgs::msg::Pose &pick_destination, const geometry_msgs::msg::Pose &place_destination)
   

Usage

1. To send a pnp request from the command line, use the following commands:

   ros2 launch pnp_actionlib ur5_pnp_actionlib.launch
   
   ros2 action send_goal /pnp pnp_msgs/action/PickAndPlace "{pick_goal:{position: {x: 0.3, y: 0.4, z: 0.3}, orientation: {x: -1.0, y: 0.0, z: 0.0, w: 0.0}}, place_goal:{position: {x: -0.2, y: 0.4, z: 0.3}, orientation: {x: -1.0, y: 0.0, z: 0.0, w: 0.0}}}"
   

2. To send a plan request from the client node, modify the pick_goal and place_goal within the pnp_client.cpp file and use the following commands:

   ros2 launch pnp_actionlib ur5_pnp_actionlib.launch
   
   ros2 run pnp_actinlib client
   

3. To send a planning request within your own function, just include the following headers in your code:

   #include <pnp_actionlib/pnp_action.hpp>
   #include <pnp_actionlib/robot_action.hpp>
   #include <pnp_actionlib/gripper_action.hpp>
   

   Then, create a client node:

   auto pnpc = std::make_shared<rsp::pnp_client>("pnp");
   rclcpp::executors::MultiThreadedExecutor pnp_executor;
   pnp_executor.add_node(pnpc);
   

   Finally, call the pnp_call function and spin the node to start the pnp action:

   auto pnp_result = pnpc->pnp_call(pick_goal, place_goal);
   pnp_executor.spin_until_future_complete(pnp_result);
   

Robotiq Description

This package basically is just the ROS2 version of the following package: robotiq
It contains the urdf and meshes for the robotiq gripper.

UR5 Gripper Description

This package combines the urdf of UR5 with the robotiq gripper. It attaches the gripper at the tool0 frame of the UR5. And a frame ee_link was created at the center of the gripper so that we can use moveit2 to do cartesian planning with respect to this frame.

UR5 Gripper Moveit Config

This package contains the srdf for the UR5 with a robotiq gripper as a end effector.

Launch Files

1. ros2 launch ur5_gripper_moveit_config ur5_gripper_moveit.launch.py
   This launch file launches the moveit_config for the UR5 robot with a robotiq gripper as its end effector.

Usage

First launch the following launch file to spawn the controller for the robot.
ros2 launch ur_bringup ur_control.launch.py ur_type:=ur5e robot_ip:=yyy.yyy.yyy.yyy use_fake_hardware:=true launch_rviz:=false initial_joint_controller:=joint_trajectory_controller

After that, launch the moveit_config using the following command:
ros2 launch ur5_gripper_moveit_config ur5_gripper_moveit.launch.py

You can now use the interactive marker to drag around the end effector to desired pose and plan and execute the motion for the robot.