From 49baf628f14fdd5d4e2b09bd53c7f7d706d9d11c Mon Sep 17 00:00:00 2001 From: cosmic_power Date: Wed, 10 Jan 2024 23:06:32 +0800 Subject: [PATCH] add base params decompose --- Identification_main.m | 7 ++- autogen/standard_regressor_Two_bar.m | 26 ++++++++++ get_Kinematics.m | 21 ++++++++ get_baseParams.m | 78 ++++++++++++++++++++++++++++ get_regressor.m | 36 ++++++++----- get_robot.m | 2 +- get_velocity.m | 7 ++- verify_regressor.m | 58 +++++++++++++++++++++ 8 files changed, 219 insertions(+), 16 deletions(-) create mode 100644 autogen/standard_regressor_Two_bar.m create mode 100644 get_baseParams.m create mode 100644 verify_regressor.m diff --git a/Identification_main.m b/Identification_main.m index 5bd6817..d6a1369 100644 --- a/Identification_main.m +++ b/Identification_main.m @@ -4,7 +4,7 @@ opt.KM_method = 'MDH'; opt.Vel_method = 'Direct'; opt.LD_method = 'Direct'; opt.debug = true; - +opt.robotName = 'Two_bar'; opt.Isreal = false; robot = get_robot(file,opt); @@ -13,4 +13,7 @@ robot = get_Kinematics(robot, opt); opt.Isreal = false; robot = get_velocity(robot, opt); -robot = get_regressor(robot,opt); \ No newline at end of file +robot = get_regressor(robot,opt); +% symbol matched +verify_regressor +robot = get_baseParams(robot, opt); \ No newline at end of file diff --git a/autogen/standard_regressor_Two_bar.m b/autogen/standard_regressor_Two_bar.m new file mode 100644 index 0000000..a7e323a --- /dev/null +++ b/autogen/standard_regressor_Two_bar.m @@ -0,0 +1,26 @@ +function out1 = standard_regressor_Two_bar(in1,in2,in3) +%standard_regressor_Two_bar +% OUT1 = standard_regressor_Two_bar(IN1,IN2,IN3) + +% This function was generated by the Symbolic Math Toolbox version 9.1. +% 10-Jan-2024 23:01:38 + +q2 = in1(2,:); +qd1 = in2(1,:); +qd2 = in2(2,:); +qdd1 = in3(1,:); +qdd2 = in3(2,:); +t2 = cos(q2); +t3 = sin(q2); +t4 = qd1+qd2; +t5 = qdd1+qdd2; +t6 = qd1.^2; +t7 = qdd1.*t2; +t8 = qdd1.*t3; +t9 = t4.^2; +t10 = t2.*t6; +t11 = t3.*t6; +t12 = -t8; +t13 = t7+t11; +t14 = t10+t12; +out1 = reshape([0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,qdd1,0.0,t2.*t13+t3.*(t8-t10),0.0,t13+t2.*t5-t3.*t9,t13,t14-t3.*t5-t2.*t9,t14,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t5,t5],[2,20]); diff --git a/get_Kinematics.m b/get_Kinematics.m index 17abdbc..c9e377d 100644 --- a/get_Kinematics.m +++ b/get_Kinematics.m @@ -2,6 +2,27 @@ function robot = get_Kinematics(robot, opt) if(opt.Isreal) switch opt.KM_method case 'SDH' + theta = robot.theta; + alpha = robot.alpha; + a = robot.a; + d = robot.d; + robot.Fkine = eye(4,4); + ndof = length(theta); % special for MDH + % init transform matrix + robot.R = zeros([3,3,ndof]); + robot.t = zeros([3,1,ndof]); + robot.T = zeros([4,4,ndof]); + for i = 1:ndof + robot.R(:,:,i) = [cos(theta(i)) -sin(theta(i))*cos(alpha(i)) sin(theta(i))*sin(alpha(i));... + sin(theta(i)) cos(theta(i))*cos(alpha(i)) -cos(theta(i))*sin(alpha(i));... + 0 sin(alpha(i)) cos(alpha(i))]; + robot.t(:,:,i) = [a(i)*cos(theta(i));a(i)*sin(theta(i));d(i)]; + Transform = eye(4,4); + Transform(1:3,1:3) = robot.R(:,:,i); + Transform(1:3,4) = robot.t(:,:,i); + robot.T(:,:,i) = Transform; + robot.Fkine = robot.Fkine*robot.T(:,:,i); + end case 'MDH' theta = robot.theta; alpha = robot.alpha; diff --git a/get_baseParams.m b/get_baseParams.m new file mode 100644 index 0000000..8ad1749 --- /dev/null +++ b/get_baseParams.m @@ -0,0 +1,78 @@ +function robot = get_baseParams(robot,opt) +% Seed the random number generator based on the current time +rng('shuffle'); +ndof = robot.ndof; +includeMotorDynamics = false; +% ------------------------------------------------------------------------ +% Set limits on posistion and velocities +% ------------------------------------------------------------------------ +q_min = -pi*ones(ndof,1); +q_max = pi*ones(ndof,1); +qd_max = 3*pi*ones(ndof,1); +q2d_max = 6*pi*ones(ndof,1); +% ----------------------------------------------------------------------- +% Find relation between independent columns and dependent columns +% ----------------------------------------------------------------------- +% Get observation matrix of identifiable paramters +W = []; +for i = 1:25 + q_rnd = q_min + (q_max - q_min).*rand(ndof,1); + qd_rnd = -qd_max + 2*qd_max.*rand(ndof,1); + q2d_rnd = -q2d_max + 2*q2d_max.*rand(ndof,1); + + if includeMotorDynamics +% Y = regressorWithMotorDynamics(q_rnd,qd_rnd,q2d_rnd); + else + standard_regressor_func = sprintf('standard_regressor_%s',opt.robotName); + Y = feval(standard_regressor_func, q_rnd,qd_rnd,q2d_rnd); + end + W = vertcat(W,Y); +end + +% QR decomposition with pivoting: W*E = Q*R +% R is upper triangular matrix +% Q is unitary matrix +% E is permutation matrix +[Q, R, E] = qr(W); + +% matrix W has rank qr_rank which is number number of base parameters +qr_rank = rank(W); + +% R = [R1 R2; +% 0 0] +% R1 is bbxbb upper triangular and reguar matrix +% R2 is bbx(c-qr_rank) matrix where c is number of standard parameters +R1 = R(1:qr_rank,1:qr_rank); +R2 = R(1:qr_rank,qr_rank+1:end); +beta = R1\R2; % the zero rows of K correspond to independent columns of WP +beta(abs(beta)