Jacobian

In the field of robotic manipulators, Denavit-Hartenberg tables, Direct Kinematics and Inverse Kinematics are the basics. To command a robot to execute smooth trajectories, we must govern how it moves through time. This transition from static posture to fluid motion brings us to the core of differential kinematics: The Jacobian Matrix.

“Robotics is the study of the Jacobian” - Prof. Bruno Siciliano

The Jacobian matrix is one of the most vital mathematical tools in robotics. It serves as the analytical bridge between two entirely different worlds: the internal world of the robot’s motors (joint space) and the external world of human perception (operational or task space).

At any given configuration, the posture of a robot is defined by a vector of joint variables \(\mathbf{\textrm{q}}\). When those joints moves at a certain velocity, they impose some linear and angular velocities on the end effector. Those velocities are pached inside the vector \(\mathbf{\dot{\textrm{q}} }\).

The Jacobian matrix \(\mathbf{\textrm{J}}(\mathbf{\textrm{q}})\) can maps these joint velocities to the end-effector velocities.

\[ \mathbf{\textrm{J}}=\frac{\partial f}{\partial x}=\left(\begin{array}{ccc} \frac{\partial f_1 }{\partial x_1 } & … & \frac{\partial f_1 }{\partial x_n }\newline … & … & …\newline \frac{\partial f_n }{\partial x_1 } & … & \frac{\partial f_n }{\partial x_n } \end{array}\right) \]

Because of the changes in the robot’s geometry during motion, the elements of \(\mathbf{\textrm{J}}(\mathbf{\textrm{q}})\) are not constant; they are non-linear functions of the current joint positions \(\mathbf{\textrm{q}}\). For this reason, the Jacobian must be re-evaluated continuously during motion.

The Jacobian is also useful for establishing the joint toques required for applying a certain force and torques to the environment.

\[ \tau ={\mathbf{\textrm{J}}}^T (\mathbf{\textrm{q}})F \]

Another important usage of the Jacobian matrix is the study of the kinematics singularities.

When it happens that \(\det (\mathbf{\textrm{J}}(\mathbf{\textrm{q}}))=0\), the manipulator loose a degree of freedom in the operational space and a singularity is obtained. Moreover, considering the relation \(\mathbf{\dot{\textrm{q}} }={\mathbf{\textrm{J}}}^{-1} (\mathbf{\textrm{q}})\mathbf{\textrm{v}}\), it is obvious that near the singularities the denominator approach zero, then the joints velocities tend to infinity; for this reason it is a wise choice to avoid singularities.

Computation of the Jacobian

The Jacobian is computed starting from the DH-table, as an example it is used the two link planar arm.

P is a polar joint, R is a revolute joint

Excluding the 4th row:

• \({\mathbf{\textrm{z}}}_{i-1}\) Joint i axis of rotation (3rd column of \({\mathbf{\textrm{T}}}_{i-1}^0\) )
• \({\mathbf{\textrm{p}}}_{i-1} \) Origin position of frame i-1 (4th column of \({\mathbf{\textrm{T}}}_{i-1}^0\) )
• \({\mathbf{\textrm{p}}}_e\) End-effector position vector (4th column of \({\mathbf{\textrm{T}}}_N^0\) )

Then joining it is possble to obtain the Jacobian matrix \(\mathbf{\textrm{J}}=[{\mathbf{\textrm{J}}}_1 ~{\mathbf{\textrm{J}}}_2 ~\cdots ~{\mathbf{\textrm{J}}}_N ]=\left\lbrack \begin{array}{c} J_P \newline J_O \end{array}\right\rbrack\)

Two Link Planar Arm

DH Table

Transformation matrix

\[ T_2^0 =\left\lbrack \begin{array}{cccc} \cos (\theta_1 +\theta_2 ) & -\sin (\theta_1 +\theta_2 ) & 0 & L_1 \cos (\theta_1 )+L_2 \cos (\theta_1 +\theta_2 )\newline \sin (\theta_1 +\theta_2 ) & \cos (\theta_1 +\theta_2 ) & 0 & L_1 \sin (\theta_1 )+L_2 \sin (\theta_1 +\theta_2 )\newline 0 & 0 & 1 & 0\newline 0 & 0 & 0 & 1 \end{array}\right\rbrack \]

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34

clear
close all
clc

L1=1;
L2=1;
% a alpha d theta
dhparams = [L1      0      0      0; 
            L2      0      0      0];

%Definition of rigidBodyTree object
planar_arm=rigidBodyTree('DataFormat', 'row');
planar_arm.BaseName='base_link';

bodies=cell(size(dhparams,1)); joints=cell(size(dhparams,1));

%Generation can be iterated
for i=1:size(dhparams,1)
    %Every joint is revolute
    bodies{i}=rigidBody("link"+i);
    joints{i}=rigidBodyJoint("joint"+i, 'revolute');
    setFixedTransform(joints{i}, dhparams(i,:),'dh');
    bodies{i}.Joint=joints{i};
