base parameters are wrong

Identification_main.m

@@ -4,19 +4,22 @@ opt.robot_def = 'direct';
 opt.KM_method = 'SCREW';
 opt.Vel_method = 'Direct';
 opt.LD_method = 'Direct';
-opt.debug = true;
+opt.debug = false;
-opt.robotName = 'R1000';
+opt.robotName = 'R1000_DVT';
 opt.reGenerate = false;
 opt.Isreal = true;
 
-robot = get_robot_R1000(file,opt);
+theta = 1000*ones(9,1);
+dtheta = zeros(9,1);
+ddtheta = 1000*ones(9,1);
+robot = get_robot_R1000(theta,dtheta,ddtheta,file,opt);
 robot = get_Kinematics(robot, opt);
 
-R1000_Dynamics_num;
+% R1000_Dynamics_num;
 % R1000_Dynamics;
-% getGravityForce;
 robot = get_velocity(robot, opt);
 robot = get_regressor(robot,opt);
 % symbol matched
 % verify_regressor_R1000;
-% robot = get_baseParams(robot, opt);
+robot = get_baseParams(robot, opt);
+% robot = estimate_dyn(robot,opt);

codegen/base_regressor_R1000_DVT.m

@@ -0,0 +1,17 @@
+function base_regrssor  = base_regressor_R1000_DVT(theta,dtheta,ddtheta)
+opt.robot_def = 'direct';
+opt.KM_method = 'SCREW';
+opt.Vel_method = 'Direct';
+opt.LD_method = 'Direct';
+opt.debug = false;
+opt.robotName = 'R1000_DVT';
+opt.reGenerate = false;
+opt.Isreal = true;
+file=[];
+
+robot = get_robot_R1000(theta,dtheta,ddtheta,file,opt);
+robot = get_Kinematics(robot, opt);
+robot = get_velocity(robot, opt);
+robot = get_regressor(robot, opt);
+robot = get_baseParams(robot, opt);
+base_regrssor = robot.baseQR.regressor;

codegen/standard_regressor_R1000_DVT.m

@@ -0,0 +1,16 @@
+function standard_regrssor  = standard_regressor_R1000_DVT(theta,dtheta,ddtheta)
+opt.robot_def = 'direct';
+opt.KM_method = 'SCREW';
+opt.Vel_method = 'Direct';
+opt.LD_method = 'Direct';
+opt.debug = false;
+opt.robotName = 'R1000_EVT';
+opt.reGenerate = false;
+opt.Isreal = true;
+file=[];
+
+robot = get_robot_R1000(theta,dtheta,ddtheta,file,opt);
+robot = get_Kinematics(robot, opt);
+robot = get_velocity(robot, opt);
+robot = get_regressor(robot, opt);
+standard_regrssor = robot.regressor.K;

estimate_dyn.m

@@ -2,22 +2,23 @@ function robot = estimate_dyn(robot,opt)
 % -------------------------------------------------------------------
 % Get datas
 % ------------------------------------------------------------------------
-time = 0:0.01:2;
+time = 0:0.1:1;
 f=1;
 q_J = sin(2*pi*f*time);
 qd_J = (2*pi*f)*cos(2*pi*f*time);
 qdd_J = -(2*pi*f)^2*sin(2*pi*f*time);
-q=[q_J;-q_J];
+q=[q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J];
-qd=[qd_J; -qd_J];
+qd=[qd_J;qd_J;qd_J;qd_J;qd_J;qd_J;qd_J;qd_J;qd_J];
-qdd=[qdd_J; -qdd_J];
+qdd=[qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J];
 g = [0; 0; -9.8];
-tau = zeros([2,101]);
+tau = zeros([robot.ndof,length(q_J)]);
 % pi -> [m;mc;I] 10 element 
