Animation 1 show the robot
moves around an environment where the robot have to avoid 9 different obstacles.
These obstacle have different rectangular shapes and sizes. Picture 1 shows
the inital point (the blue ball at the lower corner) and the goal point
(the blue ball at the left corner). Picture 2 shows the 3D RRT formed around
the obstacles. Picture 3 shows the filtered path to go from the initial
point to the goal point. Picture 4, 5, and 6 show the different view angles
of the original RRT when the obstacle were removed.
In this case, the end effector actually collide with the last link extension when transporting the workpiece to the final destination. The reason for that was I did not define the body of the robot to be part of the obstacle.