end

%Then joining bodies
for i=1:size(dhparams,1)
    if i==1
        addBody(planar_arm, bodies{i}, 'base_link');
    else
        addBody(planar_arm,bodies{i},bodies{i-1}.Name);
    end
end
planar_arm.showdetails

--------------------
Robot: (2 bodies)

 Idx    Body Name    Joint Name    Joint Type    Parent Name(Idx)   Children Name(s)
 ---    ---------    ----------    ----------    ----------------   ----------------
   1        link1        joint1      revolute        base_link(0)   link2(2)  
   2        link2        joint2      revolute            link1(1)   
--------------------

1
2
3
4
5

figure(1)
show(planar_arm,[pi/2 -pi/2]);
title("Planar Arm");
view([0.5 0.5 0.5]);
axis([-1.0 1.5 -1.0 1.5 -0.5 0.5]);

1
2

%Validation
T=getTransform(planar_arm, [pi/2 -pi/2], "link2")

T = 4x4
     1     0     0     1
     0     1     0     1
     0     0     1     0
     0     0     0     1

Symbolic math is a powerful ally helping avoiding making errors

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37

tL1=L1;tL2=L2;
syms L1 L2 theta_1 theta_2;
th12=theta_1+theta_2;

A_1=[cos(theta_1) -sin(theta_1)  0 L1*cos(theta_1);
     sin(theta_1)  cos(theta_1)  0 L1*sin(theta_1);
     0             0             1 0; 
     0             0             0 1];

A_2=[cos(theta_2) -sin(theta_2)  0 L2*cos(theta_2);
     sin(theta_2)  cos(theta_2)  0 L2*sin(theta_2);
     0             0             1 0; 
     0             0             0 1];

T=[cos(th12), -sin(th12), 0, L1*cos(theta_1)+L2*cos(th12);
   sin(th12),  cos(th12), 0, L1*sin(theta_1)+L2*sin(th12);
   0,          0,         1, 0;
   0,          0,         0, 1];

p_e=T(1:3,4);

%3th column of T_0^0 (identity)
z_0=[0;0;1];

%3th column of T_1^0
z_1=A_1(1:3,3);

%4th column of T_0^0 (identity)
p_0=[0;0;0];

%4th column of T_0^0
p_1=A_1(1:3,4);

j1=[cross(z_0,(p_e-p_0)); z_0];
j2=[cross(z_1,(p_e-p_1)); z_1];
%Jacobian
J=[j1 j2]

J =

\[ \displaystyle \left(\begin{array}{cc} -L_2 ,\sin \left(\theta_1 +\theta_2 \right)-L_1 ,\sin \left(\theta_1 \right) & -L_2 ,\sin \left(\theta_1 +\theta_2 \right)\newline L_2 ,\cos \left(\theta_1 +\theta_2 \right)+L_1 ,\cos \left(\theta_1 \right) & L_2 ,\cos \left(\theta_1 +\theta_2 \right)\newline 0 & 0\newline 0 & 0\newline 0 & 0\newline 1 & 1 \end{array}\right) \]

1
2

clear L1 L2 theta_1 theta_2;
L1=tL1;L2=tL2; clear tL1 tL2;

The rigidBodyTree class has a function able to automatically perform Jacobian calculation.

In MATLAB, the order is inverted. The calculated Jacobian will be in the form \(J_{\textrm{MATLAB}} =\left\lbrack \begin{array}{c} J_O \newline J_P \end{array}\right\rbrack\), with the angular part on top and the linear part on the bottom.

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19

for i=1:10
    q=-pi+2*pi*rand(1,2);
    if planar_arm.geometricJacobian(q,"link2")~=jacobianCalc(q)
        disp("ERROR: Jacobian is not correct!")
    end
end

function J = jacobianCalc(joints)
    L1=1;L2=1;
    theta_1=joints(1);
    theta_2=joints(2);
    t12=theta_1+theta_2;
    J=[-L2*sin(t12)-L1*sin(theta_1) -L2*sin(t12);
        L2*cos(t12)+L1*cos(theta_1) L2*cos(t12);    
        zeros(3,2);
        ones(1,2)];
    %Inversion for matching the MATLAB standard
    J=[J(4:6,:);J(1:3,:)];
end

Singularities

As mentioned, Jacobian is also used in the study of the singularities. A singularity occurs whenever the manipulator losses a degree of freedom for a maximum extension of the manipulator or joint alignment.

Usually checking if the determinant of the Jacobian is equal to zero is used to verify if the manipulator is in a singular configuration.

If the robot is underactuated or redundant it is possible to calculate the determinant of the associated matrix \(\det (\mathbf{\textrm{J}}(\mathbf{\textrm{q}}){\mathbf{\textrm{J}}}^T (\mathbf{\textrm{q}}))\) or just checking the rank.

