Very slow calculations of link orientation for 6DOF IK

Question

I have written a matlab code to solve IK for 6 DOF robotic arm. I use Newton method to numerically solve IK. Also i use Tikhonov regularization to hand bad conditioned Jacobians. It works fast and reliable when i want just to move the last link in certain position, when i use difference between X,Y,Z coordinates as condition to interrupt loop of Newtoon method. But when i want also to get into the right orientation (use difference between Euler angles as interrupt condition) it takes a very long time 2, 5, 10 minutes even more, regardless i want to get to the right coordinates also or not. So there are questions:

1. How can i accelerate calculations, or why is it so slow?
2. Can i use quternions instead of Euler angles? Quternions will increase dimension of Jacobian so it will not be a square matrix anymore and it will not be possible to use Tikhonov regularization that works so good.
3. How often people use numerical methods to solve such things? I saw may examples of using analitycal solutions but not numerical.
4. How to get sure that programm will find solution using Newton method and programm will find it in finite number of interations?

UPD: here is my matlab code, that was rewritten using damped least squares method and quaternion. But still i have the same problem. In this code we move along trajectory, but we can remove it and try to jump directly to the destination point.

%Derivative step for Jacobian composing
step = 0.01;

%Generalized coordinates for start position
q_prev = [34; 89; 1; 1; 89; 0];

%Generalized coordinates for end position. To be sure we can reach it
q_fin = [170; 150; 120; 156; 9; 158];

%get_coordinates() function returns 4x4 matrix of homogeneous transformations. It contains forward kinematics equations
%Coordinates we are at 
A_forward = get_coordinates( q_prev ); 

%Coordinates we need to reach
Dest = get_coordinates( q_fin );

%Getting rotation matrices for start and finish positions
rotmat_curr = A_forward(1:3, 1:3);
rotmat_dest = Dest(1:3, 1:3);

%Matrix_to_quat() - is my analog of rotm2quat() function
%Getting quternions for start and finish positions
quat_curr = matrix_to_quat(rotmat_curr);
quat_dest = matrix_to_quat(rotmat_dest);

%Next steps are not inmportant, but i still comment them

%Here i make a trajectory, and move along it with small steps. It was
%needed for Newton's method but also useful if it is needed to move along a
%real trajectory
                       %X coordinate    Y coordinate    Z coordinate    Quaternion
coordinates_current = [ A_forward(1,4); A_forward(2,4); A_forward(3,4); quat_curr ];
coordinates_destination = [ Dest(1,4); Dest(2,4); Dest(3,4); quat_dest ];

%Coordinate step
step_coord = 5; 

%Create table - trajectory
distance = sqrt( (coordinates_destination(1) - coordinates_current(1)).^2 + (coordinates_destination(2) - coordinates_current(2)).^2 +(coordinates_destination(3) - coordinates_current(3)).^2 );

%Find out the number of trajectory points
num_of_steps = floor(distance / step_coord);

%Initialize trajectory table
table_traj = zeros(7,(5*num_of_steps));

%Calculate steps size for each coordinate
step_x = (coordinates_destination(1) - coordinates_current(1)) / num_of_steps;
step_y = (coordinates_destination(2) - coordinates_current(2)) / num_of_steps;
step_z = (coordinates_destination(3) - coordinates_current(3)) / num_of_steps;
step_qw = (coordinates_destination(4) - coordinates_current(4)) / num_of_steps;
step_qx = (coordinates_destination(5) - coordinates_current(5)) / num_of_steps;
step_qy = (coordinates_destination(6) - coordinates_current(6)) / num_of_steps;
step_qz = (coordinates_destination(7) - coordinates_current(7)) / num_of_steps;

new_coord = coordinates_current;
%Fill trajectory table
for ind = 1:num_of_steps
    new_coord = new_coord + [step_x; step_y; step_z; step_qw; step_qx; step_qy; step_qz];
    table_traj(:,ind) = new_coord;  
end;

%Set lambda size. I found out that algorithm works better when lambda is
%small
lambda = 0.1;

