diff --git a/R1000_Dynamics_num.m b/R1000_Dynamics_num.m
index 810105b..87eb2d6 100644
--- a/R1000_Dynamics_num.m
+++ b/R1000_Dynamics_num.m
@@ -12,9 +12,9 @@ link_mass = robot.m;
 com_pos = robot.com;
 link_inertia = robot.I;
 
-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_]';
+thetalist = [q_J;zero_;zero_;zero_;zero_;q_J;zero_;zero_;zero_]';
+dthetalist = [qd_J;zero_;zero_;zero_;zero_;qd_J;zero_;zero_;zero_]';
+ddthetalist = [qdd_J;zero_;zero_;zero_;zero_;qdd_J;zero_;zero_;zero_]';
 
 % Get general mass matrix
 Glist=[];
@@ -24,17 +24,17 @@ for i = 1:N
 end
 
 % Get the com pos transformation in each joint reference frame
-Mlist_GC = [];
+Mlist_CG = [];
 for i = 0:N-1
     if i == 0
         M = robot.T(:,:,i+1)*transl(com_pos(:,i+1));
     else
         M = TransInv(transl(com_pos(:,i)))*robot.T(:,:,i+1)*transl(com_pos(:,i+1));
     end
-    Mlist_GC = cat(3, Mlist_GC, M);
+    Mlist_CG = cat(3, Mlist_CG, M);
 end
 M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
-Mlist_GC = cat(3, Mlist_GC, M);
+Mlist_CG = cat(3, Mlist_CG, M);
 
 % Get the end efforce transformation in each joint reference frame
 Mlist_ED = [];
@@ -64,9 +64,10 @@ exf=[0;0;0;0;0;0];
 for i = 1:length(q_J)
     [V1(:,:, i),Vd1(:,:, i),Adgab_mat(:,:,:,i),Fmat(:,:,i)] ...
         = InverseDynamics_debug(thetalist(i,:)', dthetalist(i,:)', ddthetalist(i,:)', ...
-        [0;0;-9.8], exf, Mlist_GC, Glist, Slist);
-    G(:,:,:,i) = FKinSpaceExpand(Mlist_GC, Slist, thetalist(i,:)');
+        [0;0;-9.8], exf, Mlist_CG, Glist, Slist);
+    G(:,:,:,i) = FKinSpaceExpand(Mlist_CG, Slist, thetalist(i,:)');
     T(:,:,:,i)=FKinSpaceExpand(Mlist_ED, Slist, thetalist(i,:)');
+    %Want to get the result from TC_delta, which means F at CG represent under frame at the last origin  
     F_Simpack(:,:,i) = getSimpackF(G(:,:,:,i),T(:,:,:,i),Mlist_ED,Fmat(:,:,i));
 end
 % plot Torque
@@ -75,6 +76,7 @@ figure(1)
 for i = 1:3
     subplot(3,1,i);
     hold on;
+    %added minus, so should be the same as simpack
     plot(time,-reshape(F_Simpack(1,i,:),[1,length(F_Simpack)]))
 %     plot(SPCK_Result.Crv(i+4).x,SPCK_Result.Crv(i+4).y)
 end
diff --git a/simpack_data/R1000-DVT-statics.sbr b/simpack_data/R1000-DVT-statics.sbr
new file mode 100644
index 0000000..29d5072
Binary files /dev/null and b/simpack_data/R1000-DVT-statics.sbr differ