Solving Inverse Kinematics using pseudo-inverse Jacobian method

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

[Sam Chak]

Hi @David Asogwa

Can you provide some examples of the inputs for the desired position of the end effector and the initial joint angles? You really need some real input values to test whether the core of your Inverse Kinematics Solver works or not. Once the results are validated, then you can take your sweet time to beautify the code to accept user input for demonstrating the Human–Computer Interaction.

How large is the search space? Are there constraints in the joint angles (they can rotate 360°)?

[David Asogwa]

Hi @Sam Chak

Yes, there are no constraints in the joint angles. They can rotate .

= transpose([4, 7])

The initial configuration =

Link length and .

Two-dimensional positional task spaces

[David Asogwa]

I made some changes, but this error keeps popping up:

clc;
syms q1 q2
goal_point = [4, 7];
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
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


%Error popping 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)));
 

[Riccardo Scorretti]

Left aside programming errors, it seems to me that the target point (4,7) is not feasible: with arms of length a1 = 2 and a2 = 3, the target point is bound to be inside a circle of radius a1+a2 = 5. Perpahs I don't have a good understanding of the problem.

[David Asogwa]

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]

Array indices must be positive. You address q(0) since your i-loop runs from 0 to 5000.

Answer 1

[Riccardo Scorretti]

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]

Answer 2

[Riccardo Scorretti]

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.

[David Asogwa]

I think this is a good way to start, but the convergence is now an issue.

Thanks man.