DAE-toolbox.maplet

# Course Modelling and Simulation Mechatronics System
# 
# Toolbox support for DAE
# 

with(LinearAlgebra):
tb_dae_debug := false:
# UTILS
# Extends diff to works with function as derivation argument
DIFF := proc( F, V )
  local __tmp;
  subs(__tmp=V,diff(subs(V=__tmp,F),__tmp));
end proc:
# Compute jacobian of a map F(vars)
JACOBIAN := proc( F_in, vars )
  local F, i, j, nr, nc, JAC;
  if type(F_in,algebraic) then
    nr := 1;
    F  := [F_in];
  elif type(F_in,list) then
    nr := nops(F_in);
    F  := F_in;
  elif type(F_in,Vector) then
    nr := Dimension(F_in);
    F  := F_in;
  elif type(F_in,Matrix) then
    nr, nc := Dimension(F_in);
    if nc = 1 then
      F := convert(F_in,Vector);
    else
      error "JACOBIAN first argument is a %d x %d matrix, expect column matrix", nr, nc;
    end
  else
    error "JACOBIAN expect as first argument a list or a Vector or an algebraic expression";
  end;
  nc  := nops(vars);
  JAC := Matrix(nr,nc);
  for i from 1 to nr do
    for j from 1 to nc do
      JAC[i,j] := DIFF(F[i],vars[j]);
    end;
  end;
  JAC;
end proc:
#interface(prettyprint=1):
#JACOBIAN(<x^2+y(t),y(t)*cos(x*y(t))>,[x,y(t)]); # check
;
# Kernel computation
# Given a matrix E find the kernel of its transpose by using LU decomposition.
# 
Kernel_build := proc( E )
  local n, m, r, P, L, U, k, j, all0, M, K, N;

  # get row and column dimension
  n, m := Dimension(E);
  # decompose the matrix as E = P * L * U
  P, L, U := LUDecomposition(E);
 
  if tb_dae_debug then
    print("Kernel_build: P,L,U",P,L,U);
  end;

  # the rank can be deduced from LU decomposition,
  # by counting the row of U that are NON zeros
  r := 0;
  for k from 1 to n do
    all0 := true;
    for j from 1 to m do
      if U[k,j] <> 0 then
        all0 := false;
        break;
      end;
    end;
    if all0 then
      break;
    end;
    r := k; # rank is at least k
  end;

  # compute M = L^(-1).P^T
  M := LinearSolve( L, Transpose(P) );

  if tb_dae_debug then
    print("Kernel_build: M", M);
    printf("Kernel_build: rank = %d\n",r);
  end;

  # return matrices
  if r > 0 then
    N := M[1..r,1..-1];
  else
    N := Matrix(0,m);
  end;
  if r < n then
    K := M[r+1..-1,1..-1];
  else
    K := Matrix(0,m);
  end;

  if tb_dae_debug then
    print("Kernel_build: K,N",K,N);
  end;

  return K, N, r;
end proc:
# Separate differential equations from algebraic equations
# 
# Given the DAE: E(x,t) x' = G(x,t) compute the matrix K(x,t) e L(x,t) such that
# K(x,t)E(x,t) = 0 so that K(x,t)G(x,t) = A(x,t) hidden constraint and
# L(x,t)E(x,t)x' = L(x,t)G(x,t) is the differential part.
# The routine return:
# 
# E1(x,t) = L(x,t)E(x,t)
# G1(x,t) = L(x,t)G(x,t)
# A(x,t) =  K(x,t)G(x,t)
DAE_separate_algebraic := proc( E, G )
  local K, L, r;
  K, L, r := Kernel_build( E );
  return L.E, # E1
         L.G, # G1
         K.G, # A
         r;
