add base params decompose

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);
@@ -14,3 +14,6 @@ robot = get_Kinematics(robot, opt);
 opt.Isreal = false;
 robot = get_velocity(robot, opt);
 robot = get_regressor(robot,opt);
+% symbol matched
+verify_regressor
+robot = get_baseParams(robot, opt);

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]);

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;

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)<sqrt(eps)) = 0; % get rid of numerical errors
+% W2 = W1*beta
+
+% Make sure that the relation holds
+W1 = W*E(:,1:qr_rank);
+W2 = W*E(:,qr_rank+1:end);
+assert(norm(W2 - W1*beta) < 1e-6,... 
+        'Found realationship between W1 and W2 is not correct\n');
+
+% -----------------------------------------------------------------------
+% Find base parmaters
+% -----------------------------------------------------------------------
+pi_lgr_sym = robot.regressor.pi;
+pi1 = E(:,1:qr_rank)'*pi_lgr_sym; % independent paramters
+pi2 = E(:,qr_rank+1:end)'*pi_lgr_sym; % dependent paramteres
+
+% all of the expressions below are equivalent
+pi_lgr_base = pi1 + beta*pi2;
+% pi_lgr_base = [eye(qr_rank) beta]*[pi1;pi2];
+% pi_lgr_base = [eye(qr_rank) beta]*E'*pi_lgr_sym;
+
+% ---------------------------------------------------------------------
+% Create structure with the result of QR decompositon a
+% ---------------------------------------------------------------------
+baseQR = struct;
+baseQR.numberOfBaseParameters = qr_rank;
+baseQR.permutationMatrix = E;
+baseQR.beta = beta;
+baseQR.motorDynamicsIncluded = includeMotorDynamics;
+baseQR.baseParams = pi_lgr_base;
+robot.baseQR = baseQR;

get_regressor.m

@@ -3,14 +3,26 @@ function robot = get_regressor(robot, opt)
 ndof = robot.ndof;
 q_sym = sym('q%d',[ndof+1,1],'real');
 qd_sym = sym('qd%d',[ndof+1,1],'real');
-q2d_sym = sym('q2d%d',[ndof+1,1],'real');
+q2d_sym = sym('qdd%d',[ndof+1,1],'real');
 % init regressor
-% robot.regressor.m = sym('m%d',[ndof,1],'real');
+robot.regressor.m = sym('m%d',[ndof,1],'real');
-% robot.regressor.com = sym('com%d',[ndof,1],'real');
+robot.regressor.mc_x = sym('mc%d_x',[ndof,1],'real');
-% robot.regressor.I = sym('I%d',[ndof,1],'real');
+robot.regressor.mc_y = sym('mc%d_y',[ndof,1],'real');
-% robot.regressor.I_vec = inertiaMatrix2Vector(robot.regressor.I);
+robot.regressor.mc_z = sym('mc%d_z',[ndof,1],'real');
-% robot.regressor.mc = robot.regressor.m.*robot.regressor.com;
+robot.regressor.ixx = sym('i%d_xx',[ndof,1],'real');
-% robot.regressor.pi = [robot.I_vec(:,i); robot.mc(:,i); robot.m(i)];
+robot.regressor.ixy = sym('i%d_xy',[ndof,1],'real');
+robot.regressor.ixz = sym('i%d_xz',[ndof,1],'real');
+robot.regressor.iyy = sym('i%d_yy',[ndof,1],'real');
+robot.regressor.iyz = sym('i%d_yz',[ndof,1],'real');
+robot.regressor.izz = sym('i%d_zz',[ndof,1],'real');
+robot.regressor.im = sym('im%d',[ndof,1],'real');
+for i = 1:ndof
+    robot.regressor.pi(:,i) = [robot.regressor.m(i),robot.regressor.mc_x(i),robot.regressor.mc_y(i),...
+        robot.regressor.mc_z(i),robot.regressor.ixx(i),robot.regressor.ixy(i),...
+        robot.regressor.ixz(i),robot.regressor.iyy(i),robot.regressor.iyz(i),robot.regressor.izz(i)]';
+end
+[nLnkPrms, nLnks] = size(robot.regressor.pi);
+robot.regressor.pi = reshape(robot.regressor.pi, [nLnkPrms*nLnks, 1]);
 % init matrix
 R = robot.R;
 P = robot.t;
