i want to make inverse kinematics for 3 dof robot arm usnig Newton Raphson in matlap but i always get this error "(Not enough input arguments. Error in robo2 (line 2) )"
조회 수: 5 (최근 30일)
이전 댓글 표시
function f = roots_fn_inv(t1,t2,t3,x,y,z,l1,l2,l3)
f = [l1*cos(t1)*(cos(t2+t3)+cos(t2))-x;
l2*sin(t1)*(cos(t2+t3)+cos(t2))-y;
l1*sin(t2+t3)+l1*sin(t2)+l3-z];
end
function f_dash = drev_roots_fn_inv(t1,t2,t3,l1,l2,l3)
f_dash = [-l1*sin(t1)*(cos(t2 + t3) + cos(t2)), -l1*cos(t1)*(sin(t2 + t3) + sin(t2)), -l1*cos(t1)*sin(t2 + t3);
l1*cos(t1)*(cos(t2 + t3) + cos(t2)), -l2*sin(t1)*(sin(t2 + t3) + sin(t2)), -l2*sin(t1)*sin(t2 + t3);
0, l1*cos(t2+t3)+l1*cos(t2), l1*cos(t2+t3)];
end
function param_vec = compute_inv (x, y, z, l1, l2, l3, epsilon)
t1 = pi/2; % initial guess
t2 = pi/9; % initial guess
t3 = pi/6; % initial guess
param_vec=[t1,t2,t3];
F_n = roots_fn_inv(t1, t2, t3, x, y, z, l1, l2, l3);
while norm(F_n) > epsilon
% compute F' matrix
F_n_dash = drev_roots_fn_inv(t1, t2, t3, l1, l2, l3);
% update parameters
param_vec = param_vec - F_n_dash\F_n;
t1 = param_vec(1);
t2 = param_vec(2);
t3 = param_vec(3);
F_n = roots_fn_inv(t1, t2, t3, x, y, z, l1, l2, l3);
end
param_vec = [t1, t2, t3];
x = 2;
y = 3;
z = 5;
l1 = 10;
l2 = 10;
l3 = 4.5;
epsilon = 0.01;
t = compute_inv(x, y, z, l1, l2, l3, epsilon);
f = roots_fn_inv(t(1), t(2), t(3), x, y, z, l1, l2, l3);
end
댓글 수: 0
답변 (1개)
Walter Roberson
2023년 12월 17일
Reorder the parts.
format long g
x = 2;
y = 3;
z = 5;
l1 = 10;
l2 = 10;
l3 = 4.5;
epsilon = 0.01;
t = compute_inv(x, y, z, l1, l2, l3, epsilon);
f = roots_fn_inv(t(1), t(2), t(3), x, y, z, l1, l2, l3);
f
function f = roots_fn_inv(t1,t2,t3,x,y,z,l1,l2,l3)
f = [l1*cos(t1)*(cos(t2+t3)+cos(t2))-x;
l2*sin(t1)*(cos(t2+t3)+cos(t2))-y;
l1*sin(t2+t3)+l1*sin(t2)+l3-z];
end
function f_dash = drev_roots_fn_inv(t1,t2,t3,l1,l2,l3)
f_dash = [-l1*sin(t1)*(cos(t2 + t3) + cos(t2)), -l1*cos(t1)*(sin(t2 + t3) + sin(t2)), -l1*cos(t1)*sin(t2 + t3);
l1*cos(t1)*(cos(t2 + t3) + cos(t2)), -l2*sin(t1)*(sin(t2 + t3) + sin(t2)), -l2*sin(t1)*sin(t2 + t3);
0, l1*cos(t2+t3)+l1*cos(t2), l1*cos(t2+t3)];
end
function param_vec = compute_inv (x, y, z, l1, l2, l3, epsilon)
t1 = pi/2; % initial guess
t2 = pi/9; % initial guess
t3 = pi/6; % initial guess
param_vec=[t1,t2,t3];
F_n = roots_fn_inv(t1, t2, t3, x, y, z, l1, l2, l3);
while norm(F_n) > epsilon
% compute F' matrix
F_n_dash = drev_roots_fn_inv(t1, t2, t3, l1, l2, l3);
% update parameters
param_vec = param_vec - F_n_dash\F_n;
t1 = param_vec(1);
t2 = param_vec(2);
t3 = param_vec(3);
F_n = roots_fn_inv(t1, t2, t3, x, y, z, l1, l2, l3);
end
param_vec = [t1, t2, t3];
end
댓글 수: 0
참고 항목
제품
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!