end proc:
# Given the DAE: F(x,x',t) = 0
# E1(x,t) = L(x,t)E(x,t)
# G1(x,t) = L(x,t)G(x,t)
# A(x,t) =  K(x,t)G(x,t)
# 
DAE_separate_algebraic_bis := proc( EQS, Dvars )
  local n, m, r, E, G, E1, G1, A;

  if not type(EQS,list) or not type(Dvars,list) then
    printf(
     "DAE_separate_algebraic_bis: first argument must be a list of equations\n"
     "second argument must be a list of differential variables\n"
    );
    error("BAD DATA");
  end;
  if nops(EQS) <> nops(Dvars) then
    printf(
     "DAE_separate_algebraic_bis: the number of equations (%d)\n"
     "must be the same of the number of variables (%d)\n",
     nops(EQS),nops(Dvars)
    );
    error("BAD DATA");
  end;

  E, G         := GenerateMatrix( EQS, Dvars ):
  E1, G1, A, r := DAE_separate_algebraic( E, G );
  return E1, G1, A, r;
end proc:
# Make DAE semi-explicit
DAE_make_semi_explicit := proc( DAE, vars )
  local i, DVARS, AVARS, ODE, Dvars;
  Dvars := diff(vars,t);
  # find differential variables and create ODE
  DVARS := [];
  AVARS := [];
  ODE   := [];
  for i from 1 to nops(Dvars) do
    if has(DAE,Dvars[i]) then
      DVARS := [op(DVARS),vars[i]];
      ODE   := [op(ODE),Dvars[i]=cat(op(0,vars[i]),__d)(t)];
      AVARS := [op(AVARS),cat(op(0,vars[i]),__d)(t)];      
    else
      AVARS := [op(AVARS),vars[i]];      
    end;
  end;
  # create ODE
  ODE, DVARS, AVARS, subs(ODE,DAE);
end proc:
# Reduce index by 1
# 
# GIven a DAE: E(x,t) x' = G(x,t) return a new DAE: E1(x,t) x' = G1(x,t).
# with index reduced by 1 and the hidden constraint A(x,t) = 0.
# If the DAE is already an ODE returned A(x,t) is empty
DAE_reduce_index_by_1_full := proc( E, G, Dvars )
  local r, E1, G1, A, DA, H, F;
  # find hidden constraint
  E1, G1, A, r := DAE_separate_algebraic( E, G );

  if tb_dae_debug then
    print("DAE_reduce_index_by_1_full: E1,G1,A",E1,G1,A);
  end;

  # Separate Algebraic and Differential part
  DA := diff( A, t );

  # E2*Dvars-G2 = DALG
  H, F := GenerateMatrix( convert(DA,list), Dvars );

  if tb_dae_debug then
    print("DAE_reduce_index_by_1_full: H,F",H,F);
  end;

  # Build the modified DAE, by substituting the algebraic equation(s)
  # with the derivative of the algebraic equation(s)
  return <E1,H>, <G1,F>, A;
end proc:
# Same routine but working directly on a list of equations
DAE_reduce_index_by_1_full2 := proc( EQS, Dvars )
  local E, G, E1, G1, ALG, DALG;
  # E*Dvars-G = EQS
  E, G := GenerateMatrix( EQS, Dvars ):

  if tb_dae_debug then
    print("DAE_reduce_index_by_1_full2: E,G",E,G);
  end;

  E1, G1, ALG := DAEreduceBy1TheIndex( E, G, Dvars ):

  if tb_dae_debug then
    print("DAE_reduce_index_by_1_full2: E1, G1, ALG",E1,G1,ALG);
  end;

  return convert(convert(E1.<Dvars>,Vector)-convert(G1,Vector),list), ALG;
end proc:
# Assume that DAE has algebraic part already separated: 

# E(x,t) x' = G(x,t)
#  0 = A(x,t)
# 
# after index reduction return

# E1(x,t) x' = G1(x,t)
#  0 = A1(x,t)
# 
# with the new algebraic part separated
DAE_reduce_index_by_1 := proc( E, G, A, Dvars )
  local n, m, n1, m1, na, r, DA, E1, G1, A1, H, F;

  # check dimensions
  n, m := Dimension(E);

  na := nops(convert(A,list));

  if na+n <> m  then
    printf(
      "DAE_reduce_index_by_1: number of row of E (%d x %d)\n"
      "plus the number of algebraic equations (%d)\n"
      "must be equal to the column of E\n", n, m, na
    );
    error("INPUT DATA ERROR\n");
  end;

  # Separate Algebraic and Differential part
  DA := diff( A, t );

  # E2*Dvars-G2 = DALG
  H, F := GenerateMatrix( convert(DA,list), Dvars );

  n1, m1 := Dimension(H);


  if n1+n <> m or m1 <> m then
    printf(
      "DAE_reduce_index_by_1: bad dimension of linear part of constraint derivative\n"
      "A' = H vars' + F, size H = %d x %d, size E = %d x %d\n",
      n1, m1, n, m
    );
    error("INPUT DATA ERROR\n");
  end;
  # find hidden constraint

  E1, G1, A1, r := DAE_separate_algebraic( <E,H>, <G,F> );

  return E1, G1, A1, r;
