Jacobian matrices of 6DOF robot not calculable

Hello together,

I am working on an exercise for school where you have to calculate the Jacobian matrix of a DOF6 Kinova Gen3 robot.I have already calculated the Forward Kinematics (see code below) .

My problem now is that matlab cannot calculate the determinant symbolically.

I am not sure if the problem lies with the arctan2 or the huge matrice, since this term cannot be simplified.

Thank you very much for the help!

clear all; close all; clc;

%% robot size and joint angles
l1 = 156.4; l2 = 128.4; l3 = 5.4; l4 = 410.0; l5 = 6.4; 
l6 = 208.4; l7 = 105.9; l8 = 105.9; l9 = 61.5;
syms phi_1 phi_2 phi_3 phi_4 phi_5 phi_6;


%% fundamental Rotation and Translation
syms alpha; syms gamma; syms beta
syms l;

R_x = [1     0           0      0;
       0 cos(gamma) -sin(gamma) 0;
       0 sin(gamma)  cos(gamma) 0;
       0     0           0      1];
R_x(gamma) = R_x;

R_y = [ cos(beta) 0 sin(beta) 0;
            0     1     0     0;
       -sin(beta) 0 cos(beta) 0;
            0     0     0     1];
R_y(beta) = R_y;

R_z = [cos(alpha) -sin(alpha) 0 0;
       sin(alpha) cos(alpha)  0 0;
       0            0         1 0;
       0            0         0 1];

R_z(alpha) = R_z;

D_x = [1 0 0 l;
       0 1 0 0;
       0 0 1 0;
       0 0 0 1];

D_x(l) = D_x;

D_y = [1 0 0 0;
       0 1 0 l;
       0 0 1 0;
       0 0 0 1];
D_y(l) = D_y;

D_z = [1 0 0 0;
       0 1 0 0;
       0 0 1 l;
       0 0 0 1];
D_z(l) = D_z;



%% Forward Kinematics for Toolframe

T_B_0 = simplify(D_z(l1))                    % Base to Zero
T_0_1 = simplify(R_z(phi_1))                 % Zero to one....
T_1_2 = simplify(D_z(l2)*D_x(l3)*R_x(phi_2))
T_2_3 = simplify(D_z(l4)*R_x(phi_3))
T_3_4 = simplify(D_x(-l5)*D_z(l6)*R_z(phi_4))
T_4_5 = simplify(D_z(l7)*R_x(phi_5))
T_5_6 = simplify(D_z(l8)*R_z(phi_6))
T_6_T = simplify(D_z(l9))                    % six to tool

T_T_B = simplify(T_B_0 * T_0_1 * T_1_2 * T_2_3 * T_3_4 * T_4_5 * T_5_6 * T_6_T)
T_T_B = simplify(T_T_B)


%% Representation as XYZ Fixed-Angle                                            
b_x_1 = T_T_B(1,4);
b_x_2 = T_T_B(2,4);
b_x_3 = T_T_B(3,4);
b_x_5 = asin(-T_T_B(3,1));
b_x_6 = atan2((T_T_B(3,2)/cos(b_x_5)),(T_T_B(3,3)/cos(b_x_5)));
b_x_4 = atan2((T_T_B(2,1)/cos(b_x_5)),(T_T_B(1,1)/cos(b_x_5)));

L_X = [b_x_1 ;b_x_2 ;b_x_3; b_x_4 ;b_x_5; b_x_6];
L_X = simplify(L_X,100)
%%
 
J_1 = [diff(b_x_1,phi_1),diff(b_x_1,phi_2),diff(b_x_1,phi_3),diff(b_x_1,phi_4),diff(b_x_1,phi_5),diff(b_x_1,phi_6)];


J_2 = [diff(b_x_2,phi_1),diff(b_x_2,phi_2),diff(b_x_2,phi_3),diff(b_x_2,phi_4),diff(b_x_2,phi_5),diff(b_x_2,phi_6)];


J_3 = [diff(b_x_3,phi_1),diff(b_x_3,phi_2),diff(b_x_3,phi_3),diff(b_x_3,phi_4),diff(b_x_3,phi_5),diff(b_x_3,phi_6)];


