├── A_matrix.m ├── A_tilde_matrix.m ├── Adjoint.m ├── B_matrix.m ├── FK.m ├── FK_new.m ├── Q_matrix.m ├── README.md ├── R_matrix.m ├── Readme.txt ├── T_matrix.m ├── cali_1995.m ├── cali_2009.m ├── cali_2016.m ├── comparision.m ├── draw_manipulator.m ├── hat.m ├── int_Adjoint.m ├── log_my.m └── orthogonal_w_generator.m /A_matrix.m: -------------------------------------------------------------------------------- 1 | function A = A_matrix( twist_matrix,theta ) 2 | % A matrix calculation 3 | A = zeros(6,42); 4 | [~,~,T_link_abs] = FK_new(twist_matrix,theta); 5 | A(:,1:6) = theta(1)*int_Adjoint(twist_matrix(:,1)*theta(1)); % integrate of adjoint, A1 6 | for i = 1:6 7 | A(:,1+i*6:6+i*6) = theta(i+1)*Adjoint(T_link_abs(:,:,i)) * int_Adjoint(twist_matrix(:,i+1)*theta(i+1)); % A2~A6 and Am 8 | end 9 | end 10 | 11 | -------------------------------------------------------------------------------- /A_tilde_matrix.m: -------------------------------------------------------------------------------- 1 | function A = A_tilde_matrix(twist,eta_matrix,P,theta) 2 | 3 | [T,~,T_link_abs] = FK(twist,theta); % calculate FK to get T_abs and T 4 | % get Q1 5 | Q_i = theta(1)*int_Adjoint(twist(:,1)*theta(1)); % Q1 6 | temp = Adjoint(T_matrix(eta_matrix(:,1)))*twist(:,1); % Adexp(eta_1)*twist_1 7 | ad_Adg = [hat(temp(4:6)),hat(temp(1:3)); % ad(Adexp) 8 | zeros(3) ,hat(temp(4:6))]*int_Adjoint(eta_matrix(:,1)); 9 | Q_tilde_i = -[eye(3) -hat(P(1:3))]*Q_i*ad_Adg; % Q1_tilde_i 10 | 11 | A = zeros(3,39); 12 | A(:,1:6) = Q_tilde_i; 13 | % get the rest of Qi 14 | for i=2:6 15 | Q_i = theta(i)*Adjoint(T_link_abs(:,:,i-1)) * int_Adjoint(twist(:,i)*theta(i)); 16 | temp = Adjoint(T_matrix(eta_matrix(:,i)))*twist(:,i); 17 | ad_Adg = [hat(temp(4:6)),hat(temp(1:3)); 18 | zeros(3) ,hat(temp(4:6))]*int_Adjoint(eta_matrix(:,i)); 19 | Q_tilde_i = -[eye(3) -hat(P(1:3))]*Q_i*ad_Adg; 20 | A(:,6*i-5:6*i) = Q_tilde_i; 21 | end 22 | Rf = T(1:3,1:3); 23 | A(:,37:39) = Rf; -------------------------------------------------------------------------------- /Adjoint.m: -------------------------------------------------------------------------------- 1 | function adj = Adjoint(g) 2 | % get adjoint matrix 3 | adj = [g(1:3,1:3), hat(g(1:3,4))*g(1:3,1:3); 4 | zeros(3,3) , g(1:3,1:3)]; 5 | end -------------------------------------------------------------------------------- /B_matrix.m: -------------------------------------------------------------------------------- 1 | function B = B_matrix(twist_matrix,measuring_type) 2 | 3 | % measuring type: 1:pose 2:point 4 | 5 | if measuring_type==1 6 | B = zeros(42,30); 7 | elseif measuring_type==2 8 | B = zeros(42,27); 9 | else 10 | error('Measuring type assert failed. Please check parameter.'); 11 | end 12 | 13 | for i=1:6 14 | qn = cross(twist_matrix(4:6,i),twist_matrix(1:3,i)); 15 | [w1,w2] = orthogonal_w_generator(twist_matrix(4:6,i)); 16 | B(6*i-5:6*i,4*i-3:4*i) = [w1,w2,cross(qn,w1),cross(qn,w2);zeros(3,2),w1,w2]; 17 | end 18 | if measuring_type==1 19 | B(37:42,25:30) = eye(6); 20 | elseif measuring_type==2 21 | B(37:42,25:27) = [eye(3);zeros(3)]; 22 | end 23 | end -------------------------------------------------------------------------------- /FK.m: -------------------------------------------------------------------------------- 1 | function [T,T_link_incr,T_link_abs] = FK( twist_matrix,theta ) 2 | %FK_NEW Summary of this function goes here 3 | % Detailed explanation goes here 4 | % link transformation incremental, i.e., T1, T2, T3, .... 5 | T_link_incr = zeros(4,4,6); 6 | % link transformation absolute, i.e., T1, T1*T2, T1*T2*T3, ... 7 | T_link_abs = zeros(4,4,6); 8 | for i = 1:6 9 | T_link_incr(:,:,i) = T_matrix(twist_matrix(:,i)*theta(i)); 10 | T_link_abs(:,:,i) = eye(4); 11 | for j = 1:i 12 | T_link_abs(:,:,i) = T_link_abs(:,:,i) * T_link_incr(:,:,j); 13 | end 14 | end 15 | T = T_link_abs(:,:,6); 16 | end -------------------------------------------------------------------------------- /FK_new.m: -------------------------------------------------------------------------------- 1 | function [T,T_link_incr,T_link_abs] = FK_new( twist_matrix,theta ) 2 | 3 | % forward kinematic 4 | 5 | % link transformation incremental, i.e., T1, T2, T3, .... 6 | T_link_incr = zeros(4,4,7); 7 | % link transformation absolute, i.e., T1, T1*T2, T1*T2*T3, ... 8 | T_link_abs = zeros(4,4,7); 9 | for i = 1:7 10 | T_link_incr(:,:,i) = T_matrix(twist_matrix(:,i)*theta(i)); % get transformation matrix 11 | T_link_abs(:,:,i) = eye(4); 12 | for j = 1:i 13 | T_link_abs(:,:,i) = T_link_abs(:,:,i) * T_link_incr(:,:,j); 14 | end 15 | end 16 | T = T_link_abs(:,:,7); 17 | end -------------------------------------------------------------------------------- /Q_matrix.m: -------------------------------------------------------------------------------- 1 | function Q = Q_matrix(twist_matrix,theta) 2 | 3 | Q = zeros(6,42); 4 | Adg = zeros(6,6,7); 5 | Adg(:,:,1) = eye(6); 6 | 7 | [~,~,T_abs] = FK( twist_matrix,theta ); 8 | for i=2:7 9 | Adg(:,:,i) = Adjoint(T_abs(:,:,i-1)); 10 | end 11 | 12 | for i=1:6 13 | Q(:,6*i-5:6*i) = Adg(:,:,i) - Adg(:,:,i+1); 14 | end 15 | Q(:,37:42) = Adg(:,:,7); 16 | end 17 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Robot_calibration_algorithm 2 | This program pack is a implementation of three papers, 3 | 1. cali_1995.m : Kinematic calibration using the product of exponentials formula [1996,F.C.Park] 4 | 2. cali_2009.m : Improved and Modified Geometric Formulation of POE Based Kinematic Calibration of Serial Robots [2009,YunJiang Lou,Yuanqing Wu] 5 | 3. cali_2016.m : POE-Based Robot Kinematic Calibration Using Axis Configuration Space and the Adjoint Error Model [2016,Yuanqing Wu,Cheng Li] 6 | 7 | -------------------------------------------------------------------------------- /R_matrix.m: -------------------------------------------------------------------------------- 1 | function R = R_matrix(omega) 2 | % calculate rotation matrix 3 | omega = omega(:); 4 | if norm(omega) < eps 5 | R = eye(3); 6 | else 7 | theta = norm(omega); 8 | omega = omega/theta; 9 | R = eye(3)*cos(theta) + hat(omega)*sin(theta) + omega*omega'*(1-cos(theta)); 10 | end -------------------------------------------------------------------------------- /Readme.txt: -------------------------------------------------------------------------------- 1 | This program pack is a implementation of three papers, 2 | 1. cali_1995.m : Kinematic calibration using the product of exponentials formula [1996,F.C.Park] 3 | 2. cali_2009.m : Improved and Modified Geometric Formulation of POE Based Kinematic Calibration of Serial Robots [2009,YunJiang Lou,Yuanqing Wu] 4 | 3. cali_2016.m : POE-Based Robot Kinematic Calibration Using Axis Configuration Space and the Adjoint Error Model [2016,Yuanqing Wu,Cheng Li] 5 | -------------------------------------------------------------------------------- /T_matrix.m: -------------------------------------------------------------------------------- 1 | function T = T_matrix(twist) 2 | % mapping from se(3) to SE(3) 3 | % twist is a vector representing a motion parameter 4 | % value is the radian that rotate or length that moves 5 | % return exponential of a twist 6 | 7 | omega = twist(4:6); 8 | omega = omega(:); 9 | v = twist(1:3); 10 | v = v(:); 11 | 12 | if norm(omega) < eps 13 | T = [eye(3),v;0 0 0 1]; 14 | else 15 | theta = norm(omega); 16 | omega = omega/theta; 17 | v = v/theta; 18 | R = eye(3)*cos(theta) + hat(omega)*sin(theta) + omega*omega'*(1-cos(theta)); 19 | p = (eye(3) - R)*hat(omega)*v + omega*omega'*v*theta; 20 | T = [R p; 0 0 0 1]; 21 | end 22 | end -------------------------------------------------------------------------------- /cali_1995.m: -------------------------------------------------------------------------------- 1 | function norm_dp = cali_1995() 2 | %% preprocess the thread 3 | % close all 4 | % clear; 5 | % clc 6 | %% parameters definition 7 | 8 | %define number of points,the more the better for calculation,but for sampling,the more,the worse. 9 | num_of_pts = 10; 10 | error_factor = 10; 11 | 12 | length_of_links = [0 1000 1000 400]; % link length 13 | 14 | q_vec_0 = [ 0 0 length_of_links(1); % q vector with no offset 15 | 0 0 length_of_links(1); 16 | 0 0 length_of_links(1)+length_of_links(2); 17 | 0 length_of_links(3) length_of_links(1)+length_of_links(2); 18 | 0 length_of_links(3) length_of_links(1)+length_of_links(2); 19 | 0 length_of_links(3) length_of_links(1)+length_of_links(2)]'; 20 | 21 | w_vec_0 = [ 0 0 1; % rotational axis matrix 22 | 1 0 0; 23 | 1 0 0; 24 | 0 1 0; 25 | 1 0 0; 26 | 0 1 0]'; 27 | 28 | g_st0 = [eye(3) [0 length_of_links(3)+length_of_links(4) length_of_links(1)+length_of_links(2)]';% g_st0 29 | 0 0 0 1]; 30 | 31 | % norminal twist_matrix_0; size = 6 x 7 32 | twist_matrix_0 = [[cross(q_vec_0,w_vec_0);w_vec_0],log_my(g_st0)]; % nominal twist 33 | twist_matrix = twist_matrix_0 + rand(6,7)/error_factor; % actual twist 34 | twist_matrix(:,1:6) = twist_matrix(:,1:6) ./ vecnorm(twist_matrix(4:6,1:6)); % normalize omega 35 | 36 | for i=1:6 37 | twist_matrix(1:3,i) = twist_matrix(1:3,i) - twist_matrix(1:3,i)'*twist_matrix(4:6,i)*twist_matrix(4:6,i); % make v perpendicular to w 38 | end 39 | 40 | gst_a = FK_new(twist_matrix,[0 0 0 0 0 0 1]); 41 | 42 | % the last joint angle shall be set to 1 in order to formalize a 43 | % exponential form 44 | theta_random_vec = zeros(num_of_pts,7); 45 | 46 | % define the maximum and minimum of joint angle. 47 | max_angle_vec = deg2rad(ones(1,6)*130); 48 | min_angle_vec = deg2rad(-ones(1,6)*130); 49 | % define the random angle value for each random point 50 | theta_random_vec(:,1:6) = (max_angle_vec - min_angle_vec) .* rand(num_of_pts,6) + min_angle_vec; 51 | theta_random_vec(:,7) = ones(num_of_pts,1); 52 | 53 | % variables declaration 54 | df_f_inv = zeros(num_of_pts*6,1); 55 | dp = zeros(num_of_pts*6,1); 56 | A = zeros(num_of_pts*6,42); 57 | j=0; 58 | 59 | norm_dp=[]; % for norm of dp visialization 60 | %% identification 61 | while j<1000 62 | for i=1:num_of_pts % repeat num_of_pts times 63 | [T_n,~,~] = FK_new(twist_matrix_0,theta_random_vec(i,:)); % Tn calculation 64 | [T_a,~,~] = FK_new(twist_matrix,theta_random_vec(i,:)); % Ta calculation, to simulate the measured data 65 | A(1+i*6-6:i*6,:) = A_matrix(twist_matrix_0,theta_random_vec(i,:)); % A matrix calculation 66 | df_f_inv(1+i*6-6:i*6) = log_my(T_a/T_n); % solve for log(df_f_inv) 67 | end 68 | dp = A\df_f_inv; % solve for dp(derive of twist) 69 | % composition of twist 70 | for i=1:6 71 | twist_matrix_0(:,i) = twist_matrix_0(:,i) + dp(1+i*6-6:i*6,1); % composition 72 | twist_matrix_0(:,i) = twist_matrix_0(:,i)/norm(twist_matrix_0(4:6,i)); % normalization 73 | twist_matrix_0(1:3,i) = twist_matrix_0(1:3,i) - twist_matrix_0(1:3,i)'*twist_matrix_0(4:6,i)*twist_matrix_0(4:6,i); % make v perpendicular to w 74 | end 75 | twist_matrix_0(:,7) = twist_matrix_0(:,7)+dp(37:42,1); % alternate the last rows 76 | 77 | j=j+1; % counter plus 1 78 | norm_dp = [norm_dp norm(dp)]; % minimization target value calculation 79 | disp (norm(dp)) % show value of norm of dp 80 | if norm(dp) < 10e-6 % check if it has convergenced 81 | converge_flag = 1; 82 | break; 83 | elseif (norm(dp)>1e4)||j>100 84 | converge_flag = 0; 85 | break; 86 | end 87 | %% plot 88 | clf; % clear plot 89 | draw_manipulator(twist_matrix,gst_a,'r',1); % draw robot frame with actual twist 90 | draw_manipulator(twist_matrix_0,T_matrix(twist_matrix_0(:,end)),'b',1); % draw robot frame with norminal twist 91 | drawnow; 92 | end 93 | %% plot again 94 | fig2 = figure(2); % open another window 95 | bar(norm_dp) % plot discrete figure 96 | %% validation 97 | % validate the result using 100 configurations 98 | number_of_validation_samples = 100; 99 | random_joint_angles = (max_angle_vec - min_angle_vec) .* rand(number_of_validation_samples,6) + min_angle_vec; 100 | orientation_error = zeros(1,100); 101 | position_error = zeros(1,100); 102 | for i = 1:number_of_validation_samples 103 | [T_n,~,~] = FK(twist_matrix_0,random_joint_angles(i,:)); % Tn calculation 104 | [T_a,~,~] = FK(twist_matrix,random_joint_angles(i,:)); % Ta calculation 105 | 106 | T_n = T_n*T_matrix(twist_matrix_0(:,end)); 107 | T_a = T_a*gst_a; 108 | 109 | deviation = log_my(T_a/T_n); % calculate deviation between nominal and actual posture 110 | orientation_error(i) = norm(deviation); 111 | position_error(i) = norm(T_a(1:3,4) - T_n(1:3,4)); 112 | end 113 | mean_orientation_error = mean(abs(orientation_error)) 114 | max_orientation_error = max(abs(orientation_error)) 115 | mean_position_error = mean(position_error) 116 | max_position_error = max(position_error) 117 | 118 | end 119 | -------------------------------------------------------------------------------- /cali_2009.m: -------------------------------------------------------------------------------- 1 | function norm_d_eta = cali_2009() 2 | %% close all unecessary windows and console and clear variables 3 | % close all 4 | % clear; 5 | % clc 6 | %% parameters definition 7 | 8 | measurment_type = 2; % 1:pose 2:points 9 | joint_error_factor = 50; 10 | EE_Error_gain = 1; 11 | 12 | % length of links 13 | length_of_links = [0 1000 2000 400]; 14 | 15 | % q(point on the rotation axis) vector without offset 16 | q_vec_0 = [ 0 0 length_of_links(1); 17 | 0 0 length_of_links(1); 18 | 0 0 length_of_links(1)+length_of_links(2); 19 | 0 length_of_links(3) length_of_links(1)+length_of_links(2); 20 | 0 length_of_links(3) length_of_links(1)+length_of_links(2); 21 | 0 length_of_links(3) length_of_links(1)+length_of_links(2)]'; 22 | 23 | % omega matrix(axis of rotation) 24 | w_vec_0 = [ 0 0 1; 25 | 1 0 0; 26 | 1 0 0; 27 | 0 1 0; 28 | 1 0 0; 29 | 0 1 0]'; 30 | 31 | %% norminal twist_matrix_a; size = 6 x 7 32 | twist_matrix_n = [cross(q_vec_0,w_vec_0);w_vec_0]; % nominal twist 33 | twist_matrix_copy = twist_matrix_n; % copy of original nominal twist 34 | twist_matrix_a = twist_matrix_n + rand(6,6)/joint_error_factor; % actual twist 35 | P_c0_n = [0 2400 1000 1]'; % nominal position of end tip 36 | P_c0_a = [P_c0_n(1:3)+rand(3,1)*EE_Error_gain;1]; 37 | gst_a = P_c0_a; 38 | 39 | %% calculate normalized omega 40 | vec_norm = vecnorm(twist_matrix_a(4:6,1:6)); % normalize rotational axis vectors 41 | for i = 1:6 42 | twist_matrix_a(:,i) = twist_matrix_a(:,i)./vec_norm(i); 43 | end 44 | 45 | %% calculate v again(by def,v is always perpendicular to omega) 46 | for i=1:6 47 | twist_matrix_a(1:3,i) = twist_matrix_a(1:3,i) - twist_matrix_a(1:3,i)'*twist_matrix_a(4:6,i)*twist_matrix_a(4:6,i); % make v perpendicular to w 48 | end 49 | 50 | %% define number of points,the more the better for calculation,but for sampling,the more the worse. 51 | num_of_pts = 50; 52 | 53 | %% the last joint angle shall be set to 0 54 | theta_random_vec = zeros(num_of_pts,6); 55 | 56 | %% define the maximum and minimum of joint angle. 57 | max_angle_vec = deg2rad(ones(1,6)*130); 58 | min_angle_vec = deg2rad(-ones(1,6)*130); 59 | 60 | %% define the random angle value for each random point 61 | theta_random_vec(:,1:6) = (max_angle_vec - min_angle_vec) .* rand(num_of_pts,6) + min_angle_vec; 62 | 63 | %% variables declaration 64 | j = 0; % number of iteration 65 | eta_matrix = zeros(6,6); 66 | d_eta = zeros(39,1); 67 | dPc = zeros(3*num_of_pts,1); 68 | A_tilde = zeros(3*num_of_pts,39); 69 | norm_d_eta = []; 70 | %% parameters identification and composition 71 | while j<1000 72 | %% calculate A matrix and df*f^-1 (parameters identification) 73 | for i=1:num_of_pts % repeat num_of_pts times 74 | [Tn,~,~] = FK(twist_matrix_n,theta_random_vec(i,:)); % nominal transformation matrix 75 | [Ta,~,~] = FK(twist_matrix_a,theta_random_vec(i,:)); % actual transfomation matrix 76 | P_n = Tn * P_c0_n; % nominal position of end point 77 | P_a = Ta * P_c0_a; % actual position of end point, simulation of measuring result 78 | dpc = P_a - P_n; % deviation of position 79 | dPc(i*3-2:i*3) = dpc(1:3); 80 | A_tilde(i*3-2:i*3,:) = A_tilde_matrix(twist_matrix_n,eta_matrix,P_n,theta_random_vec(i,:)); 81 | end 82 | d_eta = A_tilde\dPc; 83 | 84 | %% composition 85 | for i=1:6 86 | eta_matrix(:,i) = eta_matrix(:,i)+d_eta(i*6-5:i*6); % composition of eta matrix 87 | twist_matrix_n(:,i) = Adjoint(T_matrix(eta_matrix(:,i)))*twist_matrix_copy(:,i); % composition of twists 88 | end 89 | P_c0_n = [P_c0_n(1:3) + d_eta(37:39);1]; % composition of end tip position 90 | gst_n = P_c0_n; % for visualization 91 | 92 | %% data prepration of visialization 93 | j=j+1; % counter plus 1 94 | %norm_dp = [norm_dp norm(dp)]; % minimization target value calculation 95 | norm_d_eta = [norm_d_eta norm(d_eta)]; 96 | disp (norm(d_eta)) % show value of norm of dp 97 | disp (j) % show number of iteration 98 | if norm(d_eta) < 1e-4 % quit the for loop if deviation is less than 1e-5 99 | break; 100 | end 101 | %% plot 102 | clf; % clear plot 103 | draw_manipulator(twist_matrix_a,gst_a,'r',measurment_type); % draw robot frame with actual twist 104 | draw_manipulator(twist_matrix_a,gst_n,'b',measurment_type); % draw nominal axis 105 | drawnow; 106 | end 107 | %% plot again 108 | fig2 = figure(2); % create another window 109 | stem(norm_d_eta) % plot discrete data 110 | 111 | %% validation 112 | % validate the result using 100 configurations 113 | number_of_validation_samples = 100; 114 | random_joint_angles = (max_angle_vec - min_angle_vec) .* rand(number_of_validation_samples,6) + min_angle_vec; 115 | 116 | position_error = zeros(1,100); 117 | for i = 1:number_of_validation_samples 118 | [T_n,~,~] = FK(twist_matrix_n,random_joint_angles(i,:)); % Tn calculation 119 | [T_a,~,~] = FK(twist_matrix_a,random_joint_angles(i,:)); % Ta calculation 120 | 121 | Pc_a = T_a*gst_a; 122 | Pc_n = T_n*gst_n; 123 | 124 | deviation = Pc_a - Pc_n; 125 | position_error(i) = norm(deviation(1:3)); 126 | end 127 | mean_position_error = mean(position_error) 128 | max_position_error = max(position_error) 129 | 130 | end 131 | 132 | -------------------------------------------------------------------------------- /cali_2016.m: -------------------------------------------------------------------------------- 1 | function norm_k = cali_2016() 2 | %% statements 3 | % This program is a 6 axis robot calibration program base on paper 4 | % 'POE-Based Robot Kinematic Calibration Using Axis Configuration Space and the Adjoint Error Model' 5 | % Program simulate a process of robot calibration, we can choose wether 6 | % using points or pose to calibrate a robot, this algorithm is of high 7 | % efficiency,high accuracy,and can eliminate geometric error of a robot. 8 | 9 | % Alternatable variables: 10 | % 1.Change measurment type in line 14 11 | % 2.Change Joint Error factor in line 15 12 | % 3.Change End Effector error gain in line 16 13 | 14 | %% preprocess the thread 15 | % close all 16 | % clear; 17 | % clc 18 | %% parameters definition 19 | 20 | measurment_type = 1; % 1:pose 2:points 21 | joint_error_factor = 10; 22 | EE_Error_gain = 10; 23 | 24 | length_of_links = [0 1000 1000 400]; % link length 25 | 26 | q_vec_0 = [ 0 0 length_of_links(1); % q vector with no offset 27 | 0 0 length_of_links(1); 28 | 0 0 length_of_links(1)+length_of_links(2); 29 | 0 length_of_links(3) length_of_links(1)+length_of_links(2); 30 | 0 length_of_links(3) length_of_links(1)+length_of_links(2); 31 | 0 length_of_links(3) length_of_links(1)+length_of_links(2)]'; 32 | 33 | w_vec_0 = [ 0 0 1; % omega matrix 34 | 1 0 0; 35 | 1 0 0; 36 | 0 1 0; 37 | 1 0 0; 38 | 0 1 0]'; 39 | 40 | g_st0 = [eye(3) [0 length_of_links(3)+length_of_links(4) length_of_links(1)+length_of_links(2)]';% g_st0 41 | 0 0 0 1]; 42 | 43 | % norminal twist_matrix_0; size = 6 x 6 44 | twist_matrix_0 = [cross(q_vec_0,w_vec_0);w_vec_0]; % nominal twist 45 | twist_matrix = twist_matrix_0 + rand(6,6)/joint_error_factor; % actual twist 46 | if(measurment_type==1) % different gst0 based on measuring types 47 | g_st0_a = T_matrix(log_my(g_st0)+rand(6,1)/joint_error_factor); 48 | gst_a = g_st0_a; 49 | elseif measurment_type==2 50 | Pc_0_a = [g_st0(1:3,4)+rand(3,1)*EE_Error_gain ;1]; 51 | Pc_0_n = [g_st0(1:3,4);1]; 52 | gst_a = Pc_0_a; 53 | end 54 | twist_matrix(:,1:6) = twist_matrix(:,1:6) ./ vecnorm(twist_matrix(4:6,1:6)); % normalize omega 55 | 56 | for i=1:6 57 | twist_matrix(1:3,i) = twist_matrix(1:3,i) - twist_matrix(1:3,i)'*twist_matrix(4:6,i)*twist_matrix(4:6,i); % make v perpendicular to w 58 | end 59 | 60 | %define number of points,the more the better for calculation,but for sampling,the more,the worse. 61 | num_of_pts = 10; 62 | 63 | % define the maximum and minimum of joint angle. 64 | max_angle_vec = deg2rad(ones(1,6)*130); 65 | min_angle_vec = deg2rad(-ones(1,6)*130); 66 | % define the random angle value for each random point 67 | theta_random_vec(:,1:6) = (max_angle_vec - min_angle_vec) .* rand(num_of_pts,6) + min_angle_vec; 68 | 69 | % variables declaration 70 | if measurment_type == 1 71 | df_f_inv = zeros(num_of_pts*6,1); 72 | A = zeros(num_of_pts*6,42); 73 | elseif measurment_type == 2 74 | df_f_inv = zeros(num_of_pts*3,1); 75 | A = zeros(num_of_pts*3,42); 76 | end 77 | 78 | B = zeros(42,4); 79 | j=0; % times of iteration 80 | Q = zeros(6,42); 81 | d_Pc = zeros(num_of_pts*3,1); 82 | norm_k=[]; % for norm of dp visialization 83 | 84 | %% identification 85 | while j<1000 86 | for i=1:num_of_pts % repeat num_of_pts times 87 | % FK 88 | [T_n,~,~] = FK(twist_matrix_0,theta_random_vec(i,:)); % Tn calculation 89 | [T_a,~,~] = FK(twist_matrix,theta_random_vec(i,:)); % Ta calculation 90 | 91 | Q = Q_matrix(twist_matrix_0,theta_random_vec(i,:)); % Q matrix calculation 92 | % choose different algorithm base on measurment type 93 | if(measurment_type==1) 94 | T_n = T_n*g_st0; % calculate pose of end effector 95 | T_a = T_a*g_st0_a; 96 | df_f_inv(1+i*6-6:i*6) = log_my(T_a/T_n); % solve for log(df_f_inv) 97 | A(1+i*6-6:i*6,:) = Q; % calculate [Q1|Q2|Q3|...Qn|Qst] 98 | elseif(measurment_type==2) 99 | Pc_a = T_a*Pc_0_a; % position of actual end tip 100 | Pc_n = T_n*Pc_0_n; % position of nominal end tip 101 | delta_pc = Pc_a - Pc_n; % deviation 102 | df_f_inv(i*3-2:i*3) = delta_pc(1:3); 103 | A(3*i-2:3*i,:) = [eye(3) -hat(Pc_n(1:3))]*Q; 104 | end 105 | 106 | end 107 | B = B_matrix(twist_matrix_0,measurment_type); % amazing matrix 108 | k = (A*B)\df_f_inv; 109 | norm(k) 110 | 111 | % composition of twist 112 | for i=1:6 113 | twist_matrix_0(:,i) = Adjoint(T_matrix(B(i*6-5:i*6,i*4-3:i*4)*k(i*4-3:i*4)))*twist_matrix_0(:,i); 114 | end 115 | if(measurment_type==1) 116 | g_st0 = T_matrix(k(25:30))*g_st0; 117 | twist_matrix_0(:,7) = log_my(g_st0); 118 | gst_n = g_st0; 119 | elseif(measurment_type==2) 120 | Pc_0_n = [k(25:27)+Pc_0_n(1:3);1]; 121 | gst_n = Pc_0_n; 122 | end 123 | 124 | j=j+1; % counter plus 1 125 | norm_k = [norm_k norm(k)]; % minimization target value calculation 126 | disp (norm(k)) % show value of norm of dp 127 | %disp (j) % show number of iteration 128 | if norm(k) < 10e-6 % judge if it's able to quit the iteration 129 | break; 130 | elseif (norm(k)>1e4)||j>100 131 | break; 132 | end 133 | %% plot 134 | clf; 135 | draw_manipulator(twist_matrix,gst_a,'r',measurment_type); % draw robot frame with actual twist 136 | draw_manipulator(twist_matrix_0,gst_n,'b',measurment_type); % draw robot frame with norminal twist 137 | drawnow; 138 | end 139 | %% plot again 140 | % fig2 = figure(2); % open another window 141 | % bar(norm_k) % plot discrete figure 142 | 143 | %% validation 144 | % validate the result using 100 configurations 145 | number_of_validation_samples = 100; 146 | random_joint_angles = (max_angle_vec - min_angle_vec) .* rand(number_of_validation_samples,6) + min_angle_vec; 147 | if measurment_type==1 148 | orientation_error = zeros(1,100); 149 | position_error = zeros(1,100); 150 | for i = 1:number_of_validation_samples 151 | [T_n,~,~] = FK(twist_matrix_0,random_joint_angles(i,:)); % Tn calculation 152 | [T_a,~,~] = FK(twist_matrix,random_joint_angles(i,:)); % Ta calculation 153 | 154 | T_n = T_n*g_st0; 155 | T_a = T_a*g_st0_a; 156 | 157 | deviation = log_my(T_a/T_n); 158 | orientation_error(i) = norm(deviation); 159 | position_error(i) = norm(T_a(1:3,4) - T_n(1:3,4)); 160 | end 161 | mean_orientation_error = mean(abs(orientation_error)) 162 | max_orientation_error = max(abs(orientation_error)) 163 | mean_position_error = mean(position_error) 164 | max_position_error = max(position_error) 165 | elseif measurment_type==2 166 | position_error = zeros(1,100); 167 | for i = 1:number_of_validation_samples 168 | [T_n,~,~] = FK(twist_matrix_0,random_joint_angles(i,:)); % Tn calculation 169 | [T_a,~,~] = FK(twist_matrix,random_joint_angles(i,:)); % Ta calculation 170 | 171 | Pc_a = T_a*Pc_0_a; 172 | Pc_n = T_n*Pc_0_n; 173 | 174 | deviation = Pc_a - Pc_n; 175 | position_error(i) = norm(deviation(1:3)); 176 | end 177 | mean_position_error = mean(position_error) 178 | max_position_error = max(position_error) 179 | end 180 | 181 | end 182 | 183 | 184 | 185 | -------------------------------------------------------------------------------- /comparision.m: -------------------------------------------------------------------------------- 1 | norm_1995 = cali_1995; 2 | norm_2009 = cali_2009; 3 | norm_2016 = cali_2016; 4 | 5 | l95 = length(norm_1995); 6 | l09 = length(norm_2009); 7 | l16 = length(norm_2016); 8 | 9 | num = [l95 l09 l16]; 10 | [val,index] = min(num); 11 | 12 | new_arr = [norm_1995(1:val);norm_2009(1:val);norm_2016(1:val)]'; 13 | 14 | %% plot 15 | close all 16 | figure('Name','algorithm comparison','position',[500 100 1280 800]) 17 | bar(new_arr) 18 | for i = 1:val 19 | text(i-0.325,new_arr(i,1)+max(max(new_arr))/40,num2str(new_arr(i,1),'%6.2f'),'fontsize',10); 20 | text(i-0.09,new_arr(i,2)+max(max(new_arr))/40,num2str(new_arr(i,2),'%6.2f'),'fontsize',10); 21 | text(i+0.1,new_arr(i,3)+max(max(new_arr))/40,num2str(new_arr(i,3),'%6.2f'),'fontsize',10); 22 | end 23 | legend('POE result','MPOE result','ACS result') 24 | title('Comparison of three algorithms') -------------------------------------------------------------------------------- /draw_manipulator.m: -------------------------------------------------------------------------------- 1 | function draw_manipulator( twist_matrix,gst0,color,measurment_type ) 2 | 3 | % measurment_type : 1:pose 2:point 4 | % if measurment type is points,then input parameter gst0 is a column vector 5 | % of points' coordinates in homogenius form 6 | 7 | hold on; 8 | view([45 35]); 9 | axis equal; 10 | q_vec = cross(twist_matrix(4:6,1:6),twist_matrix(1:3,1:6)); 11 | [T,~,~] = FK(twist_matrix,[zeros(1,6),1]); 12 | if(measurment_type==1) 13 | gst = T*gst0; 14 | elseif(measurment_type==2) 15 | size_of_points_set = size(gst0); 16 | num_of_pts = size_of_points_set(2); 17 | gst = zeros(4,num_of_pts); 18 | for i=1:num_of_pts 19 | gst(:,i) = T*gst0(:,i); 20 | end 21 | end 22 | 23 | for i = 1:6 24 | point_set = [q_vec(:,i) - 500*twist_matrix(4:6,i),q_vec(:,i) + 500*twist_matrix(4:6,i)]'; 25 | plot3(point_set(:,1),point_set(:,2),point_set(:,3),color); 26 | end 27 | 28 | if(measurment_type==1) 29 | color_arrow = ['r' 'g' 'b']; 30 | for i = 1:3 31 | point_set = [gst(1:3,4),gst(1:3,4) + 300*gst(1:3,i)]'; 32 | plot3(point_set(:,1),point_set(:,2),point_set(:,3),color_arrow(i)); 33 | end 34 | elseif (measurment_type==2) 35 | for i=1:num_of_pts 36 | plot3(gst(1,i),gst(2,i),gst(3,i),color,'marker','*'); 37 | end 38 | end 39 | hold off; 40 | end 41 | 42 | -------------------------------------------------------------------------------- /hat.m: -------------------------------------------------------------------------------- 1 | function hatted = hat(vec) 2 | % get a skew-symmetric matrix 3 | if(length(vec)==3) 4 | hatted = [0 -vec(3) vec(2);vec(3) 0 -vec(1);-vec(2) vec(1) 0]; 5 | elseif (length(vec)==6) 6 | hatted = [hat(vec(4:6)) vec(1:3);0 0 0 0]; 7 | end 8 | end -------------------------------------------------------------------------------- /int_Adjoint.m: -------------------------------------------------------------------------------- 1 | function intAd = int_Adjoint( twist ) 2 | % integration of adjoint matrix 3 | intAd = zeros(6,6); 4 | omega = twist(4:6); 5 | omega = omega(:); 6 | v = twist(1:3); 7 | v = v(:); 8 | 9 | if norm(omega) < eps 10 | intAd(1:3,1:3) = eye(3); 11 | intAd(4:6,4:6) = eye(3); 12 | intAd(1:3,4:6) = hat(v/2); 13 | else 14 | theta = norm(omega); 15 | v = v/theta; 16 | omega = omega/theta; 17 | %Q = eye(3)*sin(theta)/theta + hat(omega)*(1-cos(theta))/theta + omega*omega'*(1-sin(theta)/theta); % ground truth 18 | Q = eye(3) + 1/theta*hat(omega)*(1-cos(theta)) + hat(omega)^2*(1 - sin(theta)/theta); % my int 19 | R = (R_matrix(omega*theta)-Q)*(omega'*v) + hat(cross(omega,v))*Q - Q*hat(cross(omega,v)); 20 | intAd(1:3,1:3) = Q; 21 | intAd(4:6,4:6) = Q; 22 | intAd(1:3,4:6) = R; 23 | end 24 | 25 | end 26 | 27 | -------------------------------------------------------------------------------- /log_my.m: -------------------------------------------------------------------------------- 1 | function twist = log_my(T) 2 | % log function,calculate inverse function of exponential 3 | R = T(1:3,1:3); 4 | p = T(1:3,4); 5 | if abs(trace(R)-3) < eps 6 | twist = [p;zeros(3,1)]; 7 | else 8 | omega = [R(3,2)-R(2,3),R(1,3)-R(3,1),R(2,1)-R(1,2)]'; 9 | theta = atan2(norm(omega),trace(R)-1); 10 | omega = omega/norm(omega); 11 | pitch = omega'*p/theta; 12 | if norm(cross(omega,p)) < eps 13 | v = pitch * omega; 14 | else 15 | p = (p - omega'*p*omega)/(2*sin(theta/2)); 16 | v = p*cos(theta/2)-cross(omega,p)*sin(theta/2)+pitch*omega; 17 | end 18 | twist = [v;omega]*theta; 19 | end 20 | end 21 | 22 | -------------------------------------------------------------------------------- /orthogonal_w_generator.m: -------------------------------------------------------------------------------- 1 | function [w1,w2] = orthogonal_w_generator(w) 2 | 3 | % this function is to generate two orthogonal unit vector that perpendicular to 4 | % the input vector 5 | 6 | w1 = randn(3,1); 7 | w1 = w1/norm(w1); 8 | w = w/norm(w); 9 | w1 = w1 - w1'*w*w; 10 | 11 | w2 = cross(w1,w); 12 | --------------------------------------------------------------------------------