Scribd
Upload
4 views2 pages

matlab Assignment2C

robotics matlab assignment

Uploaded by

Karthy Asokan
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as TXT, PDF, TXT or read online on Scribd
4 views2 pages

matlab Assignment2C

robotics matlab assignment

Uploaded by

Karthy Asokan
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
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