Two-Link Planar Robot Arm

I'm currently trying to create a plot that shows the position and velocity for each segment of a 2-segment planar robot arm. My initial conditions for theta and the angular velocity have the arm starting at 90 degrees from the horizontal, with the second segment of the arm slightly bent so that gravity pulls the system down to -90 degrees. I expect the arm to swing around before settling down to zero. My current plot shows theta for the first arm segment going to negative infinity and theta for the second segment going to positive infinity. Any ideas on how I can make everything have an initial oscillation and then converge to zero like I know it's supposed to? I'm trying to make the multi-colored picture look like the black-and-red picture.

%% Robot Arm
tvec=[0 30]; %range of time values for the function to use
smoothout=300; %the larger this number is, the smoother the function will look
range=linspace(tvec(1),tvec(2),smoothout);

%these next lines establish the initial conditions
theta1=pi/2; %position of theta1 (angle of the first arm segment)
dtheta1=0; %first derivative of theta1 (angular velocity of the first arm segment)
theta2=0.001; %position of theta2 (angle of second arm segment)
dtheta2=0; %first derivative of theta2 (angular velocity of second arm segment)
initialtheta=[theta1 dtheta1 theta2 dtheta2]; %vector of initial conditions for thetas

solution=ode45(@robdyn,range,initialtheta); %use MATLAB's built-in ODE solver

bravo=deval(solution,range); %interpolates the solution to the specified time values
plot(range,bravo(1,:),range,bravo(2,:),range,bravo(3,:),range,bravo(4,:),'linewidth',2);
legend('1st arm segment position','1st arm segment angular velocity','2nd arm segment position','2nd arm segment angular velocity','location','north');
xlabel('time (sec)','fontsize',14); title('Arm Position and Angular Velocity','fontsize',14);
grid on;

function dydt=robdyn(t,y) %this is the function for the robot's dynamics
    m1=5; m2=3; p1=0.5; p2=0.5; I1=2; I2=1; L1=1; %given dynamics parameters
    g=10; %gravitational acceleration constant
    B1=10; B2=5; %friction coefficients
    
    angle1=y(1); dangle1=y(2); angle2=y(3); dangle2=y(4);
    U=[0 0]'; %vector of torques applied to the arm. 
    %first entry is the torque applied to the first arm segment and the
    %second entry is the torque applied to the second arm segment
    
    H11=m1*p1^(2)+I1+m2*(L1^(2)+p2^(2)+2*L1*p2*cos(angle2))+I2;
    H12=m2*(p2^(2)+L1*p2*cos(angle2))+I2;
    H21=m2*(p2^(2)+L1*p2*cos(angle2))+I2;
    H22=m2*p2^(2)+I2;
    H=[H11 H12; H21 H22]; %invertible inertia matrix
    
    cmat=[dangle2 dangle1+dangle2; -dangle1 0];
    ccoef=-m2*L1*p2*sin(angle2);
    C=ccoef*cmat; %coriolis matrix
    
    B=[B1 0; 0 B2]; %friction coefficient matrix
    
    G1=(m1*p1+m2*L1)*g*cos(angle1)+m2*p2*g*cos(angle1+angle2);
    G2=m2*p2*g*cos(angle1+angle2);
    G=[G1 G2]'; %gravity vector
    
    danglemat=[dangle1; dangle2];
    %second derivative of theta1 = derivative of dtheta1 = angular
    %acceleration of the first arm segment
    ddtheta=H\(-C*danglemat-B*danglemat-G+U); %this is going to return a 2x1 vector
    
    %this is the output of the function
    dydt=[dangle1; dangle2; ddtheta]; %the output is 4x1. 2 rows plus concatenating ddtheta, which is 2x1
end

[darova]

The question is unclear for me. Can you explain more? Do you have any original formulas? Show the model you use (some sketch or drawing)

[Enrique Najera]

Sure, one second. I'm putting together a visual aid.

[Enrique Najera]

[darova]

Can you attach original euqations?

[Enrique Najera]

Sure, one second.

[Enrique Najera]

It's a 4-page project document, but the equations that I'm concerned with and are applied in the code are on page 2.

