diff --git a/R1000_Dynamics_num.m b/R1000_Dynamics_num.m
index 33a9fd7..43d8403 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_;q_J;q_J;zero_;zero_;zero_;zero_;zero_]';
+thetalist = [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 = 1000*[q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J]';
 
diff --git a/get_velocity.m b/get_velocity.m
index bba1201..189cfd7 100644
--- a/get_velocity.m
+++ b/get_velocity.m
@@ -44,15 +44,11 @@ switch opt.KM_method
         % 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);
+        % We need the vel at COM but we want to know vel at joint origin
         for i = 1:robot.ndof
-            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));
+            robot.vel.v(:,i) = vec2skewSymMat(robot.Home.R(:,:,i)'*robot.com(:,i))*robot.vel.w(:,i)+V(4:6,i+1);
+            robot.vel.dv(:,i) = vec2skewSymMat(robot.Home.R(:,:,i)'*robot.com(:,i))*robot.vel.dw(:,i)+Vd(4:6,i+1);
         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'
     case 'MDH'
         switch opt.Vel_method
diff --git a/untitled3.asv b/untitled3.asv
new file mode 100644
index 0000000..0a9b7e7
--- /dev/null
+++ b/untitled3.asv
@@ -0,0 +1,19 @@
+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)
\ No newline at end of file