revolute joint

 clc;
 clear all;
 close all;

 m_i=1; %mass of rod i
 m_j=1; %mass of rod j
 l_i = 0.1; %length of rod i
 l_j = 1; %length of rod j
 g=9.81; %acceleraation due to gravity
 T_i=0; %external torque on rod i
 T_j=0; %external torque on rod j
 anglei= -90; %initial angle between global coordinate frame and xi_i, eta_i coordinate frame in degrees
 anglej= -87; %initial angle between global coordinate frame and xi_j, eta_j coordinate frame in degrees
 thetai=anglei*pi/180; %initial angle between global coordinate frame and xi_i, eta_i coordinate frame
 thetaj=anglej*pi/180; %initial angle between global coordinate frame and xi_j, eta_j coordinate frame
 xi_eta_i_theta= pi; %constant angular position of joint in xi_i and eta_i coordinates
 xi_eta_j_theta= pi; %constant angular position of joint in xi_j and eta_j coordinates


 xi_p_i= l_i/2*cos(xi_eta_i_theta); %x position of joint in xi_i and eta_i coordinates; constant
 eta_p_i=l_i/2*sin(xi_eta_i_theta); %y position of joint in xi_i and eta_i coordinates; constant

 xi_p_j= l_j/2*cos(xi_eta_j_theta); %x position of joint in xi_j and eta_j coordinates; constant
 eta_p_j=l_j/2*sin(xi_eta_j_theta); %y position of joint in xi_j and eta_j coordinates; constant

 %%%coordinate positon of joint p
 x_p = 0; y_p = 0;

 %Position of the local coordinate origin
 x_i = x_p - xi_p_i*cos(thetai) + eta_p_i*sin(thetai) ;
 y_i = y_p - xi_p_i*sin(thetai) - eta_p_i*cos(thetai) ; %Position of local coordinate origin of body i in global coordinates

 x_j = x_p - xi_p_j*cos(thetaj) + eta_p_j*sin(thetaj) ;
 y_j = y_p - xi_p_j*sin(thetaj) - eta_p_j*cos(thetaj); %Position of local coordinate origin of body j in global coordinates

 I_i=1/3*m_i*l_i^2; %Inertia about coordinate position
 I_j=1/3*m_j*l_j^2; %Inertia about coordinate position

 M_i=[m_i 0 0; 0 m_i 0; 0 0 I_i];
 M_j=[m_j 0 0; 0 m_j 0; 0 0 I_j];

 M=[M_i, zeros(3); zeros(3),M_j]; %Mass Matrix

 %Define Numerical Parameters
 t_lim=3; %Time limit for simulation

 n=500; %Number of discretization steps

 %Setup a time vector of length n+1 with time values from 0 to t_lim
 t(n+1)=0; %Initial array
 for k=1:n+1
 t(k)=t_lim/n*(k-1);
 end
 dt=t(2)-t(1);

 %Inintial angular position at coordinate positon
 theta_i(n+1)=0; theta_j(n+1)=0;
 theta_i(1)=thetai; theta_j(1)=thetaj;

 %Initial angular velocity at coodinate position
 omega_i(n+1)=0; omega_j(n+1)=0;
 omega_i(1)=0; omega_j(1)= 0;

 %Initial angular acceleration at coordinate position
 alpha_i(n+1)=0; alpha_j(n+1)=0;
 alpha_i(1)=0; alpha_j(1)=0;

 %Initial linear velocity at coordinate position
 vx_i(n+1)=0; vy_i(n+1)=0; vx_j(n+1)=0; vy_j(n+1)=0;
 vx_i(1)=0; vy_i(1)=0; vx_j(1)=0; vy_j(1)=0;

 %Inintial linear acceleration at coordinate positon
 ax_i(n)=0; ay_i(n)=0; ax_j(n)=0; ay_j(n)=0;

 %%%Arm coordinates for ploting graph
 x_jplot(n+1)=0; x_jplot(1)=x_p+l_j*cos(theta_j(1));
 y_jplot(n+1)=0; y_jplot(1)=y_p+l_j*sin(theta_j(1));


 x_iplot(n+1)=0; x_iplot(1)=x_p+l_i*cos(theta_i(1));
 y_iplot(n+1)=0; y_iplot(1)=y_p+l_i*sin(theta_i(1));

 D(1:2,1:6,n)=0; %Jacobian Matrix initialization
 gamma(1:2,n)=0; %Gamma Vector Initialization
 acc_lagrand(1:8,n)=0; %Acceleration vector and Langrange multipliers initialization

 for k=1:n
 %Jacobian Matrix
 D(1:2,1:6,k) =[ -1 0 xi_p_i*sin(theta_i(k))+eta_p_i*cos(theta_i(k)) 1 0 -xi_p_j*sin(theta_j(k))-eta_p_j*cos(theta_j(k));
 0 -1 -xi_p_i*cos(theta_i(k))+eta_p_i*sin(theta_i(k)) 0 1 xi_p_j*cos(theta_j(k))-eta_p_j*sin(theta_j(k))];

 %Gamma vector
 gamma(1:2,k)=-[xi_p_i*cos(theta_i(k))*(omega_i(k))-eta_p_i*sin(theta_i(k))*(omega_i(k)),-xi_p_j*cos(theta_j(k))*(omega_j(k))+eta_p_j*sin(theta_j(k))*(omega_j(k));
xi_p_i*sin(theta_i(k))*(omega_i(k))+eta_p_i*cos(theta_i(k))*(omega_i(k)), -xi_p_j*sin(theta_j(k))*(omega_j(k))-eta_p_j*cos(theta_j(k))*(omega_j(k))]*[ omega_i(k); omega_j(k)];

 %Acceleration vector and Langrange multipliers vector
 acc_lagrand(1:8,k) = inv([M, (-D(1:2,1:6,k)'); D(1:2,1:6,k), zeros(2)]) * [0; 0; T_i ; 0; -m_j*g;
-m_j*g*l_j/2*cos(theta_j(k))+T_j; gamma(1:2,k)];

 %Linear Acceleration in x and y coordinate
 ax_i(k)=acc_lagrand(1:1,k);
 ay_i(k)=acc_lagrand(2:2,k);
 ax_j(k)=acc_lagrand(4:4,k);

 ay_j(k)=acc_lagrand(5:5,k);

 %Angular acceleration
 alpha_i(k)=acc_lagrand(3:3,k);
 alpha_j(k)=acc_lagrand(6:6,k);

 %linear and angular velocity of arm i
 vx_i(k+1)= vx_i(k)+ acc_lagrand(1:1,k)*dt;
 vy_i(k+1)= vy_i(k)+acc_lagrand(2:2,k)*dt;
 omega_i(k+1)=omega_i(k)+acc_lagrand(3:3,k)*dt;
 omega_i(k+1) = 0;

 %Linear and angular velocity of arm j
 vx_j(k+1)= vx_j(k)+ acc_lagrand(4:4,k)*dt;
 vy_j(k+1)= vy_j(k)+acc_lagrand(5:5,k)*dt;
 omega_j(k+1)=omega_j(k)+acc_lagrand(6:6,k)*dt;

 %omega_i(k+1)=omega_i(1);
 theta_i(k+1)=theta_i(k)+ omega_i(k+1)*dt;
 %theta_i(k+1)=theta_i(1);
 theta_j(k+1)=theta_j(k)+ omega_j(k+1)*dt;

 %%%Arm coordinates for Animation
 x_jplot(k+1)=x_p+l_j*cos(theta_j(k+1));
 y_jplot(k+1)=y_p+l_j*sin(theta_j(k+1));

 x_iplot(k+1)=x_p+l_i*cos(theta_i(k+1));
 y_iplot(k+1)=y_p+l_i*sin(theta_i(k+1));

 plot(x_iplot, y_iplot,'r', x_jplot, y_jplot,'g')
    axis([-.1 .1 -1.5 .5]); drawnow;



 end