J_4 = [diff(b_x_4,phi_1),diff(b_x_4,phi_2),diff(b_x_4,phi_3),diff(b_x_4,phi_4),diff(b_x_4,phi_5),diff(b_x_4,phi_6)];


J_5 = [diff(b_x_5,phi_1),diff(b_x_5,phi_2),diff(b_x_5,phi_3),diff(b_x_5,phi_4),diff(b_x_5,phi_5),diff(b_x_5,phi_6)];


J_6 = [diff(b_x_6,phi_1),diff(b_x_6,phi_2),diff(b_x_6,phi_3),diff(b_x_6,phi_4),diff(b_x_6,phi_5),diff(b_x_6,phi_6)];

%% Jacobian

J = [J_1;J_2;J_3;J_4;J_5;J_6]

%% det(J) for analyse of singularities

[Paul]

Hi Michael,

What do you mean by "cannot calculate the determinant symbolically"? Is there an error message? For sure, it will be a very long expression.

[Torsten]

How do you want to analyze an endless expression depending on phi_1,...,phi_6 even if you were successful in computing the determinant ?

[Walter Roberson]

Is it possible to restrict any of the symbols to real? Doing so can simplify places that would otherwise come out with conj() and real() and imag() components.

[Paul]

Even when retricting the phi* variables to be real, J still contains various real(), imag(), and terms with 1i because of how atan2(y,x) can get simplified to angle(x + 1i*y). I couldn't get anything simplified too far when I was looking at this.

Answer 1

[Walter Roberson]

Here is a performance improvement trick:

syms J_ [6 6]
detJ_ = det(J_);   %will be fast
detJ = subs(detJ_, J_, J);

This will give you a representation for the determinent of J faster than det(J) would. The result will not be as expanded as if you had used det(J) -- but if you need the expanded version then expand(detJ) formed this way will, in practice, still be faster than if you had done expand(det(J))

With all of the trig elements, there might be some identies that should ideally be processed... but that will not happen for det(J) either. It might be best to simplify(detJ) but unfortunately simplify() of a long expression takes a long time.

[Paul]

That is very cool. I've been playing around with this problem and this makes the computing the determinant extremely fast. Doing anything useful with the result, even displaying it, seems almost untenable, but that's a different problem.

[Walter Roberson]

Yup, even just displaying a long symbolic expression can take a really long time.

Internally when you do this kind of subs(), it does not need to expand out the expression. Instead, each expression in the internal version of J has a pointer to it, and when the subs() is being done, when the variable is found, the corresponding pointer can be dropped in place. So you can end up with a expression that contains a number of pointers to the same location, and that is not a problem internally for calculation purposes. But printing out the expression requires that the chain of pointers be chased repeatedly.

[Michael Zimmerli]

Thanks for all your comments and answers.

I think your are all right.

An analysis of such a determinant is not really possible. But the trick with subs has worked. I am probably in general on the wrong way to solve this task. I need to look through the theory again.

Thanks for all your inputs.

[Sam Chak]

Hi Mike, do you want to symbolically derive the analytical formula of the Jacobian matrix, or just a numerical evaluation of the Jacobian matrix for a specific configuration from the Base to the Effector of the Gen3 Kinova robot?

[Michael Zimmerli]

Hi Sam

So the excercise is to Compute the Jacobian matrix of this robot from tool flange {T} to robot base {B} . This to list all joint values where this robot is in a singularity. For that i thought i need a analytcal formula to calculate the determinent and find out when this is zero.

So i calcuated the transformation matrix from tool flange {T} to robot base {B} as a symbolically matrix with the parameter to calculate the orientation Vector of the tool as fixed-angle XYZ. But that matrices becom confusing and very big.

[Walter Roberson]

When you start looking at finding the zeros of the determinents, you should be checking if instead you should be finding eigenvalues.

Robot motion systems are notoriously intractable to analytic solutions.

[Paul]

Not a solution to the problem, but the code can be made a bit simpler.

Myabe all the lvalues shoudl be stored in a single vector and accessed via indexing. I didn't do that here because I didn't want to edit all of the l* entries below.