Two Link Planar Arm

As calculated before, \(J=\left(\begin{array}{cc} -L_2 ,\sin \left(\theta_1 +\theta_2 \right)-L_1 ,\sin \left(\theta_1 \right) & -L_2 ,\sin \left(\theta_1 +\theta_2 \right)\newline L_2 ,\cos \left(\theta_1 +\theta_2 \right)+L_1 ,\cos \left(\theta_1 \right) & L_2 ,\cos \left(\theta_1 +\theta_2 \right)\newline 0 & 0\newline 0 & 0\newline 0 & 0\newline 1 & 1 \end{array}\right)\)

Some rows of the jacobian are not influenced because the manipulator is a planar arm. Following the definition of jacobian \(\mathbf{\textrm{J}}=\left\lbrack \begin{array}{ccc} \frac{\partial f_1 }{\partial x_1 } & \cdots & \frac{\partial f_1 }{\partial x_n }\newline \vdots & \ddots & \vdots \newline \frac{\partial f_m }{\partial x_1 } & \cdots & \frac{\partial f_m }{\partial x_n } \end{array}\right\rbrack\) and considering \(\begin{array}{l} x=L_1 \cos (\theta_1 )+L_2 \cos (\theta_1 +\theta_2 )\newline y=L_1 \sin (\theta_1 )+L_2 \sin (\theta_1 +\theta_2 ) \end{array}\) then:

\[ \mathbf{J}=\left\lbrack \begin{array}{cc} \frac{\partial x}{\partial \theta_1 } & \frac{\partial x}{\partial \theta_2 }\newline \frac{\partial y}{\partial \theta_1 } & \frac{\partial y}{\partial \theta_2 } \end{array}\right\rbrack =\left\lbrack \begin{array}{cc} -L_1 \sin \theta_1 -L_2 \sin (\theta_1 +\theta_2 ) & -L_2 \sin (\theta_1 +\theta_2 )\newline L_1 \cos \theta_1 +L_2 \cos (\theta_1 +\theta_2 ) & L_2 \cos (\theta_1 +\theta_2 ) \end{array}\right\rbrack =\left\lbrack \begin{array}{cc} J_{v_x } \newline J_{v_y } \end{array}\right\rbrack \]

Here the maximum rank is 2.

Singularity for elbow folded

Any \(\theta_1\), \(\theta_2 =\pi\)

1
2
3
4
5
6
7
8
9

q=[pi/3 pi];
J=planar_arm.geometricJacobian(q,"link2");
J=J(4:5,:);
if rank(J)<2
    disp("For the configuration");
    q
    disp("Singularity is present");
    J
end

For the configuration
q = 1x2
1.0472    3.1416

Singularity is present
J = 2x2
   -0.0000    0.8660
   -0.0000   -0.5000

Singularity of maximum elongation

Any \(\theta_1\), \(\theta_2 =0\)

1
2
3
4
5
6
7
8
9

q=[-pi/7 0];
J=planar_arm.geometricJacobian(q,"link2");
J=J(4:5,:);
if rank(J)<2
    disp("For the configuration");
    q
    disp("Singularity is present");
    J
end

For the configuration
q = 1x2
   -0.4488         0

Singularity is present
J = 2x2
    0.8678    0.4339
1.8019    0.9010

3-Link Arm

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59

L1=0.35;   
L2=0.6;   
L3=0.6;   

% a alpha d theta
dhparams = [0       pi/2   L1     0;
            L2      0      0      0; 
            L3      0      0      0];

%Definition of rigidBodyTree object
threeLinkArm=rigidBodyTree('DataFormat', 'row');
threeLinkArm.BaseName='base_link';

clear bodies joints;
bodies=cell(size(dhparams,1)); joints=cell(size(dhparams,1));

%Generation can be iterated
for i=1:size(dhparams,1)
    %Every joint is revolute
    bodies{i}=rigidBody("link"+i);
    joints{i}=rigidBodyJoint("joint"+i, 'revolute');
    setFixedTransform(joints{i}, dhparams(i,:),'dh');
    bodies{i}.Joint=joints{i};
end

%Then joining bodies
for i=1:size(dhparams,1)
    if i==1
        addBody(threeLinkArm, bodies{i}, 'base_link');
    else
        addBody(threeLinkArm,bodies{i},bodies{i-1}.Name);
    end
end

syms L1 L2 L3 theta_1 theta_2 theta_3;
syms c1 s1 c2 s2 c3 s3 s23 c23; 

A_1=[cos(theta_1)  0             sin(theta_1)    0;
     sin(theta_1)  0            -cos(theta_1)    0;
     0             1             0               L1; 
     0             0             0               1];

A_2=[cos(theta_2) -sin(theta_2)  0 L2*cos(theta_2);
     sin(theta_2)  cos(theta_2)  0 L2*sin(theta_2);
     0             0             1 0; 
     0             0             0 1];

