CODEWITHSUNDEEP
matlabodesymbolic-mathsym
FluidBat
FluidBat

Reputation: 1

Is there a way to derive implict ODE function from big symbolic expression?

I'm Davide and I have a problem with the derivation of a function that later should be given to ode15i in Matlab. Basically I've derived a big symbolic expression that describe the motion of a spececraft with a flexible appendice (like a solar panel). My goal is to obtain a function handle that can be integrated using the built-in implicit solver in Matlab (ode15i).

The problem I've encounter is the slowness of the Symbolic computations, especially in the function "daeFunction" (I've run it and lost any hope for a responce after 3/4 hours had passed).

The system of equations, that is derived using the Lagrange's method is an implicit ode. The complex nature of the system arise from the flexibility modelling of the solar panel.

I am open to any suggestions that would help me in:

Thx in advance. Here after I copy the code. Note: Matlab r2021a was used.

 close all
clear 
clc


syms t
syms r(t) [3 1] 
syms angle(t) [3 1]
syms delta(t)
syms beta(t) [3 1]
mu = 3.986e14;
mc = 1600;
mi = 10;
k = 10;
kt = 10;
Ii = [1 0 0        % for the first link it is different thus I should do a functoin or something that writes everything into an array or a vector
      0 5 0
      0 0 5];

% Dimension of satellite

a = 1; 
b = 1.3;
c = 1;

Ic = 1/12*mc* [b^2+c^2 0 0 
               0 c^2+a^2 0
               0 0 a^2+b^2];
ra_c = [0 1 0]';

a = diff(r,t,t);
ddelta = diff(delta,t);
dbeta = diff(beta,t);
dddelta = diff(delta,t,t);
ddbeta = diff(beta,t,t);
R= [cos(angle1).*cos(angle3)-cos(angle2).*sin(angle1).*sin(angle3)      sin(angle1).*cos(angle3)+cos(angle2).*cos(angle1).*sin(angle3)    sin(angle2).*sin(angle3)
        -cos(angle1).*sin(angle3)-cos(angle2).*sin(angle1).*cos(angle3)     -sin(angle1).*sin(angle3)+cos(angle2).*cos(angle1).*cos(angle3)   sin(angle2).*cos(angle3)
        sin(angle2).*sin(angle3)                                          -sin(angle2).*cos(angle1)                                       cos(angle2)];

d_angle1 = diff(angle1,t);
d_angle2 = diff(angle2,t);
d_angle3 = diff(angle3,t);
dd_angle1 = diff(angle1,t,t);
dd_angle2 = diff(angle2,t,t);
dd_angle3 = diff(angle3,t,t);
d_angle = [d_angle1;d_angle2;d_angle3];
dd_angle = [dd_angle1;dd_angle2;dd_angle3];
omega = [d_angle2.*cos(angle1)+d_angle3.*sin(angle2).*sin(angle1);d_angle2.*sin(angle1)-d_angle3.*sin(angle2).*cos(angle1);d_angle1+d_angle3.*cos(angle2)]; % this should describe correctly omega_oc
d_omega = diff(omega,t);
v1 = diff(r1,t);
v2 = diff(r2,t);
v3 = diff(r3,t);
v = [v1; v2; v3];
[J,r_cgi,R_ci]= Jacobian_Rob(4,delta,beta);

% Perform matrix multiplication

for mm = 1:4
    vel(:,mm) = J(:,:,mm)*[ddelta;dbeta];
end

vel = formula(vel);
dr_Ccgi = vel(1:3,:);
omega_ci = vel(4:6,:);
assumeAlso(angle(t),'real');
assumeAlso(d_angle(t),'real');
assumeAlso(dd_angle(t),'real');
assumeAlso(r(t),'real');
assumeAlso(a(t),'real');
assumeAlso(v(t),'real');
assumeAlso(beta(t),'real');
assumeAlso(delta(t),'real');
assumeAlso(dbeta(t),'real');
assumeAlso(ddelta(t),'real');
assumeAlso(ddbeta(t),'real');
assumeAlso(dddelta(t),'real');
omega = formula(omega);

Tc = 1/2*v'*mc*v+1/2*omega'*R*Ic*R'*omega;

% kinetic energy of all appendices

for h = 1:4
    Ti(h) = 1/2*v'*mi*v+mi*v'*skew(omega)*R*ra_c+mi*v'*skew(omega)*R*r_cgi(:,h)+mi*v'*R*dr_Ccgi(:,h)+1/2*mi*ra_c'*R'*skew(omega)'*skew(omega)*R*ra_c ...
    + mi*ra_c'*R'*skew(omega)'*skew(omega)*R*r_cgi(:,h)+mi*ra_c'*R'*skew(omega)'*R*dr_Ccgi(:,h)+1/2*omega'*R*R_ci(:,:,h)*Ii*(R*R_ci(:,:,h))'*omega ...
    + omega'*R*R_ci(:,:,h)*Ii*R_ci(:,:,h)'*omega_ci(:,h)+1/2*omega_ci(:,h)'*R_ci(:,:,h)*Ii*R_ci(:,:,h)'*omega_ci(:,h)+1/2*mi*r_cgi(:,h)'*R'*skew(omega)'*skew(omega)*R*r_cgi(:,h)+mi*r_cgi(:,h)'*R'*skew(omega)'*R*dr_Ccgi(:,h)...
    + 1/2*mi*dr_Ccgi(:,h)'*dr_Ccgi(:,h);
    Ugi(h) = -mu*mi/norm(r,2)+mu*mi*r'/(norm(r,2)^3)*(R*ra_c+R*R_ci(:,:,h)*r_cgi(:,h));
