Inverse Kinematics of UR5 Robot
The inverse kinematics of a robot arm consists in finding the joint coordinates that lead to a desired configuration (position and orientation) of the end-effector.
Here, I wrote a program that implements the Newton-Raphson algorithm to solve the inverse kinematics of the UR5 robot from Universal Robots. This method uses a iterative numerical approach.
First, it takes as input the desired end-effector configuration T_sd, and an initial "guess" vector of joint coordinates, and calculates the transformation matrix T_sb using forward kinematics. A transformation matrix is a representation of the orientation and position of a rigid body. With T_sb and T_sd, it calculates the body twist Vb that, if followed for unit time, would cause a motion from T_sb to the desired configuration T_sd.
From the body twist Vb, it calculates the vector norms of its angular and linear components, and compares them with the angular and linear error tolerances, defined in the program. If the diference or error is not within tolerance, it calculates a new vector of joint coodinates, using the body twist Vb and the pseudoinverse of the end-effector body Jacobian. The algorithm iterates again until the angular and linear errors are within tolerance.
For this case, I used an initial vector of joint coordinates (2,1,1,3,1,1) rad, an angular error of 0.001, a linear error of 0.0001, and a desired end-effector configuration
T_sd = [ [ 0, 1, 0, -0.5 ],
[ 0, 0, -1, 0.1 ],
[ -1, 0, 0, 0.1 ],
[ 0, 0, 0, 1.0 ]]
The algorithm converges after 5 iterations, to the joint coordinates (2.5862,-0.6621,1.7397,2.0641,0.553,1.5706).
The 5 iterations are showed in the animation video.
Simulation Newton-Raphson Method
What I liked the most about this project is how the Newton-Raphson algorithm is adapted from the single variable case (x, f(x), x_d) to the robot multi-variable case, where the Vb twist is the analog of the slope at the point (x_d - f(x)), and the new vector of joint coordinates is calculated from that twist Vb and the Jacobian, analog to the intersection of the slope with the horizontal axis that gives the new variable for iteration.
I used Python 3.7 to develop this program, with the libraries numpy and csv. I also used the library 'modern_robotics' provided in the Modern Robotics wiki website.
http://hades.mech.northwestern.edu/index.php/Modern_Robotics
For the simulation, I used the software V-REP EDU PRO software.
I followed the assignments in the Course 2 Robot Kinematics, which is part of the online specialization Modern Robotics: Mechanics, Planing and Control, offered by Nortwestern University and Coursera.