@@ -21,7 +33,7 @@ switch opt.LD_method
     case 'Direct'
         switch opt.KM_method
             case 'MDH'
-                for i = 2:ndof+1
+                for i = 1:ndof
                     p_skew(:,:,i) = vec2skewSymMat(P(:,:,i));
                     w_skew(:,:,i) = vec2skewSymMat(w(:,i));
                     dw_skew(:,:,i) = vec2skewSymMat(dw(:,i));
@@ -29,7 +41,7 @@ switch opt.LD_method
                     w_l(:,:,i) = vec2linearSymMat(w(:,i));
                     dw_l(:,:,i) = vec2linearSymMat(dw(:,i));
                     % size of matrix A is 6*10, need to -1
-                    robot.regressor.A(:,:,i-1) = [dv(:,i),dw_skew(:,:,i)+w_skew(:,:,i)*w_skew(:,:,i),zeros(3,6); ...
+                    robot.regressor.A(:,:,i) = [dv(:,i),dw_skew(:,:,i)+w_skew(:,:,i)*w_skew(:,:,i),zeros(3,6); ...
                         zeros(3,1),-dv_skew(:,:,i),dw_l(:,:,i)+w_skew(:,:,i)*w_l(:,:,i)];
                 end
                 % construct matrix U, size: [6*n,10*n]
@@ -51,9 +63,7 @@ switch opt.LD_method
                 robot.regressor.U = U_;
                 delta_ = zeros([ndof,6*ndof]);
                 for i = 1:ndof
-                    for j = 1:ndof
+                    delta_(i,6*i) = 1;
-                        delta_(i,6*j) = 1;
-                    end
                 end
                 robot.regressor.K = delta_*robot.regressor.U;
                 if(opt.debug)
@@ -61,6 +71,8 @@ switch opt.LD_method
                     sprintf('size of K=%dx%d.',size(robot.regressor.K));
                 end
         end
+        matlabFunction(robot.regressor.K,'File',sprintf('autogen/standard_regressor_%s',opt.robotName),...
+            'Vars',{q_sym,qd_sym,q2d_sym});
 %     matlabFunction(Y_f,'File','autogen/standard_regressor_Two_bar',...
 %                    'Vars',{q_sym,qd_sym,q2d_sym});
     case 'Lagrange'

get_robot.m

@@ -20,7 +20,7 @@ switch opt.robot_def
             % Create symbolic generilized coordiates, their first and second deriatives
             q_sym = sym('q%d',[ndof+1,1],'real');
             qd_sym = sym('qd%d',[ndof+1,1],'real');
-            q2d_sym = sym('q2d%d',[ndof+1,1],'real');
+            q2d_sym = sym('qdd%d',[ndof+1,1],'real');
             q_sym(ndof+1) = 0;
             qd_sym(ndof+1) = 0;
             q2d_sym(ndof+1) = 0;

get_velocity.m

@@ -5,7 +5,9 @@ switch opt.KM_method
         switch opt.Vel_method
             case 'Direct'
                 Z = [0,0,1]';
-                w0 = zeros(3,1); dw0 = zeros(3,1);dv0 = robot.gravity;
+                w0 = zeros(3,1); dw0 = zeros(3,1);
+%                 dv0 = robot.gravity;
+                v0 = zeros(3,1);dv0 = zeros(3,1);
                 % w = zeros(3,number);
                 % dw = zeros(3,number);
                 % dv = zeros(3,number);
@@ -17,15 +19,18 @@ switch opt.KM_method
                 % 1-n外推公式
                 %第一关节
                 w(:,1) = R(:,:,1)' * w0 + dtheta(1) * Z;
+                v(:,1) = R(:,:,1)' * v0 + cross(w0,P(:,1));
                 dw(:,1) = R(:,:,1)' * dw0 + cross(R(:,:,1)' * w0, dtheta(1) * Z) + ddtheta(1) * Z;
                 dv(:,1) = R(:,:,1)' * (cross(dw0,P(:,1)) + cross(w0,cross(w0, P(:,1))) + dv0);
                 %后面n-1关节
                 for i = 1:robot.ndof
                     w(:,i+1) = R(:,:,i+1)' * w(:,i) + dtheta(i+1) * Z ;
