Implementation of a Real-Time Object Pick-and-Place System Based on a Changing Strategy for Rapidly-Exploring Random Tree

An object pick-and-place system with a camera, a six-degree-of-freedom (DOF) robot manipulator, and a two-finger gripper is implemented based on the robot operating system (ROS) in this paper. A collision-free path planning method is one of the most fundamental problems that has to be solved before the robot manipulator can autonomously pick-and-place objects in complex environments. In the implementation of the real-time pick-and-place system, the success rate and computing time of path planning by a six-DOF robot manipulator are two essential key factors. Therefore, an improved rapidly-exploring random tree (RRT) algorithm, named changing strategy RRT (CS-RRT), is proposed. Based on the method of gradually changing the sampling area based on RRT (CSA-RRT), two mechanisms are used in the proposed CS-RRT to improve the success rate and computing time. The proposed CS-RRT algorithm adopts a sampling-radius limitation mechanism, which enables the random tree to approach the goal area more efficiently each time the environment is explored. It can avoid spending a lot of time looking for valid points when it is close to the goal point, thus reducing the computing time of the improved RRT algorithm. In addition, the CS-RRT algorithm adopts a node counting mechanism, which enables the algorithm to switch to an appropriate sampling method in complex environments. It can avoid the search path being trapped in some constrained areas due to excessive exploration in the direction of the goal point, thus improving the adaptability of the proposed algorithm to various environments and increasing the success rate. Finally, an environment with four object pick-and-place tasks is established, and four simulation results are given to illustrate that the proposed CS-RRT-based collision-free path planning method has the best performance compared with the other two RRT algorithms. A practical experiment is also provided to verify that the robot manipulator can indeed complete the specified four object pick-and-place tasks successfully and effectively.


Introduction
Due to the presence of various objects in the working environment, path planning for robots is one of the most important topics in robotics research and is widely discussed [1][2][3][4]. If the robot does not have a good path planning method to choose a collision-free path, various collision situations may occur. Once a collision occurs, unpredictable or large losses may be caused. Therefore, many researchers devote themselves to the field of collisionfree path planning. Based on differential search methods, path planning algorithms are divided into three categories: search-based, heuristic-based, and sampling-based. The A* algorithm proposed by Hart et al. is a search-based path planning algorithm [5]. It There are six sections in this paper. In Section 1, the background is introduced. In Section 2, an implemented object pick-and-place system based on ROS is described. In Section 3, three sampling strategies used in the RRT algorithms are described. In Section 4, a basic RRT algorithm and an improved RRT algorithm based on an existing RRT algorithm are described. In Section 5, an experimental environment with four pick-and-place tasks is setup, and some simulation and actual experimental results are presented to illustrate that the implemented object pick-and-place system using the proposed RRT algorithm can allow a robot manipulator to pick and place objects in real time. Finally, conclusions and future work are described in Section 6.

ROS-Based Object Pick-and-Place System
The system architecture diagram of the implemented ROS-based object pick-and-place system is shown in Figure 1. The input of this system is the RGB image captured by Microsoft Azure Kinect DK (a RGB-D camera), and the outputs are the control commands of Universal Robots UR5 (six-degree-of-freedom (DOF) robot manipulator) and Robotiq 2F-85 (a two-finger parallel gripper). Azure Kinect DK has the features of a wide field of view and easy installation; it is directly installed on a bracket to capture images on the table. UR5 has some features, such as a light weight, a user-friendly interface, and collision detection capability. The related unified robot description format (URDF) files for the specific MoveIt applications are also provided. These features make experiments easy to perform and avoid collisions between the robot manipulator and surrounding objects during the experiment. Robotiq 2F-85 is easily integrated into robot manipulators. It has the feature that it can avoid damage to the gripper itself and prevent the robot manipulator from injuring the object during the grasping task. ready available for more than 150 robots. Therefore, in the design of motion p MoveIt is used to complete the required motion of the robot manipulator.
There are six sections in this paper. In Section 1, the background is introd Section 2, an implemented object pick-and-place system based on ROS is desc Section 3, three sampling strategies used in the RRT algorithms are described. In 4, a basic RRT algorithm and an improved RRT algorithm based on an existing gorithm are described. In Section 5, an experimental environment wi pick-and-place tasks is setup, and some simulation and actual experimental re presented to illustrate that the implemented object pick-and-place system u proposed RRT algorithm can allow a robot manipulator to pick and place objec time. Finally, conclusions and future work are described in Section 6.

