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;

% 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];

for i = 1:N
gravityForces(:,i) = Glist(:,:,i)*[zeros(3,1);gravity];
Jacoblist(:,:,i) = JacobianSpace(Slist(:,1:i),thetalist(1:i));
end

for i = N:-1:1
    gravityTorques(i) = transpose(Jacoblist(:,:,i))*gravityForces(:,i);
end