end proc:
# Index 3 DAE
# Special routine for index 3 DAE
# 
DAE3_get_ODE_and_invariants := proc( Mass, Phi_in, f_in, qvars, vvars, lvars )
  local tbl, n, m, f, q_D, v_D, v_dot,
        ODE_POS, ODE_VEL, tmp,
        Phi, Phi_P, Phi_t, A, A_rhs, g, bigM, bigRHS, bigVAR;

  if type(f_in,'Vector') then
    f := f_in;
  else
    f := convert(f_in,Vector);
  end;


  if type(Phi_in,'Vector') then
    Phi := Phi_in;
  else
    Phi := convert(Phi_in,Vector);
  end;

  n, m:= Dimension( Mass );
  if n <> m or n <> nops(vvars) then
    print(
      "DAE3_get_ODE_and_invariants: mass marrix must be square\n"
      "and of the same size of the length of vvars", Mass, vvars
    );
  end;

  n := nops( qvars );
  m := Dimension( Phi );

  if n <> nops(vvars) then
    print(
      "DAE3_get_ODE_and_invariants: qvars and vvars\n"
      "must have the same length", qvars, vvars
    );
  end;

  if m <> nops(lvars) then
    print(
      "DAE3_get_ODE_and_invariants: lvars must have\n"
      "the same length the number of constraints", lvars, Phi
    );
  end;

  # differential variables
  q_D := map( diff, qvars, t );
  v_D := map( diff, vvars, t );

  # definition of variable "derivative of velocities"
  v_dot := map( map( cat, map2( op, 0, vvars ), __d ), (t) );

  # ode position part q' = v
  ODE_POS := zip( (x,y)-> x = y, q_D, vvars );

  # ode velocity part v' = v__d
  ODE_VEL := zip( (x,y)-> x = y, v_D, v_dot );

  # hidden contraint/invariant A(q,v,t)
  A := subs( ODE_POS, diff(Phi,t) );

  Phi_P, A_rhs := GenerateMatrix(convert(A,list),vvars);
  # hidden invariant Phi_P v__d - g(q,v,t)
  tmp, g := GenerateMatrix(diff(convert(A,list),t),v_D);

  if tb_dae_debug then
    print("DAE3_get_ODE_and_invariants: Phi_P, tmp",Phi_P, tmp);
  end;

  # big linear system
  bigM   := <<Mass,Phi_P>|<Transpose(Phi_P),Matrix(m,m)>>;
  bigRHS := convert(<f,subs(ODE_POS,g)>,Vector);
  bigVAR := [op(v_dot),op(lvars)];
  # return the computed block
  return table([
    "m"       = m,
    "n"       = n,
    "PVARS"   = qvars,
    "VVARS"   = vvars,
    "LVARS"   = lvars,
    "VDOT"    = v_dot,
    "ODE_RHS" = [op(map(rhs,ODE_POS)),op(map(rhs,ODE_VEL))],
    "ODE_POS" = ODE_POS,
    "ODE_VEL" = ODE_VEL,
    "Phi"     = Phi,
    "Phi_P"   = Phi_P,
    "A_rhs"   = A_rhs,
    "A"       = A,
    "f"       = f,
    "g"       = subs(ODE_POS,g),
    "bigVAR"  = bigVAR,
    "bigM"    = bigM,
    "bigRHS"  = bigRHS
  ]);
