diff --git a/untitled2.m b/untitled2.m
index a69ce23..a228bba 100644
--- a/untitled2.m
+++ b/untitled2.m
@@ -3,8 +3,8 @@ 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));
+        %         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
@@ -15,11 +15,11 @@ for i = 0:N-1
         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));
+        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
@@ -39,15 +39,37 @@ 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;
+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:7
+for i = 1:9
     if i == 1
         ct(:,i) = com_pos_R1(:,i);
-    elseif i< 8
-        ct(:,i) = -com_pos_R2(:,i-1)+com_pos_R1(:,i);
-    elseif i==8
-        % ct(:,)
+    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
\ No newline at end of file
+end
+plot3(ct(1,:),ct(2,:),ct(3,:),'o','Color','r')
+axis equal
+grid on
+% plot 3D: Get 3D coordinate of COM
+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
\ No newline at end of file