A_3=[cos(theta_3) -sin(theta_3)  0 L3*cos(theta_3);
     sin(theta_3)  cos(theta_3)  0 L3*sin(theta_3);
     0             0             1 0; 
     0             0             0 1];
trigon=[cos(theta_1), sin(theta_1), cos(theta_2), ...
    sin(theta_2), cos(theta_3), sin(theta_3), ...
    cos(theta_2 + theta_3), sin(theta_2 + theta_3)];

compact=[c1, s1, c2, s2, c3, s3, c23, s23];

T=simplify(A_1*A_2*A_3);
T=subs(T,trigon,compact)

T =

\[ \displaystyle \left(\begin{array}{cccc} c_1 ,c_{23} & -c_1 ,s_{23} & s_1 & c_1 ,{\left(L_2 ,c_2 +L_3 ,c_{23} \right)}\newline c_{23} ,s_1 & -s_1 ,s_{23} & -c_1 & s_1 ,{\left(L_2 ,c_2 +L_3 ,c_{23} \right)}\newline s_{23} & c_{23} & 0 & L_1 +L_2 ,s_2 +L_3 ,s_{23} \newline 0 & 0 & 0 & 1 \end{array}\right) \]

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28

p_e=T(1:3,4);

%3th column of T_0^0 (identity)
z_0=[0;0;1];

%3th column of T_1^0
z_1=A_1(1:3,3);

%3th column of T_2^0
T_2_0=A_1*A_2;
z_2=T_2_0(1:3,3);

%4th column of T_0^0 (identity)
p_0=[0;0;0];
% 
%4th column of T_1^0 
p_1=A_1(1:3,4);
% 
%4th column of T_2^0
p_2=T_2_0(1:3,4);

j1=simplify([cross(z_0,(p_e-p_0)); z_0]);
j2=simplify([cross(z_1,(p_e-p_1)); z_1]);
j3=simplify([cross(z_2,(p_e-p_2)); z_2]);
%Jacobian
J=simplify([j1 j2 j3]);
J=expand(subs(J,trigon,compact))

J =

\[ \displaystyle \begin{array}{l} \left(\begin{array}{ccc} -L_2 ,c_2 ,s_1 -L_3 ,c_{23} ,s_1 & -L_2 ,c_1 ,s_2 -L_3 ,c_1 ,s_{23} & -L_3 ,c_1 ,s_{23} \newline L_2 ,c_1 ,c_2 +L_3 ,c_1 ,c_{23} & -L_2 ,s_1 ,s_2 -L_3 ,s_1 ,s_{23} & -L_3 ,s_1 ,s_{23} \newline 0 & L_2 ,{c_1 }^2 ,c_2 +L_3 ,{c_1 }^2 ,c_{23} +L_2 ,c_2 ,{s_1 }^2 +\sigma_1 & L_3 ,c_{23} ,{c_1 }^2 +\sigma_1 \newline 0 & s_1 & s_1 \newline 0 & -c_1 & -c_1 \newline 1 & 0 & 0 \end{array}\right)\\mathrm{}\\textrm{where}\\mathrm{}\;;\sigma_1 =L_3 ,c_{23} ,{s_1 }^2 \end{array} \]

1
2

detJp=simplify(J(1,1)*(J(2,2)*J(3,3)-J(2,3)*J(3,2))...
    -J(2,1)*(J(1,2)*J(3,3)-J(1,3)*J(3,2)))

detJp = \(\displaystyle -L_2 ,L_3 ,{\left(L_2 ,c_2 +L_3 ,c_{23} \right)},{\left(c_2 ,s_{23} -c_{23} ,s_2 \right)},{{\left({c_1 }^2 +{s_1 }^2 \right)}}^2 \)

1
2
3
4
5
6

clear L1 L2 L3 theta_1 theta_2 theta_3;
clear c1 s1 c2 s2 c3 s3 s23 c23;

L1=0.35;   
L2=0.6;   
L3=0.6; 

Considering the \(\mathbf{J_p }\) part only

\[ \mathbf{J_p }=\left(\begin{array}{ccc} -(L_2 c_2 +L_3 c_{23} )s_1 & -(L_2 s_2 +L_3 s_{23} )c_1 & -L_3 c_1 s_{23} \newline (L_2 c_2 +L_3 c_{23} )c_1 & -(L_2 s_2 +L_3 s_{23} )s_1 & -L_3 s_1 s_{23} \newline 0 & L_2 c_2 +L_3 c_{23} & L_3 c_{23} \end{array}\right) \]

\[ \det (\mathbf{J_p })=-L_2 ,L_3 ,\left(L_2 ,c_2 +L_3 ,c_{23} \right),\left(c_2 ,s_{23} -c_{23} ,s_2 \right) \]

