PD+gravity Control 2DoF Planar Robotic Manipulator

This code presents a simple PD+gravity control design for a two-degree-of-freedom manipulator.

Time definition and discretization is in lines 4-8.

The degree of freedom of the arm is in line 10.

The start and end points are defined in lines 12-15.

The physical parameters of the robot are set in lines 17-26.

Inverse kinematics is presented in liens 27-37, details can be found in chapter 3 of:

Schilling, R. J. (1996). Fundamentals of robotics: analysis and control. Simon & Schuster Trade.

Dynamics: lines 54-60. M, C, g matrix and vectors can be obtained in:

https://es.mathworks.com/matlabcentral/fileexchange/75554-symbolic-derivation-of-manipulator-dyn-denavit-hartenberg?s_tid=prof_contriblnk

PD controller is in lines 62-64. Details can be found in chapter 7 of:

Schilling, R. J. (1996). Fundamentals of robotics: analysis and control. Simon & Schuster Trade.

Simple PD can be implemented by replacing line 64 with:

u(:,i)=-Kp*(q-x_des(1:2))-Kd*(dq-x_des(3:4));