Scribd
Upload
4 views

matlab Assignment2C

robotics matlab assignment

Uploaded by

Karthy Asokan
Copyright
© © All Rights Reserved
Available Formats
Download as TXT, PDF, TXT or read online on Scribd
4 views

matlab Assignment2C

robotics matlab assignment

Uploaded by

Karthy Asokan
Copyright
© © All Rights Reserved
Available Formats
Download as TXT, PDF, TXT or read online on Scribd
You are on page 1/ 2

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);

You might also like