\[ \begin{array}{l} \sin (\alpha \pm \beta )=\sin (\alpha )\cos (\beta )\pm \cos (\alpha )\sin (\beta )\newline \cos (\alpha \pm \beta )=\cos (\alpha )\cos (\beta )\mp \sin (\alpha )\sin (\beta ) \end{array} \]

\[ \begin{array}{l} c_2 s_{23} -c_{23} s_2 =c_2 (s_2 c_3 +c_2 s_3 )-s_2 (c_2 c_3 -s_2 s_3 )=\newline =s_2 c_2 c_3 +c_2^2 s_3 -s_2 c_2 c_3 +s_2^2 s_3 =\newline =s_3 \end{array} \]

\[ \det (\mathbf{J_p })=-L_2 ,L_3 ,s_3 ,\left(L_2 ,c_2 +L_3 ,c_{23} \right) \]

The determinant is zero if \(\sin (\theta_3 )=k\pi\) or \(L_2 ,c_2 +L_3 ,c_{23} =0\)

1
2

q=[pi/3,pi/5,-pi/6];
J=threeLinkArm.geometricJacobian(q,"link3")

J = 6x3
    0.0000    0.8660    0.8660
   -0.0000   -0.5000   -0.5000
1.0000    0.0000    0.0000
   -0.9371   -0.2077   -0.0314
    0.5411   -0.3597   -0.0543
    0.0000    1.0821    0.5967

1
2
3
4

%Selecting reducted Jacobian for the job
J_p=J(4:6,:);
max_rank=3;
rank_J_p=rank(J_p)

rank_J_p = 3

Singularity of maximum elongation

Any \(\theta_1 ,\theta_2 \) \( \theta_3 =0\)

 1
 2
 3
 4
 5
 6
 7
 8
 9
10

q=[pi/12 pi/5 0];
J=threeLinkArm.geometricJacobian(q,"link3");
J_p=J(4:6,:);
if rank(J_p)<max_rank
    disp("For the configuration");
    q
    disp("Singularity is present");
    J_p
    rank_J_p=rank(J_p)
end

For the configuration
q = 1x3
    0.2618    0.6283         0

Singularity is present
J_p = 3x3
   -0.2513   -0.6813   -0.3407
    0.9377   -0.1826   -0.0913
         0    0.9708    0.4854

rank_J_p = 2

1
2
3
4
5

show(threeLinkArm,q);
title("Singularity for maximum elongation");
xlim([0 1]);
ylim([-1 0.5]);
zlim([0.0 1.2]);

Singularity for elbow folded

Any \(\theta_1 ,\theta_2\), \(\theta_3 =\pi\)

 1
 2
 3
 4
 5
 6
 7
 8
 9
10

q=[pi/3, pi/5, pi];
J=threeLinkArm.geometricJacobian(q,"link3");
J_p=J(4:6,:);
if rank(J_p)<max_rank
    disp("For the configuration");
    q
    disp("Singularity is present");
    J_p    
    rank_J_p=rank(J_p)
end

For the configuration
q = 1x3
1.0472    0.6283    3.1416

Singularity is present
J_p = 3x3
    0.0000   -0.0000    0.1763
   -0.0000   -0.0000    0.3054
    0.0000   -0.0000   -0.4854

rank_J_p = 1

1
2
3
4
5

show(threeLinkArm,q);
title("Elbow Folded");
xlim([-0.5 0.5]);
ylim([-0.5 0.5]);
zlim([0.0 0.8]);

Singularity for shoulder alignment

Whenever the condition \(L_2 ,c_2 +L_3 ,c_{23} =0\) is present, the end effector is aligned with the rotational joint axis of the robot shoulder. It is possible to demonstrate this condition with a little bit of trigonometry.

\[ \begin{array}{l} x_2 =L_2 \cos (\theta_2 )\newline y_2 =L_2 \sin (\theta_2 ) \end{array} \]

\[ \begin{array}{l} x_3 =x_2 +L_3 \cos (\theta_2 +\theta_3 )\newline y_3 =y_3 +L_3 \sin (\theta_2 +\theta_3 ) \end{array} \]

To achieve axis alignment \(x_3 =0\).

\[ x_3 =L_2 \cos (\theta_2 )+L_3 \cos (\theta_2 +\theta_3 )=0 \]

Then it is possible to find a relation between the angles able to achieve this condition.

\[ \begin{array}{l} L_2 \cos (\theta_2 )+L_3 \cos (\theta_2 +\theta_3 )=0\newline L_2 \cos (\theta_2 )+L_3 \cos (\theta_2 )\cos (\theta_3 )-L_3 \sin (\theta_2 )\sin (\theta_3 )=0\newline \cos (\theta_2 )(L_2 +L_3 \cos (\theta_3 ))-L_3 \sin (\theta_2 )\sin (\theta_3 )=0\newline (L_2 +L_3 \cos (\theta_3 ))-L_3 \frac{\sin (\theta_2 )}{cos(\theta_2 )}\sin (\theta_3 )=0 \end{array} \]

