This is an example output of the RRT algorithm with a 5-link serial manipulator.

This is an example output of the PRM algorithm with a 5-link serial manipular.

The overall structure of all functions used in the task 1 code is outlined in figure 1 where each arrow points from a script or function in which a function was called to the function that was called.  The task1_main script is run to plan a path using RRT and PRM and create videos of each named “RRT_nlink” or “PRM_nlink.” These videos are saved as .avi files in the directory from Matlab and then manually converted to .mov files for submission.

Environment and obstacles:

• The base of the 2D n-link manipulator is fixed at the origin of the world. Input parameter r in both RRT and PRM controls the length of each link in the manipulator.
• The 2D world has an x-axis from [-20 to 25] and y-axis [-10 20].
• There are three rectangular obstacles in the environment described by input variable cell array O, which contains the vertices of each obstacle in CCW order.
• The initial position of the end of the nth link of the manipulator is represented with a red square and the goal position of the end of the nth link is represented with a green rhombus.

RRT Inputs:

• q_init and q_goal: both are  n_linksx1 vectors representing the initial and goal joint angles of the robot. Each row represents the joint angle for one link.
• O: cell array containing the vertices of each obstacle in CCW order.
• r: a scalar value representing the length of each link in the manipulator (A length of about 5 works well for this environment, shorter links will likely allow for faster path planning.
• dq: a scalar between 0 and 1 representing the step size.

PRM Inputs:

• q_init and q_goal: both are  n_linksx1 vectors representing the initial and goal joint angles of the robot. Each row represents the joint angle for one link.
• O: cell array containing the vertices of each obstacle in CCW order.
• r: a scalar value representing the length of each link in the manipulator (A length of about 5 works well for this environment, shorter links will likely allow for faster path planning.
• n: the scalar number of random configurations to be generated. Increasing n makes the system more likely to be able to find a collision-path, but increases runtime.
• K: the scalar number of nearest neighbors we should consider for path finding.

RRT and PRM outputs:

• path: an n_linksxP array representing the path that the robot should take where P is the number of configurations in the path. 

Both the RRT and PRM functions use the point_collides function which takes input q, an n-linksx1 vector of joint angles and then converts q into the workspace to check each link for a collision with each obstacle using the isintersect_linepolygon function from homework 8. The path_collides function takes an input of two different 1xn-links vectors of joint angles and performs a 21-step linear interpolation between the two configurations calling the point_collides function at each step to check for collision. RRT also uses the helper function nearest_node to search through the nodes already added to the child array for the closest one to the randomly generated configuration and build_path to reconstruct the path once the goal is reached using the arrays of stored parent and child configurations.

For PRM, the Astar pathfinding algorithm was chosen instead of djikstra because Astar works more quickly since it does not need to check every node in the graph. The heuristic function used in Astar is the same as the function used in Homework 2 problem 2a, meaning that Astar favors nodes which have fewer steps to take to get to the goal.

The make_video function uses the path output of either RRT or PRM to create the videos in our submission of the robot following the path to reach the goal configuration while avoiding the obstacles. It plots the initial and goal points of the robot end effector in the workspace...