%% R1000
N=9;
time = 0:0.01:1;
f=1;    
q_J = sin(2*pi*f*time);
qd_J = (2*pi*f)*cos(2*pi*f*time);
qdd_J = -(2*pi*f)^2*sin(2*pi*f*time);
zero_ = zeros(1,length(q_J));


% thetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
thetalist = zeros(N,1);

Mlist_CG = robot.kine.Mlist_CG;
Slist=robot.slist;

% Dynamics parameters
link_mass = robot.m;
com_pos = robot.com;
link_inertia = robot.I;
% Get general mass matrix
Glist=[];
for i = 1:N
    Gb= [link_inertia(:,:,i),zeros(3,3);zeros(3,3),link_mass(i)*diag([1,1,1])];
    Glist = cat(3, Glist, Gb);
end

gravity = [0;0;-9.806];
gravityForcelist = zeros(6,N);
for i = 1:N
gravityForcelist(:,i)  = Glist(:,:,i)*[zeros(3,1);gravity];
end

% JacobianMatrix = zeros(6*N,6*N);
% for i = 1:N
%     for j = 1:N
%         JacobianMatrix(1+6*(i-1):6*i,1+6*(j-1):6*j) = JacobianSpace(Slist(:,i:j),thetalist(i:j));
%     end
% end
JacobianMatrix = JacobianSpace(Slist,thetalist);
gravityTorques = transpose(JacobianMatrix)*gravityForcelist;