20+ DoF Robot

Potential Field Algorithm
To allow for flexibility in the robots motion, this reactive algorithm was implemented. I created a motion planning based upon a quadratic potential model, drawing inspiration from Coulomb’s inverse square law. After tuning parameters, the robot successfully navigated to the goal position.

PRM and Djiktra’s Algorithm
Rather than using an uniform distribution, a probability distribution biased towards narrow passages and areas of high object density was created. A blurred version of the C-space was created using a gaussian filter, then subtracted from the original. Next, the edge graph was blurred to create a map with higher values at the edges of obstacles. To convert this to a probability distribution, it was normalised. A nested for loop then iterates through each generated point checking if it is acceptable.

The points generated with PRM were used to create a graph, and min/max edge lengths were defined. Edges that collided with obstacles were removed, then Dijkstra’s Algorithm implemented to find the optimal (within the graph) route.

Exploiting Redundancy
By calculating the set of redundant poses, the robot could move a brick avoiding certain areas. The function starts by creating a joint array and then loops through and stores the joint values, up to the elbow. After that, the function calculates the Jacobian for the elbow, then completing the full size Jacobian by filling in a zero matrix that represents the six joints of the robot arm. Finally, it returns only the linear part of the Jacobian. With the Jacobian, the desired joint velocities can be found.

Destroy and Clean Up
The robot can interact using forces and torques, calculated using the mass matrix, Coriolis, joint kinematics and required torques to counter external forces. A simple PD controller was implemented, and forces specified to ‘pull back’ then throw the brick towards the wall. To ensure desired motion was achieved, flags were used. Once the movement reaches the furthest away point from the chosen reference point, the flag is set to True. At this point, the force is set to launch the brick towards the wall.

The robot then cleans the mess - applying linear forces. In order to solve control issues, rotational components were added to the derivative term of the force being applied in order to compensate for the twist.