%% robot size and joint angles
%lvec = [156.4; 128.4; 5.4; 410.0; 6.4; 208.4; 105.9; 105.9; 61.5];
l1 = 156.4; l2 = 128.4; l3 = 5.4; l4 = 410.0; l5 = 6.4; 
l6 = 208.4; l7 = 105.9; l8 = 105.9; l9 = 61.5;

Similarly, consider making phi a sym vector, and assume the angles are real, which can help simplify things sometimes. Might also want to consider using assumeAlso to represent other phyiscal constraints on the angles.

phi = sym('phi',[1 6],'real')

phi = 

%syms phi_1 phi_2 phi_3 phi_4 phi_5 phi_6 real

The rotation and translation matrices can be defined as symfun objects from the jump

%% fundamental Rotation and Translation
syms alpha gamma beta
syms l;
R_x(gamma) = [1     0           0      0;
       0 cos(gamma) -sin(gamma) 0;
       0 sin(gamma)  cos(gamma) 0;
       0     0           0      1];
%R_x(gamma) = R_x;
R_y(beta) = [ cos(beta) 0 sin(beta) 0;
            0     1     0     0;
       -sin(beta) 0 cos(beta) 0;
            0     0     0     1];
%R_y(beta) = R_y;
R_z(alpha) = [cos(alpha) -sin(alpha) 0 0;
       sin(alpha) cos(alpha)  0 0;
       0            0         1 0;
       0            0         0 1];
%R_z(alpha) = R_z;
D_x(l) = [1 0 0 l;
       0 1 0 0;
       0 0 1 0;
       0 0 0 1];
%D_x(l) = D_x;
D_y(l) = [1 0 0 0;
       0 1 0 l;
       0 0 1 0;
       0 0 0 1];
%D_y(l) = D_y;
D_z(l) = [1 0 0 0;
       0 1 0 0;
       0 0 1 l;
       0 0 0 1];
%D_z(l) = D_z;
%% Forward Kinematics for Toolframe
T_B_0 = simplify(D_z(l1));                    % Base to Zero
T_0_1 = simplify(R_z(phi(1)));                 % Zero to one....
T_1_2 = simplify(D_z(l2)*D_x(l3)*R_x(phi(2)));
T_2_3 = simplify(D_z(l4)*R_x(phi(3)));
T_3_4 = simplify(D_x(-l5)*D_z(l6)*R_z(phi(4)));
T_4_5 = simplify(D_z(l7)*R_x(phi(5)));
T_5_6 = simplify(D_z(l8)*R_z(phi(6)));
T_6_T = simplify(D_z(l9));                    % six to tool
T_T_B = simplify(T_B_0 * T_0_1 * T_1_2 * T_2_3 * T_3_4 * T_4_5 * T_5_6 * T_6_T);
%T_T_B = simplify(T_T_B)

I'll just not here that b_x_6 is computed before b_x_4

%% Representation as XYZ Fixed-Angle                                            
b_x_1 = T_T_B(1,4);
b_x_2 = T_T_B(2,4);
b_x_3 = T_T_B(3,4);
b_x_5 = asin(-T_T_B(3,1));
b_x_6 = atan2((T_T_B(3,2)/cos(b_x_5)),(T_T_B(3,3)/cos(b_x_5)));
b_x_4 = atan2((T_T_B(2,1)/cos(b_x_5)),(T_T_B(1,1)/cos(b_x_5)));
L_X = [b_x_1 ;b_x_2 ;b_x_3; b_x_4 ;b_x_5; b_x_6];
L_X = simplify(L_X);

Use jacobian to compute J, instead all those calls to diff

J = jacobian(L_X,phi);
J

J = 

Maybe this can be simplified down some more, I wasn't able to get very far. Unless the atan2 are known to be abolutely necessary, I'd start with trying ordinary atan.

Instead of checking the determinant of J, maybe a miracle will occur with rref, or eig as @Walter Roberson suggested, from which one could determine when J loses rank. But I wouldn't hold my breath.

[Sam Chak]

Hi @Michael Zimmerli

I found an Accepted Answer by @Paul that is related to solving the determinant of the Jacobian matrix to identify the joint configurations that lead to singularities. You can check it out here:

https://www.mathworks.com/matlabcentral/answers/1574528-solving-jacobian-matrix-for-singularities