python humanoid robot walking : position control [1] : get inverse matrix (6 x 6) for inverse kinematics, using matlab jacobian inverse for IK

use matlab, get inverse jacobian matrix , robot is darwin-op3

above image is not op3 , op

get jacobian matrix

1. Create symbolic 6 by 6 matrix

A = sym(zeros(6));
% Fill the matrix with the symbolic variables A01 to A66
for i = 1:6
    for j = 1:6
        varName = sprintf('A%d%d', i, j);
        A(i, j) = sym(varName);
    end
end
disp(A);

2. get determinant of 6 by 6 matrix

A_det=det(A);
disp(A_det);
fid = fopen('A_det.txt', 'wt');
fprintf(fid, '%s\n', char(A_det));
fclose(fid);

A_inv = A_adj / A_det;
disp(A_inv)

you can copy this to python, but keep in mind that using this code directly might result in errors depending on the envioronment you’re using. Therefore, you need to break it down and use it in segments. Refer to the image below for more details.

3. get adjoint matrix

Of course, we can obtain the adjoint matrix of A using Python, but it’s extremely slow. so we get the adjoint matrix using MATLAB

A_adj=adjoint(A);
disp(A_adj);

fid = fopen('A_adj.txt', 'wt');
for i = 1:6
    for j = 1:6
        
        a=char(A_adj(i,j));
        half_length=floor(length(a))/2
        a_1=a(1:half_length);
        a_2=a(half_length+1:end);
        fprintf(fid,'J_inv_1=%s\n',a_1);
        fprintf(fid,'J_inv_2=%s\n',a_2);
        fprintf(fid,'J_inv[%d][%d]=(J_inv_1+J_inv_2)/J_det\n',char(i-1),char(j-1));
       
    end
end
% fprintf(fid, '%s\n', char(A_adj));
fclose(fid);

at disp(A_adj); you can see

result is 95017 length!!! it’s too long that when you convert the python code, there might be errors due to the lengthy computation in one line. So, we need to split the result. Inside the for-loop, we have the following code:

        half_length=floor(length(a))/2
        a_1=a(1:half_length);
        a_2=a(half_length+1:end);
        fprintf(fid,'J_inv_1=%s\n',a_1);
        fprintf(fid,'J_inv_2=%s\n',a_2);
        fprintf(fid,'J_inv[%d][%d]=(J_inv_1+J_inv_2)/J_det\n',char(i-1),char(j-1));

finally we can get

and copy to python

done

you can get inverse of 6 by 6 matrix in python.

result

thank you