diff --git a/R1000_Dynamics_num.m b/R1000_Dynamics_num.m
index 7b5d61b..f48d273 100644
--- a/R1000_Dynamics_num.m
+++ b/R1000_Dynamics_num.m
@@ -13,9 +13,9 @@ 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 = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
-ddthetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
+ddthetalist = [q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J]';
 
 % Get general mass matrix
 Glist=[];
diff --git a/get_robot_R1000.m b/get_robot_R1000.m
index 0e13a8f..98fe59e 100644
--- a/get_robot_R1000.m
+++ b/get_robot_R1000.m
@@ -119,12 +119,7 @@ switch opt.robot_def
         if(opt.Isreal)
             %FIXME: only consider the theta temply
             robot.dtheta = zeros([ndof,1]);
-            robot.ddtheta = zeros([ndof,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.ddtheta = pi/4*ones([ndof,1]);
             robot.link_type = ['R','R','R','R','R','R','R','R','P'];
             switch opt.KM_method
                 case 'SDH'
@@ -158,8 +153,8 @@ switch opt.robot_def
                         [0;0;0;1;0;0]];
                     robot.theta = zeros([ndof,1]);
 %                     robot.theta(end-1) = pi/4;
-                    robot.theta(3) = pi/4;
-                    robot.theta(4) = pi/4;
+%                     robot.theta(3) = pi/4;
+%                     robot.theta(4) = pi/4;
                 otherwise
                     disp('Bad opt.KM_method!')
                     return;