Solving Inverse Kinematics using pseudo-inverse Jacobian method

107 views (last 30 days)
I need help with writing the code to solve this equation attached. I have tried severally but my head seems to be running in a circle and I need an extra eye.
where: ζis a constant value, say 0.1.
"i" is the number of iterations.
k is the kinematics matrix: . Where q1 and q2 are joint angles to be calculated.
and are lengths of manipulator arm.
J is the Jacobian of the kinematics k(q).
xf is the final position of the end effector
I need to solve the equation iteratively until the following condition is met: , where or less.
This is the code I have written but seems like trash:
function [] = Jacobian_Pseudo_Inverse_Method()
clc;
disp('Jacobian Pseudo-Inverse Method selected');
syms q1 q2 q3 q4 q5
syms a1 a2 a3 a4 a5
%select the degree of freedom(DOF)
disp(['Select Manipulators DOF' newline '1. 2-DOF' newline '2. 3-DOF' newline '3. 4-DOF' newline '4. 5-DOF']);
dof = str2double(input('DOF: ','s'));
%Global variables
goal_point = input('Goal point (xf) = ');
xf = transpose(goal_point);
xi = input('ξ = ');
error_margin = input('Error margin (ε) = ');
%The section below is a switch case function to select which method to
%execute
switch dof
case 1
%inputs
init_config = input('Initial Config. (q0) = ');
q(0) = transpose(init_config);
kinematics = input('Kinematics Matrix (k) = ');
a1 = input('link length 1 = ');
a2 = input('link length 2 = ');
%calculate Jacobian and Jacobian transpose of kinematics matrix
Ja = jacobian(kinematics,[q1;q2]);
Jt = transpose(Ja);
JJt = Ja*Jt;
JJt_inverse = inv(JJt);
J_hash = Jt*JJt_inverse;
%J_hash = Jt/JJt_inverse;
%calculate qi
for i = 0:5000
q(i+1) = q(i) + xi*J_hash(q(i))*(xf - kinematics*(q(i)));
dist = norm(xf - kinematics*(q(i)));
while dist >= error_margin
i = i + 1;
end
disp('Number of iterations = %d',i);
end
%Find the values of q1 and q2
%S = solve(kinematics,[q1 q2]);
%theta1 = simplify(S.q1)
%theta2 = simplify(S.q2)
end
end
  6 Comments
David Asogwa
David Asogwa on 27 Apr 2022
You have a point there, the goal point is not actually feasible. Even after changing it to be within the radius range, the error still comes up.
Array indices must be positive integers or logical values.
Error in sym/subsref (line 997)
R_tilde = builtin('subsref',L_tilde,Idx);
Error in code_testing (line 29)
q(i+1) = q(i) + xi*J_hash(q(i))*(xf - kinematics(q(i)));
Torsten
Torsten on 27 Apr 2022
Array indices must be positive. You address q(0) since your i-loop runs from 0 to 5000.

Sign in to comment.

Accepted Answer