-robot_pi1=[1;1/2;0;0;1+1/4;0;0;1+1/4;0;1+1/4];
+[nLnkPrms, nLnks] = size(robot.pi);
-robot_pi2=[2;1;0;0;1+1/4;0;0;1+1/4;0;1+1/4];
+robot_pi = reshape(robot.pi, [nLnkPrms*nLnks, 1]);
-robot_pi=[robot_pi1;robot_pi2];
 for i = 1:length(q_J)
-    regressor = standard_regressor_Two_bar(q(:,i),qd(:,i),qdd(:,i));
+    %     regressor = standard_regressor_Two_bar(q(:,i),qd(:,i),qdd(:,i));
+    standard_regressor_func = sprintf('standard_regressor_%s',opt.robotName);
+    regressor = feval(standard_regressor_func,q(:,i),qd(:,i),qdd(:,i));
     tau(:,i)=regressor*robot_pi;
 end
 idntfcnTrjctry.t = time;

get_baseParams.m

@@ -15,7 +15,7 @@ q2d_max = 6*pi*ones(ndof,1);
 % -----------------------------------------------------------------------
 % Get observation matrix of identifiable paramters
 W = [];    
-for i = 1:25
+for i = 1:250
     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);
@@ -25,6 +25,8 @@ for i = 1:25
     else
         standard_regressor_func = sprintf('standard_regressor_%s',opt.robotName);
         Y = feval(standard_regressor_func, q_rnd,qd_rnd,q2d_rnd);
+          %FIXME: better compute standard_regressor
+%           Y =  standard_regressor_func(q_rnd,qd_rnd,q2d_rnd); 
     end
     W = vertcat(W,Y);
 end
@@ -57,6 +59,14 @@ assert(norm(W2 - W1*beta) < 1e-6,...
 % -----------------------------------------------------------------------
 % Find base parmaters
 % -----------------------------------------------------------------------
+if(opt.Isreal)
+    pi_lgr_num = robot.pi;
+    [nLnkPrms, nLnks] = size(pi_lgr_num);
+    pi_lgr_num = reshape(pi_lgr_num, [nLnkPrms*nLnks, 1]);
+    pi1 = E(:,1:qr_rank)'*pi_lgr_num; % independent paramters
+    pi2 = E(:,qr_rank+1:end)'*pi_lgr_num; % dependent paramteres
+    pi_lgr_base = pi1 + beta*pi2;
+else
     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
@@ -65,7 +75,7 @@ pi2 = E(:,qr_rank+1:end)'*pi_lgr_sym; % dependent paramteres
     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;
-
+end
 % ---------------------------------------------------------------------
 % Create structure with the result of QR decompositon a
 % ---------------------------------------------------------------------
@@ -76,13 +86,18 @@ baseQR.beta = beta;
 baseQR.motorDynamicsIncluded = includeMotorDynamics;
 baseQR.baseParams = pi_lgr_base;
 robot.baseQR = baseQR;
+robot.baseQR.regressor = robot.regressor.K*robot.baseQR.permutationMatrix(:,1:robot.baseQR.numberOfBaseParameters);
 
+if(opt.reGenerate)
+    tic
+    disp('compiling robot.baseQR.regressor');
     % ---------------------------------------------------------------------
     % Gen base_regressor
     % ---------------------------------------------------------------------
     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');
-robot.baseQR.regressor = robot.regressor.K*robot.baseQR.permutationMatrix(:,1:robot.baseQR.numberOfBaseParameters);
     matlabFunction(robot.baseQR.regressor,'File',sprintf('autogen/base_regressor_%s',opt.robotName),...
         'Vars',{q_sym,qd_sym,q2d_sym});
+    fprintf("The total compile time was: = %d minutes, %d seconds\n", floor(compileTime/60), ceil(rem(compileTime,60)));
+end

get_regressor.m

@@ -84,11 +84,11 @@ 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});
         if(opt.reGenerate)
             tic
