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
%%
for i = 1:9
tt(:,i)=robot.TW(1:3,1:3,i)*com_pos_R1(:,i);
end
kk=[];
for i = 1:9
kk(:,:,i)=TransInv(robot.TW(:,:,i))*Mlist_CG_Base(:,:,i);
end

%%
yy=eye(4,4);
for i =1:9
yy = yy*Mlist_CG(:,:,i)
end
%%
A=pascal(6)
AA = A;
B=A*E1gen(A,6,1)*E1gen(A,5,2)
% AA(:,[1,2,6]) = AA(:,[6,5,1])