regressor work but don't know why

Identification_main.asv

@@ -0,0 +1,22 @@
+close all;clc;clear
+file = [];
+opt.robot_def = 'direct';
+opt.KM_method = 'SCREW';
+opt.Vel_method = 'Direct';
+opt.LD_method = 'Direct';
+opt.debug = true;
+opt.robotName = 'R1000';
+opt.reGenerate = false;
+opt.Isreal = true;
+
+robot = get_robot_R1000(file,opt);
+robot = get_Kinematics(robot, opt);
+
+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);

R1000_Dynamics_num.m

@@ -14,8 +14,8 @@ com_pos = robot.com;
 link_inertia = robot.I;
 
 thetalist = [zero_;zero_;q_J;q_J;zero_;zero_;zero_;zero_;zero_]';
-dthetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
+dthetalist = 1000*[q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J]';
-ddthetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
+ddthetalist = 1000*[q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J]';
 
 % Get general mass matrix
 Glist=[];

get_Kinematics.m

@@ -60,6 +60,16 @@ if(opt.Isreal)
             M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
             Mlist_CG = cat(3, Mlist_CG, M);
             robot.kine.Mlist_CG = Mlist_CG;
+
+            % Get the end efforce transformation in each joint reference frame
+            Mlist_ED = [];
+            for i = 1:length(thetalist)
+                M = robot.T(:,:,i);
+                Mlist_ED = cat(3, Mlist_ED, M);
+            end
+            M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
+            Mlist_ED = cat(3, Mlist_ED, M);
+            robot.kine.Mlist_ED = Mlist_ED;
         case 'SDH'
             theta = robot.theta;
             alpha = robot.alpha;

get_robot_R1000.m

@@ -118,13 +118,8 @@ switch opt.robot_def
         % ------------------------------------------------------------------------
         if(opt.Isreal)
             %FIXME: only consider the theta temply
-            robot.dtheta = zeros([ndof,1]);
+            robot.dtheta = 1000*pi/4*ones([ndof,1]);
-            robot.ddtheta = zeros([ndof,1]);
+            robot.ddtheta = 1000*pi/4*ones([ndof,1]);  
-%             robot.theta(end-1) = pi/4;
-%             robot.dtheta(end-2) = pi/4;
-%             robot.ddtheta(end-1) = 100*pi/4;
-%             robot.ddtheta(end-2) = 100*pi/4;
-%             robot.ddtheta(end-3) = 100*pi/4;
             robot.link_type = ['R','R','R','R','R','R','R','R','P'];
             switch opt.KM_method
                 case 'SDH'
@@ -158,8 +153,8 @@ switch opt.robot_def
                         [0;0;0;1;0;0]];
                     robot.theta = zeros([ndof,1]);
 %                     robot.theta(end-1) = pi/4;
-                    robot.theta(3) = pi/4;
+%                     robot.theta(3) = pi/4;
-                    robot.theta(4) = pi/4;
+%                     robot.theta(4) = pi/4;
                 otherwise
                     disp('Bad opt.KM_method!')
                     return;

get_velocity.m

@@ -17,7 +17,7 @@ switch opt.KM_method
             % 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);
+            robot.pi(2:4,i) = robot.m(i)*robot.Home.R(:,:,i)'*robot.com(:,i);
             com_vec2mat = vec2skewSymMat(robot.com(:,i)); %com
             % get joint inertial
             robot.I(:,:,i) = robot.I(:,:,i)-link_mass(i)*com_vec2mat*com_vec2mat;
@@ -45,11 +45,12 @@ switch opt.KM_method
         robot.vel.w = V(1:3,2:end);
         robot.vel.dw = Vd(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)'*robot.com(:,i);
+            robot.vel.v(:,i) = V(4:6,i+1);
             robot.vel.dv(:,i) = Vd(4:6,i+1) - vec2skewSymMat(robot.vel.dw(:,i))*robot.Home.R(:,:,i)'*robot.com(:,i)...
                 -  vec2skewSymMat(robot.vel.w(:,i))*(vec2skewSymMat(robot.vel.w(:,i))*robot.Home.R(:,:,i)'*robot.com(:,i));
         end
 %         robot.vel.v = V(4:6,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'