How to use each step in ode45 function to be used in the function for a array?

The issue is that I need to call each point of theta1 in the function as each step occurs, I do not know how to do this or if it is even possible. Any help would be greatly appriciated.

clc;
clear;
close all;
%% Parameters
param.m1 = 0.2;
param.m2 = 0.2;
param.L1 = 0.25;
param.L2 = 0.25;
param.g = 9.81;
param.I1 = 1/12;
param.I2 = 1/12;
t = linspace(0,20,1000);
%% Initial Values
th10 = -pi()/4;
th20 = -pi()/4;
dth10 = 0;
dth20 = 0;
ddth10 = 0;
ddth20 = 0;
X1 = param.L1*cos(th10);
Y1 = param.L1*sin(th10);
X2 = X1 + param.L2*cos(th10 + th20);
Y2 = Y1 + param.L2*sin(th10 + th20);
iC1 = [ th10 dth10];
iC2 = [ th20 dth20];
param.r1 = sqrt(X1^2+Y1^2);
param.r2 = sqrt(X2^2+Y2^2);
%% User Input
figure(1)
viscircles([0 0],param.L1 + param.L2,'color','k');
hold on
plot(0,0,'ko','color','k','LineWidth',6)
title('Click Point on Graph in Circle')
i = 0;
while i == 0
    [X,Y] = ginput(1);
    if sqrt(X^2 + Y^2) <= param.L1+param.L2
        i = 1;
    end
end
plot(X,Y,'ko','color','g')
%% Inverse Kinematics
gamma = atan2(Y,X);
beta = acos((param.L1^2+param.L2^2-X^2-Y^2)/(2*param.L1*param.L2));
alpha = acos((X^2+Y^2+param.L1^2-param.L2^2)/(2*param.L1*sqrt(X^2+Y^2)));
theta1desired = gamma - alpha;
theta2desired = pi() - beta;
ee = [theta1desired theta2desired];
%% PD Controller
options = odeset('RelTol',1e-9,'AbsTol',1e-9);
[tlist, statevarlist] = ode45(@singlelinkodefile_PD,t,iC1,options,ee,param);
theta1list = statevarlist(:,1);
[tlist, statevarlist] = ode45(@twolinkodefilePD,t,iC2,options,ee,param,theta1list);
theta2list = statevarlist(:,1);
%% Graphing
title('Double Link Pendulum');
h1A = plot([0 X1],[0 Y1],'b');
h2A = plot([X1 X2],[Y1 Y2],'b');
for i = 1:1000
    theta1 = theta1list(i);
    theta2 = theta2list(i);
    X1 = param.L1*cos(theta1);   
    Y1 = param.L1*sin(theta1);
    X2 = X1 + param.L2*cos(theta1 + theta2);
    Y2 = Y1 + param.L2*sin(theta1 + theta2);
    set(h1A,'xdata',[0 X1],'ydata',[0 Y1]);
    set(h2A,'xdata',[X1 X2],'ydata',[Y1 Y2]);
    pause(0.01)
end
%% Functions
function dstatevar = singlelinkodefile_PD(t,iC1,ee,param)
% SINGLELINKODEFILE with PD control
% unpacking the statevar vector
theta1 = iC1(1);
dtheta1 = iC1(2);
% unpacking the param variable for the physical parameters
g = param.g; m1 = param.m1; r1 = param.L1; I1 = param.I1;
% tau1 = 0; % torque = 0 means no control, its a pendulum
theta1desired = ee(1);
% solving for kp and kv
omega = 3.43;
zeta = 1;
kp = m1 * omega^2;
kv = m1 * 2 * omega * zeta;
tau1 = -kp*(theta1-theta1desired)-kv*dtheta1;
dstatevar = [dtheta1; (-g*m1*r1*sin(theta1)+tau1)/(m1*r1^2+I1)];
end
function dstatevar = twolinkodefilePD(t,iC2,ee,param,theta1list)
%Zeta and Omega
zeta = 1;
omega = 3.43;
%Unloading iCs
theta2 = iC2(1);
dtheta2 = iC2(2);
theta1 = interp1(theta1list,t);
dtheta1 = [0 diff(theta1)];
%Unloading param
g = param.g; 
m1 = param.m1; 
m2 = param.m2; 
L1 = param.L1;
L2 = param.L2; 
I1 = param.I1; 
I2 = param.I2;
r1 = param.r1;
r2 = param.r2;
%desired
theta1desired = ee(1);
theta2desired = ee(2);
% Solving for kp and kv
kp = (m1+m2)*omega^2;
kv = (m1+m2)*2*omega*zeta;
%Solving
tau1 = -kp*(theta1-theta1desired)-kv*(dtheta1);
tau2 = -kp*(theta2-theta2desired)-kv*(dtheta2);
E = zeros(2,1);
E(1) = L1*m2*r2*sin(theta2)*dtheta1^2 - tau2 + g*m2*r2*cos(theta1 + theta2);
E(2) = -L1*m2*r2*sin(theta2)*dtheta2^2-2*L1*dtheta1*m2*r2*sin(theta2)*dtheta2 - tau1 + g*m2*r2*cos(theta1 + theta2) + L1*g*m2*cos(theta1) + g*m1*r1*cos(theta1);
M = zeros(2,2);
M(1,1) = -m2*r2^2-L1*m2*cos(theta2)*r2-I2;
M(1,2) = -m2*r2^2-I2;
M(2,1) = -m2*L1^2-2*m2*cos(theta2)*L1*r2-m1*r1^2-m2*r2^2-I1-I2;
M(2,2) = -m2*r2^2-L1*m2*cos(theta2)*r2-I2;
C = M\E;
dstatevar = C;
end

[Torsten]

Why don't you solve the four ODEs simultaneously ? Then you have theta1 and dtheta1 available when needed for the calculation of theta2 and dtheta2.

Answer 1

[Maneet Kaur Bagga]

Hi Andrew,

As per my understanding, to call each point of "theta1" in the function as each step occurs, please refer to the below changes in the code:

• Inside the "twolinkodefilePD" function, add an additional input parameter "index" to keep track of the current index in "theta1list":

function dstatevar = twolinkodefilePD(t,iC2,ee,param,theta1list,index)

• Modify the line where "theta1" is calculated to use the "index":

theta1 = theta1list(index);

• Inside the main script, modify the loop where the graph is updated to pass the current index to the "twolinkodefilePD" function:

for i = 1:1000
    theta1 = theta1list(i);
    theta2 = theta2list(i);
    X1 = param.L1*cos(theta1);   
    Y1 = param.L1*sin(theta1);
    X2 = X1 + param.L2*cos(theta1 + theta2);
    Y2 = Y1 + param.L2*sin(theta1 + theta2);
    set(h1A,'xdata',[0 X1],'ydata',[0 Y1]);
    set(h2A,'xdata',[X1 X2],'ydata',[Y1 Y2]);
    pause(0.01)
    
    [tlist, statevarlist] = ode45(@twolinkodefilePD,t,iC2,options,ee,param,i);
    theta2list = statevarlist(:,1);
end

By passing the current index "i" to the "twolinkodefilePD" function, the corresponding "theta1" value can be retrieved from "theta1list".

Hope this helps!

Thank You

Maneet Bagga