% plot_theta.m

clear; close all;

[LABEL,DATA] = MBDynLoad('double_rigid_pendulum_2.jnt');

a1z = DATA(:,16,LABEL==1);
a2z = DATA(:,16,LABEL==2);

t = 1.e-3*[0:length(a1z)-1]';

theta1 = 30-a1z;
theta2 = 30-a2z;

plot(t,theta1,t,theta2,'LineWidth',1.5)
grid on;
set(gca,'FontSize',14);
xlabel('Time','FontSize',14);
ylabel('Angle (deg)','FontSize',14);
title('Joint Angles');
legend('\theta_1','\theta_2',0);