\[ \begin{array}{l} (L_2 +L_3 \cos (\theta_3 ))-L_3 \tan (\theta_2 )\sin (\theta_3 )=0\newline \tan (\theta_2 )=\frac{(L_2 +L_3 \cos (\theta_3 ))}{L_3 \sin (\theta_3 )} \end{array} \]

Then the condition is:

Any \(\theta_1\), \(\tan (\theta_2 )=\frac{(L_2 +L_3 \cos (\theta_3 ))}{L_3 \sin (\theta_3 )}\)

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14

%Chosing theta_3=-pi/4
theta_3=-pi/3;
theta_2=atan2((L2+L3*cos(theta_3)),(L3*sin(theta_3)));

q=[2*pi/3, theta_2, theta_3];
J=threeLinkArm.geometricJacobian(q,"link3");
J_p=J(4:6,:);
if rank(J_p)<max_rank
    disp("For the configuration");
    q
    disp("Singularity elbow is present");
    J_p    
    rank_J_p=rank(J_p)
end

For the configuration
q = 1x3
    2.0944    2.0944   -1.0472

Singularity elbow is present
J_p = 3x3
    0.0000    0.5196    0.2598
    0.0000   -0.9000   -0.4500
    0.0000   -0.0000    0.3000

rank_J_p = 2

It is possible to visualize the result.

1
2
3
4
5

show(threeLinkArm,q);
title("End-Effector Aligned with the axis of theta_1");
xlim([-0.5 0.5]);
ylim([-0.5 0.5]);
zlim([-0.1 1.5]);

6dof Arm - Puma 560

In order to match the importable model, the DH-table is modified with an offset from the zero of 0.6718. The model is imported from the Peter Corke’s toolbox.

Below it is possible to obtain the symbolic T

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35

clear

% a alpha d theta
dhparams = [0         pi/2         0.6718          0; 
            0.4318    0            0         0; 
            0.0203   -pi/2         0.15005   0;
            0         pi/2         0.4318    0;
            0        -pi/2         0         0;
            0         0            0         0];

theta=sym('theta_', [1 6]);
a=dhparams(:,1);
alpha=dhparams(:,2);
d=dhparams(:,3);
A=cell(6,1);
for i=1:6
    s_i=sin(theta(i));
    c_i=cos(theta(i));
    s_a_i=sin(alpha(i));
    c_a_i=cos(alpha(i));
    a_i=a(i);
    d_i=d(i);
    A_i=[c_i    -s_i*c_a_i    s_i*s_a_i   a_i*c_i;
         s_i     c_i*c_a_i   -c_i*s_a_i   a_i*s_i;
           0         s_a_i        c_a_i       d_i;
           0             0            0        1];

    A{i}=A_i;
end
T=eye(4,4);
for i=1:6
    T=simplify(T*A{i});
end
puma=loadrvcrobot("puma",DataFormat="row");
ee="link6";

Safe position

1
2
3
4
5
6

q=[pi/2 -pi/2 pi pi/5 pi/3 pi/3];
show(puma,q);
title("Puma 560 safe position");
xlim([-0.5 0.5]);
ylim([-0.5 0.5]);
zlim([0 1]);

1

J=puma.geometricJacobian(q,"link6")

J = 6x6
   -0.0000    1.0000    1.0000   -0.0000    0.8090    0.5090
   -0.0000   -0.0000   -0.0000   -1.0000         0   -0.5000
1.0000   -0.0000   -0.0000    0.0000    0.5878   -0.7006
    0.4318    0.0000    0.0000    0.0000         0         0
    0.1501    0.4115   -0.0203   -0.0000         0         0
   -0.0000   -0.4318   -0.4318    0.0000         0         0

1

rank_J=rank(J)

rank_J = 6

Singularity of maximum elongation

When the robot is fully stretched we loose rank.

1
2
3
4
5
6

q=[0 pi/2 -pi/2 0 0 0];
show(puma,q);
title("Puma 560 maximum elongation")
xlim([-0.5 0.5]);
ylim([-0.5 0.5]);
zlim([0 1.8]);

1
2
3
4
5
6
7
8

J=puma.geometricJacobian(q,ee);
if det(J)<=1e-15
    disp("For the configuration");
    q
    disp("Singularity is present");
    J    
    rank_J=rank(J)
end

For the configuration
q = 1x6
         0    1.5708   -1.5708         0         0         0

Singularity is present
J = 6x6
         0         0         0         0         0         0
         0   -1.0000   -1.0000         0   -1.0000         0
1.0000    0.0000    0.0000    1.0000    0.0000    1.0000
    0.1500   -0.8636   -0.4318         0         0         0
    0.0203    0.0000    0.0000         0         0         0
         0    0.0203    0.0203         0         0         0