ROS-Based Object Pick-and-Place System
The system architecture diagram of the implemented ROS-based pick-and-place system is shown in Figure 1. The input of this system is the RG captured by Microsoft Azure Kinect DK (a RGB-D camera), and the outputs are trol commands of Universal Robots UR5 (six-degree-of-freedom (DOF) robot m tor) and Robotiq 2F-85 (a two-finger parallel gripper). Azure Kinect DK has the of a wide field of view and easy installation; it is directly installed on a bracket to images on the table. UR5 has some features, such as a light weight, a user-friend face, and collision detection capability. The related unified robot description (URDF) files for the specific MoveIt applications are also provided. These featur experiments easy to perform and avoid collisions between the robot manipul surrounding objects during the experiment. Robotiq 2F-85 is easily integrated in manipulators. It has the feature that it can avoid damage to the gripper itself and the robot manipulator from injuring the object during the grasping task. The object detection module is implemented by the you only look once (YO gorithm [14] to obtain the position information of the object on the table from The object detection module is implemented by the you only look once (YOLO) algorithm [14] to obtain the position information of the object on the table from the captured RGB image. The task strategy module is used to decide the destination of the robot manipulator when a pick-and-place task is given. The path planning module is implemented by the improved RRT algorithm, which can quickly select a collision-free path so that the robot manipulator and the two-finger gripper can successfully complete the given tasks in real-time.
In the system integration of the proposed pick-and-place system, ROS is used to handle the communication between each module. The robot motion information is calculated by MoveIt through ROS. The version of ROS Melodic with Ubuntu 18.04 is used. MoveIt is an open-source motion planning software that is a state-of-the-art implementation of robot motion and path planning. Thus, it is used for the motion planning of the robot manipulator. MoveIt provides a variety of functional packages for users to choose from and integrates various functional plugins such as kinematics, collision detection, and motion planning so that it can provide the desired motion planning for various robots. Moreover, MoveIt is a package of ROS, and it is highly integrated with ROS so that the results of motion planning by MoveIt can be easily transmitted to the robot manipulator through ROS. Users can use the 3D visualization tool RViz to visually present the motion planning results in the ROS. For MoveIt, the unified robot description format (URDF) and semantic robot description format (SRDF) are used to describe robots. The proposed RRT algorithm is designed for the robot manipulator UR5 in this paper, but it can be used for the other six DOF robot manipulators. MoveIt imports the URDF file to set the parameters of the robot and the simulated environment, then sends a request to the default library, the open motion planning library (OMPL), to design a suitable motion trajectory. After calculating a path, MoveIt will divide this path into the same distances and add information such as speed, acceleration, and the consumed time of the robot at each piece of the path. In addition, OMPL is the main library of sampling-based planning algorithms, which includes many modules of common RRT algorithms. Because of its modular program design, it is easy for users to add custom motion planning algorithms. Therefore, MoveIt is adopted as the motion planning software for the robot manipulator in this paper.
As shown in Figure 1, the image information is sent by the object detection module, and the motion information is calculated by MoveIt through ROS. Since ROS can transmit or receive different data through messages and services, the proposed system integrated through ROS can be applied to various input and output devices, so it has good applicability. In the communication mode between the object detection module, task strategy module, MoveIt, and UR5, there are mainly three two-way communication services in the implemented pick-and-place system, which are, respectively, named Service1, Service2, and Service3.
In order to ensure that the task strategy module can indeed receive the object position information from the object detection module, Service1 is used to make the control command of the task strategy module for the robot manipulator to move only after it has received the object position information. The nodes of the server and client of Service1 are the task strategy module and the object detection module, respectively. The request sent by the task strategy module to the server has a status value of 0 (false) or 1 (true) while the control command is received. The response is given by the object detection module as the client after it receives the request for the object coordinates (x, y) on the table. The nodes of the server and client of Service2 are the task strategy module and MoveIt, respectively. The request sent by the task strategy module as the server is the target position (x, y, z) of the end effector and the quaternion of the robot manipulator pose (w, x, y, z). The response given by MoveIt as the client after receiving the request is the result of forward and inverse kinematics and the motion trajectory obtained by RRT. The nodes of the server and client of Service3 are MoveIt and UR5, respectively. The request sent by MoveIt as the server is the joint motion trajectory of the robot manipulator. The response given by UR5 as the client after receiving the request is a status value of 0 (false) or 1 (true), depending on whether UR5 is busy or not.

Sampling Strategy
Random sampling is the sampling strategy used by the basic RRT algorithm. The algorithm randomly samples the entire space to determine sampling points. The simulation result of 1000 random samplings in a 200 × 200 two-dimensional space is shown in Figure 2, where the black point is the goal point. It can be seen that the sampling points randomly fall throughout the entire space. For the RRT algorithms, random sampling can fully explore the environment, find a path from the start point to the goal point, and avoid obstacles in the environment. However, due to its randomness, it spends a lot of time exploring invalid areas in most environments. Therefore, many improved sampling strategies were proposed to reduce computing time.

Sampling Strategy
Random sampling is the sampling strategy used by the basic RRT algorithm. The algorithm randomly samples the entire space to determine sampling points. The simulation result of 1000 random samplings in a 200 × 200 two-dimensional space is shown in Figure 2, where the black point is the goal point. It can be seen that the sampling points randomly fall throughout the entire space. For the RRT algorithms, random sampling can fully explore the environment, find a path from the start point to the goal point, and avoid obstacles in the environment. However, due to its randomness, it spends a lot of time exploring invalid areas in most environments. Therefore, many improved sampling strategies were proposed to reduce computing time. Goal-biased sampling is an improved sampling strategy over random sampling. By adding a random variable and set: if is less than the specified probability , then the point will be selected as a sampling point. Otherwise, use the original random sampling method to randomly sample the space. This sampling strategy causes the random tree of the RRT algorithm to take the goal point to sample with a certain probability. In this case, random trees can approach the goal area faster and reduce computing time while maintaining random sampling to fully explore the environment. The simulation result of 1000 goal-biased sampling points in a 200 × 200 two-dimensional space is shown in Figure 3. It can be seen that more sampling points fall within the goal area. Bounded sampling is a sampling strategy to limit the sampling area to a radius from the goal point and let the random tree explore toward the goal area. The simulation result of 1000 bounded sampling in a 200 × 200 two-dimensional space is shown in Figure 4. It Goal-biased sampling is an improved sampling strategy over random sampling. By adding a random variable rand and set: if rand is less than the specified probability p, then the point will be selected as a sampling point. Otherwise, use the original random sampling method to randomly sample the space. This sampling strategy causes the random tree of the RRT algorithm to take the goal point to sample with a certain probability. In this case, random trees can approach the goal area faster and reduce computing time while maintaining random sampling to fully explore the environment. The simulation result of 1000 goal-biased sampling points in a 200 × 200 two-dimensional space is shown in Figure 3. It can be seen that more sampling points fall within the goal area.

Sampling Strategy
Random sampling is the sampling strategy used by the basic RRT algorithm. The algorithm randomly samples the entire space to determine sampling points. The simulation result of 1000 random samplings in a 200 × 200 two-dimensional space is shown in Figure 2, where the black point is the goal point. It can be seen that the sampling points randomly fall throughout the entire space. For the RRT algorithms, random sampling can fully explore the environment, find a path from the start point to the goal point, and avoid obstacles in the environment. However, due to its randomness, it spends a lot of time exploring invalid areas in most environments. Therefore, many improved sampling strategies were proposed to reduce computing time. Goal-biased sampling is an improved sampling strategy over random sampling. By adding a random variable and set: if is less than the specified probability , then the point will be selected as a sampling point. Otherwise, use the original random sampling method to randomly sample the space. This sampling strategy causes the random tree of the RRT algorithm to take the goal point to sample with a certain probability. In this case, random trees can approach the goal area faster and reduce computing time while maintaining random sampling to fully explore the environment. The simulation result of 1000 goal-biased sampling points in a 200 × 200 two-dimensional space is shown in Figure 3. It can be seen that more sampling points fall within the goal area. Bounded sampling is a sampling strategy to limit the sampling area to a radius from the goal point and let the random tree explore toward the goal area. The simulation result of 1000 bounded sampling in a 200 × 200 two-dimensional space is shown in Figure 4. It  Bounded sampling is a sampling strategy to limit the sampling area to a radius from the goal point and let the random tree explore toward the goal area. The simulation result of 1000 bounded sampling in a 200 × 200 two-dimensional space is shown in Figure 4. It can be seen that the sampling points are limited within a radial space centered on the goal point. Compared with the RRT algorithm using a goal-biased sampling strategy, the RRT algorithm using bounded sampling can make the random tree explore the goal area stably. By gradually reducing the sampling radius, this algorithm can find a path to the goal point. However, since such bounded sampling may be overly focused on exploring towards the goal area, random trees can easily get trapped in some complex environments. can be seen that the sampling points are limited within a radial space centered on the goal point. Compared with the RRT algorithm using a goal-biased sampling strategy, the RRT algorithm using bounded sampling can make the random tree explore the goal area stably. By gradually reducing the sampling radius, this algorithm can find a path to the goal point. However, since such bounded sampling may be overly focused on exploring towards the goal area, random trees can easily get trapped in some complex environments.

Changing Strategy RRT Algorithm
In the design of the sampling-based path planning method, two items, such as computing time and path length, are usually considered. For offline path planning, the path length is usually the main consideration. On the other hand, for online path planning, computing time is the priority consideration. Real-time object picking and placing tasks require online path planning, so we mainly focus on how to reduce the computing time of the improved RRT algorithm.
The basic RRT algorithm, as a sampling-based path planning method, is mainly designed to perform random sampling in the configuration space [15]. A schematic illustration of the basic RRT algorithm for finding a path from a starting point (S) to a goal point (G) in a two-dimensional space is shown in Figure 5. It can be seen that the random tree fully explores the environment. The advantage of this method is that it does not require modeling the entire environment. Such path planning algorithms can explore two-dimensional spaces faster than other path planning algorithms. Therefore, it is suitable for solving the path planning problem in complex or constrained environments. The proposed algorithm is named changing strategy RRT (CS-RRT), which is improved on the basis of the method of gradually changing the sampling area based on RRT (CSA-RRT) [9]. The pseudocode of the CSA-RRT algorithm is shown in Algorithm 1.

Changing Strategy RRT Algorithm
In the design of the sampling-based path planning method, two items, such as computing time and path length, are usually considered. For offline path planning, the path length is usually the main consideration. On the other hand, for online path planning, computing time is the priority consideration. Real-time object picking and placing tasks require online path planning, so we mainly focus on how to reduce the computing time of the improved RRT algorithm.
The basic RRT algorithm, as a sampling-based path planning method, is mainly designed to perform random sampling in the configuration space [15]. A schematic illustration of the basic RRT algorithm for finding a path from a starting point (S) to a goal point (G) in a two-dimensional space is shown in Figure 5. It can be seen that the random tree fully explores the environment. The advantage of this method is that it does not require modeling the entire environment. Such path planning algorithms can explore two-dimensional spaces faster than other path planning algorithms. Therefore, it is suitable for solving the path planning problem in complex or constrained environments.
RRT algorithm using bounded sampling can make the random tree explor stably. By gradually reducing the sampling radius, this algorithm can find goal point. However, since such bounded sampling may be overly focused towards the goal area, random trees can easily get trapped in some com ments.

Changing Strategy RRT Algorithm
In the design of the sampling-based path planning method, two i computing time and path length, are usually considered. For offline path path length is usually the main consideration. On the other hand, for onl ning, computing time is the priority consideration. Real-time object pickin tasks require online path planning, so we mainly focus on how to reduce time of the improved RRT algorithm.
The basic RRT algorithm, as a sampling-based path planning method signed to perform random sampling in the configuration space [15]. A sc tration of the basic RRT algorithm for finding a path from a starting poin point (G) in a two-dimensional space is shown in Figure 5. It can be seen th tree fully explores the environment. The advantage of this method is that quire modeling the entire environment. Such path planning algorithm two-dimensional spaces faster than other path planning algorithms. There able for solving the path planning problem in complex or constrained envi  The proposed algorithm is named changing strategy RRT (CS-RRT), which is improved on the basis of the method of gradually changing the sampling area based on RRT (CSA-RRT) [9]. The pseudocode of the CSA-RRT algorithm is shown in Algorithm 1. It needs to calculate the distance D max between the two nodes q f ar and q goal , where q f ar is the node farthest from the goal point q goal . Since the tree only has the initial node q start at the beginning, the algorithm will initially use q start as q f ar to calculate D max , which is the initial sampling radius R of bounded sampling. When the dimension of the space is s, the maximum distance D max is calculated by: The CSA-RRT algorithm uses the random sampling method to randomly select a sampling point q rand in the space. The distance D rand between q rand and q goal is calculated by: Compare the distance between D rand and R to make sure that the sampling point q rand is inside R. If D rand is less than R, then q rand is considered a valid sampling point. Conversely, if D rand is greater than R, it means that q rand is outside R, and the algorithm will resample until q rand is inside R. If a new node q new is successfully added to the random tree in an iteration, it means that there is no obstacle between q new and the nearest node q near . When the new point q new is closer to the goal point q goal , it becomes the nearest node. At this time, the value of R is changed to the distance from the new nearest node q new to the goal point q goal . Conversely, it means that an obstacle is encountered during the expansion process. At this time, a step size ε of k times is added to R, which means that the sampling area is expanded so that the random tree can avoid nearby obstacles. The value of k is a positive integer for adjusting the sampling radius, which needs to be manually adjusted according to the complexity of the environment. This allows the algorithm to explore the direction of q goal as much as possible while having the ability to randomly explore the environment. The comparison results of the CSA-RRT algorithm and the 10% goal-biased RRT algorithm are shown in Figure 6. It can be seen that the CSA-RRT algorithm can reduce the generation of invalid nodes more than the goal-biased RRT algorithm. The CSA-RRT algorithm has the advantage that the invalid nodes of the CSA-RRT algorithm are much lower than those of the goal-biased RRT algorithm. However, the computing time of the CSA-RRT algorithm is not much faster than that of the goal-biased RRT algorithm. After observation, we found that although the sampling radius will gradually shrink as gets closer to the goal area, thereby reducing the generation of invalid points. However, on the other hand, because of the reduction in , selected by random sampling becomes more and more difficult to fall within . As shown in Figure 7, when the new point is close to the goal point , will become smaller and smaller. This results in a very small chance that the sampling point will fall within . As a result, the CSA-RRT algorithm spends a lot of time doing computation at certain stages. Therefore, the CSA-RRT algorithm has the advantage of generating fewer invalid nodes, but it still cannot significantly reduce the computing time of path planning. This becomes more apparent when sampling in larger environments.
A sampling-radius limitation mechanism is adopted to solve this problem that is difficult to fall into when is close to . An additional statement is used to determine whether the random tree is approaching . Whenever is added to the random tree, R and are compared before the next sampling. If is greater than one-fifth of , it means that the random tree is still far away from . Thus, it continues to use the random sampling of the CSA-RRT algorithm to select . On the other hand, if R is smaller than one-fifth of , it means that the random tree is close to . At this time, a sampling-radius limitation mechanism based on bounded sampling is adopted to limit the sampling area within the radius from the goal point. In this way, the problem that cannot successfully fall within when it is close to the goal point can be solved. This makes the proposed CS-RRT algorithm not only quickly find valid nodes but also reduce the computing time of path planning. In the case of two-dimensional simulations, the CS-RRT algorithm improves by about 0.5 times compared with the CSA-RRT algorithm.  In addition, the CSA-RRT algorithm has the advantage that it can quickly find an initial path to the goal area. However, as shown in Figure 8, if the CSA-RRT algorithm is The CSA-RRT algorithm has the advantage that the invalid nodes of the CSA-RRT algorithm are much lower than those of the goal-biased RRT algorithm. However, the computing time of the CSA-RRT algorithm is not much faster than that of the goal-biased RRT algorithm. After observation, we found that although the sampling radius R will gradually shrink as q new gets closer to the goal area, thereby reducing the generation of invalid points. However, on the other hand, because of the reduction in R, q rand selected by random sampling becomes more and more difficult to fall within R. As shown in Figure 7, when the new point q new is close to the goal point q goal , R will become smaller and smaller. This results in a very small chance that the sampling point q rand will fall within R. As a result, the CSA-RRT algorithm spends a lot of time doing computation at certain stages. Therefore, the CSA-RRT algorithm has the advantage of generating fewer invalid nodes, but it still cannot significantly reduce the computing time of path planning. This becomes more apparent when sampling in larger environments.
if Distance(q rand ,q goal ) > R then 6.
end if 20. end for 21. return Failed; invalid points. However, on the other hand, because of the reduction in , selected by random sampling becomes more and more difficult to fall within . As shown in Figure 7, when the new point is close to the goal point , will become smaller and smaller. This results in a very small chance that the sampling point will fall within . As a result, the CSA-RRT algorithm spends a lot of time doing computation at certain stages. Therefore, the CSA-RRT algorithm has the advantage of generating fewer invalid nodes, but it still cannot significantly reduce the computing time of path planning. This becomes more apparent when sampling in larger environments.
A sampling-radius limitation mechanism is adopted to solve this problem that is difficult to fall into when is close to . An additional statement is used to determine whether the random tree is approaching . Whenever is added to the random tree, R and are compared before the next sampling. If is greater than one-fifth of , it means that the random tree is still far away from . Thus, it continues to use the random sampling of the CSA-RRT algorithm to select . On the other hand, if R is smaller than one-fifth of , it means that the random tree is close to . At this time, a sampling-radius limitation mechanism based on bounded sampling is adopted to limit the sampling area within the radius from the goal point. In this way, the problem that cannot successfully fall within when it is close to the goal point can be solved. This makes the proposed CS-RRT algorithm not only quickly find valid nodes but also reduce the computing time of path planning. In the case of two-dimensional simulations, the CS-RRT algorithm improves by about 0.5 times compared with the CSA-RRT algorithm.  In addition, the CSA-RRT algorithm has the advantage that it can quickly find an initial path to the goal area. However, as shown in Figure 8, if the CSA-RRT algorithm is A sampling-radius limitation mechanism is adopted to solve this problem that q rand is difficult to fall into R when q new is close to q goal . An additional statement is used to determine whether the random tree is approaching q goal . Whenever q new is added to the random tree, R and D max are compared before the next sampling. If R is greater than onefifth of D max , it means that the random tree is still far away from q goal . Thus, it continues to use the random sampling of the CSA-RRT algorithm to select q rand . On the other hand, if R is smaller than one-fifth of D max , it means that the random tree is close to q goal . At this time, a sampling-radius limitation mechanism based on bounded sampling is adopted to limit the sampling area within the radius R from the goal point. In this way, the problem that q rand cannot successfully fall within R when it is close to the goal point can be solved. This makes the proposed CS-RRT algorithm not only quickly find valid nodes but also reduce the computing time of path planning. In the case of two-dimensional simulations, the CS-RRT algorithm improves by about 0.5 times compared with the CSA-RRT algorithm.
In addition, the CSA-RRT algorithm has the advantage that it can quickly find an initial path to the goal area. However, as shown in Figure 8, if the CSA-RRT algorithm is performed in a complex environment and the value of k is not adjusted properly, the tree may be trapped due to focusing too much on the goal point. As a result, the random tree keeps expanding in the same area but cannot find an escape path until the number of node expansions of the algorithm reaches the maximum limit of expansions and fails. In order to solve this problem, a node counting mechanism is adopted to appropriately switch the sampling strategy to an appropriate sampling method in complex environments. It can avoid the search path being trapped in some constrained areas and improve the adaptability of the proposed CS-RRT algorithm to various environments.
performed in a complex environment and the value of is not adjusted properly, the tree may be trapped due to focusing too much on the goal point. As a result, the random tree keeps expanding in the same area but cannot find an escape path until the number of node expansions of the algorithm reaches the maximum limit of expansions and fails. In order to solve this problem, a node counting mechanism is adopted to appropriately switch the sampling strategy to an appropriate sampling method in complex environments. It can avoid the search path being trapped in some constrained areas and improve the adaptability of the proposed CS-RRT algorithm to various environments.
The CSA-RRT algorithm with the node counting mechanism will calculate the distance from the node that is closest to the goal point and set the node count variable to zero during initialization. As shown in Algorithm 2, in the sampling stage, the algorithm will choose which sampling method to use according to the value of . If is less than the set threshold, the sampling method of CSA-RRT is used to make the random tree quickly extend to the goal point. Otherwise, the random sampling method, which fully explores the environment, is used.  After completing the collision detection stage in each iteration, the algorithm calculates the Euclidean distance from to , no matter if is successfully added to the random tree. After this, compare with in the CheckEnvironment() function. The pseudocode of the CheckEnvironment() function is shown in Algorithm 3. If is smaller than , then is set to zero. At this time, is closer to , which means that the tree is approaching the goal area, so there is no need to change the sampling strategy. After that, change into as the basis for the next check of the expansion status. On the other hand, if is greater than , it means that is not closer to . At this time, +1. If continues to be greater than for the next few times, it is considered that the random tree is trapped in the current area. Then the algorithm will switch the sampling method to random sampling in the SelectSample() function to try to escape the current area until is smaller than . In addition, an upper limit is set to avoid the algorithm wasting too much time using random sampling to explore the space. Therefore, when The CSA-RRT algorithm with the node counting mechanism will calculate the distance D min from the node q min that is closest to the goal point q goal and set the node count variable nodecnt to zero during initialization. As shown in Algorithm 2, in the sampling stage, the algorithm will choose which sampling method to use according to the value of nodecnt. If nodecnt is less than the set threshold, the sampling method of CSA-RRT is used to make the random tree quickly extend to the goal point. Otherwise, the random sampling method, which fully explores the environment, is used. if nodecnt < 20 then 2.
return q rand ; After completing the collision detection stage in each iteration, the algorithm calculates the Euclidean distance D new from q new to q goal , no matter if q new is successfully added to the random tree. After this, compare D new with D min in the CheckEnvironment() function. The pseudocode of the CheckEnvironment() function is shown in Algorithm 3. If D new is smaller than D min , then nodecnt is set to zero. At this time, q new is closer to q goal , which means that the tree is approaching the goal area, so there is no need to change the sampling strategy. After that, change q new into q min as the basis for the next check of the expansion status. On the other hand, if D new is greater than D min , it means that q new is not closer to q goal . At this time, nodecnt+1. If D new continues to be greater than D min for the next few times, it is considered that the random tree is trapped in the current area. Then the algorithm will switch the sampling method to random sampling in the SelectSample() function to try to escape the current area until D new is smaller than D min . In addition, an upper limit is set to avoid the algorithm wasting too much time using random sampling to explore the space. Therefore, when nodecnt reaches the set upper limit, it will reset to zero immediately. A schematic illustration of node count adjustment is shown in Figure 9.
Sensors 2023, 23, x FOR PEER REVIEW 10 of 19 reaches the set upper limit, it will reset to zero immediately. A schematic illustration of node count adjustment is shown in Figure 9.
(a) (b)  In short, the proposed CS-RRT algorithm is based on the CSA-RRT algorithm and uses the sampling-radius limitation mechanism and the node counting mechanism to solve the problems existing in the CSA-RRT algorithm. The sampling-radius limitation mechanism allows the random tree to finish the sampling stage more quickly when it is close to the goal area, so that the proposed CS-RRT algorithm can further reduce the computing time. The node counting mechanism makes the algorithm avoid overfocusing on the goal area, so the proposed CS-RRT algorithm also has good adaptability to the environment. The results of the proposed CS-RRT algorithm performed in two different environments are shown in Figure 10. When encountering simple environments, as shown in Figure 10a, the proposed CS-RRT algorithm can quickly find a path. When encountering complex environments, as shown in Figure 10b, the proposed CS-RRT algorithm can also prevent trapping by switching sampling strategies. Comparing the results shown in Figure 8, we can see that the CSA-RRT algorithm is trapped in this environment. With these improvements, the proposed CS-RRT algorithm indeed not only reduces computing time but also improves environmental adaptability.   In short, the proposed CS-RRT algorithm is based on the CSA-RRT algorithm and uses the sampling-radius limitation mechanism and the node counting mechanism to solve the problems existing in the CSA-RRT algorithm. The sampling-radius limitation mechanism allows the random tree to finish the sampling stage more quickly when it is close to the goal area, so that the proposed CS-RRT algorithm can further reduce the computing time. The node counting mechanism makes the algorithm avoid overfocusing on the goal area, so the proposed CS-RRT algorithm also has good adaptability to the environment. The results of the proposed CS-RRT algorithm performed in two different environments are shown in Figure 10. When encountering simple environments, as shown in Figure 10a, the proposed CS-RRT algorithm can quickly find a path. When encountering complex environments, as shown in Figure 10b, the proposed CS-RRT algorithm can also prevent trapping by switching sampling strategies. Comparing the results shown in Figure 8, we can see that the CSA-RRT algorithm is trapped in this environment. With these improvements, the proposed CS-RRT algorithm indeed not only reduces computing time but also improves environmental adaptability.
Sensors 2023, 23, x FOR PEER REVIEW 10 of 19 reaches the set upper limit, it will reset to zero immediately. A schematic illustration of node count adjustment is shown in Figure 9.
(a) (b)  In short, the proposed CS-RRT algorithm is based on the CSA-RRT algorithm and uses the sampling-radius limitation mechanism and the node counting mechanism to solve the problems existing in the CSA-RRT algorithm. The sampling-radius limitation mechanism allows the random tree to finish the sampling stage more quickly when it is close to the goal area, so that the proposed CS-RRT algorithm can further reduce the computing time. The node counting mechanism makes the algorithm avoid overfocusing on the goal area, so the proposed CS-RRT algorithm also has good adaptability to the environment. The results of the proposed CS-RRT algorithm performed in two different environments are shown in Figure 10. When encountering simple environments, as shown in Figure 10a, the proposed CS-RRT algorithm can quickly find a path. When encountering complex environments, as shown in Figure 10b, the proposed CS-RRT algorithm can also prevent trapping by switching sampling strategies. Comparing the results shown in Figure 8, we can see that the CSA-RRT algorithm is trapped in this environment. With these improvements, the proposed CS-RRT algorithm indeed not only reduces computing time but also improves environmental adaptability.

Simulation Results of Robot Manipulator
The simulation setup of the robot manipulator and experimental environment is shown in Figure 11. In order to demonstrate the performance of the motion planning of the robot manipulator based on the proposed CS-RRT algorithm, a series of tasks (Task 1~Task 4) is designed, and then the success rates and average computing time after 50 experiments of each algorithm, the proposed CS-RRT, CSA-RRT, and the 10% goal-bias RRT algorithm, provided by the open motion planning library (OMPL), are compared. The tasks are described as follows: ensors 2023, 23, x FOR PEER REVIEW 1

Simulation Results of Robot Manipulator
The simulation setup of the robot manipulator and experimental environm shown in Figure 11. In order to demonstrate the performance of the motion plann the robot manipulator based on the proposed CS-RRT algorithm, a series of tasks 1~Task 4) is designed, and then the success rates and average computing time af experiments of each algorithm, the proposed CS-RRT, CSA-RRT, and the 10% goa Task 2: The implemented pick-and-place system commands the robot manipula place the grasped object at a certain location on the upper layer of the cabine sides of the cabinet can be considered obstacles for the movement of the manipulator. This task verifies the motion planning performance of algor from open space to restricted space.
Task 3: The implemented pick-and-place system commands the robot manipula move from the upper layer of the cabinet to the lower layer and grasp the Ob that was placed on the lower layer of the cabinet. This task verifies the m planning performance of algorithms in two restricted spaces.
Task 4: The implemented pick-and-place system commands the robot manipula move from the lower layer of the cabinet to the grasping position in Task 1 a place the grasped object. This task verifies the motion planning performa algorithms from a restricted space to an open space. Figure 12 illustrates the motion flow from Task 1 to Task 4. Note that the gra orange robot manipulators indicate the initial and finish positions of each task, re tively. Four simulation results for each task are illustrated as follows:  In the experimental scenarios of Task 1, as shown in Figure 12a, the robot manipulator is commanded to move from its initial position to grasp the object on the table. In the experimental scenarios of Task 1, as shown in Figure 12a, the robot manipulator is commanded to move from its initial position to grasp the object on the table. Figure 13a,b are bar graphs of the success rate and the average computing time of the three algorithms executed 50 times in Task 1, respectively. From the results in Figure 13, it can be seen that the success rate and the average computing time of the proposed CS-RRT and the 10% goal-biased RRT are the same. We infer that the proposed CS-RRT needs to compute the sampling radius at each iteration. Therefore, searching in an open space without obstacles, does not have the advantage of taking less computing time. In addition, CSA-RRT makes it difficult for the sampling points to fall within the sampling radius when they are close to the goal area. Therefore, the computing time of the CSA-RRT is higher than that of the proposed CS-RRT.  Figure 13, it can be seen that the success rate and the average computing time of the proposed CS-RRT and the 10% goal-biased RRT are the same. We infer that the proposed CS-RRT needs to compute the sampling radius at each iteration. Therefore, searching in an open space without obstacles, does not have the advantage of taking less computing time. In addition, CSA-RRT makes it difficult for the sampling points to fall within the sampling radius when they are close to the goal area. Therefore, the computing time of the CSA-RRT is higher than that of the proposed CS-RRT. Remark: The experiment sets the maximum path planning time to 1 second because it is basically not regarded as real-time motion planning after more than 1 second. In addition, the robot manipulator may rotate a full circle and cause damage to surrounding objects due to the randomness of the RRT algorithms. Thus, we restrict each axis of the robot manipulator to rotating between plus and minus 180 degrees. In the experimental scenarios of Task 2, as shown in Figure 12b, the robot manipulator is commanded to move from the finished position of Task 1 to a specific position on the upper layer of the cabinet to place the grasped object. Although moving in a straight line is the fastest way to reach the destination, it will collide with the sides of the cabinet. Therefore, for the robot manipulator to reach its destination safely, it needs a collision-free path to avoid collisions with the sides of the cabinet. Task 2 tests the motion planning performance of algorithms from an open space to a restricted space. From the results shown in Figure 14, it can be seen that the proposed CS-RRT has the highest success rate and the shortest average computing time. Therefore, the proposed CS-RRT has better performance than the other two algorithms when planning a motion in a restricted environment.  Remark: The experiment sets the maximum path planning time to 1 second because it is basically not regarded as real-time motion planning after more than 1 second. In addition, the robot manipulator may rotate a full circle and cause damage to surrounding objects due to the randomness of the RRT algorithms. Thus, we restrict each axis of the robot manipulator to rotating between plus and minus 180 degrees.
In the experimental scenarios of Task 2, as shown in Figure 12b, the robot manipulator is commanded to move from the finished position of Task 1 to a specific position on the upper layer of the cabinet to place the grasped object. Although moving in a straight line is the fastest way to reach the destination, it will collide with the sides of the cabinet. Therefore, for the robot manipulator to reach its destination safely, it needs a collision-free path to avoid collisions with the sides of the cabinet. Task 2 tests the motion planning performance of algorithms from an open space to a restricted space. From the results shown in Figure 14, it can be seen that the proposed CS-RRT has the highest success rate and the shortest average computing time. Therefore, the proposed CS-RRT has better performance than the other two algorithms when planning a motion in a restricted environment.
In the experimental scenarios of Task 3, as shown in Figure 12c, the robot manipulator is commanded to move from the upper layer of the cabinet to a specific position on the lower layer of the cabinet. Since the robot manipulator has entered the upper cabinet, the environment of the two restricted areas in Task 3 is more complex than that in Task 1 and Task 2. In this experiment, we found that the default step size ε using OMPL is too small. As a result, the random tree of the three algorithms grows slowly, and the motion planning cannot be completed within the specified time. Therefore, the step size ε is increased by three times in Task 3. In other words, the expansion distance of each iteration of the random tree is increased. From the results shown in Figure 15, we can see that the proposed CS-RRT has the highest success rate and the shortest average computing time too. Therefore, the proposed CS-RRT has better performance than the 10% goal-biased RRT algorithm and CSA-RRT when planning in a complex environment.
Therefore, for the robot manipulator to reach its destination safely, it needs a collision-free path to avoid collisions with the sides of the cabinet. Task 2 tests the motion planning performance of algorithms from an open space to a restricted space. From the results shown in Figure 14, it can be seen that the proposed CS-RRT has the highest success rate and the shortest average computing time. Therefore, the proposed CS-RRT has better performance than the other two algorithms when planning a motion in a restricted environment.  In the experimental scenarios of Task 3, as shown in Figure 12c, the robot manipulator is commanded to move from the upper layer of the cabinet to a specific position on the lower layer of the cabinet. Since the robot manipulator has entered the upper cabinet, the environment of the two restricted areas in Task 3 is more complex than that in Task 1 and Task 2. In this experiment, we found that the default step size ε using OMPL is too small. As a result, the random tree of the three algorithms grows slowly, and the motion planning cannot be completed within the specified time. Therefore, the step size ε is increased by three times in Task 3. In other words, the expansion distance of each iteration of the random tree is increased. From the results shown in Figure 15, we can see that the proposed CS-RRT has the highest success rate and the shortest average computing time too. Therefore, the proposed CS-RRT has better performance than the 10% goal-biased RRT algorithm and CSA-RRT when planning in a complex environment. In the experimental scenarios of Task 4, as shown in Figure 12d, the robot manipulator is commanded to move from the lower layer of the cabinet to the top of the table. This task verifies the motion planning performance of algorithms from a restricted space to an open space. From the results shown in Figure 16, we can see that the success rate and the average computing time of the proposed CS-RRT and 10% goal-biased RRT are the same. However, the average computing time of CSA-RRT under Task 4 is still longer than that of CS-RRT and 10% goal-biased RRT. Judging from the fact that the success rate of all three algorithms is 100 percent, we infer that once the robot manipulator comes out of the cabinet, there are multiple ways to move to the destination. Since the restricted area is near the initial point in Task   In the experimental scenarios of Task 4, as shown in Figure 12d, the robot manipulator is commanded to move from the lower layer of the cabinet to the top of the table. This task verifies the motion planning performance of algorithms from a restricted space to an open space. From the results shown in Figure 16, we can see that the success rate and the average computing time of the proposed CS-RRT and 10% goal-biased RRT are the same. However, the average computing time of CSA-RRT under Task 4 is still longer than that of CS-RRT and 10% goal-biased RRT. Judging from the fact that the success rate of all three algorithms is 100 percent, we infer that once the robot manipulator comes out of the cabinet, there are multiple ways to move to the destination. Since the restricted area is near the initial point in Task 4, once the robot manipulator leaves the restricted area, all three algorithms can easily find a collision-free path to the destination in an open space with only a few explorations.
Based on these results shown in Figures 13-16, the simulation results of three algorithms in the four scenarios of Tasks 1~4 are summarized in Table 1. Compared with goal-biased RRT and CSA-RRT, the implemented pick-and-place system based on the proposed CS-RRT algorithm has a higher success rate and requires less computing time. We can see that the results of CS-RRT are similar to those of the 10% goal-biased RRT when planning is performed in non-complex environments, such as Task 1 and Task 4. However, the advantages of the proposed CS-RRT in terms of success rate and computing time can be seen when planning is performed in complex environments with space restrictions, such as Task 2 and Task 3.
the same. However, the average computing time of CSA-RRT under Task 4 is still longer than that of CS-RRT and 10% goal-biased RRT. Judging from the fact that the success rate of all three algorithms is 100 percent, we infer that once the robot manipulator comes out of the cabinet, there are multiple ways to move to the destination. Since the restricted area is near the initial point in Task 4, once the robot manipulator leaves the restricted area, all three algorithms can easily find a collision-free path to the destination in an open space with only a few explorations.

