diff --git a/R1000_Dynamics_num.asv b/R1000_Dynamics_num.asv
index 0fd64f9..b929aca 100644
--- a/R1000_Dynamics_num.asv
+++ b/R1000_Dynamics_num.asv
@@ -7,20 +7,20 @@ 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);
 zero_ = zeros(1,length(q_J));
-q_J = ones(1,length(q_J));
+q_J = pi/4*ones(1,length(q_J));
 % Dynamics parameters
 link_mass = robot.m;
 com_pos = robot.com;
 link_inertia = robot.I;
 
 thetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
-dthetalist = [zero_;q_J;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
+dthetalist = [zero_;zero_;zero_;q_J;zero_;zero_;zero_;zero_;zero_]';
 ddthetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
 
 % Get general mass matrix
 Glist=[];
 for i = 1:N
-    link_inertia(:,:,i) = robot.Home.R(:,:,i)'*link_inertia(:,:,i)
+    link_inertia(:,:,i) = robot.Home.R(:,:,i)'*link_inertia(:,:,i)*robot.Home.R(:,:,i);
     Gb= [link_inertia(:,:,i),zeros(3,3);zeros(3,3),link_mass(i)*diag([1,1,1])];
     Glist = cat(3, Glist, Gb);
 end
diff --git a/R1000_Dynamics_num.m b/R1000_Dynamics_num.m
index f58ad63..7b5d61b 100644
--- a/R1000_Dynamics_num.m
+++ b/R1000_Dynamics_num.m
@@ -7,14 +7,14 @@ 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);
 zero_ = zeros(1,length(q_J));
-q_J = ones(1,length(q_J));
+q_J = pi/4*ones(1,length(q_J));
 % Dynamics parameters
 link_mass = robot.m;
 com_pos = robot.com;
 link_inertia = robot.I;
 
-thetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
-dthetalist = [zero_;q_J;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
+thetalist = [zero_;zero_;q_J;q_J;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_]';
 
 % Get general mass matrix
diff --git a/get_regressor.m b/get_regressor.m
index 91d904e..a84d946 100644
--- a/get_regressor.m
+++ b/get_regressor.m
@@ -44,8 +44,10 @@ switch opt.LD_method
                     dw_l(:,:,i) = vec2linearSymMat(dw(:,i));
                     % size of matrix A is 6*10, need to -1
                     % Traversaro el 2015
+%                     robot.regressor.A(:,:,i) = [dv(:,i)+w_skew(:,:,i)*v(:,i),dw_skew(:,:,i)+w_skew(:,:,i)*w_skew(:,:,i),zeros(3,6); ...
+%                         zeros(3,1),-(dv_skew(:,:,i)+w_skew(:,:,i)*v_skew(:,:,i)-v_skew(:,:,i)*w_skew(:,:,i)),dw_l(:,:,i)+w_skew(:,:,i)*w_l(:,:,i)];
                     robot.regressor.A(:,:,i) = [dv(:,i)+w_skew(:,:,i)*v(:,i),dw_skew(:,:,i)+w_skew(:,:,i)*w_skew(:,:,i),zeros(3,6); ...
-                        zeros(3,1),-(dv_skew(:,:,i)+w_skew(:,:,i)*v_skew(:,:,i)-v_skew(:,:,i)*w_skew(:,:,i)),dw_l(:,:,i)+w_skew(:,:,i)*w_l(:,:,i)];
+                        zeros(3,1),-vec2skewSymMat((dv(:,i)+w_skew(:,:,i)*v(:,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);
                 end
diff --git a/get_robot_R1000.m b/get_robot_R1000.m
index 68a8901..0e13a8f 100644
--- a/get_robot_R1000.m
+++ b/get_robot_R1000.m
@@ -120,7 +120,11 @@ switch opt.robot_def
             %FIXME: only consider the theta temply
             robot.dtheta = zeros([ndof,1]);
             robot.ddtheta = zeros([ndof,1]);
-            robot.dtheta(2) = 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'
@@ -153,9 +157,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) = pi/4;
-%                     robot.theta(4) = 1;
-%                     robot.theta(6) = 1;
+%                     robot.theta(end-1) = pi/4;
+                    robot.theta(3) = pi/4;
+                    robot.theta(4) = pi/4;
                 otherwise
                     disp('Bad opt.KM_method!')
                     return;
diff --git a/get_velocity.asv b/get_velocity.asv
index 1c9eee6..dacf6b3 100644
--- a/get_velocity.asv
+++ b/get_velocity.asv
@@ -12,8 +12,17 @@ switch opt.KM_method
             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;
+            %             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
@@ -33,9 +42,10 @@ switch opt.KM_method
         % 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 = i:robot.ndof
-        robot.vel.v = V(4:6,i+1)+robot.vel.w*robot.kine.t();
+        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)];
diff --git a/get_velocity.m b/get_velocity.m
index 13d6509..f069b3c 100644
--- a/get_velocity.m
+++ b/get_velocity.m
@@ -19,6 +19,7 @@ switch opt.KM_method
             % 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
+            % get joint inertial
             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));
@@ -42,12 +43,14 @@ switch opt.KM_method
         % 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);
+        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.pi(2:4,i)/robot.m(i);
+            robot.vel.v(:,i) = V(4:6,i+1) - vec2skewSymMat(robot.vel.w(:,i))*robot.Home.R(:,:,i)'*robot.com(:,i);
+            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.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'