rank_J = 5

Wrist Singularity

When the 4th and the 6th axis are aligned, we have a wrist singularity. This happen when \(q_5 =0\) or \(q_5 =\pm \pi\), depending on the phisical structure of the robot and mechanical limitations.

1
2
3
4
5
6
7
8
9

q = [pi/2 pi/4 -2.2515 -pi/3 0 pi/4];
J=puma.geometricJacobian(q,ee);
if det(J)<1e-10
    disp("For the configuration");
    q
    disp("Wrist singularity is present");
    J    
    rank_J=rank(J)
end

For the configuration
q = 1x6
1.5708    0.7854   -2.2515   -1.0472         0    0.7854

Wrist singularity is present
J = 6x6
   -0.0000    1.0000    1.0000    0.0000    0.5000    0.0000
    0.0000   -0.0000   -0.0000    0.9945   -0.0905    0.9945
1.0000    0.0000    0.0000    0.1045    0.8613    0.1045
   -0.7369   -0.0000   -0.0000   -0.0000         0         0
    0.1501   -0.3303   -0.0249   -0.0000         0         0
   -0.0000    0.7369    0.4316    0.0000         0         0

rank_J = 5

1
2
3
4
5

show(puma,q);
title("Puma560 Wrist Singularity");
xlim([-0.4 0.4]);
ylim([-0.4 0.8]);
zlim([0 1.2]);

Effect of singularities on joint velocities

If a manipulator is near a singularity position, it is needed a lot of effort to move the end effector from that position.

The more near to the singularity, the more effort is needed.

1
2
3
4

q=[0 pi/2 -pi/2 0 0 0];
%Moving the 5th joint of 5deg
q(5)=deg2rad(5);
J=puma.geometricJacobian(q,"link6")

J = 6x6
    0.0000         0         0    0.0000    0.0000   -0.0872
    0.0000   -1.0000   -1.0000    0.0000   -1.0000   -0.0000
1.0000    0.0000    0.0000    1.0000    0.0000    0.9962
    0.1500   -0.8636   -0.4318    0.0000         0         0
    0.0203    0.0000    0.0000    0.0000         0         0
   -0.0000    0.0203    0.0203   -0.0000         0         0

Following the formula \(\mathbf{\textrm{v}}=\mathbf{\textrm{J}}(\mathbf{\textrm{q}})\mathbf{\dot{\textrm{q}} }\), trying to move the end-effector by \(0.1\textrm{m/s}\) in the z direction, it is possible to notice the huge effort the joints should achieve in order to satisfy the requirement.

\[ \mathbf{\textrm{v}}=\mathbf{\textrm{J}}(\mathbf{\textrm{q}})^{-1} \left\lbrack \begin{array}{c} 0\newline 0\newline 0\newline 0\newline 0\newline 0.1 \end{array}\right\rbrack \]

1

v=inv(J)*[0 0 0 0 0 0.1]'

v = 6x1
   -0.0000
   -4.9261
    9.8522
    0.0000
   -4.9261
   -0.0000

In this configuration, the 3th joint (the elbow) needs a velocity of \(9.8\textrm{rad/s}\), that is really closer to the limits of the actuators.

Ellipsoid

The manipulability ellipsoid is a mathematical tool used to evaluate the manipulation capabilities of the end-effector at a certain position.

The shape of the ellipsoid represents the velocity capabilities in the operational space that the end-effector can achieve from a starting position when providing joint velocities of unitary norm.

By looking at the shape of the velocity ellipsoid, it is possible to have an idea of the robot kinematic capabilities.

Along the major axis, the robot can move very fast in that direction with minimal joint effort.

Along the minor axis, moving the end-effector along this path requires the motors to spin very fast for very little physical displacement.

If the ellipsoid is a perfect sphere the robot is in an ideal state, with the result that it can move with equal speed and equal effort in all directions. Whenever the end-effector approaches a singularity, the ellipsoid tends to flatten or shrink along the direction of the lost degree of freedom.

Puma 560

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22

clear
puma=loadrvcrobot("puma",DataFormat="row");
ee="link6";
q=[pi/3 pi/4 -pi/3 pi/2 pi/3 0.8];
J=puma.geometricJacobian(q,ee);