+                    v(:,i+1) = R(:,:,i+1)' * v(:,i) + cross(w(:,i), P(:,i+1));
                     dw(:,i+1) = R(:,:,i+1)' * dw(:,i) + cross(R(:,:,i+1)' * w(:,i), dtheta(i+1) * Z)+ ddtheta(i+1) * Z;
                     dv(:,i+1) = R(:,:,i+1)' * (cross(dw(:,i), P(:,i+1)) + cross(w(:,i), cross(w(:,i), P(:,i+1))) + dv(:,i));
                 end
                 robot.vel.w = w;
+                robot.vel.v = v;
                 robot.vel.dw = dw;
                 robot.vel.dv = dv;
             otherwise

verify_regressor.m

@@ -0,0 +1,58 @@
+% function robot = verify_regressor(robot, opt)
+% verify: If full regressor dynamics is the same as basic dynamics
+ndof = robot.ndof;
+q_sym = sym('q%d',[ndof+1,1],'real');
+qd_sym = sym('qd%d',[ndof+1,1],'real');
+q2d_sym = sym('qdd%d',[ndof+1,1],'real');
+pi1=[2;1/2;0;0;1+1/4;0;0;1+1/4;0;1+1/4];
+pi2=[1;1/2;0;0;1+1/4;0;0;1+1/4;0;1+1/4];
+% pi2=zeros([10,1]);
+pi=[pi1;pi2];
+regressor = standard_regressor_Two_bar(q_sym,qd_sym,q2d_sym);
+tau=regressor*pi;
+%% Two-bar
+N=2;
+thetalist = q_sym(1:N);
+dthetalist = qd_sym(1:N);
+ddthetalist = q2d_sym(1:N);
+
+Gb= [diag([1,1,1]),zeros(3,3);
+    zeros(3,3),diag([1,1,1])];
+Glist = cat(3, Gb, Gb);
+% Glist = cat(3, Gb, zeros([6,6]));
+M01 = [[1, 0, 0, 1/2]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
+M12 = [[1, 0, 0, 1]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
+M23 = [[1, 0, 0, 1/2]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
+Mlist = cat(3, M01, M12, M23);
+Slist=[[0;0;1;0;0;0],...
+    [0;0;1;0;-1;0]];
+Adgab_mat = sym(zeros(6,6,N+1));
+Fmat=sym(zeros(N,6));
+F1=sym(zeros(N,6));
+V1=sym(zeros(6,N+1));
+G=sym(zeros(4,4,N));
+T=sym(zeros(4,4,N));
+Vlinear=sym(zeros(3,3));
+Vd1=sym(zeros(6,N+1));
+Gb= [diag([1,1,1]),zeros(3,3);
+    zeros(3,3),diag([1,1,1])];
+J=sym(zeros(6,N));
+exf=[0;0;0;0;0;0];
+
+[V1,Vd1,Adgab_mat,Fmat,tau_mat] ...
+    = InverseDynamics_sym(thetalist, dthetalist, ddthetalist, ...
+    [0;0;0], exf, Mlist, Glist, Slist);
+G = FKinSpaceExpand_Sym(Mlist, Slist, thetalist);
+M01 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
+M12 = [[1, 0, 0, 1]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
+M23 = [[1, 0, 0, 1]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
+Mlist = cat(3, M01, M12, M23);
+T=FKinSpaceExpand_Sym(Mlist, Slist, thetalist);
+F_Simpack = getSimpackF_Sym(G,T,Mlist,Fmat);
+% Use Body Twist cal linear vel, but can't cal the end frame vel
+[V2] = InverseDynamics_sym(thetalist, dthetalist, ddthetalist, ...
+    [0;0;0], exf, Mlist, Glist, Slist);
+j=1;
+Vlinear(:, j+1) = BodyVelToLinearVel(V2(:,j+1),G(:,:,j)*M12);
+j=2;
+Vlinear(:, j+1) = BodyVelToLinearVel(V2(:,j+1),G(:,:,j)*M23);