Inverse Kinematics

The inverse Kinematics algorithm used was the Inverse Jacobian. We used the Inverse Jacobian because it is highly parallizable which was a key aspect of our project because we wanted to distribute the calculations across different controllers. This algorithm also allows us easily preform inverse kinematics with a joint locked because we can remove that column from the Jacobian matrix. After the Jacobian is computed the thetas are changed in small iterations. In the code all that needs to be done is change the desired target position's x,y,z,roll,pitch,yaw coordinate.