Experimental Results of Real Robot Manipulator
In the practical experimental demonstration, a real pick-and-place system with a depth camera (Microsoft Azure Kinect DK), a six DOF robot manipulator (UR5), and a two-finger parallel gripper (Robotiq 2F-85) is presented to illustrate the efficiency of the proposed CS-RRT algorithm applied in the pick-and-place system. The camera is installed above the table and the YOLOv4 algorithm is used to obtain the position coordinates of Object A, which is randomly placed on the table. Code runs on the robot with ROS implemented in Python. The communication method of ROS Services is used to ensure that the task strategy module actually receives the coordinate information and MoveIt is used to execute the motion planning results of the proposed CS-RRT algorithm to complete the four pick-and-place tasks described in the previous section. The video of the demonstration of the real pick-and-place task can be viewed on this website: https://youtu.be/lcdy2byIG_g (accessed on 20 January 2023). The snapshots of the real robot manipulator performing Task 1, Task 2, Task 3, and Task 4 are, respectively, shown in Figures 17-20. The procedure can be described as follows: Step 1: Obtain the position coordinates of object A randomly placed on the table through the camera installed above the table and the YOLOv4 algorithm.
Step 2: Move to the top of Object A.
Step 3: Move downward to grasp Object A.
Step 4: Move upward from the table.
Step 5: Move to the outside of the upper layer of the cabinet.
Step 6: Move into the upper interior of the upper layer of the cabinet.
Step 7: Move downward to place Object A.
Step 8: Move to the outside of the upper layer of the cabinet.
Step 9: Move to the outside of the lower layer of the cabinet.
Step 10: Move to the top of Object B in the lower layer of the cabinet.
Step 11: Move downward to grasp Object B.
Step 12: Move to the outside of the lower layer of the cabinet.
Step 13: Move to the top of the initial position of Object A, where it was originally placed on the table.
Step 14: Move downward to place Object B on the table.
Step 15: Move to the top of the object placed on the table.
Step 16: Return to the initial position of the robot manipulator. Step 14: Move downward to place Object B on the table.
Step 15: Move to the top of the object placed on the table.
Step 16: Return to the initial position of the robot manipulator.  Step 14: Move downward to place Object B on the table.
Step 15: Move to the top of the object placed on the table.
Step 16: Return to the initial position of the robot manipulator. Step 14: Move downward to place Object B on the table.
Step 15: Move to the top of the object placed on the table.
Step 16: Return to the initial position of the robot manipulator.

