diff --git a/Identification_main.m b/Identification_main.m
index 166d17c..06d078f 100644
--- a/Identification_main.m
+++ b/Identification_main.m
@@ -13,7 +13,7 @@ robot = get_robot_R1000(file,opt);
 robot = get_Kinematics(robot, opt);
 %TODO verify kinematics via robotics toolbox or other software result
 
-R1000_Dynamics;
+R1000_Dynamics_num;
 % opt.Isreal = false;
 % robot = get_velocity(robot, opt);
 % robot = get_regressor(robot,opt);
diff --git a/R1000_Dynamics_num.m b/R1000_Dynamics_num.m
index fa5b892..69fd256 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 = [q_J;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
-dthetalist = [qd_J;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
-ddthetalist = [qdd_J;zero_;zero_;zero_;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_]';
 
 % Get general mass matrix
 Glist=[];
@@ -62,7 +62,7 @@ J=sym(zeros(6,N));
 exf=[0;0;0;0;0;0];
 
 for i = 1:length(q_J)
-    [V1(:,:, i),Vd1(:,:, i),Adgab_mat(:,:,:,i),Fmat(:,:,i)] ...
+    [V1(:,:, i),Vd1(:,:, i),Adgab_mat(:,:,:,i),Fmat(:,:,i),taumat(:,i)] ...
         = InverseDynamics_debug(thetalist(i,:)', dthetalist(i,:)', ddthetalist(i,:)', ...
         [0;0;-9.8], exf, Mlist_CG, Glist, Slist);
     G(:,:,:,i) = FKinSpaceExpand(Mlist_CG, Slist, thetalist(i,:)');
@@ -80,7 +80,9 @@ 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(time,-reshape(F_Simpack(8,i,:),[1,length(F_Simpack)]))
+    xlabel('time(s)')
+    ylabel('Torque(NM)')
 %     plot(SPCK_Result.Crv(i+4).x,SPCK_Result.Crv(i+4).y)
 end