diff --git a/Identification_main.m b/Identification_main.m
index 3db19f1..f95dfe1 100644
--- a/Identification_main.m
+++ b/Identification_main.m
@@ -15,8 +15,8 @@ robot = get_Kinematics(robot, opt);
 R1000_Dynamics_num;
 % R1000_Dynamics;
 % getGravityForce;
-% robot = get_velocity(robot, opt);
-% robot = get_regressor(robot,opt);
+robot = get_velocity(robot, opt);
+robot = get_regressor(robot,opt);
 % symbol matched
 % verify_regressor_R1000;
 % robot = get_baseParams(robot, opt);
diff --git a/R1000_Dynamics_num.m b/R1000_Dynamics_num.m
index e226c83..d9a898b 100644
--- a/R1000_Dynamics_num.m
+++ b/R1000_Dynamics_num.m
@@ -13,7 +13,7 @@ link_mass = robot.m;
 com_pos = robot.com;
 link_inertia = robot.I;
 
-thetalist = [zero_;zero_;zero_;q_J;zero_;zero_;zero_;zero_;zero_]';
+thetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
 dthetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
 ddthetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
 
diff --git a/get_regressor.m b/get_regressor.m
index 2e6bc78..66bde4f 100644
--- a/get_regressor.m
+++ b/get_regressor.m
@@ -40,11 +40,15 @@ switch opt.LD_method
                     dv_skew(:,:,i) = vec2skewSymMat(dv(:,i));
                     w_l(:,:,i) = vec2linearSymMat(w(:,i));
                     dw_l(:,:,i) = vec2linearSymMat(dw(:,i));
+                    % 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.TW(1:3,1:3,i)'*robot.pi(2:4,i);
                     % size of matrix A is 6*10, need to -1
                     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)];
                     %? match screw method
-                    robot.regressor.A(:,:,i) = Adjoint(RpToTrans(robot.TW(1:3,1:3,i),[0;0;0]))'*robot.regressor.A(:,:,i);
+%                     robot.regressor.A(:,:,i) = Adjoint(RpToTrans(robot.TW(1:3,1:3,i),[0;0;0]))'*robot.regressor.A(:,:,i);
                 end
                 % construct matrix U, size: [6*n,10*n]
                 % U_ = sym(zeros([6*ndof,10*ndof]));
@@ -67,10 +71,11 @@ switch opt.LD_method
                 for i = 1:ndof
                     delta_(i,6*i) = 1;
 
-%                     %FIXME: use link type
-%                     if(i==ndof)
-%                         delta_(i,6*i) = 0;
-%                     end
+                    %FIXME: use link type, hack now
+                    if(i==ndof)
+                        delta_(i,6*i) = 0;
+                        delta_(ndof,6*(ndof-1)+3) = 1;
+                    end
                 end
                 robot.regressor.K = delta_*robot.regressor.U;
                 if(opt.debug)
diff --git a/get_robot_R1000.m b/get_robot_R1000.m
index f60f627..7876683 100644
--- a/get_robot_R1000.m
+++ b/get_robot_R1000.m
@@ -152,6 +152,9 @@ switch opt.robot_def
                         [0;-1;0;cross(-[0;-1;0],co(:,8))]...
                         [0;0;0;1;0;0]];
                     robot.theta = zeros([ndof,1]);
+%                     robot.theta(2) = 1;
+%                     robot.theta(4) = 1;
+%                     robot.theta(6) = 1;
                 otherwise
                     disp('Bad opt.KM_method!')
                     return;
diff --git a/get_velocity.m b/get_velocity.m
index ffd3b1f..beaddd4 100644
--- a/get_velocity.m
+++ b/get_velocity.m
@@ -35,8 +35,8 @@ switch opt.KM_method
         robot.vel.w = V(1:3,:);
         robot.vel.v = V(4:6,:);
         robot.vel.dw = Vd(1:3,:);
-%         robot.vel.dv = Vd(4:6,:);
-        robot.vel.dv=[zeros(2,robot.ndof);-robot.gravity(end)*ones(1,robot.ndof)];
+        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