SDRE+Gravity Compensation for a 2 DoF Planar Robot Control

This code shows a simple SDRE control with gravity compensation for point-to-point regulation of 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.

SDRE control parameters are set in lines 12-13.

Line 26 defines the mass of the load on the end-effector, mp.

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

The physical parameters of the robot are set in lines 20-29.

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

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

Dynamics: lines 57-63. 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

SDRE controller is in lines 65-70. Details can be found at: https://doi.org/10.1049/cth2.12206 Nekoo, S. R., Acosta, J. Á., & Ollero, A. (2022). Gravity compensation and optimal control of actuated multibody system dynamics. IET Control Theory & Applications, 16(1), 79-93.