[Enrique Najera]

UPDATE: I finally got the shape I was looking for. MATLAB was taking a long time to run the original code that I had (shown above), so I copied and pasted everything into a brand new script file, and then I reorganized my vector of initial conditions to read [theta1 theta2 dtheta1 dtheta2] instead of [theta1 dtheta1 theta2 dtheta2] and then double-checked that I was calling the initial conditions into the funciton correctly and verified my math.

Here's my output now:

[Enrique Najera]

Now I'm going to work on creating an animation gof the arm swinging down like I showed in my drawings.

[Enrique Najera]

Well actually now that I look at the plot again, that blue line on the bottom left should be going to zero, but it's not. everything's supposed to have a decaying oscillation towards zero.

[Enrique Najera]

Here's the new code I pieced together. It's not much different than the original, but the shape of the output plot seems to be correct.

clear; clc;

smoothout=300;
trange=linspace(0,15,smoothout);

icond=[pi/2 0.001 0 0]'; %theta1 theta2 dtheta1 dtheta2

sol=ode45(@dyn,trange,icond);
bravo=deval(sol,trange);

plot(trange,bravo(1,:),trange,bravo(2,:),trange,bravo(3,:),trange,bravo(4,:))
legend('angular position of first arm segment','angular position of second arm segment','angular velocity of first arm segment','angular velocity of second arm segemnt')


function dynamo=dyn(t,y) %this is the function for the robot's dynamics
    m1=5; m2=3; p1=0.5; p2=0.5; I1=2; I2=1; L1=1; %given dynamics parameters
    g=10; %gravitational acceleration constant
    dynamo=zeros(4,1);
    
    theta1=y(1); theta2=y(2); dtheta1=y(3); dtheta2=y(4);
    U=[0 0]'; %vector of torques applied to the arm. 
    %first entry is the torque applied to the first arm segment and the
    %second entry is the torque applied to the second arm segment
    
    Hfirst=m1*p1^2 + I1 + m2*(L1^2 + p2^2 + 2*L1*p2*cos(theta2)) + I2;
    Hsecond=m2*(p2^2 + L1*p2*cos(theta2)) + I2;
    Hthird=m2*(p2^2 + L1*p2*cos(theta2)) + I2;
    Hfourth=m2*p2^2 + I2;
    H=[Hfirst Hsecond; Hthird Hfourth]; %invertible inertia matrix
    
    cmat=[dtheta2 (dtheta1 + dtheta2); -dtheta1 0];
    ccoef=-m2*L1*p2*sin(theta2);
    C=ccoef*cmat; %coriolis and centrifugal matrix
    
    B1=10; B2=5; %friction coefficients
    B=[B1 0; 0 B2]; %friction coefficient matrix
    
    Gfirst=(m1*p1 + m2*L1) * g * cos(theta1) + m2*p2 * g * cos((theta1 + theta2));
    Gsecond=m2*p2 * g * cos((theta1 + theta2));
    G=[Gfirst Gsecond]'; %gravity vector
    
    dthetamat=[dtheta1; dtheta2];
    %second derivative of theta1 = derivative of dtheta1 = angular
    %acceleration of the first arm segment
    ddtheta=H\(-C*dthetamat-B*dthetamat-G+U); %this is going to return a 2x1 vector
    
    %this is the output of the function
    dynamo(1)=dtheta1;
    dynamo(2)=dtheta2;
    dynamo(3)=ddtheta(1);
    dynamo(4)=ddtheta(2);
end

The issue now is figuring out what's happening with the blue line, which is the angular position of the first arm segment.

[darova]

You changed this line?

dydt=[dangle1; dangle2; ddtheta]; %the output is 4x1. 2 rows plus concatenating ddtheta, which is 2x1

[Enrique Najera]

Yes, I renamed it to dynamo and I just did this

%this is the output of the function
    dynamo(1)=dtheta1;
    dynamo(2)=dtheta2;
    dynamo(3)=ddtheta(1);
    dynamo(4)=ddtheta(2);

[Enrique Najera]

Instead of concatenating ddtheta into the 3rd and 4th entries of dynamo, I handpicked the entries and assigned them.