end

Ugc = -mu*mc/norm(r,2);
Ue = 1/2*kt*(delta)^2+sum(1/2*k*(beta).^2);
U = Ugc+sum(Ugi)+Ue;
L = Tc + sum(Ti) - U;
D = 1/2 *100* (ddelta^2+sum(dbeta.^2));

%% Equation of motion derivation

eq = [diff(jacobian(L,v),t)'-jacobian(L,r)';
     diff(jacobian(L,d_angle),t)'-jacobian(L,angle)';
     diff(jacobian(L,ddelta),t)'-jacobian(L,delta)'+jacobian(D,ddelta)';
     diff(jacobian(L,dbeta),t)'-jacobian(L,beta)'+jacobian(D,dbeta)'];

%% Reduction to first order sys

[sys,newVars,R1]=reduceDifferentialOrder(eq,[r(t); angle(t); delta(t); beta(t)]);

DAEs = sys;
DAEvars = newVars;


%% ode15i implicit solver   

pDAEs = symvar(DAEs);
pDAEvars = symvar(DAEvars);
extraParams = setdiff(pDAEs,pDAEvars);
f = daeFunction(DAEs,DAEvars,'File','ProvaSum');

y0est = [6778e3 0 0 0.01 0.1 0.3 0 0.12 0 0 0 7400 0 0 0 0 0 0 0 0]';
yp0est = zeros(20,1);
opt = odeset('RelTol', 10.0^(-7),'AbsTol',10.0^(-7),'Stats', 'on');
[y0,yp0] = decic(f,0,y0est,[],yp0est,[],opt);
    
% Integration  

[tSol,ySol] = ode15i(f,[0 0.5],y0,yp0,opt);


%% Funcitons 

function [J,p_cgi,R_ci]=Jacobian_Rob(N,delta,beta)
% Function to compute Jacobian see Robotics by Siciliano
% N total number of links 
% delta [1x1]  beta [N-1x1] variable that describe position of the solar
% panel elements


beta = formula(beta);

L_link = [1 1 1 1]'; % Length of each link elements in [m], later to be derived from file or as function input

for I = 1 : N
A1 = Homog_Matrix(I,delta,beta);
A1 = formula(A1);
R_ci(:,:,I) = A1(1:3,1:3);
if I ~= 1
    p_cgi(:,I) = A1(1:3,4) + A1(1:3,1:3)*[1 0 0]'*L_link(I)/2;
else 
    p_cgi(:,I) = A1(1:3,4) + A1(1:3,1:3)*[0 0 1]'*L_link(I)/2;
end

for j = 1:I
    A_j = formula(Homog_Matrix(j,delta,beta));
    z_j = A_j(1:3,3);
    Jp(:,j) = skew(z_j)*(p_cgi(:,I)-A_j(1:3,4));
    Jo(:,j) = z_j;
end
if N-I > 0
    Jp(:,I+1:N) = zeros(3,N-I);
    Jo(:,I+1:N) = zeros(3,N-I);
end
J(:,:,I)= [Jp;Jo];
end
J = formula(J);
p_cgi = formula(p_cgi);
R_ci = formula(R_ci);
end

function [A_CJ]=Homog_Matrix(J,delta,beta)
% This function is  made sopecifically for the solar panel 

% define basic rotation matrices

Rx = @(angle) [1            0           0
               0      cos(angle) -sin(angle)
               0      sin(angle)  cos(angle)];
Ry = @(angle) [ cos(angle)  0     sin(angle)
               0            1           0
               -sin(angle)  0     cos(angle)];
Rz = @(angle) [cos(angle) -sin(angle)   0
               sin(angle)  cos(angle)   0
               0            0           1];
if isa(beta,"sym")
    beta = formula(beta);
end
L_link = [1 1 1 1]'; % Length of each link elements in [m], later to be derived from file or as function input

% Rotation matrix how C sees B 
R_CB = Rz(-pi/2)*Ry(-pi/2); % Clarify notation: R_CB represent the rotation matrix that describe the frame B how it is seen by C
                            % it is the same if it was wrtitten R_B2C
                            % becouse bring a vector written in B to C
                            % frame --> p_C = R_CB p_B

% same convention used in siciliano how C sees B frame

A_AB = [R_CB    zeros(3,1)
       zeros(1,3) 1];

A_B1 = [Rz(delta) zeros(3,1)
        zeros(1,3)      1];
A_12 = [Ry(-pi/2)*Rx(-pi/2)*Rz(beta(1)) L_link(1)*[0 0 1]'
        zeros(1,3)      1];

if J == 1
    A_CJ = A_AB*A_B1;
elseif J == 0
    A_CJ = A_AB;
else 
    A_CJ = A_AB*A_B1*A_12;
end

for j = 3:J
    A_Jm1J = [Rz(beta(j-1))      L_link(j-1)*[1 0 0]'
    zeros(1,3)          1];
    A_CJ = A_CJ*A_Jm1J;
end
end

function [S]=skew(r)
S = [ 0 -r(3) r(2); r(3) 0 -r(1); -r(2) r(1) 0];
end

Upvotes: 0

Views: 99

Answers (1)

Yasien Abdelaziz Ali Shaaban A
Yasien Abdelaziz Ali Shaaban A

Reputation: 11

I found your question beautiful. My suggestion is to manipulate the problem numerically. symbolic manipulation in Matlab is good but is much slower than numerical calculation. you can define easily the ode into a system of first-order odes and solve them using numerical integration functions like ode45. Your code is very lengthy and I couldn't manage to follow its details.

All the Best. Yasien

Upvotes: -1

Related Questions