%In next steps i inialize Jacobian, build new destination matrix, calculate
%orientation error at the first step. As orientation error i use max
%element of quaternions difference.
for ind = 1:num_of_steps
    J = zeros(7, 6);

    %quat_to_matrix() - analog of quat2rotm() function
    rot_matr = quat_to_matrix(table_traj(4:7, ind));

    Dest = [ rot_matr, table_traj(1:3, ind);
             0, 0, 0, 1  ]; 

    %mat_to_coord_quat() function takes matrix of homogeneous
    %transformations and returns 7x1 vector of coorditaes
    %X Y Z and quaternion
    differ = mat_to_coord_quat(Dest) - mat_to_coord_quat(A_forward);
    error = max(abs(differ(4:6)));

    %Here is the algorithm. It works until we have coordinates and
    %orientation error less that was set
     while (abs(differ(1)) > 0.05) || (abs(differ(2)) > 0.05) || (abs(differ(3)) > 0.05) || error > 0.01

         %first - calculating Jacobian
         for ind2 = 1:6  %for every coordinate
                %Calculating of partial derivatives:
                q_prev_m1 = q_prev;
                q_prev_m1(ind2) = q_prev_m1(ind2) - step;
                q_prev_p1 = q_prev;
                q_prev_p1(ind2) = q_prev_p1(ind2) + step;
                Fn1 = mat_to_coord_quat(get_coordinates(q_prev_m1)); % in q_prev vector ind1 element is one step smaller than in original q_prev
                Fn2 = mat_to_coord_quat(get_coordinates(q_prev_p1)); % in q_prev vector ind1 element is one step bigger than in original q_prev
                deltaF = Fn2 - Fn1; %delta functions vector
                deltaF = deltaF/(2*step); %devide by step to get partial derivatives for every function
                %composing Jacobian from column of partian derivatives
                J(:,ind2) = deltaF;
        end;
        %Next according to damped least squares method
        %calculate velosities along all coordinates
            A_forward = get_coordinates( q_prev );
            differ = mat_to_coord_quat(Dest) - mat_to_coord_quat(A_forward);

            %calculating generalized coordinates velosities
            dq = (J.'*J + lambda * eye(6))\ J.' * differ;

            %integrate generalized coordinates velosities
            q_prev = q_prev + dq;

            %calculate max orientation error
            error = max(abs(differ(4:7)));   
    end;
end;

Answer 1

How can i accelerate calculations, or why is it so slow?

I think the reason it is really slow is that your calculated joint velocities do not steer the system towards a solution. Consider the time derivative of the forward kinematics equation

$$ \begin{bmatrix} v\\ \omega \end{bmatrix} = \begin{bmatrix} J_v\\ J_\omega \end{bmatrix} \dot{q}, $$ where $v, \omega$ are the end-effector linear and angular velocities, respectively, and $J_v, J_\omega$ map the joint velocity to $v, \omega$ respectively. Difference in X, Y, Z coordinates is actually $v$ in the above equation, that is why you can get $\dot{q}$ (or rather $dq$) that drives the system towards the correct position.

Now the problem is that the difference in Euler angles do not give you angular velocity suitable to use in the above equation. Changing the orientation representation to axis-angle representation should solve the problem.

Can i use quternions instead of Euler angles? Quternions will increase dimension of Jacobian so it will not be a square matrix anymore and it will not be possible to use Tikhonov regularization that works so good.

Yes, you can. But the lower part of the Jacobian, i.e., $J_\omega$, needs to be changed to something like $J_{quat}$ which is a $4 \times n$ matrix ($n$ is the dimension of the joint vector). So the velocity equation above will be

$$ \begin{bmatrix} v \\ u \end{bmatrix} = \begin{bmatrix} J_v\\ J_{quat} \end{bmatrix} \dot{q}, $$ where $u$ can be thought of as a difference in quaternion. (Actually OpenRAVE does have a function for calculating Jacobians suitable to use with quaternions.) It's true that the overall Jacobian will then not a square matrix but as far as I know, that should not prevent you from using Tikhonov regularization.

How often people use numerical methods to solve such things? I saw may examples of using analitycal solutions but not numerical.

I'm not sure how most people do but solving for IK solutions numerically is useful when one needs their robot to track some end-effector trajectory. Although one may be able to solve for an IK solution analytically, there can be multiple solutions for a single end-effector pose and it is not obvious how one can choose a solution for each time step such that the resulting joint trajectory is smooth. Instead, the IK solution for the consecutive time step along the trajectory is computed numerically by using a differential IK method, which is actually based on the Jacobian equation above.

How to get sure that programm will find solution using Newton method and programm will find it in finite number of iterations?

Convergence of Newton method depends on a number of factors. I'm not sure if we can have such universal criteria.

Answer 2

If you have 6 DOF in your joints, and you want a particular 6 DOF pose, and you have some kind of preferred movement envelope, then there is at most one solution. There is also a closed-form solution to this that you can calculate. Typically, you'll want to design your armature to make this solution easy.

If you have a known desired XYW position and UVW orientation of the end effector, and each axis of rotation is individually controlled, then you can start by posing the endmost three axes (presumably with short translations) to get you to an "ideal plane" for positioning, and then post the other three axes (presumably with longer translations) to get to the point of wherever the third-outmost actuator ended up when getting orientation right.

Another option, which is useful when you have a "desired shape" and "desired end goal" incrementally from the current position, is to simply minimize the amount of joint movement (or some other penalty function, such as desired shape) to move between the current state and the desired end state. As long as the end state varies smoothly, the incremental movement between each solution step is small, and the solution converges quickly.

That being said, 1 minutes to solve a 6DOF system sounds wrong. Either you're running into numerical problems (denormalized numbers?) or you're implementing the solution wrong, or you're running on some piece of hardware that is not suitable for the task (like, an 8-bit microcontroller without floating point support.)