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:
- How can i accelerate calculations, or why is it so slow?
- 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.
- How often people use numerical methods to solve such things? I saw may examples of using analitycal solutions but not numerical.
- 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;