Conclusions and Future Work
An ROS-based object pick-and-place system is implemented, and a CS-RRT algorithm is proposed so that the robot manipulator can efficiently pick-and-place objects in real time. There are three main contributions in this paper. (1) Path planning for robots is one of the most important topics in robotics research. In the research on robot manipulators for picking and placing objects in a constrained environment, most of the research only completed simulation results to verify the effectiveness of their path planning methods. Many improved RRT algorithms have been proposed, but they are rarely applied to actual robot manipulators for object pick-and-place tasks in real time. Both simulation and actual experiments are used to demonstrate that the proposed CS-RRT algorithm and the implemented system can allow the robot manipulator to effectively avoid obstacles and pick-and-place objects in real time. (2) Some disadvantages of existing RRT algorithms are addressed, and two mechanisms of sampling radius counting, and node counting are adopted in the proposed CS-RRT algorithm. The sampling-radius limitation mechanism, by limiting the sampling radius, can make the random tree finish the sampling stage faster when the tree is close to the goal point. It can reduce the computing time of the proposed CS-RRT algorithm. The node counting mechanism allows the algorithm to switch to an appropriate sampling method in a complex environment. It can avoid excessive exploration in the direction of the goal point so that the random tree does not trap itself in constrained areas. It can make the proposed CS-RRT algorithm have better environmental adaptability. In addition, an experimental environment with four object picking and placement tasks has been established. Experimental results show that the object pick-and-place system based on the proposed CS-RRT algorithm has a higher success rate and lower computing time compared with the other two path planning algorithms. (3) The robot operating system (ROS) is used to implement the object pick-and-place system. By implementing the proposed CS-RRT algorithm in the open motion planning library (OMPL), MoveIt can be used to plan the motion of the robot