-            disp('compiling robot.regressor.K');
+            disp('compiling symbol robot.regressor.K');
+            %         matlabFunction(robot.regressor.K,'File',sprintf('autogen/standard_regressor_%s',opt.robotName),...
+            %             'Vars',{q_sym,qd_sym,q2d_sym});
             matlabFunction(robot.regressor.K,'File',sprintf('autogen/standard_regressor_%s',opt.robotName),...
                 'Vars',{q_sym});
             compileTime = toc;

get_robot_R1000.m

@@ -1,4 +1,4 @@
-function robot = get_robot_R1000(file,opt)
+function robot = get_robot_R1000(theta,dtheta,ddtheta,file,opt)
 switch opt.robot_def
     case 'direct'
         ndof = 9;
@@ -118,8 +118,9 @@ switch opt.robot_def
         % ------------------------------------------------------------------------
         if(opt.Isreal)
             %FIXME: only consider the theta temply
-            robot.dtheta = 1000*pi/4*ones([ndof,1]);
+            robot.theta = theta;
-            robot.ddtheta = 1000*pi/4*ones([ndof,1]);  
+            robot.dtheta = dtheta;
+            robot.ddtheta = ddtheta;  
             robot.link_type = ['R','R','R','R','R','R','R','R','P'];
             switch opt.KM_method
                 case 'SDH'
@@ -151,10 +152,6 @@ switch opt.robot_def
                         [0;0;-1;cross(-[0;0;-1],co(:,7))]...
                         [0;-1;0;cross(-[0;-1;0],co(:,8))]...
                         [0;0;0;1;0;0]];
-                    robot.theta = zeros([ndof,1]);
-%                     robot.theta(end-1) = pi/4;
-%                     robot.theta(3) = pi/4;
-%                     robot.theta(4) = pi/4;
                 otherwise
                     disp('Bad opt.KM_method!')
                     return;

get_velocity.asv

