clear;
clc;
% Parameters
L1 = 1; % Length of link 1
L2 = 1; % Length of link 2
dt = 0.01; % Time step size
t = 0:dt:1; % Time vector
% Initial joint angles
theta1 = 0;
theta2 = pi;
% Initial end-effector position
x_init = L1 * cos(theta1) + L2 * cos(theta1 + theta2);
y_init = L1 * sin(theta1) + L2 * sin(theta1 + theta2);
% Desired final end-effector position
x_final = 1;
y_final = 1;
% Total change in x and y, divided by the number of steps
deltaX_total = x_final - x_init;
deltaY_total = y_final - y_init;
deltaX_step = deltaX_total / (length(t) - 1);
deltaY_step = deltaY_total / (length(t) - 1);
% Initialize position and delta theta arrays
x = x_init;
y = y_init;
dTheta1 = zeros(1, length(t)-1);
dTheta2 = zeros(1, length(t)-1);
% Loop to compute incremental joint angles for each time step
for i = 1:length(t)-1
% Update end-effector position incrementally
x = x + deltaX_step;
y = y + deltaY_step;
% Compute Jacobian matrix
J = [-L1*sin(theta1) - L2*sin(theta1 + theta2), -L2*sin(theta1 + theta2);
L1*cos(theta1) + L2*cos(theta1 + theta2), L2*cos(theta1 + theta2)];
% Use pseudoinverse to handle near-singularities
J_inv = pinv(J);
% Position change vector
deltaXY = [deltaX_step; deltaY_step];
% Calculate joint angle increments
deltaTheta = J_inv * deltaXY;
% Store incremental joint angle changes
dTheta1(i) = deltaTheta(1);
dTheta2(i) = deltaTheta(2);
% Update joint angles for the next iteration
theta1 = theta1 + dTheta1(i);
theta2 = theta2 + dTheta2(i);
end
% Store results in a matrix
dTheta = [dTheta1; dTheta2];
% Display the results
disp('Delta theta values (dTheta1 and dTheta2) for each time step:');
disp(dTheta);