The purpose of this project was to implement or integrate an RRT-based planner that takes the initial and goal states as input. The planner creates a tree in the full configuration space and finds a solution path. To achieve this, several components were suggested, including a graph data structure for storing vertices and edges, nearest neighbor search, collision checker, local controller for moving the main shaft assembly in 3D space (dynamics optional), and sampling method.

In the above figure, using the 3D scene in MATLAB's robotics toolbox. I have created a number of rectangular and cylindrical obstacles using the collisionBox and collisionCylinder functions, respectively, and set their positions and orientations using transformation matrices generated by trvec2tform and axang2tform.

I then added the rectangular obstacles and some of the cylindrical obstacles to an array box_obstacles, and adds the remaining cylindrical obstacles to cylinder_obstacles. Finally, used the show function to display each obstacle in the 3D scene, with some of the boxes colored black.

Psuedo Code for RRT Algo

1. Define the boundary for plotting and robot information.
2. Define the starting and goal points.
3. Plot the starting and goal points.
4. Define the generateRRT function with the input arguments as start, goal, box_obstacles, cylinder_obstacles, boundary, branch_length, main_shaft, and main_shaft_bearings.
5. Inside the generateRRT function, check if the start and goal points are in obstacle-free space.
6. If the start or goal points are not in obstacle-free space, set the PATH variable as an empty array and display an error message.
7. If the start and goal points are in obstacle-free space, set the maximum number of samples to be generated (N) to 10000.
8. Define vertex_start as the start point, and edges_start as an empty array.
9. Loop through N times.
10. Within the loop, sample a random point using rand() for x, y, z, and theta values.
11. Check if the current iteration is a multiple of 5, if so, set the random point as the goal point.
12. If the current iteration is not a multiple of 5, find the nearest node in the tree (vertex_start) using the find_nearest_node function.
13. Check if the node can be extended to the random node using the can_it_extend function.
14. If the node can be extended, add the new_node to vertex_start, add the edge to edges_start, and plot the edge connecting the nodes.
15. Check if the new_node is equal to the goal node, if so, break the loop.
16. Select the best path in V and E using the select_best_path function.
17. Display the PATH variable and the first row of PATH.
18. Plot the final path using an animated line.
19. Inside a for loop, update the main_shaft and main_shaft_bearings poses for each point in the path.
20. Delete the previous main_shaft and main_shaft_bearings objects.
21. Set the new main_shaft and main_shaft_bearings poses using the PATH variable.
22. Show the new main_shaft and main_shaft_bearings objects.

This is the initial setup for the main shaft inside the gear box. It will then utilizes RRT algo to move the shaft from the gear box out to the goal location.

We can observe the tree-like structure for the RRT Search Algorithm. We can see on of the branch is going extending towards the red Star i.e. goal.

Here we can see the RRT search Algo select the best path and it is highlighted by red color.

• http://lavalle.pl/planning/ch5.pdf
• https://www.mathworks.com/help/matlab/2-and-3d-plots.html?s_tid=CRUX_lftnav