@@ -1,119 +0,0 @@
-function robot = get_velocity(robot, opt)
-switch opt.KM_method
-    case 'SCREW'
-        Mlist_CG = robot.kine.Mlist_CG;
-        % Get general mass matrix
-        link_mass = robot.m;
-        com_pos = robot.com;
-        link_inertia = robot.I;
-        Slist=robot.slist;
-        Glist=[];
-        for i = 1:robot.ndof
-            Gb= [link_inertia(:,:,i),zeros(3,3);zeros(3,3),link_mass(i)*diag([1,1,1])];
-            Glist = cat(3, Glist, Gb);
-            % ?
-            %             mc = robot.TW(1:3,1:3,i)*robot.m(i)*robot.com(:,i);
-            %             robot.pi(2:4,i) = mc;
-            % hack: because we only know the com in the world frame
-            % the inv of rotation is beacause we want to the com in
-            % our defined frame. R*com is the wrong result
-            robot.pi(2:4,i) = robot.Home.R(:,:,i)'*robot.pi(2:4,i);
-            com_vec2mat = vec2skewSymMat(robot.com(:,i)); %com
-            robot.I(:,:,i) = robot.I(:,:,i)-link_mass(i)*com_vec2mat*com_vec2mat;
-            robot.I(:,:,i) = robot.Home.R(:,:,i)'*robot.I(:,:,i)*robot.Home.R(:,:,i);
-            robot.I_vec(:,i) = inertiaMatrix2Vector(robot.I(:,:,i));
-            robot.pi(5:end,i) = robot.I_vec(:,i);
-        end
-        if opt.Isreal
-            % init q
-            q = robot.theta;
-            qd = robot.dtheta;
-            qdd = robot.ddtheta;
-            [V,Vd,~,~,~] = InverseDynamics_debug(q, qd, qdd, ...
-                robot.gravity, [0;0;0;0;0;0],Mlist_CG, Glist, Slist);
-        else
-            % init q
-            q = robot.theta;
-            qd = robot.dtheta;
-            qdd = robot.ddtheta;
-            [V,Vd,~,~,~] = InverseDynamics_sym(q, qd, qdd, ...
-                robot.gravity, [0;0;0;0;0;0],Mlist_CG, Glist, Slist);
-        end
-        % FIXME: twist is not equal to velocity
-        % FIXME: Need to get the velocity represent at base frame
-        robot.vel.w = V(1:3,2:end);
-        for i = 1:robot.ndof
-            robot.vel.v(:,i) = V(4:6,i+1)-vec2skewSymMat(robot.vel.w(:,i))*robot.Home.R(:,:,i)'*;
-        end
-%         robot.vel.v = V(4:6,2:end);
-        robot.vel.dw = Vd(1:3,2:end);
-        robot.vel.dv = Vd(4:6,2:end);
-%         robot.vel.dv=[zeros(2,robot.ndof);-robot.gravity(end)*ones(1,robot.ndof)];
-    case 'SDH'
-    case 'MDH'
-        switch opt.Vel_method
-            case 'Direct'
-                Z = [0,0,1]';
-                w0 = zeros(3,1); dw0 = zeros(3,1);
-                dv0 = robot.gravity;
-                v0 = zeros(3,1);
-                link_type = robot.link_type;
-                % init q
-                q = robot.theta;
-                qd = robot.dtheta;
-                qdd = robot.ddtheta;   
-%                 for i = 1:robot.ndof
-%                     switch link_type(i)
-%                         case 'R'
-%                             %Do nothing
-%                         case 'P'
-%                             q(i) = robot.d(i);
-%                             qd(i) = robot.vd(i);
-%                             qdd(i) = robot.accd(i);
-%                     end
-%                 end
-                R = robot.kine.R;
-                P = robot.kine.t;
-                % 1-n外推公式 参考robotics toolbox
-                %第一关节
-                switch link_type(1)
-                    case 'R' %revolute
-                        w(:,1) = R(:,:,1)' * w0 + qd(1) * Z;
-                        v(:,1) = R(:,:,1)' * v0 + cross(w0,P(:,1));
-                        dw(:,1) = R(:,:,1)' * dw0 + cross(R(:,:,1)' * w0, qd(1) * Z) + qdd(1) * Z;
-                        dv(:,1) = R(:,:,1)' * (cross(dw0,P(:,1)) + cross(w0, cross(w0, P(:,1))) + dv0);
-                    case 'P' %prismatic
-                        w(:,1) = R(:,:,1)' * w0;
-                        v(:,1) = R(:,:,1)' * (Z*qd(1)+v0) + cross(w0, P(:,1));
-                        dw(:,1) = R(:,:,1)' * dw0;
-                        dv(:,1) = R(:,:,1)' * (cross(dw0, P(:,1)) + cross(w0, cross(w0, P(:,1))) + dv0)+...
-                            2*cross(R(:,:,1)' * w0, Z * qd(1)) + Z * qdd(1);
-                end
-                %后面n-1关节
-                for i = 1:robot.ndof-1
-%                     switch link_type(i)
-%                         case 'R' %revolute
-                            w(:,i+1) = R(:,:,i+1)' * w(:,i) + qd(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), qd(i+1) * Z)+ qdd(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));
-%                         case 'P' %prismatic
-%                             w(:,i+1) = R(:,:,i+1)' * w(:,i);
-%                             v(:,i+1) = R(:,:,i+1)' * (Z*qd(:,i)+v(:,i)) + cross(w(:,i), P(:,i+1));
-%                             dw(:,i+1) = R(:,:,i+1)' * dw(:,i);
-%                             dv(:,i+1) = R(:,:,i+1)' * (cross(dw(:,i), P(:,i+1)) + cross(w(:,i), cross(w(:,i), P(:,i+1))) + dv(:,i))+...
-%                                 2*cross(R(:,:,i+1)' * w(:,i), Z * qd(:,i)) + Z * qdd(:,i);
-%                     end
-                end
-                robot.vel.w = w;
-                robot.vel.v = v;
-                robot.vel.dw = dw;
-                robot.vel.dv = dv;
-            otherwise
-                disp('Bad opt.Vel_method!')
-                return;
-        end
-    otherwise
-        disp('Bad opt.KM_method!')
-        return;
-end

untitled3.asv

@@ -1,19 +0,0 @@
-R = robot.kine.R;
-P = robot.kine.t;
-F1 = Adjoint(RpToTrans(RotX(pi/4),[1;2;3]))*robot.regressor.A(:,:,end)*robot.pi(:,end)
-F2 = robot.regressor.A(:,:,end-1)*robot.pi(:,end-1)+F1
-
-FF1 = Adjoint(TransInv(RpToTrans(RotX(pi/4),[1;2;3])))'*F_Simpack(end,:,1)'
-FF2 = F_Simpack(end-1,:,1)'+FF1
-%%
-F1 = robot.regressor.A(:,:,end)*robot.pi(:,end);
-F3 = robot.regressor.A(:,:,end-2)*robot.pi(:,end-2);
-%%
-F_Simpack(end,:,1)
-F_Simpack(end-2,:,1)
-%%
-robot_pi_vecoter = reshape(robot.pi,[90,1]);
-F = robot.regressor.U*robot_pi_vecoter;
-FF = reshape(F,[6,9])
-%%
-F_Simpack(:,:,1)

untitled3.m

@@ -17,3 +17,49 @@ F = robot.regressor.U*robot_pi_vecoter;
 FF = reshape(F,[6,9])
 %%
 F_Simpack(:,:,1)
+%%
+robot_pi_vecoter = reshape(robot.pi,[90,1]);
+tau_standard  = robot.regressor.K*robot_pi_vecoter;
+tau_standard = reshape(tau_standard,[1,9])
+
+tau_base  = robot.baseQR.regressor*robot.baseQR.baseParams;
+tau_base = reshape(tau_base,[1,9])
+%%
+tau_estimate  = robot.baseQR.regressor*robot.sol.pi_b;
+tau_estimate = reshape(tau_estimate,[1,9])
+%%
+time = 0:0.1:1;
+f=1;
+q_J = sin(2*pi*f*time);
+qd_J = (2*pi*f)*cos(2*pi*f*time);
+qdd_J = -(2*pi*f)^2*sin(2*pi*f*time);
+q=[q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J];
+qd=[qd_J;qd_J;qd_J;qd_J;qd_J;qd_J;qd_J;qd_J;qd_J];
+qdd=[qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J];
+g = [0; 0; -9.8];
+tau = zeros([robot.ndof,length(q_J)]);
+% pi -> [m;mc;I] 10 element
+[nLnkPrms, nLnks] = size(robot.pi);
+robot_pi = reshape(robot.pi, [nLnkPrms*nLnks, 1]);
+Wb = []; Tau = [];
+for i = 1:length(q_J)
+    %     regressor = standard_regressor_Two_bar(q(:,i),qd(:,i),qdd(:,i));
+    standard_regressor_func = sprintf('standard_regressor_%s',opt.robotName);
+    regressor = feval(standard_regressor_func,q(:,i),qd(:,i),qdd(:,i));
+    tau=regressor*robot_pi;
+    Tau = vertcat(Tau, tau);
+end
+for i = 1:1:length(q_J)
+    base_regressor_func = sprintf('base_regressor_%s',opt.robotName);
+    Yb = feval(base_regressor_func, q(:,i),qd(:,i),qdd(:,i));
+    Wb = vertcat(Wb, Yb);
+end
+%%
+qr_rank = robot.baseQR.numberOfBaseParameters;
+E = robot.baseQR.permutationMatrix;
+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
+beta = robot.baseQR.beta;
+% all of the expressions below are equivalent
+pi_lgr_base = pi1 + beta*pi2;