Riccardo Scorretti
Riccardo Scorretti on 28 Apr 2022
Hi David, there were several errors in your script (see below, see also the remark of @Torsten). After fixing them, your program works.
However, in my opinion you are using the Symbolic Computational Toolbox in a very unefficient way: at each step of the loop you have to use subs to replace the symbolic variables q1, q2 with the values stocked in q. This takes time uselessly.
I think that the solution I proposed in a previous post is more efficient. If you really want to have MATLAB to compute the Jacobian matrix, it is better to use the function matlabFunction to generate automatically and reliably an .m function which computes numerically the Jacobian (see https://fr.mathworks.com/help/symbolic/matlabfunction.html).
clc;
syms q1 q2
% goal_point = [4, 7];
goal_point = [3, -2];
xf = transpose(goal_point);
xi = 0.1;
error_margin = 0.01;
init_config = [15, -45];
q0 = transpose(init_config);
a1 = 3;
a2 = 2;
kinematics = [a1*cos(q1) + a2*cos(q1+q2); a1*sin(q1) + a2*sin(q1+q2)];
%calculate Jacobian and Jacobian transpose of kinematics matrix
Ja = jacobian(kinematics,[q1;q2]);
Jt = transpose(Ja);
JJt = Ja*Jt;
JJt_inverse = inv(JJt);
J_hash = Jt*JJt_inverse;
%J_hash = Jt/JJt_inverse;
%Find the values of q1 and q2
%S = solve(kinematics,[q1 q2]);
%S = solve(J_hash,[q1 q2]);
%theta1 = simplify(S.q1);
%theta2 = simplify(S.q2);
%calculate qi
q = q0;
for i = 1:5000
% *** you must replace symbolic variables q1, q2 with q(:,i) ***
kinematics_ = subs(kinematics, [q1 q2], q(:,i).');
J_hash_ = subs(J_hash, [q1 q2], q(:,i).');
q(:,i+1) = q(:,i) + xi*J_hash_*(xf - kinematics_);
dist = norm(xf - kinematics_);
% *** This boils up into an infinite loop
% while dist >= error_margin
% i = i + 1;
% end
if dist < error_margin , break ; end
% *** You cannot use disp in this way: use fprintf instead
fprintf('Number of iterations = %d\n',i);
end
Number of iterations = 1 Number of iterations = 2 Number of iterations = 3 Number of iterations = 4 Number of iterations = 5 Number of iterations = 6 Number of iterations = 7 Number of iterations = 8 Number of iterations = 9 Number of iterations = 10 Number of iterations = 11 Number of iterations = 12 Number of iterations = 13 Number of iterations = 14 Number of iterations = 15 Number of iterations = 16 Number of iterations = 17 Number of iterations = 18 Number of iterations = 19 Number of iterations = 20 Number of iterations = 21 Number of iterations = 22 Number of iterations = 23 Number of iterations = 24 Number of iterations = 25 Number of iterations = 26 Number of iterations = 27 Number of iterations = 28 Number of iterations = 29 Number of iterations = 30 Number of iterations = 31 Number of iterations = 32 Number of iterations = 33 Number of iterations = 34 Number of iterations = 35 Number of iterations = 36 Number of iterations = 37 Number of iterations = 38 Number of iterations = 39 Number of iterations = 40 Number of iterations = 41 Number of iterations = 42 Number of iterations = 43 Number of iterations = 44 Number of iterations = 45 Number of iterations = 46 Number of iterations = 47 Number of iterations = 48 Number of iterations = 49 Number of iterations = 50 Number of iterations = 51 Number of iterations = 52 Number of iterations = 53 Number of iterations = 54 Number of iterations = 55
% Let's see if it worked ...
figure
plot(q(1,:), 'r') ; hold on ; plot(q(2,:), 'b');
ylabel('Q') ; grid on ; box on
fprintf('kinematics = [%f , %f]\n', kinematics_(1), kinematics_(2));
kinematics = [2.993323 , -2.006209]

More Answers (1)

Riccardo Scorretti
Riccardo Scorretti on 27 Apr 2022
Hi David,
in your code you tried to use the Symbolic Computational Toolbox. Personally, for such a problem I prefer to go numerically. Let me know if it is what your are looking for:
deg = pi/180; % = conversion factor degrees --> radians
% Set the initial configuration
Q0 = [15 -30]'*deg;
A = [3 2]';
% Set the desired final configuration
Xf = compute_jacobian(A, [45*deg 90*deg]');
% Set the required tollerance
toll = 1.0E-3;
% Set the parameter chi
chi = 0.1;
% Here we go ...
Q = Q0;
[X, Jac] = compute_jacobian(A, Q0);
Q_rec = Q0 ; X_rec = X; % This is just to record the iterations
while norm(X - Xf) > toll
Q = Q + chi*pinv(Jac)*(Xf - X);
[X, Jac] = compute_jacobian(A, Q);
Q_rec(:,end+1) = Q ; X_rec(:,end+1) = X; % This is just to record the iterations
end
nbIter = size(X_rec, 2);
% Let's see if it worked ...
figure
subplot(2, 1, 1);
plot(Q_rec(1,:)/deg, 'r.-') ; hold on ; plot(Q_rec(2,:)/deg, 'b.-');
ylabel('Q') ; grid on ; box on
subplot(2, 1, 2);
plot(X_rec(1,:), 'r.-') ; hold on ; plot(X_rec(2,:), 'b.-');
plot([0 nbIter], [Xf(1) Xf(1)], 'r--');
plot([0 nbIter], [Xf(2) Xf(2)], 'b--');
ylabel('X') ; grid on ; box on
xlabel('Iteration');
function [Kmat, Jmat] = compute_jacobian(A, Q)
a1 = A(1) ; a2 = A(2);
q1 = Q(1) ; q2 = Q(2);
Kmat = [ ...
a1*cos(q1)+a2*cos(q1+q2) ; ...
a1*sin(q1)+a2*sin(q1+q2) ...
];
Jmat = [ ...
-a1*sin(q1)-a2*sin(q1+q2) -a2*sin(q1+q2) ; ...
a1*cos(q1)+a2*cos(q1+q2) a2*cos(q1+q2) ...
];
end
Interestingly (at least for me), when I programmed it, I expected that Q converged to the value [45 90]*rad, which I used to compute a feasible value of Xf. This is not the case, but one sees that the algorithm found as well an acceptable solution.
By the way, a test ought to be added to stop the algorithm if the target Xf is not feasible.

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!