Conclusions and Future Work
An ROS-based object pick-and-place system is implemented, and a CS-RRT algorithm is proposed so that the robot manipulator can efficiently pick-and-place objects in real time. There are three main contributions in this paper. (1) Path planning for robots is one of the most important topics in robotics research. In the research on robot manipulators for picking and placing objects in a constrained environment, most of the research only completed simulation results to verify the effectiveness of their path planning methods. Many improved RRT algorithms have been proposed, but they are rarely applied to actual robot manipulators for object pick-and-place tasks in real time. Both simulation and actual experiments are used to demonstrate that the proposed CS-RRT algorithm and the implemented system can allow the robot manipulator to effectively avoid obstacles and pick-and-place objects in real time.
(2) Some disadvantages of existing RRT algorithms are addressed, and two mechanisms of sampling radius counting, and node counting are adopted in the proposed CS-RRT algorithm. The sampling-radius limitation mechanism, by limiting the sampling radius, can make the random tree finish the sampling stage faster when the tree is close to the goal point. It can reduce the computing time of the proposed CS-RRT algorithm. The node counting mechanism allows the algorithm to switch to an appropriate sampling method in a complex environment. It can avoid excessive exploration in the direction of the goal point so that the random tree does not trap itself in constrained areas. It can make the proposed CS-RRT algorithm have better environmental adaptability. In addition, an experimental environment with four object picking and placement tasks has been established. Experimental results show that the object pick-and-place system based on the proposed CS-RRT algorithm has a higher success rate and lower computing time compared with the other two path planning algorithms. (3) The robot operating system (ROS) is used to implement the object pick-and-place system. By implementing the proposed CS-RRT algorithm in the open motion planning library (OMPL), MoveIt can be used to plan the motion of the robot manipulator. According to the imported URDF file, MoveIt can also perform motion planning for different robot manipulators, so the proposed method can be easily applied to different robot manipulators.
There are two parts to the future work: (1) In the part of switching strategy and step size adjustment, switching sampling strategy can improve the adaptability of the proposed algorithm to the environment, but its own parameters need to be manually designed according to the environment. In addition, the step size also needs to be chosen according to the environment. Therefore, some optimization methods can be used in the future to select appropriate parameters for the switching strategy and step size according to the environment.
(2) In the part of the sampling method where the distance needs to be calculated. In the path planning of the six-dimensional joint space of the robot manipulator, more parameters are needed to calculate the distance, which increases the computing time of the proposed algorithm. Therefore, the number of calculation distances can be reduced in the future to reduce the computing time needed to meet the system requirements.