Problem with calculating forward dynamics using ode45

The dynamics of a 3 link robot written has the following form:

Inverse dynamics solves for the torques given the generalized coordinates (simple linear algebra), and forward dynamics solves for the coordinates given the torques (requires solving an ODE).

In my code, I first define a smooth trajectory and obtain the torques. Then, with the torques as an input, I solve for the angles. I expect to recover the trajectory I started with, but the results are significantly different. There could be a trivial mistake but I have looked through the code many times and I am not catching it. The script and the functions used to evaluate M, C, G matrices are attached.

clear all;

n_dof = 3;
T = 1.4; N = T*500; h = T/N;

startpos = (90 - [0 0 0])'*pi/180; endpos = (90 - [38 -80 -7])'*pi/180;

n_points = N + 2;
start_point = 2;
end_point = N + 1;

[q, qdot, qddot] = generate_traj(startpos, endpos, T, h, n_points);
%% Inverse dynamics
otau = zeros(n_dof, n_points);
for t = start_point:end_point
    M = M_func(q(1,t),q(2,t),q(3,t));
    C = C_func(q(1,t),q(2,t),q(3,t),qdot(1,t),qdot(2,t),qdot(3,t));
    G = G_func(q(1,t),q(2,t),q(3,t));

    otau(:,t) = M*qddot(:,t) + C*qdot(:,t) + G;
end
%% Forward dynamics
t_span = linspace(0,T,N);
x0 = [q(:,start_point);qdot(:,start_point)]; 
[t,x] = ode45(@(t,x) robot_eq(t,x,otau(:,start_point:end_point),t_span),t_span,x0);
%% Plots
figure; hold on;
title('Joint 1');
plot(t_span, x(:,1));
plot(t_span, q(1,start_point:end_point));
title('Joint 1');
xlabel('Time (s)');
ylabel('Angle (rad)');
legend('ode45','Original','Location','best')
print('-dpng','joint1');

figure; hold on;
plot(t_span, x(:,2));
plot(t_span, q(2,start_point:end_point));
title('Joint 2');
xlabel('Time (s)');
ylabel('Angle (rad)');
legend('ode45','Original','Location','best')
print('-dpng','joint2');

figure; hold on;
plot(t_span, x(:,3));
plot(t_span, q(3,start_point:end_point));
title('Joint 3');
xlabel('Time (s)');
ylabel('Angle (rad)');
legend('ode45','Original','Location','best')
print('-dpng','joint3');





%% Robot equation
function [xdot] = robot_eq(t, x, torques, t_span)

    tau_1 = interp1(t_span,torques(1,:),t);
    tau_2 = interp1(t_span,torques(2,:),t);
    tau_3 = interp1(t_span,torques(3,:),t);
    
    tau = [tau_1; tau_2; tau_3];
    
    q_1 = x(1);
    q_2 = x(2);
    q_3 = x(3);
    qd_1 = x(4);
    qd_2 = x(5);
    qd_3 = x(6);
    
    qdot = [qd_1;qd_2;qd_3];
    
    M = M_func(q_1,q_2,q_3);
    C = C_func(q_1,q_2,q_3,qd_1,qd_2,qd_3);
    G = G_func(q_1,q_2,q_3); 
    
    qddot = M\(-C*qdot - G + tau);
    
    xdot = [qdot;qddot];

end





%% Trajectory generation
function [q,qdot,qddot] = generate_traj(startpos, endpos, T, h, n_points)

    n_dof = length(startpos);
    
    t0 = 0;
    tf = T+h;
    A_mat = [t0^3, t0^2, t0, 1;
        tf^3, tf^2, tf, 1;
        3*t0^2, 2*t0, 1, 0;
        3*tf^2, 2*tf, 1, 0];
    B_mat1 = [startpos(1);endpos(1);0;0];
    B_mat2 = [startpos(2);endpos(2);0;0];
    B_mat3 = [startpos(3);endpos(3);0;0];
    
    coeffs1 = A_mat\B_mat1;
    coeffs2 = A_mat\B_mat2;
    coeffs3 = A_mat\B_mat3;
    
    ot = zeros(n_dof, n_points);
    otdot = zeros(n_dof, n_points);
    otddot = zeros(n_dof, n_points);
    
    t_vec = linspace(0, T + 2*h, n_points);
    ot(1,:) = coeffs1(1)*t_vec.^3 + coeffs1(2)*t_vec.^2 + coeffs1(3)*t_vec + coeffs1(4);
    ot(2,:) = coeffs2(1)*t_vec.^3 + coeffs2(2)*t_vec.^2 + coeffs2(3)*t_vec + coeffs2(4);
    ot(3,:) = coeffs3(1)*t_vec.^3 + coeffs3(2)*t_vec.^2 + coeffs3(3)*t_vec + coeffs3(4);
    
    otdot(1,:) = 3*coeffs1(1)*t_vec.^2 + 2*coeffs1(2)*t_vec + coeffs1(3);
    otdot(2,:) = 3*coeffs2(1)*t_vec.^2 + 2*coeffs2(2)*t_vec + coeffs2(3);
    otdot(3,:) = 3*coeffs3(1)*t_vec.^2 + 2*coeffs3(2)*t_vec + coeffs3(3);
    
    otddot(1,:) = 6*coeffs1(1)*t_vec + 2*coeffs1(2);
    otddot(2,:) = 6*coeffs2(2)*t_vec + 2*coeffs2(2);
    otddot(3,:) = 6*coeffs3(3)*t_vec + 2*coeffs3(2);
    
    % Generalized coordinates from absolute angles.
    q = [ot(1,:);ot(2,:)-ot(1,:);ot(3,:)-ot(2,:)];
    qdot = [otdot(1,:);otdot(2,:)-otdot(1,:);otdot(3,:)-otdot(2,:)];
    qddot = [otddot(1,:);otddot(2,:)-otddot(1,:);otddot(3,:)-otddot(2,:)];

end