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

1 次查看（过去 30 天）
Andrew Quakenbush
Andrew Quakenbush 2022-12-10
回答： Maneet Kaur Bagga 2023-9-20
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
  1 个评论
Torsten
Torsten 2022-12-10
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.

请先登录，再进行评论。

请先登录，再回答此问题。

回答（1 个）

Maneet Kaur Bagga
Maneet Kaur Bagga 2023-9-20
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

类别

在 Help Center 和 File Exchange 中查找有关 Ordinary Differential Equations 的更多信息

标签

产品


版本

R2022a

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by