Why the following code is returning X as NaN from Ode45 ?

• M is a 6x6 matrix
• C is a 6x6 matrix
• G is a 6x1 matrix
• th1, th2, th3, th4,th5,th6, th1d, th2d, th3d,th4d,th5d, th6d are symbolic variables that are substituted out and replaced by my state variables.

Any help would be greatly appreciated.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function []= ControlmodelPDg(M,C,G, th1, th2, th3, th4,th5,th6, th1d, th2d, th3d,th4d,th5d, th6d)

x0= [0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1]; % Initial Condition

tf=10;

%%Solve the closed-loop system nonlinear differential equation (PlanarArmODE) via ode45
%%ode45 solves the differential equation and returns X with respect to T.
global torque
torque=[];

[T,X] = ode45(@(t,x)planarArmODE(t,x),[0 tf],x0);

torque=[];

%%Definging Functions

      function dx = planarArmODE(t,x)
          theta_d=[1.57;1.57;1.57;1.57;1.57;1.57]; % Desired Set-Point Position
          dtheta_d=[0;0;0;0;0;0]; % Desired velocity (Derivative of theta_d)
          ddtheta_d=[0;0;0;0;0;0];
          theta= x(1:6,1); 
          dtheta= x(7:12,1); %thetadots

          global Mmat Cmat Gmat
          %ie) th1= theta1 th1d= theata1 dot
          oldv=[th1,th2,th3,th4,th5,th6,th1d,th2d,th3d,th4d,th5d,th6d];
          newv=[x(1),x(2),x(3),x(4),x(5),x(6),x(7),x(8),x(9),x(10),x(11),x(12)];
          Mmat = subs(M,oldv,newv);
          disp('M Matrix')
          disp(Mmat)
          Cmat = subs(C,oldv,newv);
          disp('C Matrix')
          disp(Cmat)
          %Mmat=eye(6);
          %Cmat=eye(6);
          Gmat = subs(G,oldv,newv);

          invM = inv(Mmat);
          invMC = invM*Cmat;
          invMG = invM*Gmat;
          tau = PDControl(theta_d,dtheta_d,ddtheta_d,theta,dtheta); %Control Law inputs

          %state space rep
          torque =[torque, tau];
          dx=zeros(12,1);
          dx(1) = x(7);  %dtheta1
          dx(2) = x(8);  %dtheta2
          dx(3) = x(9);  %dtheta3
          dx(4) = x(10); %dtheta4
          dx(5) = x(11); %dtheta5
          dx(6) = x(12); %dtheta6
          dx(7:12) = -invMC*x(7:12)+invM*tau;-invMG; % because ddot theta = -M^{-1}(C \dot Theta) + M^{-1} tau
      end

   function tau = PDControl(theta_d,dtheta_d,ddtheta_d,theta,dtheta) %Control Law 
       global Gmat   
       Kp=100*eye(6);
          Kv=100*eye(6);
          e=theta_d-theta; % position error (q tilda)
          de = dtheta_d - dtheta; % velocity error
          tau = Kp*e+Kv*de+Gmat;
      end

disp('Finished PDg Control');

end

[Walter Roberson]

You probably shouldn't be using syms inside your ode function. You should probably only be using numerics there -- which might involve using odeFunction() to generate the appropriate function. See the example in odeFunction to see the workflow.

Answer 1

[Walter Roberson]

At t = 0, initial conditions, your M matrix has column 6 all zero, and also row 5 all zero. The rank of M is 5 rather than 6. You then compute Mmat from M, and take inv() of Mmat, so you are taking inv() of a singular matrix. That is giving you all inf, and the next computation step the calculations based upon those all inf values give you nan values.

You can trace the M back to about lines 185, where you will see that all of the M5* variables come out 0.