end proc:
DAE3_get_ODE_and_invariants_full := proc( Mass, Phi, f, qvars, vvars, lvars )
  local tbl, i, n, m, bigETA;

  tbl := DAE3_get_ODE_and_invariants( Mass, Phi, f, qvars, vvars, lvars );

  n := tbl["n"];
  m := tbl["m"];

  bigETA := convert(tbl["bigM"].<seq(mu||i,i=1..n+m)>,Vector);

  tbl["bigETA"]  := bigETA;
  tbl["JbigETA"] := JACOBIAN(bigETA,[op(qvars),op(vvars)]);
  tbl["JbigRHS"] := JACOBIAN(tbl["bigRHS"],[op(qvars),op(vvars)]);

  # return the computed block
  return tbl;
end proc:
# 
DAE3_get_ODE_and_invariants_baumgarte := proc( Mass, Phi, f_in, qvars, vvars, lvars )
  local tbl;
  tbl                 := DAE3_get_ODE_and_invariants_full( Mass, Phi, f_in, qvars, vvars, lvars );
  tbl["h"]            := tbl["g"]-2*eta*omega*tbl["A"]-omega^2*tbl["Phi"];
  tbl["bigRHS_stab"]  := convert(<tbl["f"] ,tbl["h"] >,Vector);
  tbl["JbigRHS_stab"] := JACOBIAN(tbl["bigRHS_stab"],[op(qvars),op(vvars)]);
  return tbl;
end:
# Code Generation
F_TO_MATLAB := proc( F_in, vars, name )
  local i, F, str, LST, vals, vv, ind;
  if type(F_in,list) then
    F := F_in;
  else
    F := convert(F_in,list);
  end;
  ind := "    ";
  LST  := []:
  vals := "":
  for i from 1 to nops(F) do
    vv := simplify(F[i]);
    if evalb(vv <> 0) then
      LST  := [op(LST), convert("res__"||i,symbol) = vv ];
      vals := cat(vals,sprintf("%s  res__%s(%d) = res__%d;\n",ind,name,i,i));
    end;
  end;
  str := CodeGeneration[Matlab](LST,optimize=true,coercetypes=false,deducetypes=false,output=string);
  printf("%sfunction res__%s = %s( self, t, vars__ )\n",ind,name,name);
  printf("\n%s  %% extract states\n",ind);
  for i from 1 to nops(vars) do
    printf("%s  %s = vars__(%d);\n",ind,convert(vars[i],string),i);
  end;
  printf("\n%s  %% evaluate function\n",ind);
  printf("%s  %s\n",ind,StringTools[SubstituteAll](  str, "\n", cat("\n  ",ind) ));
  printf("\n%s  %% store on output\n",ind);
  printf("%s  res__%s = zeros(%d,1);\n",ind,name,nops(F));
  printf("%s\n%send\n",vals,ind);
end proc:
JF_TO_MATLAB := proc( JF, vars, name )
  local val, pat, NR, NC, i, j, str, ind, LST;
  NR  := LinearAlgebra[RowDimension](JF);
  NC  := LinearAlgebra[ColumnDimension](JF);
  LST := []:
  pat := "";
  ind := "    ";
  for i from 1 to NR do
    for j from 1 to NC do
      val := simplify(JF[i,j]);
      if evalb(val <> 0) then
        LST := [op(LST), convert("res__"||i||_||j,symbol) = val];
        pat := cat(pat,sprintf("%s  res__%s(%d,%d) = res__%d_%d;\n",ind,name,i,j,i,j));
      end;
    end;
  end;
  str := CodeGeneration[Matlab](LST,optimize=true,coercetypes=false,deducetypes=false,output=string);
  printf("%sfunction res__%s = %s( self, t, vars__ )\n",ind,name,name);
  printf("\n%s  %% extract states\n",ind);
  for i from 1 to nops(vars) do
    printf("%s  %s = vars__(%d);\n",ind,convert(vars[i],string),i);
  end;
  printf("\n%s  %% evaluate function\n",ind);
  printf("%s  %s\n",ind,StringTools[SubstituteAll]( str, "\n", cat("\n  ",ind) ));
  printf("%s  %% store on output\n",ind);
  printf("%s  res__%s = zeros(%d,%d);\n",ind,name,NR,NC);
  printf("%s",pat);
  printf("%send\n",ind);
end proc: