inverse kinematics of 7 dof robot using newton raphson method anyone can help. i am not getting desired position of end effector

main()
function main
% Desired end-effector position and orientation
T = [0 1 0 -5;-0.0008 0 1 0.0191;1 0 0.0008 -24; 0 0 0 1];
I = eye(4);
% Initial guess for the joint angles
thetha = [0;0;0;0;0;0;0];
% Tolerance and maximum number of iterations
tolerance = 1e-4;
max_iters = 10000;
alpha = 0.05;
% Main IK loop using Newton-Raphson
for i = 1:max_iters

g = gmatrix(thetha);
G = inv(g);
d = G*T -I ;
% Compute the error between current and desired pose
D = [d(1,4);d(2,4);d(3,4);d(3,2);d(1,3);d(2,1)];

% Check if the solution is within the tolerance
if norm(D) < tolerance
fprintf(‘Converged to a solution after %d iterations.n’, i);
disp(‘Joint angles (radians):’);
disp(thetha);
return;
end

% Compute the Jacobian matrix at the current joint angles
j = jac(thetha);
% Calculate the pseudoinverse of the Jacobian
J_pinv = pinv(j); % Moore-Penrose pseudoinverse

% Update the joint angles
thetha = thetha + alpha*J_pinv * D;
thetha = round(thetha,3);
end
display(thetha)

fprintf(‘Maximum iterations reached without convergence.n’);
a = [0 2 25 3 2 2 1];
a1 = [-1.57 1.57 -1.57 1.57 -1.57 0 0];
d = [0 0 0 0 0 0 0];
th = thetha;

T1=eye(4);
for i=1:7

end
function g1 = gmatrix(th)
a = [0 2 25 3 2 2 1];
a1 = [-1.57 1.57 -1.57 1.57 -1.57 0 0];
d = [0 0 0 0 0 0 0];
T=eye(4);
for i=1:7

end
function J = jac(th)

a = [0 2 25 3 2 2 1];
a1 = [-1.57 1.57 -1.57 1.57 -1.57 0 0];
d = [0 0 0 0 0 0 0];

T=eye(4);
for i=1:7

A=[cos(th(i)) -sin(th(i)) 0 a(i); sin(th(i))*cos(a1(i)) cos(th(i))*cos(a1(i)) -sin(a1(i)) -sin(a1(i))*d(i); sin(th(i))*sin(a1(i)) cos(th(i))*sin(a1(i)) cos(a1(i)) cos(a1(i))*d(i); 0 0 0 1];
T=T*A;
z(1:3,i)=T(1:3,3);
p(1:3,i)=T(1:3,4);
end
ro = [0; 0; 1;];
J=[cross(ro,p(:,7)) cross(z(:,1),(p(:,7)-p(:,1))) cross(z(:,2),(p(:,7)-p(:,2))) cross(z(:,3),(p(:,7)-p(:,3))) cross(z(:,4),(p(:,7)-p(:,4))) cross(z(:,5),(p(:,7)-p(:,5))) cross(z(:,6),(p(:,7)-p(:,6))) ;ro z(:,1) z(:,2) z(:,3) z(:,4) z(:,5) z(:,6)];

endmain()
function main
% Desired end-effector position and orientation
T = [0 1 0 -5;-0.0008 0 1 0.0191;1 0 0.0008 -24; 0 0 0 1];
I = eye(4);
% Initial guess for the joint angles
thetha = [0;0;0;0;0;0;0];
% Tolerance and maximum number of iterations
tolerance = 1e-4;
max_iters = 10000;
alpha = 0.05;
% Main IK loop using Newton-Raphson
for i = 1:max_iters

g = gmatrix(thetha);
G = inv(g);
d = G*T -I ;
% Compute the error between current and desired pose
D = [d(1,4);d(2,4);d(3,4);d(3,2);d(1,3);d(2,1)];

% Check if the solution is within the tolerance
if norm(D) < tolerance
fprintf(‘Converged to a solution after %d iterations.n’, i);
disp(‘Joint angles (radians):’);
disp(thetha);
return;
end

% Compute the Jacobian matrix at the current joint angles
j = jac(thetha);
% Calculate the pseudoinverse of the Jacobian
J_pinv = pinv(j); % Moore-Penrose pseudoinverse

% Update the joint angles
thetha = thetha + alpha*J_pinv * D;
thetha = round(thetha,3);
end
display(thetha)

fprintf(‘Maximum iterations reached without convergence.n’);
a = [0 2 25 3 2 2 1];
a1 = [-1.57 1.57 -1.57 1.57 -1.57 0 0];
d = [0 0 0 0 0 0 0];
th = thetha;

T1=eye(4);
for i=1:7

end
function g1 = gmatrix(th)
a = [0 2 25 3 2 2 1];
a1 = [-1.57 1.57 -1.57 1.57 -1.57 0 0];
d = [0 0 0 0 0 0 0];
T=eye(4);
for i=1:7

end
function J = jac(th)

a = [0 2 25 3 2 2 1];
a1 = [-1.57 1.57 -1.57 1.57 -1.57 0 0];
d = [0 0 0 0 0 0 0];

T=eye(4);
for i=1:7

end main()
function main
% Desired end-effector position and orientation
T = [0 1 0 -5;-0.0008 0 1 0.0191;1 0 0.0008 -24; 0 0 0 1];
I = eye(4);
% Initial guess for the joint angles
thetha = [0;0;0;0;0;0;0];
% Tolerance and maximum number of iterations
tolerance = 1e-4;
max_iters = 10000;
alpha = 0.05;
% Main IK loop using Newton-Raphson
for i = 1:max_iters

g = gmatrix(thetha);
G = inv(g);
d = G*T -I ;
% Compute the error between current and desired pose
D = [d(1,4);d(2,4);d(3,4);d(3,2);d(1,3);d(2,1)];

% Check if the solution is within the tolerance
if norm(D) < tolerance
fprintf(‘Converged to a solution after %d iterations.n’, i);
disp(‘Joint angles (radians):’);
disp(thetha);
return;
end

% Compute the Jacobian matrix at the current joint angles
j = jac(thetha);
% Calculate the pseudoinverse of the Jacobian
J_pinv = pinv(j); % Moore-Penrose pseudoinverse

% Update the joint angles
thetha = thetha + alpha*J_pinv * D;
thetha = round(thetha,3);
end
display(thetha)

fprintf(‘Maximum iterations reached without convergence.n’);
a = [0 2 25 3 2 2 1];
a1 = [-1.57 1.57 -1.57 1.57 -1.57 0 0];
d = [0 0 0 0 0 0 0];
th = thetha;

T1=eye(4);
for i=1:7

end
function g1 = gmatrix(th)
a = [0 2 25 3 2 2 1];
a1 = [-1.57 1.57 -1.57 1.57 -1.57 0 0];
d = [0 0 0 0 0 0 0];
T=eye(4);
for i=1:7

end
function J = jac(th)

a = [0 2 25 3 2 2 1];
a1 = [-1.57 1.57 -1.57 1.57 -1.57 0 0];
d = [0 0 0 0 0 0 0];

T=eye(4);
for i=1:7

end matlab, inverse kinematics, 7 dof robot, jacobian matrix MATLAB Answers — New Questions

Cart

Cart