figure;
%Translation Ellipsoid
subplot(1, 2, 1);
Jt=J(4:6,:);
Et=inv(Jt * Jt');
plotellipsoid(Et);
title('Traslational Velocity Ellipsoid');
grid on;

%Rotational Ellipsoid
subplot(1,2,2);
Jr=J(1:3,:);
Ev=inv(Jr*Jr');
plotellipsoid(Ev);
title('Rotational Velocity Ellipsoid');
grid on;

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19

q=[0 pi/2 -pi/2+0.08 0 0 0];
J=puma.geometricJacobian(q,ee);

figure;
%Translation Ellipsoid
subplot(1, 2, 1);
Jt=J(4:6,:);
Et=inv(Jt * Jt');
plotellipsoid(Et);
title('Traslational Velocity Ellipsoid');
grid on;

%Rotational Ellipsoid
subplot(1,2,2);
Jr=J(1:3,:);
Ev=inv(Jr*Jr');
plotellipsoid(Ev);
title('Rotational Velocity Ellipsoid');
grid on;

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19

q=[0 pi/2 -pi/2+deg2rad(10) 0 0 0];
J=puma.geometricJacobian(q,ee);

figure;
%Translation Ellipsoid
subplot(1, 2, 1);
Jt=J(4:6,:);
Et=inv(Jt * Jt');
plotellipsoid(Et);
title('Traslational Velocity Ellipsoid');
grid on;

%Rotational Ellipsoid
subplot(1,2,2);
Jr=J(1:3,:);
Ev=inv(Jr*Jr');
plotellipsoid(Ev);
title('Rotational Velocity Ellipsoid');
grid on;

Two Link Planar Arm

For a better visualization of the ellipsoid it is useful to visualize it reducing a dimension of the spece, using the Two Link Planar Arm as an example.

It is easy to see how the ellipsoid progressivelly shrinks when the end-effector aproach a singularity.

1
2
3

clear

planar_arm=ETS2.Rz("q1")*ETS2.Tx(1)*ETS2.Rz("q2")*ETS2.Tx(1);

1
2

%Good conditions
planar_arm.teach([0 pi/2],"vellipse")

1
2

%Near singularity
planar_arm.teach([0 deg2rad(15)],"vellipse")

1
2

%Very close to the singularity
planar_arm.teach([0 deg2rad(3)],"vellipse");

Pseudo-Jacobian

In general, if the number of actuated joints is less than 6 it is possible to face some problems when inverting the jacobian.

In this case more or zero rows are always present, even if the manipulaotr is not in a conventional singular condition, collapsing the determinant to zero and not ermitting the inversion of the Jacobian.

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35

L1=0.35;   
L2=0.6;   
L3=0.6;   

% a alpha d theta
dhparams = [0       pi/2   L1     0;
            L2      0      0      0; 
            L3      0      0      0];

%Definition of rigidBodyTree object
threeLinkArm=rigidBodyTree('DataFormat', 'row');
threeLinkArm.BaseName='base_link';

clear bodies joints;
bodies=cell(size(dhparams,1)); joints=cell(size(dhparams,1));

%Generation can be iterated
for i=1:size(dhparams,1)
    %Every joint is revolute
    bodies{i}=rigidBody("link"+i);
    joints{i}=rigidBodyJoint("joint"+i, 'revolute');
    setFixedTransform(joints{i}, dhparams(i,:),'dh');
    bodies{i}.Joint=joints{i};
end

%Then joining bodies
for i=1:size(dhparams,1)
    if i==1
        addBody(threeLinkArm, bodies{i}, 'base_link');
    else
        addBody(threeLinkArm,bodies{i},bodies{i-1}.Name);
    end
end
q=[pi/3 -pi/3 pi/4];
J=threeLinkArm.geometricJacobian(q,"link2");

During the previous example, only the \({\mathbf{\textrm{J}}}_p\) part where selected, giving the possibility to do all the calculations.

In general, when facing those problems, it is possible to use the Pseudo-Jacobian.

\[ J(q)^+ =[J(q)^T J(q)]^{-1} J(q)^T \]

In MATLAB it is possible to calculate it using the function pinv

1

J_pinv=pinv(J)

J_pinv = 3x6
   -0.0000   -0.0000    0.9174   -0.2384    0.1376   -0.0000
    0.6368   -0.3676   -0.0000    0.1910    0.3309    0.2206
         0         0         0         0         0         0

Then it is possible to use it for the calculations \(\mathbf{\textrm{v}}=\mathbf{\textrm{J}}(\mathbf{\textrm{q}})\mathbf{\dot{\textrm{q}} }\), \(\mathbf{\dot{\textrm{q}} }={\mathbf{\textrm{J}}}^{-1} (\mathbf{\textrm{q}})\mathbf{\textrm{v}}\)

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13

q=[1 1 1];
J=threeLinkArm.geometricJacobian(q,"link3");
J_pinv=pinv(J);
J_p=J(4:6,:);

%Calculating joint velocities required
%To achieve the desired motion
%Imposing 0 rad/s, 0.1 m/s along X, 0.2 m/s along Y
v_des=[0 0 0 0 0.1 0.2]';
qd = J_pinv*v_des;

%Reversing
v_obtained=J*qd

v_obtained = 6x1
   -0.0724
    0.0465
    0.0040
   -0.0045
   -0.0065
    0.0567

1
2

%Error calculation
error=norm(v_des-v_obtained)

error = 0.1983