clc % Segments lengths a1 = 5; a2 = 3; a3 = 2; % Initial configuration (joints angles) tetha1 = 0; tetha2 = 0; tetha3 = 0; dt = 1e-3; % ax1_0 = a1*cos(tetha1); ay1_0 = a1*sin(tetha1); ax2_0 = a2*cos(tetha1+tetha2); ay2_0 = a2*sin(tetha1+tetha2); ax3_0 = a3*cos(tetha1+tetha2+tetha3); ay3_0 = a3*sin(tetha1+tetha2+tetha3); OP1_0 = [ax1_0 ay1_0 0]; P1P2_0 = [ax2_0 ay2_0 0]; OP2_0 = OP1_0 + P1P2_0; P2P3_0 = [ax3_0 ay3_0 0]; OP3_0 = OP2_0 + P2P3_0; ax_0 = OP3_0(1); ay_0 = OP3_0(2); zero_vector = [0 0 0]; theta1_0 = zero_vector; theta2_0 = zero_vector; theta3_0 = zero_vector; OT1 = [4 6 0]; % position de la premiere cible OT2 = [-2 6 0]; % position de la deuxieme cible OT3 = [4 6 0]; % position de la troisieme cible OT4 = [1 8 0]; % position de la quatrieme cible OT5 = [1 4 0]; % position de la cinqieme cible OT6 = [1 8 0]; % position de la sixieme cible OT7 = [3 7.5 0]; OT8 = [-1 4.5 0]; OT9 = [3 7.5 0]; OT10 = [-1 7.5 0]; OT11 = [3 4.5 0]; OT12 = [-1 7.5 0]; th = 1e-1; g = 1; % To simulate parkinsonian set lower than 1 % weights for different segments movements w1 = 1/100; w2 = 1/50; w3 = 1/20; delay = 1;