diff --git a/Identification_main.m b/Identification_main.m
index 02488e6..34d5916 100644
--- a/Identification_main.m
+++ b/Identification_main.m
@@ -31,7 +31,7 @@ com_pos_R2(:,8)=[2.2355855e+02 -3.6281380e-01  1.4875409e+01]'*10^-3; % don't us
 com_pos_R1(:,9)=[-9.6776846e-02  1.4179201e-01 -3.4242667e+01]'*10^-3;
 
 % Get 3D coordinate of CO
-co=[]
+co=[];
 for i = 1:8
     if i == 1
         co(:,i) = com_pos_R1(:,i)-com_pos_R2(:,i);
@@ -46,17 +46,16 @@ end
 co = [zeros(3,1),co];
 
 % temp slist
-robot.slist=[[0;0;1;0;0;0],...
-    [0;-1;0;cross(-[0;-1;0],[0.2;0;0])]...
-    [0;-1;0;cross(-[0;-1;0],[0.2+0.5;0;0])]...
-    [0;-1;0;cross(-[0;-1;0],[0.2+0.5+0.45;0;0])]...
-    [0;0;1;cross(-[0;0;1],[0.2+0.5+0.45+0.12;0;0])]...
-    [1;0;0;cross(-[1;0;0],[0.2+0.5+0.45+0.12+0.28;0;0])]...
-    [0;0;-1;cross(-[0;0;-1],[0.2+0.5+0.45+0.12+0.28;0;-0.4])]...
-    [0;-1;0;cross(-[0;-1;0],[0.2+0.5+0.45+0.12+0.28;0;-0.4])]...
+robot.slist=[[0;0;1;co(:,1)],...
+    [0;-1;0;cross(-[0;-1;0],co(:,2))]...
+    [0;-1;0;cross(-[0;-1;0],co(:,3))]...
+    [0;-1;0;cross(-[0;-1;0],co(:,4))]...
+    [0;0;1;cross(-[0;0;1],co(:,5))]...
+    [1;0;0;cross(-[1;0;0],co(:,6))]...
+    [0;0;-1;cross(-[0;0;-1],co(:,7))]...
+    [0;-1;0;cross(-[0;-1;0],co(:,8))]...
     [0;0;0;1;0;0]];
 
-robot.slist(4:6,:) =  co;
 robot.Home.R(:,:,1) = [[1;0;0],[0;1;0],[0;0;1]];
 robot.Home.R(:,:,2) = [[1;0;0],[0;0;1],[0;-1;0]];
 robot.Home.R(:,:,3) = [[1;0;0],[0;0;1],[0;-1;0]];
@@ -74,7 +73,7 @@ robot.link_type = ['R','R','R','R','R','R','R','R','P'];
 
 robot = get_Kinematics(robot, opt);
 
-% R1000_Dynamics_num;
+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 c7c2ec8..a3bdfca 100644
--- a/R1000_Dynamics_num.m
+++ b/R1000_Dynamics_num.m
@@ -24,51 +24,94 @@ for i = 1:N
 end
 
 % Get the com pos transformation in each joint reference frame
-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_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_CG = cat(3, Mlist_CG, M);
+% 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_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_CG = cat(3, Mlist_CG, M);
 
 % Get the com pos transformation in each joint reference frame
 % FIXME: BUG here
-Mlist_CG=[];
-for i = 0:N-1
-    if i == 0
-        M=robot.T(:,:,i+1)*transl(com_pos(:,i+1));
-    else
-        rotation_i = diag([1,1,1]);
-        for j = 1:i
-            rotation_i = rotation_i*TransToRp(robot.T(:,:,i));
-            rotation_j = rotation_i*TransToRp(robot.T(:,:,i+1));
-        end
-        M = TransInv(RpToTrans(rotation_i,rotation_i*com_pos(:,i)))*robot.T(:,:,i)*RpToTrans(rotation_j,rotation_j*com_pos(:,i+1));
-    end
-    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_CG = cat(3, Mlist_CG, M);
+% Mlist_CG=[];
+% for i = 0:N-1
+%     if i == 0
+%         M=robot.T(:,:,i+1)*transl(com_pos(:,i+1));
+%     else
+%         rotation_i = diag([1,1,1]);
+%         for j = 1:i
+%             rotation_i = rotation_i*TransToRp(robot.T(:,:,i));
+%             rotation_j = rotation_i*TransToRp(robot.T(:,:,i+1));
+%         end
+%         M = TransInv(RpToTrans(rotation_i,rotation_i*com_pos(:,i)))*robot.T(:,:,i+1)*RpToTrans(rotation_j,rotation_j*com_pos(:,i+1));
+%     end
+%     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_CG = cat(3, Mlist_CG, M);
+
+% ct=[];
+% Mlist_CG=[];
+% for i = 1:N
+%     if i == 1
+%         ct(:,i) = com_pos_R1(:,i);
+%     elseif i< 9
+%         ct(:,i) = -com_pos_R2(:,i-1)+com_pos_R1(:,i);
+%     else
+%         ct(:,i) = -com_pos_R1(:,i-1)-[0;0;0.05896]+com_pos_R1(:,i);
+%     end
+%     robot.Home.com(:,i) = ct(:,i);
+%     M = RpToTrans(robot.T(1:3,1:3,i),robot.Home.R(:,:,i)*robot.Home.com(:,i));
+%     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_CG = cat(3, Mlist_CG, M);
 
 % Mlist_CG=[];
 % for i = 1:N
 %         if i == 1
 %         M = [diag([1,1,1]),com_pos_R1(:,i);zeros(1,3),1];
 %     elseif i<=8
-%         M = RpToTrans(TransToRp(robot.T(:,:,i)),TransToRp(robot.T(:,:,i))*(-com_pos_R2(:,i-1)+com_pos_R1(:,i)));
+%         M = RpToTrans(TransToRp(robot.T(:,:,i)),robot.Home.R(:,:,i)*(-com_pos_R2(:,i-1)+com_pos_R1(:,i)));
 %     elseif i==9
-%         M = RpToTrans(TransToRp(robot.T(:,:,i)),TransToRp(robot.T(:,:,i))*(-[0;0;0.05896]+com_pos_R1(:,i-1)+com_pos_R1(:,i)));
+%         M = RpToTrans(TransToRp(robot.T(:,:,i)),robot.Home.R(:,:,i)*(-[0;0;0.05896]+com_pos_R1(:,i-1)+com_pos_R1(:,i)));
 %         end
 %         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_CG = cat(3, Mlist_CG, M);
 
+ct=[];
+Mlist_CG_Base=[];
+for i = 1:N
+    if i == 1
+        ct(:,i) = com_pos_R1(:,i);
+    elseif i< 9
+        ct(:,i) = ct(:,i-1)-com_pos_R2(:,i-1)+com_pos_R1(:,i);
+    else
+        ct(:,i) = ct(:,i-1)-com_pos_R1(:,i-1)-[0;0;0.05896]+com_pos_R1(:,i);
+    end
+    robot.Home.com(:,i) = ct(:,i);
+    M_CG_Base = RpToTrans(robot.Home.R(:,:,i),robot.Home.com(:,i));
+    Mlist_CG_Base = cat(3, Mlist_CG_Base, M_CG_Base);
+end
+Mlist_CG=[];
+for i = 1:N
+    if i == 1
+        Mlist_CG(:,:,i) = Mlist_CG_Base(:,:,i);
+    else
+        Mlist_CG(:,:,i) = TransInv(Mlist_CG_Base(:,:,i-1))*Mlist_CG_Base(:,:,i);
+    end
+end
+M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
+Mlist_CG = cat(3, Mlist_CG, M);
+
+
 % Get the end efforce transformation in each joint reference frame
 Mlist_ED = [];
 for i = 1:N
@@ -80,15 +123,7 @@ Mlist_ED = cat(3, Mlist_ED, M);
 
 %TODO: Get Slist form DH table method
 % RRRRRRRRP
-Slist=[[0;0;1;0;0;0],...
-    [0;-1;0;cross(-[0;-1;0],[0.2;0;0])]...
-    [0;-1;0;cross(-[0;-1;0],[0.2+0.5;0;0])]...
-    [0;-1;0;cross(-[0;-1;0],[0.2+0.5+0.45;0;0])]...
-    [0;0;1;cross(-[0;0;1],[0.2+0.5+0.45+0.12;0;0])]...
-    [1;0;0;cross(-[1;0;0],[0.2+0.5+0.45+0.12+0.28;0;0])]...
-    [0;0;-1;cross(-[0;0;-1],[0.2+0.5+0.45+0.12+0.28;0;-0.4])]...
-    [0;-1;0;cross(-[0;-1;0],[0.2+0.5+0.45+0.12+0.28;0;-0.4])]...
-    [0;0;0;1;0;0]];
+Slist=robot.slist;
 
 Vlinear=sym(zeros(3,3));
 J=sym(zeros(6,N));
diff --git a/get_Kinematics.m b/get_Kinematics.m
index 232e1dc..7bed40f 100644
--- a/get_Kinematics.m
+++ b/get_Kinematics.m
@@ -12,7 +12,7 @@ if(opt.Isreal)
                 end
                 robot.TW(:,:,j) = T;
             end
-            for i = 1:size(thetalist)
+            for i = 1:length(thetalist)
                 if i == 1
                     robot.T(:,:,i) = robot.TW(:,:,i);
                 else
diff --git a/get_robot_R1000.m b/get_robot_R1000.m
index b085191..673cbe9 100644
--- a/get_robot_R1000.m
+++ b/get_robot_R1000.m
@@ -15,6 +15,8 @@ switch opt.robot_def
                     robot.link_type = ['R','R','R','R','R','R','R','R','P'];
                 case 'SCREW'
                     robot.theta = zeros(1,9);
+                    robot.theta(5) = pi/3;
+                    robot.theta(7) = pi/3;
                 otherwise
                     disp('Bad opt.KM_method!')
                     return;
diff --git a/untitled2.asv b/untitled2.asv
new file mode 100644
index 0000000..e755b92
--- /dev/null
+++ b/untitled2.asv
@@ -0,0 +1,81 @@
+Mlist_CG=[]
+for i = 0:N-1
+    if i == 0
+        M=robot.T(:,:,i+1)*transl(com_pos(:,i+1));
+    else
+        %         rotation_i = TransToRp(robot.T(:,:,i+1))
+        M = TransInv(transl(com_pos(:,i)))*robot.T(:,:,i+1)*transl(com_pos(:,i+1));
+    end
+    Mlist_CG = cat(3, Mlist_CG, M);
+end
+%%
+Mlist_CG=[]
+for i = 0:N-1
+    if i == 0
+        M=robot.T(:,:,i+1)*transl(com_pos(:,i+1));
+    else
+        rotation_i = diag([1,1,1]);
+        for j = 1:i
+            rotation_i = rotation_i*TransToRp(robot.T(:,:,i));
+            rotation_j = rotation_i*TransToRp(robot.T(:,:,i+1));
+        end
+        M = TransInv(RpToTrans(rotation_i,com_pos(:,i)))*robot.T(:,:,i+1)*RpToTrans(rotation_j,com_pos(:,i+1));
+    end
+    Mlist_CG = cat(3, Mlist_CG, M);
+end
+%%
+com_pos_R1(:,1)=[9.7435250e+01  8.3517655e-01  1.2246547e+02]'*10^-3;
+com_pos_R2(:,1)=[-1.0040117e+02 -9.4164823e+01 -3.4522260e+01]'*10^-3;
+com_pos_R1(:,2)=[3.7345395e+02 -5.5328829e+01 -4.4313141e-02]'*10^-3;
+com_pos_R2(:,2)=[ -1.2654643e+02  9.6371171e+01 -4.4094190e-02]'*10^-3;
+com_pos_R1(:,3)=[1.8811711e+02 -7.9651429e+00 -4.9225523e-04 ]'*10^-3;
+com_pos_R2(:,3)=[-2.6188289e+02  2.8348571e+00 -4.9225523e-04 ]'*10^-3;
+com_pos_R1(:,4)=[ 5.4048685e+01  5.8463901e+01 -5.0915631e+00 ]'*10^-3;
+com_pos_R2(:,4)=[ -6.5951315e+01 -9.0360991e+00  5.2908437e+01]'*10^-3;
+com_pos_R1(:,5)=[1.3028528e+02  4.8953539e-02  4.6198421e+01]'*10^-3;
+com_pos_R2(:,5)=[-9.5814715e+01  4.8953539e-02 -1.2301579e+01 ]'*10^-3;
+com_pos_R1(:,6)=[4.9059639e+01  5.9928547e-02 -2.8858572e+01]'*10^-3;
+com_pos_R2(:,6)=[-4.7403608e+00  5.9928547e-02  6.2741428e+01]'*10^-3;
+com_pos_R1(:,7)=[2.3210318e-02 -9.5031144e+00 -1.0242124e+02]'*10^-3;
+com_pos_R2(:,7)=[2.3210318e-02 -9.5031144e+00  2.0257876e+02 ]'*10^-3;
+com_pos_R1(:,8)=[-3.6571935e+01 -3.6282658e-01 -4.7124267e+01]'*10^-3;
+com_pos_R2(:,8)=[2.2355855e+02 -3.6281380e-01  1.4875409e+01]'*10^-3; % don't use this
+com_pos_R1(:,9)=[-9.6776846e-02  1.4179201e-01 -3.4242667e+01]'*10^-3;
+% plot 3D: Get 3D coordinate of COM
+ct=[];
+for i = 1:9
+    if i == 1
+        ct(:,i) = com_pos_R1(:,i);
+    elseif i< 9
+        ct(:,i) = ct(:,i-1)-com_pos_R2(:,i-1)+com_pos_R1(:,i);
+    else
+        ct(:,i) = ct(:,i-1)-com_pos_R1(:,i-1)-[0;0;0.05896]+com_pos_R1(:,i);
+    end
+end
+plot3(ct(1,:),ct(2,:),ct(3,:),'o','Color','r')
+axis equal
+grid on
+% plot 3D: Get 3D coordinate of CO
+co=[];
+for i = 1:8
+    if i == 1
+        co(:,i) = com_pos_R1(:,i)-com_pos_R2(:,i);
+    elseif i<8
+        %From base to ISA Origin
+        co(:,i) = co(:,i-1)+com_pos_R1(:,i)-com_pos_R2(:,i);
+    else
+        %From base to ISA Origin
+        co(:,i) = co(:,i-1)-[0;0;0.05896];
+    end
+end
+co = [zeros(3,1),co];
+hold on
+plot3(co(1,:),co(2,:),co(3,:),'o','Color','b')
+axis equal
+grid on
+%%
+for i = 1:9
+co(:,i+1)-co(:,i);
+end
+%%
+robot.
\ No newline at end of file
diff --git a/untitled2.m b/untitled2.m
index a1f8c23..a9fbcb9 100644
--- a/untitled2.m
+++ b/untitled2.m
@@ -42,7 +42,7 @@ com_pos_R1(:,8)=[-3.6571935e+01 -3.6282658e-01 -4.7124267e+01]'*10^-3;
 com_pos_R2(:,8)=[2.2355855e+02 -3.6281380e-01  1.4875409e+01]'*10^-3; % don't use this
 com_pos_R1(:,9)=[-9.6776846e-02  1.4179201e-01 -3.4242667e+01]'*10^-3;
 % plot 3D: Get 3D coordinate of COM
-ct=[]
+ct=[];
 for i = 1:9
     if i == 1
         ct(:,i) = com_pos_R1(:,i);
@@ -56,7 +56,7 @@ plot3(ct(1,:),ct(2,:),ct(3,:),'o','Color','r')
 axis equal
 grid on
 % plot 3D: Get 3D coordinate of CO
-co=[]
+co=[];
 for i = 1:8
     if i == 1
         co(:,i) = com_pos_R1(:,i)-com_pos_R2(:,i);
@@ -73,6 +73,16 @@ hold on
 plot3(co(1,:),co(2,:),co(3,:),'o','Color','b')
 axis equal
 grid on
+%%
 for i = 1:9
-co(:,i+1)-co(:,i)
+co(:,i+1)-co(:,i);
+end
+%%
+for i = 1:9
+tt(:,i)=robot.TW(1:3,1:3,i)*com_pos_R1(:,i);
+end
+%%
+yy=eye(4,4);
+for i =1:9
+yy = yy*Mlist_CG(:,:,i)
 end
\ No newline at end of file