# A maple code for the dual Leipniz-Hopf(LH) algebra
# Description: This code includes functions to compute
# the orverlapping product and the conjugation in the dual
# Leipniz-Hopf algebra
# Author: S. Kaji (Yamaguchi University, Japan)
# Version history: initial version -- 2 Feb. 2014.
#                  current version -- 2 Jan. 2015.
# Global variables:
# T[]: basis for LH-algebra
# S[]: basis for dual LH-algebra
# P[]: Cartan-Serre basis for Steenrod algebra
# Q[]: Cartan-Serre basis for dual Steenrod algebra
# x[]: Milnor basis for dual Steenrod algebra

with(LinearAlgebra[Modular]);

## the dual Leibniz-Hopf algebra
# overlapping shuffle product
os := proc (f::polynom) local ff, L, t; global p;
    ff := expand(f) mod p;
    if degree(ff) < 2 then
        return ff
    elif op(0, ff) = `+` then
        return expand(map(os, ff)) mod p
    elif op(0, ff) = `*` then
        if type(op(1, ff), 'numeric') then
            return op(1, ff)*os(ff/op(1, ff)) mod p;
        elif nops(ff) = 2 and type(op(1, ff), 'indexed') and type(op(2, ff), 'indexed') then
            L := osw(op(1, op(1, ff)), op(1, op(2, ff)));
            return `mod`(add(S[l], l in L), p)
        else
            return `mod`(expand(os(os(op(1, ff))*os(mul(op(i, ff), i = 2 .. nops(ff))))), p)
        end if;
    elif op(0, ff) = `^` and type(op(1, ff), 'indexed') then
        t := floor(log[p](op(2, ff)));
        if 0 < t then
            return `mod`(os(S[p^t*op(1, op(1, ff))]*op(1, ff)^(op(2, ff)-p^t)), p)
        else L := osw(op(1, op(1, ff)), op(1, op(1, ff)));
            return os(add(S[l], l in L)*op(1, ff)^(op(2, ff)-2))
        end if;
    else
        print(ff, op(0, ff), "Error: no rule to compute the product")
    end if;
end proc;

#overlapping shuffle of two words
osw := proc (v::list, w::list) local L, i; option remember;
    if nops(w) = 0 then
        return [v]
    end if;
    if nops(v) = 0 then
        return [w]
    end if;
    L := [seq([w[1], op(x)], x in osw(v, w[2 .. -1]))];
    for i to nops(v) do
        L := [op(L), seq([op(v[1 .. i]), w[1], op(x)], x in osw(v[i+1 .. -1], w[2 .. -1]))];
        L := [op(L), seq([op(v[1 .. i-1]), v[i]+w[1], op(x)], x in osw(v[i+1 .. -1], w[2 .. -1]))]
    end do;
    return L;
end proc;

# pi image of milnor's dual basis
xi := proc (n) local i, x; global p;
    if type(n, 'integer') then
        if n = 0 then
            return 1
        else
            return S[[seq(p^(n-i), i = 1 .. n)]]
        end if
    elif type(n, 'list') then
        x := 1;
        for i to nops(n) do
            x := x*xi(i)^n[i]
        end do;
        return x
    else print("Error: xi")
    end if
end proc;

# convert a polynomial in x to one in S
x2xi := proc(f::polynom) local s,ind;
    ind := [];
    for s in indets(f) do
        if op(0,s)=`x` and type(s,'indexed') then
            ind := [op(ind),op(1,s)]
        end if;
    end do;
    return eval(f,[seq(x[i]=xi(i),i=ind)]);
end proc:

# conjugation for dual LH-algebra
chi := proc (f::polynom) global p;
    if degree(f) < 1 then
        return f
    elif type(f, 'indexed') then
        return add((-1)^nops(op(1, f))*S[x], x in coarsening(ListTools[Reverse](op(1, f))))
    elif op(0, f) = `+` then
        return `mod`(map(chi, f), p)
    elif op(0, f) = `*` then
        if type(op(1, f), 'numeric') then
            return `mod`(op(1, f)*chi(f/op(1, f)), p)
        else
            return `mod`(chi(op(1, f))*chi(mul(op(i, f), i = 2 .. nops(f))), p)
        end if
    elif op(0, f) = `^` then
        return `mod`(chi(op(1, f))^op(2, f), p)
    else
        print("cannot compute chi:", f, op(0, f))
    end if
end proc;

# conjugation for LH-algebra
chi2 := proc (f::polynom) global p;
    if degree(f) < 1 then
        return f
    elif type(f, 'indexed') then
        return add((-1)^nops(op(1, f))*T[x], x in refinement(ListTools[Reverse](op(1, f))))
    elif op(0, f) = `+` then
        return `mod`(map(chi2, f), p)
    elif op(0, f) = `*` then
        if type(op(1, f), 'numeric') then
            return `mod`(op(1, f)*chi2(f/op(1, f)), p)
        else
            return `mod`(chi2(op(1, f))*chi2(mul(op(i, f), i = 2 .. nops(f))), p)
        end if
    elif op(0, f) = `^` then
        return `mod`(chi2(op(1, f))^op(2, f), p)
    else
        print("cannot compute chi2:", f, op(0, f))
    end if
end proc;

# retraction onto the dual steenrod algebra (taking admissble terms)
retract := proc (f::polynom) local ff, i, w; global p;
    ff := expand(f);
    if 1 < degree(ff) then
        print(ff, "input must be linear")
    elif op(0, ff) = `+` then
        return map(retract, ff)
    elif type(ff, 'indexed') then
        if op(0,ff) != `S` then return ff; end if;
        w := op(1, ff);
        for i to nops(w)-1 do
            if w[i] < p*w[i+1] then
                return 0
            end if
        end do;
        return Q[w]
    elif op(0, ff) = `*` and type(op(1, ff), 'numeric') then
        return `mod`(op(1, ff)*retract(ff/op(1, ff)), p)
    else
        print("cannot compute retract:",ff, op(0, ff));
        return 0
    end if
end proc;

# projection onto the steenrod algebra (applying Adem relations)
projection := proc (f::polynom) local ff, i, w, t, j; global p;
    ff := expand(f) mod p;
    if 1 < degree(ff) then
        print(ff, "input must be linear")
    elif type(ff,'numeric') then
        return ff;
    elif op(0, ff) = `+` then
        return map(procname, ff) mod p
    elif type(ff, 'indexed') then
        if op(0,ff) != `T` and op(0,ff) != `P` then return ff; end if;
        w := op(1, ff);
        for i to nops(w)-1 do
            if w[i] < p*w[i+1] then
                t := ((-1)^(w[i]))*binomial((p-1)*w[i+1]-1,w[i])
                      *P[[op(w[1..i-1]),w[i]+w[i+1],op(w[i+2..-1])]];
                for j to floor(w[i]/p) do
                    t:=t + ((-1)^(w[i]+j))*binomial((p-1)*(w[i+1]-j)-1,w[i]-p*j)
                      *P[[op(w[1..i-1]),w[i]+w[i+1]-j,j,op(w[i+2..-1])]];
                end do;
                return procname(t mod p);
            end if
        end do;
        return P[w]
    elif op(0, ff) = `*` and type(op(1, ff), 'numeric') then
        return `mod`(op(1, ff)*procname(ff/op(1, ff)), p)
    else
        print("cannot compute projection:",ff, op(0, ff));
        return 0
    end if
end proc;

## Combinatorics on words
# ordered partition of an integer
ordpart := proc (n::integer) local L, V; option remember;
    if n = 0 then
        return []
    elif n = 1 then
        return [[1]]
    end if;
    L := ordpart(n-1);
    V[1] := [seq([1, op(v)], v in L)];
    V[2] := [seq([v[1]+1, op(v[2 .. -1])], v in L)];
    return [op(V[1]), op(V[2])]
end proc;
# ordered partition of a list
ordpartlist := proc (L::list) local V, u, i, w, pos;
    V := [];
    for u in ordpart(nops(L)) do
        w := [];
        pos := 1;
        for i in u do
            w := [op(w), L[pos .. pos+i-1]];
            pos := pos+i
        end do;
        V := [op(V), w]
    end do;
    return V
end proc;

# coarsening of a word
coarsening := proc (w::list) local L, p, i, l, v; option remember;
    L := [];
    for p in ordpart(nops(w)) do
        v := [];
        l := 1;
        for i to nops(p) do
            v := [op(v), add(w[j], j = l .. l+p[i]-1)];
            l := l+p[i]
        end do;
        L := [op(L), v]
    end do;
    return L
end proc;

# refinement of a word
refinement := proc (w::list) local L, l, v, U; option remember;
    if nops(w)=1 then
        return ordpart(w[1]);
    else
        U := [];
        L := refinement(w[2..-1]);
        for v in ordpart(w[1]) do
            for l in L do
                U := [op(U), [op(v),op(l)]];
            end do;
        end do;
        return U;
    end if;
end proc:

# list monomials of degree "totaldeg" consisting of "gens",
# whose degrees are specified by "degs"
list_monom := proc (gens::list, degs::(list(integer)), totaldeg::integer) local v, L, V;
    L := wpartition(degs, totaldeg);
    V := [];
    for v in L do
        V := [op(V), mul(gens[i]^v[i], i = 1 .. nops(v))]
    end do;
    return V
end proc;

# weighted partition; express "totaldeg" as linear combinations of "degs"
wpartition := proc (degs::(list(integer)), totaldeg::integer) local i, L, V;
    L := [];
    if totaldeg = 0 then
        return [[seq(0, i = 1 .. nops(degs))]]
    elif nops(degs) = 0 then
        return []
    elif nops(degs) = 1 then
        if `mod`(totaldeg, degs[1]) = 0 then
            return [[totaldeg/degs[1]]]
        else
            return []
        end if
    end if;
    for i from floor(totaldeg/degs[-1]) by -1 to 0 do
        V := wpartition(degs[1 .. -2], totaldeg-i*degs[-1]);
        L := [op(L), seq([op(v), i], v in V)]
    end do;
    return L
end proc;

## Enumerating basis and the conjugation invariants
# enumerate admissible sequences with lex order
admseq := proc (totaldeg::integer) local n, L; global p;
    n := floor(log[p]((p-1)*totaldeg+1));
    L := wpartition([seq((p^i-1)/(p-1), i = 1 .. n)], totaldeg);
    return map(ex2adm, L)
end proc;

# convert excess representation to admissible sequence
ex2adm := proc (ex::list) local n; global p;
    n := nops(ex);
    while ex[n] = 0 do
        n := n-1
    end do;
    return [seq(add(p^(i-j)*ex[i], i = j .. nops(ex)), j = 1 .. n)]
end proc;

# enumerate pi image of milnor monomials of degree "deg"
milnor_images := proc (deg::integer) local L, V, i, s, n; global p;
    V := [];
    n := floor(log[p]((p-1)*deg+1));
    L := list_monom([seq(x[i], i = 1 .. n)], [seq((p^i-1)/(p-1), i = 1 .. n)], deg);
    for s in L do
        V := [op(V), s = os(eval(s, [seq(x[i] = xi(i), i = 1 .. n)]))]
    end do;
    return V
end proc;

# polynomial associated to a list
pp := proc (L::list) local x, w;
    x := 0;
    for w in L do
        x := x+mul(S[u], u in w)
    end do;
    return x
end proc;

# linear equation solver
# Solves equations L in terms of indeterminants A
sol := proc (L::list, A::list) local X, V, i, j, v, n, Y,Z, rp, d, R, U, r; global p;
    n := nops(L);
    X := Matrix(n, n+nops(A));
    for i to n do
        X[i, nops(A)+i] := 1;
        for j to nops(A) do
            X[i, j] := coeff(op(2, L[i]), A[j])
        end do
    end do;
    X := Mod(p, X, integer[]);
    U := Mod(p, X, integer[]);
    Y, rp, d := RowEchelonTransform(p, U, true, true, false, false);
    r := nops(rp); U := U[1 .. n, 1 .. n];
    Z := LinearAlgebra[IdentityMatrix](n, compact = false);
    for i to LinearAlgebra[Dimension](Y) do
        LinearAlgebra[RowOperation](Z, [i, Y[i]], inplace = true)
    end do;
    Z := Mod(p, Z, integer[]);
    X := Multiply(p, Z, X);
    X := Multiply(p, U, X);
    V := [];
    for v in convert(X, listlist) do
        V := [op(V), add(v[i]*A[i], i = 1 .. nops(A)) = add(v[nops(A)+i]*op(1, L[i]), i = 1 .. nops(L))]
    end do;
    return V
end proc;

# compute conjugation invariants in degree "deg" in the dual Steenrod algebra
conjinvQ := proc (deg::integer) local L, V, U, v, w; global p;
    L := milnor_images(deg);
    V := [];
    for v in L do
        V := [op(V), op(1, v) = `mod`(chi(op(2, v))-op(2, v), p)]
    end do;
    U := [];
    for v in sol(V, [seq(S[w], w=admseq(deg))]) do
        if op(1,v)=0 then
            U:=[op(U), [op(2, v),retract(os(x2xi(op(2,v))))]];
        end if;
    end do:
    return U;
end proc;
# compute conjugation invariants in degree "deg" in the dual LH algebra
conjinvS := proc (deg::integer) local L, V, U, v, w; global p;
    L := ordpart(deg);
    V := [];
    for v in L do
        V := [op(V), S[v] = `mod`(chi(S[v])-S[v], p)]
    end do;
    U:=[];
    for v in sol(V, [seq(S[w], w=L)]) do
        if op(1, v) = 0 then
            U:=[op(U),srt(op(2, v))]
        end if;
    end do;
    return U
end proc;
# compute conjugation invariants in degree "deg" in the LH algebra
conjinvT := proc (deg::integer) local L, V, U, v, w; global p;
    L := ordpart(deg);
    V := [];
    for v in L do
        V := [op(V), T[v] = `mod`(chi2(T[v])-T[v], p)]
    end do;
    U:=[];
    for v in sol(V, [seq(T[w], w=L)]) do
        if op(1, v) = 0 then
            U:=[op(U),srt(op(2, v))]
        end if;
    end do;
    return U
end proc;
# compute conjugation invariants in degree "deg" in the Steenrod algebra
conjinvP := proc (deg::integer) local L, V, U, v; global p;
    L := admseq(deg);
    V := [];
    for v in L do
        V := [op(V), P[v] = projection(chi2(T[v]) - T[v]) mod p]
    end do;
    U:=[];
    for v in sol(V, [seq(P[w],w=L)]) do
        if op(1, v) = 0 then
            U:=[op(U),srt(op(2, v))]
        end if;
    end do;
    return U
end proc;

# sort terms in reverse lex order
srt := proc (f::polynom) local L;
    if degree(f) <> 1 then
        print("degree must be one", f);
        return f
    end if;
    L := sort([seq(ListTools[Reverse](op(1, s)), s in indets(f))]);
    L := [seq(S[ListTools[Reverse](l)], l in L)];
    return
    sort(f, L)
end proc;

# duality between S_I and T^J
# Note that S_J is conjugation invariant <=> So is T^K in non-dual LH-algebra
dual := proc (f::{polynom,list}) local J,K,k,Sym;
    if op(0, f) = `+` or type(f,'list') then
        return map(dual, f);
    elif op(0, f) = `*` then
        if type(op(1, f), 'numeric') then
            return op(1, f)*dual(f/op(1, f));
        else
            print("cannot compute dual :",f);
        end if;
    elif type(f, 'indexed') then
        if op(0,f)=`S` then
            Sym := T;
        else
            Sym := S;
        end if;
        k := 0;
        J := op(1,f);
        K := [];
        while nops(J)>0 do
            k := k+1;
            if J[1]=1 then
                J := J[2..-1];
                if nops(J)=0 then
                    K := [op(K),k];
                end if;
            else
                J[1] := J[1]-1;
                K := [op(K),k];
                k := 0;
            end if;
        end do;
        return Sym[K];
    else
            print("cannot compute dual:",f);
    end if;
end proc:


(*
 Copyright (C) 2020 Susi Lehtola

 This Source Code Form is subject to the terms of the Mozilla Public
 License, v. 2.0. If a copy of the MPL was not distributed with this
 file, You can obtain one at http://mozilla.org/MPL/2.0/.
*)

(* type: mgga_exc *)
(* prefix:
  mgga_k_csk_loc_params *params;

  assert(p->params != NULL);
  params = (mgga_k_csk_loc_params * )(p->params);
*)

$include "mgga_k_csk.mpl"

(* Equation (21) *)
csk_z  := (p, q) -> 1 + params_a_csk_cp*p + params_a_csk_cq*q - (1 + 5*p/3):


# This file is about Singer systems of endomorphisms of the additive formal
# group.  Such a system has groups G(i,j) (for 0 <= i <= j <= n say),
# all of which are equal to the standard additive formal group.  There
# are morphisms G(j,k) -> G(i,l) whenever i <= j <= k <= l, with the
# evident functoriality condition.  The horizontal maps are linear
# and the vertical maps are short isogenies.  Every subdiagram
# G(i,i) -> G(i,j) <- G(j,j) is of the form G -> G/a <- G//a.

W0V := (k) -> mul(V[i],i=0..k-1):

WW := proc(k,l,m)
  local J,P;
  J := [seq(j,j=k..l-1)];
  P := combinat[choose](J,l-k-m);
  return add(mul(V[u[t]]^(2^(k+m+t-1-u[t])),t=1..l-k-m),u in P);
end:

WWW := (i,j,k,l,m) -> WW(k,l,m) * W0V(j)^(2^(k+m-j)) / W0V(i)^(2^(l-i)):

# G(j,k) to G(i,l)
f := proc(i,j,k,l)
 local x;
 return unapply(sort(collect(expand(add(WWW(i,j,k,l,m) * x^(2^m),m=0..l-k)),x)),x):
end:

vV_rule := {seq(v[i]=V[i]/W0V(i),i=0..20)}:

Vv_rule := {seq(
 V[i] = v[i] * mul(v[j]^(2^(i-j-1)),j=0..i-1),
 i=0..20)};

check_id := proc(i::nonnegint,k::nonnegint)
 local x;

 if not (i <= k) then return FAIL; fi;

 _ASSERT(f(i,i,k,k)(x) = x,"identity maps",[i,k]);

end:

check_compose := proc(h::nonnegint,i::nonnegint,j::nonnegint,
                      k::nonnegint,l::nonnegint,m::nonnegint)
 local x,err;

 if not(h <= i and i <= j and j <= k and k <= l and l <= m) then
  return FAIL;
 fi;

 err := modp(expand(f(h,i,l,m)(f(i,j,k,l)(x)) - f(h,j,k,m)(x)),2);

 _ASSERT(err = 0,"maps compose correctly",[h,i,j,k,l]);
end:

check_linear := proc(i::nonnegint,j::nonnegint,k::nonnegint)
 local x,y,err;

 if not(i <= j and j <= k) then
  return FAIL;
 fi;

 y := f(i,j,k,k)(x);
 err := expand(y - coeff(y,x,1) * x);

 _ASSERT(err = 0,"horizontal map is linear",[i,j,k]);
end:

check_short_isogeny := proc(i::nonnegint,k::nonnegint,l::nonnegint)
 local x,y,err;

 if not(i <= k and k <= l) then
  return FAIL;
 fi;

 y := f(i,i,k,l)(x);

 _ASSERT(degree(y,x) = 2^(l-k) and coeff(y,x,2^(l-k)) = 1,
         "vertical map is a short isogeny",[i,k,l]);
end:

check_dilation := proc(i::nonnegint,p::nonnegint)
 local x,y,z,err;

 y := f(i,i,i,i+p)(x);
 z := f(i,i+p,i+p,i+p)(x);
 _ASSERT(expand(coeff(y,x,1) - coeff(z,x,1)) = 0,"dilation condition",[i,j,k]);
end:

check_vV_rules := proc(n)
 _ASSERT(
  `and`(seq(evalb(subs(vV_rule,subs(Vv_rule,V[i])) = V[i]),i=0..n)) and
  `and`(seq(evalb(subs(Vv_rule,subs(vV_rule,v[i])) = v[i]),i=0..n)),
  "rules for converting v[i] to V[i] and vice versa"
 );

end:


######################################################################

`is_element/WK` := (A::set) -> proc(TTxl)
 local TT,x,l,TT1,T,n,u,children;
 global reason;

 if not(type(TTxl,list) and nops(TTxl) = 3) then
  reason := [convert(procname,string),"TTxl cannot be split as [TT,x,l]",TTxl];
  return false;
 fi;

 TT,x,l := op(TTxl);

 if not(`is_element/trees`(A)(TT)) then
  reason := [convert(procname,string),"TT is not a tree on A",TT,A];
  return false;
 fi;

 TT1 := select(U -> nops(U) > 1,TT);

 if not(type(x,table)) then
  reason := [convert(procname,string),"x is not a table",x];
  return false;
 fi;

 if map(op,{indices(x)}) <> TT1 then
  reason := [convert(procname,string),"x is not indexed by TT1",x,TT1];
  return false;
 fi;

 children := `children_map`(A)(TT);
 for T in TT1 do
  if not(`is_element/stasheff_star`(children[T])(x[T])) then
   reason := [convert(procname,string),"x[T] has invalid type",T,x[T],reason];
   return false;
  fi;
 od;

 if not(type(l,table)) then
  reason := [convert(procname,string),"l is not a table",l];
  return false;
 fi;

 if map(op,{indices(l)}) <> TT then
  reason := [convert(procname,string),"l is not indexed by TT",l,TT];
  return false;
 fi;

 n := nops(A);

 for T in TT do
  if not(`is_element/RR`(l[T]) and l[T] >= 0 and l[T] <= 1) then
   reason := [convert(procname,string),"l[T] is not in the unit interval",T,l[T]];
   return false;
  fi;

  if nops(T) = 1 and l[T] <> 1 then
   reason := [convert(procname,string),"l[{a}] <> 1",op(T),l[T]];
   return false;
  fi;

  if nops(T) = n and l[T] <> 1 then
   reason := [convert(procname,string),"l[A] <> 1",T,l[T]];
   return false;
  fi;
 od;

 return true;
end;

######################################################################

`is_interior/WK` := (A::set) -> proc(TTxl)
 local TT,x,l,n,T;
 global reason;

 TT,x,l := op(TTxl);
 n := nops(A);

 for T in TT do
  if nops(T) > 1 and nops(T) < n and l[T] = 1 then
   reason := [convert(procname,string),"not interior, l[T] = 1",T];
   return false;
  fi;
 od:

 return true;
end;

######################################################################

`is_reduced/WK` := (A::set) -> proc(TTxl)
 local TT,x,l,T;
 global reason;

 TT,x,l := op(TTxl);

 for T in TT do
  if l[T] = 0 then
   reason := [convert(procname,string),"not reduced, l[T] = 0",T];
   return false;
  fi;
 od:

 return true;
end;

######################################################################

`reduce_once/WK` := (A::set) -> proc(TTxl,T)
 local TT,x,l,parent,children,n,TT1,x1,l1,U,B,C,X,p,y,z;

 TT,x,l := op(TTxl);

 parent := `parent_map`(A)(TT);
 children := `children_map`(A)(TT);

 TT1 := TT minus {T};

 U := parent[T];
 C := children[U];
 B := C minus {T} union children[T];

 p := table();
 for X in C minus {T} do p[X] := X; od;
 for X in children[T] do p[X] := T; od;

 y := table();
 for X in C minus {T} do y[X] := `eta/stasheff_star`({X}); od;
 y[T] := x[T];

 z := `gamma/stasheff_star`(B,C)(p)(x[U],y);

 x1 := table();
 l1 := table();

 for X in TT1 do
  l1[X] := l[X];
  if nops(X) > 1 then
   x1[X] := `if`(X = U,eval(z),eval(x[X]));
  fi;
 od;

 return([TT1,eval(x1),eval(l1)]);
end;

`reduce/WK` := (A::set) -> proc(TTxl)
 local is_reduced,X,TT,x,l,T;

 is_reduced := false;
 X := TTxl;

 while not(is_reduced) do
  TT,x,l := op(X);
  is_reduced := true;

  for T in TT do
   if l[T] = 0 then
    is_reduced := false;
    X := `reduce_once/WK`(A)(X,T);
    break;
   fi;
  od:
 od;

 return X;
end;

######################################################################

`is_equal/WK` := (A::set) -> proc(X0,X1)
 local Y0,Y1,TT0,TT1,x0,x1,l0,l1,children,T;
 global reason;

 Y0 := `reduce/WK`(A)(X0);
 Y1 := `reduce/WK`(A)(X1);
 TT0,x0,l0 := op(Y0);
 TT1,x1,l1 := op(Y1);

 if TT0 <> TT1 then
  reason := [convert(procname,string),"TT0 <> TT1",TT0,TT1];
  return false;
 fi;

 children := `children_map`(A)(TT0);

 for T in TT0 do
  if l0[T] <> l1[T] then
   reason := [convert(procname,string),"l0[T] <> l1[T]",T,l0[T],l1[T]];
   return false;
  fi;

  if nops(T) > 1 then
   if not(`is_equal/stasheff_star`(children[T])(x0[T],x1[T])) then
    reason := [convert(procname,string),"x0[T] <> x1[T]",T,x0[T],x1[T],reason];
    return false;
   fi;
  fi;
 od;

 return true;
end;

######################################################################

`is_leq/WK` := NULL;

######################################################################

`random_element/WK` := (A::set) -> proc()
 local TT,x,l,TT1,T,n,children,d;

 d := 3;
 TT := `random_element/full_trees`(A)();

 TT1 := select(U -> nops(U) > 1,TT);

 x := table();
 l := table();

 children := `children_map`(A)(TT);
 for T in TT1 do
  x[T] := `random_element/stasheff_star`(children[T])();
 od;

 n := nops(A);

 for T in TT do
  if nops(T) = 1 or nops(T) = n then
   l[T] := 1;
  else
   l[T] := rand(0..d)()/d;
  fi;
 od;

 return [TT,eval(x),eval(l)];
end;

`list_elements/WK` := NULL;
`count_elements/WK` := NULL;

######################################################################

`eta/WK` := proc(A::set)
 if nops(A) <> 1 then return FAIL; fi;
 return [[op(A)],table([])];
end;

`gamma/WK` := (A::set,B::set) -> (p) -> proc(M,N)
 local F,TT,SS,UU,x,y,z,l,m,n,pi,T,T1,C,C1,U,b,L,children;

 F := fibres(A,B)(p);

 TT,x,l := op(M);
 children := children_map(B)(TT);
 pi := table();
 for T in TT do
  pi[T] := map(op,map(b -> F[b],T));
 od;

 SS := table();
 y := table();
 m := table();

 for b in B do
  SS[b],y[b],m[b] := op(N[b]);
 od;

 UU := {seq(op(SS[b]),b in B)};
 z := table();
 n := table();

 for T in TT do
  T1 := pi[T];
  if nops(T1) > 0 then
   UU := {op(UU),T1};
   n[T1] := l[T];
   if nops(T) = 1 then
    if nops(T1) > 1 then
     z[T1] := eval(y[op(T)][T1]);
    fi;
   else
    C := children[T];
    C1 := map(U -> pi[U],C);
    z[T1] :=  `act/stasheff_star`(C,C1)(pi)(x[T]);
   fi;
  fi;
 od;

 for b in B do
  for U in SS[b] do
   n[U] := m[b][U];
   if nops(U) > 1 then
    z[U] := y[b][U];
   fi;
  od;
 od;

 L := [UU,eval(z),eval(n)];
 L := `reduce/WK`(A)(L);
 return L;
end;

######################################################################

`phi/WK/K` := (A::set) -> proc(TTxl)
 local SS,TT,x,l,TT1,s,t,u,RR,R,T,f,parent,children,J,J1,JJ,i,j,k,n,m;

 TT,x,l := op(TTxl);

 TT1 := select(U -> nops(U) > 1,TT);
 parent := `parent_map`(A)(TT);
 children := `children_map`(A)(TT);

 s := table():
 RR := table():

 for T in TT1 do
  RR[T],s[T] := op(x[T]);
 od:

 R := `phi/WK/K/aux`(A,RR);
 u := table():
 n := nops(A);
 JJ := {seq(seq({seq(R[k],k=i..j)},j=i..n),i=1..n)};

 for J in JJ do
  if member(J,TT) then
   u[J] := (1+l[J])/2;
  else
   SS := select(T -> (J minus T = {}),TT);
   m := min(op(map(nops,SS)));
   T := select(T -> (nops(T) = m),SS)[1];
   J1 := select(U -> (U intersect J <> {}),children[T]);
   if {op(map(op,J1))} = J then
    u[J] := s[T][J1]/2;
   else
    u[J] := 0;
   fi;
  fi;
 od;

 return [R,eval(u)];
end;

########################################

`phi/WK/K/aux` := proc(T,UU)
  if nops(T) = 1 then
   return [op(T)];
  else
   map(op,map(U -> `phi/WK/K/aux`(U,UU),UU[T]));
  fi;
 end;

######################################################################

`theta/K/WK` := (A::set) -> proc(Rt)
 local R,t,r,T,TT,RR,s,l,x,C,m,J,J1,i,j,k,children;

 R,t := op(Rt);

 r := table();
 for i from 1 to nops(R) do
  r[R[i]] := i;
 od;

 TT := select(J -> t[J] > 1/2,map(op,{indices(t)}));
 RR := table():
 s := table():
 l := table():
 x := table():

 children := `children_map`(A)(TT);
 for T in TT do
  l[T] := 2*t[T] - 1;
  C := [op(children[T])];
  C := sort(C,(U0,U1) -> (r[U0[1]] < r[U1[1]]));
  RR[T] := C;
  m := nops(C);
  if m > 1 then
   s[T] := table():
   for i from 1 to m do
    for j from i to m do
     J1 := {seq(C[k],k=i..j)};
     J := map(op,J1);
     s[T][J1] := min(1,2*t[J]);
    od;
   od;
   x[T] := [RR[T],eval(s[T])];
  fi;
 od:

 return [TT,eval(x),eval(l)];
end:


# Kinematik-Berechnung 3. Arm KAS5
# Beschreibung
# Berechnung der Kinematik der parallelen Struktur: Stellung des Kompensationsmechanismus in Abhängigkeit der gemessenen Winkel der Haupt-Armkinematik
# Zu Geometrie und Winkeln, siehe Bild (Unterlagen)
#
# Modell m3: Kurbel N7 ist starr mit Körper K2 gekoppelt (über die Achse)
#
# Berechne alle notwendigen Winkel in kintmp in Abhängigkeit konstanter Größen und der verallgemeinerten Koordinaten
#
# Berechne zuerst nur Sinus und Cosinus jedes Winkels. Diese werden für die Rotationsmatrizen benötigt.
# Die Berechnung der Winkel selbst (zur Kontrolle) benötigt die arctan-Funktion, die längere Ausdrücke nur sehr langsam auswertet.
# Daher werden zunächst alle Winkel mit %arctan (inert-Funktion berechnet) und am Ende erst ausgewertet.
# Die Zeitableitung der Winkel (in späteren Skripten) wird aus der inert-Funktion berechnet.
#
# Hinweise:
# Der Befehl arctan hat die Funktionalität von atan2
#
# Eingabe:
# "../codeexport/KAS5m3_definitions"
# * enthält die Definitionen aller Konstanten und verallgemeinerter Koordinaten
#
# Ausgabe:
# "../codeexport/%s/tmp/kinematic_constraints_maple_inert.m"
# * Erzeugt die Ausdrücke für Winkel der Parallelstruktur (kintmp), Ersetzungsausdrücke für sin,cos (kintmp_subsexp) und die Federlänge (lpar) in Abhängigkeit der verallgemeinerten Koordinaten.
# Quellen
# [1] Wang, Jiegao and Gosselin, ClémentM.: A New Approach for the Dynamic Analysis of Parallel Manipulators (1998)
# Autor
# Moritz Schappler, schappler@irt.uni-hannover.de, 2015-11
# Institut fuer Regelungstechnik, Leibniz Universitaet Hannover
# Initialisierung
restart:
kin_constraints_exist := true: # Für Speicherung
;
with(StringTools): # Für Zeitausgabe
with(LinearAlgebra):
with(codegen):
with(CodeGeneration):
codegen_act := true:
codegen_opt := 1: # Geringerer Optimierungsgrad. Sonst zu lange.
codegen_debug := 0: # Zur Code-Generierung auch für Nicht-Inert-Ausdrücke
;
read "../helper/proc_MatlabExport":
read "../transformation/proc_rotx":
read "../transformation/proc_roty":
read "../transformation/proc_rotz":
read "../helper/proc_convert_s_t":
read "../helper/proc_convert_t_s":
read "../robot_codegen_constraints/proc_subs_kintmp_exp":
read "../helper/proc_intersect_circle":
with(RealDomain): # Schränkt alle Funktionen auf den reellen Bereich ein. Muss nach Definition von MatlabExport kommen. Sonst geht dieses nicht.
;
read "../robot_codegen_definitions/robot_env":
read sprintf("../codeexport/%s/tmp/tree_floatb_definitions", robot_name):
# Variable mit Winkeln der Nebenstruktur nur in Abhängigkeit der verallgemeinerten Koordinaten
kintmp_qs := Matrix(RowDimension(kintmp_s),1):
# Konstante Winkel bereits hineinschreiben
for i from 1 to RowDimension(kintmp_s) do
  if diff(kintmp_s(i,1), t) = 0 then
    kintmp_qs(i,1) := kintmp_s(i,1):
  end if:
end do:
# Variablen definieren für die Hilfswinkel
gamma5_s := kintmp_s(5,1):
delta_s := kintmp_s(6..23,1):
delta_qs := kintmp_qs(6..23,1):
# Ersetzungsausdrücke definieren.
# Speichere Sinus und Cosinus der Winkel direkt ab, da diese in den Rotationsmatrizen direkt auftreten.
# Spalte 1: Zu suchender Ausdruck (sin oder cos eines Winkels)
# Spalte 2: Einzusetzender Ausdruck.
# Dadurch werden arctan-Ausdrücke in der direkten Kinematik reduziert.
# Ähnliches Vorgehen wie in [1].
kintmp_subsexp := Matrix(2*RowDimension(kintmp_s),2):
for i from 1 to RowDimension(kintmp_s) do
  kintmp_subsexp(2*i-1, 1) := sin(kintmp_s(i,1)):
  kintmp_subsexp(2*i,   1) := cos(kintmp_s(i,1)):
  # Initialisierung der rechten Spalte mit gleichen Werten. Später nur Ersetzung, wenn Vorteilhaft.
  kintmp_subsexp(2*i-1, 2) := kintmp_subsexp(2*i-1, 1):
  kintmp_subsexp(2*i,   2) := kintmp_subsexp(2*i,   1):
end do:
# Annahmen über Konstanten: Längen sind alle Positiv.
# (Wird anscheinend für arctan gebraucht). Ansonsten steht csgn in exportiertem Code.
assume(l1>0):
assume(l2>0):
assume(l3>0):
assume(l4>0):
assume(l5>0):
assume(l6>0):
assume(l7>0):
assume(l8>0):
assume(l9>0):
assume(l10>0):
assume(l11>0):
assume(l12>0):
assume(l13>0):
assume(l14>0):
assume(l15>0):
assume(l16>0):
assume(l17>0):
assume(l18>0):
assume(l19>0):
assume(l20>0):
assume(l21>0):
assume(l22>0):
# Schalter mit Optionen für die Code-Generierung.
# Wähle, ob trigonometrische arctan-Ausdrücke ausgewertet werden, oder ob Sinus- und Cosinussatz angewendet werden sollen.
# Dies hat großen Einfluss auf die Rechendauer.
# Die Optimierungen sind so gewählt, dass
# * Kreisschnittpunktsfunktionen nicht mit den komplizierten Eingabedaten aufgerufen, sondern mit allgemeinen Platzhaltern, die dann ersetzt werden,
# * Keine Zwischenoptimierungen durchgeführt werden. Durch simplify und combine werden die symbolischen Ausdrücke größer, was später die Rechenzeit enorm erhöht
# * Sinus und Cosinus der Parallelwinkel ersetzt werden und kein sin(arctan(...)) ausgerechnet wird.
Schalter_Opt := <1;1;0;0;0;0;0;1>:
printf("Beginn der Berechnungen. %s\n", FormatTime("%Y-%m-%d %H:%M:%S")):
st := time():
# Reihenfolge, in der die Winkel der Parallelstruktur berechnet werden. Hilft beim exportieren von Code, wenn beim Debuggen in der gleichen Reihenfolge ausgegeben wird.
Reihenfolge_kintmp := <12; 5; 21; 4; 9; 16; 3; 8; 24; 11; 2; 1; 6; 7; 10; 13; 14; 15; 17; 18; 19; 20; 22; 23>:
# Berechne delta7
# Gl. (30)
delta_qs(7) := kintmp_qs(22) + convert_t_s(theta(4)):
kintmp_qs(12) := delta_qs(7):
# Berechne gamma5
# Gl.(2)
gamma5_qs := - kintmp_qs(12) + Pi + convert_t_s(theta(5)):
kintmp_qs(5) := gamma5_qs:
kintmp_subsexp(9,2) := sin(gamma5_qs):
kintmp_subsexp(10,2) := cos(gamma5_qs):
# Berechne delta16
# Kreisschnittpunkt berechnen
r_4_G4_G5 := <l11;0;0>:
r_12_G5_B := <l14;0;0>:
R_4_12 := rotz(Pi+delta_qs(7)):
r_4_G5_B := R_4_12 . r_12_G5_B:
save r_4_G5_B, "../codeexport/KAS5m3_kinematic_constraints_r_4_G5_B_debug_maple":
R_3_4 := rotz(convert_t_s(theta(4))):
R_4_3 := Transpose(R_3_4):
# Vektor D-G4 (ausgedrückt in KS 3)
r_3_D_G4 := <l5-l6;0;0>:
r_4_G4_D := R_4_3 . (-r_3_D_G4):
# Vektor G4-G5 (ausgedrückt in KS 4)
r_4_G4_G5 := <l11;0;0>:
r_4_G5_D := -r_4_G4_G5 + r_4_G4_D:
save r_4_G5_D, "../codeexport/KAS5m3_kinematic_constraints_r_4_G5_D_noopt_debug_maple":
if Schalter_Opt(7) = 1 then
  r_4_G5_D := simplify(r_4_G5_D, trig):
  r_4_G5_D := simplify(r_4_G5_D):
  r_4_G5_D := combine(r_4_G5_D, trig):
end if:
save r_4_G5_D, "../codeexport/KAS5m3_kinematic_constraints_r_4_G5_D_opt_debug_maple":
if Schalter_Opt(8) = 1 then
  # Rufe die Kreisschnittpunkt-Funktion mit allgemeinen Werten auf und ersetze das Ergebnis.
  subs_ic := <r_4_G5_B_Allg_x; r_4_G5_B_Allg_y; r_4_G5_D_Allg_x; r_4_G5_D_Allg_y>:
  subs_ic_KAS := <r_4_G5_B(1..2,1); r_4_G5_D(1..2,1)>:
  ReturnValue_Allg := intersect_circle(<r_4_G5_B_Allg_x; r_4_G5_B_Allg_y>, <r_4_G5_D_Allg_x; r_4_G5_D_Allg_y>, l13, l22):
  ReturnValue := ReturnValue_Allg:
  for i from 1 to 4 do
    ReturnValue := subs({subs_ic(i) = subs_ic_KAS(i)}, ReturnValue):
  end do:
else
  ReturnValue := intersect_circle(r_4_G5_B, r_4_G5_D, l13, l22):
end if:
printf("Erster Kreisschnittpunkt berechnet. %s. CPU-Zeit bis hier: %1.2fs.\n", FormatTime("%Y-%m-%d %H:%M:%S"), time()-st):
r_4_G5_C := <ReturnValue(1..2, 2); 0>: # Alternative 2
;
save r_4_G5_C, "../codeexport/KAS5m3_kinematic_constraints_r_4_G5_C_debug_noopt_maple":
if Schalter_Opt(4) = 1 then
  r_4_G5_C := simplify(r_4_G5_C, trig):
  r_4_G5_C := simplify(r_4_G5_C):
  r_4_G5_C := combine(r_4_G5_C, trig):
end if:
printf("CPU-Zeit bis hier: %1.2fs\n", time()-st):
save r_4_G5_C, "../codeexport/KAS5m3_kinematic_constraints_r_4_G5_C_debug_opt_maple":
r_4_D_C := - r_4_G5_D + r_4_G5_C:
r_3_D_C := R_3_4 . r_4_D_C:
save r_3_D_C, "../codeexport/KAS5m3_kinematic_constraints_r_3_D_C_debug_noopt_maple":
if Schalter_Opt(6) = 1 then
  r_3_D_C := simplify(r_3_D_C, trig):
  r_3_D_C := simplify(r_3_D_C):
  r_3_D_C := combine(r_3_D_C, trig):
end if:
save r_3_D_C, "../codeexport/KAS5m3_kinematic_constraints_r_3_D_C_debug_opt_maple":
# Gl. (m3.5)
delta_qs(16) := %arctan(-r_3_D_C(2), r_3_D_C(1)):
sin_delta16_qs := -r_3_D_C(2)/l22:
cos_delta16_qs := r_3_D_C(1)/l22:
kintmp_qs(21) := delta_qs(16):
kintmp_subsexp(41,2) := sin_delta16_qs:
kintmp_subsexp(42,2) := cos_delta16_qs:
# Berechne gamma3
# Berechnungen für Hebel E-F-G3
# Zweiter Kreismittelpunkt für Schnittpunkt-Algorithmus
r_3_G3_D := <l6;0;0>:
r_3_D_G3 := -r_3_G3_D:
r_10_D_E := rotz(delta_qs(9)) . <l21;0;0>:
# Stelle Rotationsmatrix mit Zeitabhängigen Winkel delta16 auf (ohne Ersetzung mit delta16(q), da dort arctan vorkommt.
# Ersetze sin(delta16), cos(delta16) mit Ausdrücken von oben. Bei arctan(sin(...)) entstehen kompliziertere Ausdrücke
if Schalter_Opt(1) = 1 then
  R_3_10_s := rotz(- delta_s(16)):
  R_3_10 := subs_kintmp_exp(R_3_10_s):
else
  R_3_10 := rotz(- delta_qs(16)):
end if:
r_3_D_E := R_3_10 . r_10_D_E:
save r_3_D_E, sprintf("../codeexport/%s/tmp/kinematic_constraints_r_3_D_E_noopt_debug_maple", robot_name):
if Schalter_Opt(5) = 1 then
  r_3_D_E := simplify(r_3_D_E, trig):
  r_3_D_E := simplify(r_3_D_E):
  r_3_D_E := combine(r_3_D_E, trig):
end if:
save r_3_D_E, sprintf("../codeexport/%s/tmp/kinematic_constraints_r_3_D_E_opt_debug_maple", robot_name):
if Schalter_Opt(2) = 1 then
  # Rufe die Kreisschnittpunkt-Funktion mit allgemeinen Werten auf und ersetze das Ergebnis.
  # dies führt zu einer wesentlichen Verringerungen der Rechenzeit, da das Einsetzen und Weiterrechnen der komplizierten Ausdrücke in der Unterfunktion entfällt.
  subs_ic := <r_3_D_E_Allg_x; r_3_D_E_Allg_y; r_3_D_G3_Allg_x; r_3_D_G3_Allg_y>:
  subs_ic_KAS := <r_3_D_E(1..2,1); r_3_D_G3(1..2,1)>:
  ReturnValue_Allg := intersect_circle(<r_3_D_E_Allg_x; r_3_D_E_Allg_y>, <r_3_D_G3_Allg_x; r_3_D_G3_Allg_y>, l20, l4):
  ReturnValue := ReturnValue_Allg:
  for i from 1 to 4 do
    ReturnValue := subs({subs_ic(i) = subs_ic_KAS(i)}, ReturnValue):
  end do:
else
  ReturnValue := intersect_circle(r_3_D_E, r_3_D_G3, l20, l4):
end if:
printf("Zweiter Kreisschnittpunkt berechnet. %s. CPU-Zeit bis hier: %1.2fs.\n", FormatTime("%Y-%m-%d %H:%M:%S"), time()-st):
r_3_D_F := <ReturnValue(1..2, 2); 0>: # Alternative 2
;
save r_3_D_F, sprintf("../codeexport/%s/tmp/kinematic_constraints_r_3_D_F_noopt_debug_maple", robot_name):
if Schalter_Opt(3) = 1 then
  r_3_D_F := simplify(r_3_D_F, trig):
  r_3_D_F := simplify(r_3_D_F):
  r_3_D_F := combine(r_3_D_F, trig):
end if:
r_3_F_G3 := -r_3_D_F + r_3_D_G3:
printf("CPU-Zeit bis hier: %1.2fs\n", time()-st):
save r_3_D_F, sprintf("../codeexport/%s/tmp/kinematic_constraints_r_3_D_F_opt_debug_maple", robot_name):
save ReturnValue, r_3_D_F, r_3_F_G3, sprintf("../codeexport/%s/tmp/kinematic_constraints_gamma3_qs_debug_maple", robot_name):
# Gl. (m3.6)
gamma3_qs := %arctan(r_3_F_G3(2,1), -r_3_F_G3(1,1)):
sin_gamma3_qs := r_3_F_G3(2,1) / l4:
cos_gamma3_qs := (-r_3_F_G3(1,1)) / l4:
kintmp_qs(4) := gamma3_qs:
kintmp_subsexp(7,2) := sin_gamma3_qs:
kintmp_subsexp(8,2) := cos_gamma3_qs:
save ReturnValue, r_3_D_F, r_3_F_G3, gamma3_qs, sprintf("../codeexport/%s/tmp/kinematic_constraints_gamma3_qs_debug_maple", robot_name):
save kintmp_qs, kintmp_subsexp, sprintf("../codeexport/%s/tmp/kinematic_constraints_maple_debug_nach_gamma3.m", robot_name):
printf("Winkel gamma3 berechnet. %s. CPU-Zeit bis hier: %1.2fs.\n", FormatTime("%Y-%m-%d %H:%M:%S"), time()-st):
# Berechne delta4
# Gl. (22)
delta_qs(4) := Pi/2 - delta_qs(16) - delta_qs(12):
kintmp_qs(9) := delta_qs(4):
# Berechne delta11
# Gl. (3)
delta_qs(11) := delta_qs(10) - delta_qs(4):
kintmp_qs(16) := delta_qs(11):
# Berechne beta2
# Gl. (m3.7)
# Siehe auch: KAS5m3_sym_codegen_kinematic_constraints_hilfsberechnungen.mw
d7b2 := %arctan((sin(delta_qs(7)) * l14 + r_4_G5_C(2)), (cos(delta_qs(7)) * l14 + r_4_G5_C(1))):
beta2_qs := d7b2 - delta_qs(7):
kintmp_qs(3) := beta2_qs:
# Nutze Additionstheoreme zur Bestimmung von Sinus und Cosinus.
cos_d7b2 := (cos(delta_qs(7)) * l14 + r_4_G5_C(1)) / l13:
sin_d7b2 := (sin(delta_qs(7)) * l14 + r_4_G5_C(2)) / l13:
sin_beta2_qs := sin_d7b2*cos(delta_qs(7)) - cos_d7b2*sin(delta_qs(7)):
cos_beta2_qs := cos_d7b2*cos(delta_qs(7)) + sin_d7b2*sin(delta_qs(7)):
kintmp_subsexp(5,2) := sin_beta2_qs:
kintmp_subsexp(6,2) := cos_beta2_qs:
save kintmp_qs, kintmp_subsexp, sprintf("../codeexport/%s/tmp/kinematic_constraints_maple_debug_nach_beta2.m", robot_name):
# Berechne delta3
# Gl. (m3.9)
# Siehe auch: KAS5m3_sym_codegen_kinematic_constraints_hilfsberechnungen.mw
r_4_D_B := -r_4_G5_D + r_4_G5_B:
t7 := convert_t_s(theta(4)) + delta_qs(16):
# Nutze Additionstheoreme zur Bestimmung von Sinus und Cosinus.
# Dadurch kann Sinus/Cosinus von delta16 direkt genutzt werden und es tritt kein arctan auf.
cos_t7 := cos(convert_t_s(theta(4)))*cos_delta16_qs - sin(convert_t_s(theta(4)))*sin_delta16_qs:
sin_t7 := sin(convert_t_s(theta(4)))*cos_delta16_qs + cos(convert_t_s(theta(4)))*sin_delta16_qs:
# Hilfswinkel t8
t8 := %arctan((sin_t7 * l22 + r_4_D_B(2)), (cos_t7 * l22 - r_4_D_B(1))):
sin_t8 := (sin_t7 * l22 + r_4_D_B(2))/l13:
cos_t8 := (cos_t7 * l22 - r_4_D_B(1))/l13:
delta_qs(3) := t8 - t7:
cos_delta3_qs := (cos_t8)*(cos_t7) + (sin_t8)*(sin_t7):
sin_delta3_qs := (sin_t8)*(cos_t7) - (cos_t8)*(sin_t7):
kintmp_qs(8) := delta_qs(3):
kintmp_subsexp(15,2) := sin_delta3_qs:
kintmp_subsexp(16,2) := cos_delta3_qs:
save kintmp_qs, kintmp_subsexp, sprintf("../codeexport/%s/tmp/kinematic_constraints_maple_debug_nach_delta3.m", robot_name):
# Berechne rho3
# Gl. (11)
rho3_qs := gamma3_qs + delta_qs(8) - Pi/2:
# Nutze Additionstheoreme zur Bestimmung von Sinus und Cosinus.
# Dadurch kann Sinus/Cosinus von gamma3 direkt genutzt werden und es tritt kein arctan auf.
sin_rho3_qs := (sin_gamma3_qs)*(cos(delta_qs(8) - Pi/2)) + (cos_gamma3_qs)*(sin(delta_qs(8) - Pi/2)):
cos_rho3_qs := (cos_gamma3_qs)*(cos(delta_qs(8) - Pi/2)) - (sin_gamma3_qs)*(sin(delta_qs(8) - Pi/2)):
kintmp_qs(24) := rho3_qs:
kintmp_subsexp(47,2) := sin_rho3_qs:
kintmp_subsexp(48,2) := cos_rho3_qs:
save kintmp_qs, kintmp_subsexp, sprintf("../codeexport/%s/tmp/kinematic_constraints_maple_debug_nach_rho3.m", robot_name):
printf("Winkel rho3 berechnet. %s. CPU-Zeit bis hier: %1.2fs.\n", FormatTime("%Y-%m-%d %H:%M:%S"), time()-st):
# Berechne delta6
# Gl. (m3.8)
# Siehe auch: KAS5m3_sym_codegen_kinematic_constraints_hilfsberechnungen.mw
r_3_G3_E := r_3_G3_D + r_3_D_E:
t1 := -rho3_qs + delta_qs(8):
t2 := %arctan((-sin_t1 * l4 + r_3_G3_E(1)), (cos_t1 * l4 + r_3_G3_E(2))):
# Nutze Additionstheoreme zur Bestimmung von Sinus und Cosinus.
cos_t1 :=  cos_rho3_qs*cos(delta_qs(8)) + sin_rho3_qs*sin(delta_qs(8)):
sin_t1 := -sin_rho3_qs*cos(delta_qs(8)) + cos_rho3_qs*sin(delta_qs(8)):
delta_qs(6) := t1 + t2:
kintmp_qs(11) := delta_qs(6):
# Nutze Additionstheoreme zur Bestimmung von Sinus und Cosinus.
cos_t2 := ( cos_t1 * l4 + r_3_G3_E(2)) / l20:
sin_t2 := (-sin_t1 * l4 + r_3_G3_E(1)) / l20:
cos_delta6_qs := cos_t2*cos_t1 - sin_t2*sin_t1:
sin_delta6_qs := sin_t2*cos_t1 + cos_t2*sin_t1:
kintmp_subsexp(21,2) := sin_delta6_qs:
kintmp_subsexp(22,2) := cos_delta6_qs:
save kintmp_qs, kintmp_subsexp, sprintf("../codeexport/%s/tmp/kinematic_constraints_maple_debug_nach_delta6.m", robot_name):
# Kompensationsfeder
R_5_6:=rotz(convert_t_s(theta(6))):
r_6_A_H := <l17; 0; 0>:
r_5_A_H:=R_5_6 . r_6_A_H:
r_5_G5_G6:=<l12;0;0>:
r_6_G6_H:=<0; -l18; 0>:
r_5_G6_H:=R_5_6 . r_6_G6_H:
r_5_G5_H:= r_5_G5_G6 + r_5_G6_H: # Weg G5-G6-H
;
R_12_5_s:=rotz(gamma5_s): # Rotationsmatrix mit allgemeinem zeitabhängigem Ausdruck für den Winkel gamma5
;
R_12_5 := subs_kintmp_exp(R_12_5_s): # sinus und cosinus von gamma5 direkt ersetzen, ohne den Winkel einzusetzen.
;
R_5_12:=Transpose(R_12_5):
r_12_B_G5:=<-l14;0;0>:
r_5_G5_B:=R_5_12 . (-r_12_B_G5):
r_5_AB_2 := r_5_A_H - r_5_G5_H + r_5_G5_B: # Weg AB aus A-H-G5-B
;
l16_c := sqrt(r_5_AB_2[1,1]^2 + r_5_AB_2[2,1]^2): # Länge AB entspricht Federlänge
;
lpar_qs := l16_c:
printf("Kompensationsfederlänge berechnet. %s.  CPU-Zeit bis hier: %1.2fs.\n", FormatTime("%Y-%m-%d %H:%M:%S"), time()-st):
# Berechne beta1
# Gl. (29)
# Siehe auch: KAS5_m4_sym_codegen_kinematic_constraints_hilfsberechnungen.mw
t2 := cos(convert_t_s(theta(6))):
t1 := sin(convert_t_s(theta(6))):
t3 := %arctan(-(t2 * l18 - sin(gamma5_qs) * l14 + t1 * l17), (t2 * l17 + cos(gamma5_qs) * l14 - t1 * l18 - l12)):
beta1_qs := t3 - gamma5_qs:
cos_t3 := (t2 * l17 + cos(gamma5_qs) * l14 - t1 * l18 - l12) / l16_c:
sin_t3 := (-(t2 * l18 - sin(gamma5_qs) * l14 + t1 * l17)) / l16_c:
kintmp_qs(2) := beta1_qs:
# Nutze Additionstheoreme zur Bestimmung von Sinus und Cosinus.
# Zu erledigen: In den Sinus/Cosinus-Ausdrücken kommt l16 vor. Muss noch generiert werden.
cos_beta1_qs := cos_t3*cos(gamma5_qs) + sin_t3*sin(gamma5_qs):
sin_beta1_qs := sin_t3*cos(gamma5_qs) - cos_t3*sin(gamma5_qs):
kintmp_subsexp(3,2) := sin_beta1_qs:
kintmp_subsexp(4,2) := cos_beta1_qs:
# Exportiere Code für folgende Skripte
# Entferne Annahmen aus Variablen
l1 := 'l1':
l2 := 'l2':
l3 := 'l3':
l4 := 'l4':
l5 := 'l5':
l6 := 'l6':
l7 := 'l7':
l8 := 'l8':
l9 := 'l9':
l10 := 'l10':
l11 := 'l11':
l12 := 'l12':
l13 := 'l13':
l14 := 'l14':
l15 := 'l15':
l16 := 'l16':
l17 := 'l17':
l18 := 'l18':
l19 := 'l19':
l20 := 'l20':
l21 := 'l21':
l22 := 'l22':
# Die Annahmen sind im Ausdruck bereits in den Variablen gespeichert. Ersetze das "~"-Zeichen.
# Quelle: http://www.mapleprimes.com/questions/207601-Remove-Assumptions
nms:=convert(indets(kintmp_qs,name),list): # Liste der Symbole
;
nmsS:=convert~(nms,string): # Liste der Symbole als String
;
L:=StringTools:-Substitute~(nmsS,"~",""): #Removing "~"
;
L1:=parse~(L):
S:=nms=~L1:
kintmp_qs:=subs(S,kintmp_qs):
lpar_qs:=subs(S,lpar_qs):
kintmp_subsexp:=subs(S,kintmp_subsexp):
kintmp_qt := convert_s_t(kintmp_qs):
lpar_qt := convert_s_t(lpar_qs):
# Speichere Maple-Ausdruck (Eingabe-Format und internes Format)
save kintmp_subsexp, sprintf("../codeexport/%s/tmp/kinematic_constraints_kintmp_subsexp_maple", robot_name):
save kintmp_subsexp, sprintf("../codeexport/%s/tmp/kinematic_constraints_kintmp_subsexp_maple.m", robot_name):
printf("Ausdrücke für kintmp_subsexp gespeichert (Maple). %s. CPU-Zeit bis hier: %1.2fs.\n", FormatTime("%Y-%m-%d %H:%M:%S"), time()-st):
for i from 1 to RowDimension(kintmp_s) do
  tmp := kintmp_qs(i):
  save tmp, sprintf("../codeexport/%s/tmp/kinematic_constraints_maple_inert_kintmpq_%d", robot_name, i):
  save tmp, sprintf("../codeexport/%s/tmp/kinematic_constraints_maple_inert_kintmpq_%d.m", robot_name, i):
end do:
save kin_constraints_exist, kintmp_qs, kintmp_qt, lpar_qs, lpar_qt, kintmp_subsexp, sprintf("../codeexport/%s/tmp/kinematic_constraints_maple_inert", robot_name):
save kin_constraints_exist, kintmp_qs, kintmp_qt, lpar_qs, lpar_qt, kintmp_subsexp, sprintf("../codeexport/%s/tmp/kinematic_constraints_maple_inert.m", robot_name):
save kintmp_qs, sprintf("../codeexport/%s/tmp/kinematic_constraints_kintmp_qs_maple_inert", robot_name):
# Exportieren des vollständigen Ausdruckes
if codegen_act then
  MatlabExport(kintmp_subsexp, sprintf("../codeexport/KAS5_kinematik_parallel_kintmp_subsexp_matlab_noopt.m"), 0):
  MatlabExport(kintmp_subsexp, sprintf("../codeexport/KAS5_kinematik_parallel_kintmp_subsexp_matlab_opt.m"), 1):
end if:
printf("Ausdrücke mit Inert-Arctan exportiert (Matlab). %s. CPU-Zeit bis hier: %1.2fs.\n", FormatTime("%Y-%m-%d %H:%M:%S"), time()-st):
# Liste mit abhängigen konstanten Kinematikparametern erstellen (wichtig für Matlab-Funktionsgenerierung)
read "../helper/proc_list_constant_expressions";
kc_symbols := Matrix(list_constant_expressions( kintmp_subsexp(..,2) )):
save kc_symbols, sprintf("../codeexport/%s/tmp/kinematic_constraints_symbols_list_maple", robot_name):
MatlabExport(Transpose(kc_symbols), sprintf("../codeexport/%s/tmp/kinematic_constraints_symbols_list_matlab.m", robot_name), 2):
printf("Fertig. %s. CPU-Zeit bis hier: %1.2fs.\n", FormatTime("%Y-%m-%d %H:%M:%S"), time()-st):


#@ Not autoload

MU_n_max := 10;

make_fgl_Lazard := proc(d)
 global a,m,ma,am;
 local e,T,u,x,y,q,i,j,k,Fm,Fmp,err,sol;

 T := table();
 T["degree"] := d;
 Order := d+2;

 T["log_m"] := unapply(x + add(m[i]*x^(i+1),i=1..d-1),x);
 T["exp_m"] :=
   unapply(
   convert(
    solve(x = series(T["log_m"](y) + sin(y)^(d+1),y=0,d+1),y),
    polynom,x
   ),
   x
  );

 Fm := x+y;
 for k from 2 to d do
  Fmp[0] := 1;
  Fmp[1] := subs({x=e*x,y=e*y},Fm);
  for j from 2 to k do
   Fmp[j] := rem(expand(Fmp[1] * Fmp[j-1]),e^(k+1),e);
  od;
  err := expand(Fmp[1] - e*x - e*y +
   add(m[i]*(Fmp[i+1] - e^(i+1)*(x^(i+1)+y^(i+1))),i=1..k-1));
  Fm := Fm - subs(e=1,err);
 od:

 T["sum_m"] := unapply(Fm,x,y):

 am[1] := m[1];
 for k from 2 to d-1 do
  q := igcd_alt([seq(binomial(k+1,i),i=1..k)])[2];
  am[k] := -add(q[i]*coeff(coeff(Fm,x,i),y,k+1-i),i=1..k);
 od:
 sol := solve({seq(a[i]=am[i],i=1..d-1)},{seq(m[i],i=1..d-1)}):
 for i from 1 to d-1 do ma[i] := subs(sol,m[i]); od:

 T["log_a"] := unapply(collect(expand(eval(subs(m = ma,T["log_m"](x)))),x),x);
 T["exp_a"] := unapply(collect(expand(eval(subs(m = ma,T["exp_m"](x)))),x),x);
 T["sum_a"] := unapply(collect(expand(eval(subs(m = ma,T["sum_m"](x,y)))),{x,y}),x,y);

 return eval(T);
end:

MU_vars := plex(seq(a[i],i=1..MU_n_max));
MUMU_vars := plex(seq(a[i],i=1..MU_n_max),seq(b[i],i=1..MU_n_max));


# Inverse Dynamics for Parallel Robot Platform
# Einleitung
# Berechnung der inversen Dynamik der Roboter-Plattform
#
# Dateiname:
# robot -> Berechnung für allgemeinen Roboter
# para -> Berechnung für eine parallelen Roboter
# plattform -> Bezogen auf Plattform der PKM
# rotmat -> Kinematik wird mit Rotationsmatrizen berechnet
# dynamics -> Berechnung der Dynamik
# Autor
# Tim Job (Studienarbeit bei Moritz Schappler), 2018-12
# Moritz Schappler, moritz.schappler@imes.uni-hannover.de
# (C) Institut für Mechatronische Systeme, Universität Hannover
# Sources
# [Abdellatif2007] Modellierung, Identifikation und robuste Regelung von Robotern mit parallelkinematischen Strukturen
# Initialization
interface(warnlevel=0): # Unterdrücke die folgende Warnung.
restart: # Gibt eine Warnung, wenn über Terminal-Maple mit read gestartet wird.
interface(warnlevel=3):
with(LinearAlgebra):
with(codegen):
with(CodeGeneration):
with(StringTools):
with(VectorCalculus):
# Einstellungen für Code-Export: Optimierungsgrad (2=höchster) und Aktivierung jedes Terms.
codegen_dynpar := 1:
codegen_opt := 2:
codeexport_invdyn := false:
codeexport_grav := false:
codeexport_corvec := false:
codeexport_inertia := false:
read "../helper/proc_MatlabExport":
read "../helper/proc_vec2skew":
read "../robot_codegen_definitions/robot_env_par":
read sprintf("../codeexport/%s/tmp/tree_floatb_definitions", leg_name):
# Definitionen für parallelen Roboter laden
read "../robot_codegen_definitions/robot_env_par":
read sprintf("../codeexport/%s/tmp/para_definitions", robot_name):
r_P_sP_P := -r_P_sP:
s_P_P_sP := s_P_sP:

# Lade "robotics_repo_path"-File mit Link zum "imes-robotics-matlab"-Repo
read("../robotics_repo_path"):
# Lade die Funktionen aus dem "imes-robotics-matlab"-Repo
read(sprintf("%s/transformation/maple/proc_eul%s2r", robotics_repo_path, angleConv)):
read(sprintf("%s/transformation/maple/proc_eul%sjac", robotics_repo_path, angleConv)):

# Additional Kinematics
# Berechnung der Rotationsmatrizen

R_0_0_E_t := parse(sprintf("eul%s2r",angleConv))(xE_t(4..6)):
R_0_0_E_s := parse(sprintf("eul%s2r",angleConv))(xE_s(4..6)):
RPYjac_0_t := parse(sprintf("eul%sjac",angleConv))(xE_t(4..6)):
RPYjac_0_s := parse(sprintf("eul%sjac",angleConv))(xE_s(4..6)):
r_0_sP_P := R_0_0_E_s.r_P_sP_P:
if codegen_dynpar = 1 then
  J_P_P := J_SP + mE*Multiply(Transpose(vec2skew(r_P_sP_P)),vec2skew(r_P_sP_P)):
else
  J_P_P := J_P_P:
end if:
J_0_P := R_0_0_E_s.J_P_P.Transpose(R_0_0_E_s):

r_0_sP_P := R_0_0_E_t.r_P_sP_P:
rD_0_sP_P := diff~(r_0_sP_P,t):
s_0_P_sP := R_0_0_E_t.s_P_P_sP:
sD_0_P_sP := diff~(s_0_P_sP,t):
for i to 3 do
   for j to 6 do
      rD_0_sP_P(i) := subs({xED_t(j)=xED_s(j),xE_t(j)=xE_s(j)},rD_0_sP_P(i)):
      sD_0_P_sP(i) := subs({xED_t(j)=xED_s(j),xE_t(j)=xE_s(j)},sD_0_P_sP(i)):
   end do:
end do:
s_0_P_sP := R_0_0_E_s.s_P_P_sP:
r_0_sP_P := R_0_0_E_s.r_P_sP_P:

# Berechnung der H-Matrix und deren Ableitung nach Abdellatif2007 S.20
#
RPYjac_E_t := simplify(Multiply(Transpose(R_0_0_E_t),RPYjac_0_t)):
RPYjac_E_s := simplify(Multiply(Transpose(R_0_0_E_s),RPYjac_0_s)):
w_E_0_E_s := RPYjac_E_s.xED_s(4..6,1):
w_0_0_E_s := RPYjac_0_s.xED_s(4..6,1):
w_E_0_E_t := RPYjac_E_t.xED_t(4..6,1):
dRPYjac_E_t := diff~(RPYjac_E_t,t):
dRPYjac_0_t := diff~(RPYjac_0_t,t):
dRPYjac_E_s := Copy(dRPYjac_E_t):
dRPYjac_0_s := Copy(dRPYjac_0_t):
# Substituiere die zeitabhängigen Koordinaten in der H-Matrix mit zeitunabhängigen Koordinaten
for i to 3 do
  for j to 3 do
    for k from 4 to 6 do
      dRPYjac_E_s(i,j) := subs({xEDD_t(k)=xEDD_s(k),xED_t(k)=xED_s(k),xE_t(k)=xE_s(k)},dRPYjac_E_s(i,j)):
      dRPYjac_0_s(i,j) := subs({xEDD_t(k)=xEDD_s(k),xED_t(k)=xED_s(k),xE_t(k)=xE_s(k)},dRPYjac_0_s(i,j)):
    end do:
  end do:
end do:

wD_E_0_E_s := simplify(dRPYjac_E_s.xED_s(4..6,1)+RPYjac_E_s.xEDD_s(4..6,1)):
wD_0_0_E_s := simplify(dRPYjac_0_s.xED_s(4..6,1)+RPYjac_0_s.xEDD_s(4..6,1)):

# Abdellatif2007 S.20 (2.19)

H := <IdentityMatrix(3,3),ZeroMatrix(3);
      ZeroMatrix(3),RPYjac_0_s>:
Hinv := MatrixInverse(H):
dH := <ZeroMatrix(3),ZeroMatrix(3);
      ZeroMatrix(3),dRPYjac_0_s>:

# Gravitational-Vector gE of the platform
# Berechnung des Gravitationsvektors
# Abdellatif2007 S.34 (3.20)
xAll := <x;y;z;an;bn;cn>:
if codegen_dynpar = 1 then
  gE_z := xE_s(1..3,1) - r_0_sP_P:
else
  gE_z := mE*xE_s(1..3,1) + s_0_P_sP:
end if:
for j to 3 do
  for i from 1 to 6 do
    gE_z(j) := subs(xE_s(i)=xAll(i),gE_z(j)):
  end do:
end do:
dgEdz_final := Matrix(6,1):
dgEdz := Matrix(3,1):

for j to 6 do
  for i to 3 do
     dgEdz(i) := diff(gE_z(i),xAll(j)):
  end do:
  if codegen_dynpar = 1 then
    dgEdz_final(j) := mE*Transpose(g_world(..,1)).dgEdz(..,1):
  else
    dgEdz_final(j) := Transpose(g_world(..,1)).dgEdz(..,1):
  end if:
end do:
for j to 6 do
  for i from 1 to 6 do
    dgEdz_final(j) := subs(xAll(i)=xE_s(i),dgEdz_final(j)):
  end do:
end do:
gE := simplify(Transpose(Hinv).dgEdz_final):

if codeexport_grav then
  MatlabExport(gE, sprintf("../codeexport/%s/tmp/gravload_platform_eul%s_matlab.m", robot_name, angleConv), codegen_opt):
end if:

# Mass-Matrix ME of the platform
# Berechnung Massenmatrix für die EE-Plattform und die Winkelgeschwindigkeit
# Abdellatif2007 S.39 (3.30)

if codegen_dynpar = 1 then
  ME := simplify(<mE*Matrix(3,shape=identity),mE*vec2skew(r_0_sP_P);
         mE*Transpose(vec2skew(r_0_sP_P)),J_0_P>):
else
  ME := simplify(<mE*Matrix(3,shape=identity),vec2skew(-s_0_P_sP);
         Transpose(vec2skew(-s_0_P_sP)),J_0_P>):
end if:

if codeexport_inertia then
  MatlabExport(ME, sprintf("../codeexport/%s/tmp/inertia_platform_eul%s_matlab.m", robot_name, angleConv), codegen_opt):
end if:

# Coriolis-Vector cE of the platform
# Coriolis-Matrix
# Abdellatif2007 S.39 (3.31)

if codegen_dynpar = 1 then
  cE := simplify(<ZeroMatrix(3,3),mE*vec2skew(rD_0_sP_P);
         ZeroMatrix(3,3),Multiply(vec2skew(w_0_0_E_s),J_0_P)>):
else
  cE := simplify(<ZeroMatrix(3,3),vec2skew(-sD_0_P_sP);
         ZeroMatrix(3,3),Multiply(vec2skew(w_0_0_E_s),J_0_P)>):
end if:

if codeexport_corvec then
  # Der Term ist nicht der Coriolis-Vektor, sondern eine Matrix, die dazu führt. Daher kein Export.
  # MatlabExport(cE, sprintf("../codeexport/%s/tmp/coriolisvec_platform_eul%s_matlab.m", robot_name, angleConv), codegen_opt):
end if:

# Torque at the platform
# Inverse Dynamik der Plattform
# Abdellatif2007 S.39 (3.29)

MME := ME.H:
cvecE := Multiply(ME.dH+cE.H,xED_s):
tauE := Multiply(MME,xEDD_s) + cvecE - gE:
# Maple-Export
save MME, cvecE , gE, tauE, H, dH, sprintf("../codeexport/%s/tmp/floatb_platform_dynamic_maple.m", robot_name):
# Matlab Code Export
if codeexport_invdyn then
  MatlabExport(tauE, sprintf("../codeexport/%s/tmp/invdyn_platform_eul%s_matlab.m", robot_name, angleConv), codegen_opt):
end if:
#


# Inverse Dynamics for Robot based on MDH frames
# Einleitung
# Berechnung der inversen Dynamik
#
# Dateiname:
# robot -> Berechnung für allgemeinen Roboter
# tree -> Berechnung für eine beliebige Baumstruktur (ohne Schleifen)
# floatb -> floating base wird durch base twist (Geschwindigkeit der Basis) oder vollständige Orientierung (Euler-Winkel) berücksichtigt
# rotmat -> Kinematik wird mit Rotationsmatrizen berechnet
# dynamics -> Berechnung der Dynamik
# worldframe -> Berechnung basierend auf Energien aus Welt-KS (KS W)
# par12 -> Parametersatz 1 (Schwerpunkt als Parameter: SX,SY,SZ) oder Parametersatz 2 (1. und 2. Moment MX,MY,MZ,...)
# Autor
# Moritz Schappler, schappler@irt.uni-hannover.de, 2016-03
# (C) Institut fuer Regelungstechnik, Leibniz Universitaet Hannover
# Sources
# [GautierKhalil1990] Direct Calculation of Minimum Set of Inertial Parameters of Serial Robots
# [KhalilDombre2002] Modeling, Identification and Control of Robots
# [Ortmaier2014] Vorlesungsskript Robotik I
# Initialization
interface(warnlevel=0): # Unterdrücke die folgende Warnung.
restart: # Gibt eine Warnung, wenn über Terminal-Maple mit read gestartet wird.
interface(warnlevel=3):
with(LinearAlgebra):
with(ArrayTools):
with(codegen):
with(CodeGeneration):
with(StringTools):
# Einstellungen für Code-Export: Optimierungsgrad (2=höchster) und Aktivierung jedes Terms.
codegen_act := true:
codegen_debug := false:
codegen_opt := 2:
codeexport_grav := true:
codeexport_corvec := true:
codeexport_cormat := true:
codeexport_inertia := true:
codeexport_inertiaD := true:
codeexport_invdyn := true:
read "../helper/proc_convert_s_t":
read "../helper/proc_convert_t_s":
read "../helper/proc_MatlabExport":
read "../helper/proc_simplify2":
read "../helper/proc_index_symmat2vector":
read "../helper/proc_symmat2vector":
read "../transformation/proc_rotx":
read "../transformation/proc_roty":
read "../transformation/proc_rotz":
read "../transformation/proc_trotx":
read "../transformation/proc_troty":
read "../transformation/proc_trotz":
read "../transformation/proc_transl":
read "../transformation/proc_trafo_mdh":
read "../robot_codegen_definitions/robot_env":
read sprintf("../codeexport/%s/tmp/tree_floatb_definitions", robot_name):
read sprintf("../codeexport/%s/tmp/kinematic_constraints_maple_inert.m", robot_name):
kin_constraints_exist := kin_constraints_exist: # nur zum Abschätzen der Komplexität
;
# Einstellungen für Term-Vereinfachungen: 0=keine, 1=Massenmatrix, 2=Coriolis-Matrix, 3=Massenmatrix-Zeitableitung, Coriolis-Vektor, Invers-Dynamik
# Motivation: Massenmatrix ist Voraussetzung für andere symbolische Berechnungen (lohnt sich damit mehr)
# Coriolis-Matrix lässt sich besser vereinfachen, da viele Einträge getrennt voneinander betrachtet werden.
# Coriolis-Vektor bzw. Invers-Dynamik am kompliziertesten.
if not assigned(simplify_options) or simplify_options(9)=-1 then # Standard-Einstellungen:
  if not kin_constraints_exist then # normale serielle Ketten und Baumstrukturen
    use_simplify := 0: # Standardmäßig aus
  else # mit kinematischen Zwangsbedingungen
    # pauschale Annahme für kinematische Zwangsbedingungen.
    use_simplify := 1: # nur Massenmatrix
  end if:
else # Benutzer-Einstellungen:
 use_simplify := simplify_options(9): # neunter Eintrag ist für Dynamik
end if:

# Kennung des Parametersatzes, für den die Dynamikfunktionen erstellt werden sollen. Muss im Repo und in der mpl-Datei auf 1 gelassen werden, da die folgende Zeile mit einem Skript verarbeitet wird.
codegen_dynpar := 1:
# Ergebnisse des Lagrange-Formalismus laden
read sprintf("../codeexport/%s/tmp/floatb_%s_lagrange_dUdq_s_par%d_maple.m", robot_name, base_method_name, codegen_dynpar):
dUdq_s := dUdq_s:
read sprintf("../codeexport/%s/tmp/floatb_%s_lagrange_dTdq_s_par%d_maple.m", robot_name, base_method_name, codegen_dynpar):
dTdq_s := dTdq_s:
read sprintf("../codeexport/%s/tmp/floatb_%s_lagrange_dTdqDdt_s_par%d_maple.m", robot_name, base_method_name, codegen_dynpar):
dTdqDdt_s := dTdqDdt_s:

DynString := "Term:":
if codeexport_grav then
  DynString := sprintf("%s g",DynString):
end if:
if codeexport_corvec then
  DynString := sprintf("%s c",DynString):
end if:
if codeexport_cormat then
  DynString := sprintf("%s C",DynString):
end if:
if codeexport_inertia then
  DynString := sprintf("%s M",DynString):
end if:
if codeexport_inertiaD then
  DynString := sprintf("%s MD",DynString):
end if:
if codeexport_invdyn then
  DynString := sprintf("%s tau",DynString):
end if:
printf("%s. Generiere Dynamik (%s) für %s mit Parametersatz %d und %s\n", \
  FormatTime("%Y-%m-%d %H:%M:%S"), DynString, robot_name, codegen_dynpar, base_method_name):

# Gravitational Load
# Generate
taug_s := dUdq_s:
save taug_s, sprintf("../codeexport/%s/tmp/gravload_par%d_maple.m", robot_name, codegen_dynpar):
# Matlab Export
# Belastung der Basis (ist falsch für floatb_twist, da das Moment durch diese Wahl der verallgemeinerten Koordinaten (floatb_twist) nicht berechnet werden kann!)
# Ist korrekt für floatb_eulxyz.
# Die Berechnungen werden deshalb nicht für floatb_twist durchgeführt.
if codegen_act and codeexport_grav and not(base_method_name="twist") then
  MatlabExport(taug_s(1..6), sprintf("../codeexport/%s/tmp/gravload_base_floatb_%s_par%d_matlab.m", robot_name, base_method_name, codegen_dynpar), codegen_opt):
end if:
# Belastung der Gelenke
if codegen_act and codeexport_grav then
  MatlabExport(taug_s(7..NQ), sprintf("../codeexport/%s/tmp/gravload_joint_floatb_%s_par%d_matlab.m", robot_name, base_method_name, codegen_dynpar), codegen_opt):
end if:
# Kompletter Vektor
if codegen_act and codeexport_grav and not(base_method_name="twist") then
  MatlabExport(taug_s(1..NQ), sprintf("../codeexport/%s/tmp/gravload_floatb_%s_par%d_matlab.m", robot_name, base_method_name, codegen_dynpar), codegen_opt):
end if:
# Mass Matrix
# Generate
tauMM_s := dTdqDdt_s:
MM_s := Matrix(NQ, NQ):

if codeexport_inertia then
  for i to NQ do
    for j to NQ do
    	 if i > j then # linke untere Dreiecksmatrix
    	   MM_s[i, j] := MM_s[j,i]: # nehme symmetrischen Teil (wurde schon berechnet)
    	   next:
    	 end if:
      MM_s[i, j] := diff(tauMM_s[i, 1], qDD_s[j, 1]):
    end do:
  end do:
  # Terme vereinfachen
  if use_simplify>=1 then
    tmp_t0:=time():
    tmp_l0 := length(MM_s):
    printf("%s: Beginne Elementweise Vereinfachung Massenmatrix (Param. %d). Länge: %d.\n", \
      FormatTime("%Y-%m-%d %H:%M:%S"), codegen_dynpar, tmp_l0):
    for i to NQ do
      for j to NQ do
    	   if i > j then # linke untere Dreiecksmatrix
    	     MM_s[i, j] := MM_s[j,i]: # nehme symmetrischen Teil (wurde schon vereinfacht)
    	     next:
    	   end if:
        tmp_t1:=time():
        tmp_l1 := length(MM_s[i,j]):
        printf("%s: Beginne Vereinfachung Massenmatrix (Param. %d; Elem. %d,%d). Länge: %d.\n", \
          FormatTime("%Y-%m-%d %H:%M:%S"), codegen_dynpar, i, j, tmp_l1):
        MM_s[i,j] := simplify2(MM_s[i,j]):
        tmp_t2:=time():
        tmp_l2 := length(MM_s[i,j]):
        printf("%s: Massenmatrix (Param. %d; Elem. %d,%d) vereinfacht. Länge: %d->%d. Rechenzeit %1.1fs.\n", \
          FormatTime("%Y-%m-%d %H:%M:%S"), codegen_dynpar, i, j, tmp_l1, tmp_l2, tmp_t2-tmp_t1):
      end do:
    end do:
    tmp_l2 := length(MM_s):
    printf("%s: Massenmatrix (Param. %d) vereinfacht. Länge: %d->%d. Rechenzeit %1.1fs.\n", \
      FormatTime("%Y-%m-%d %H:%M:%S"), codegen_dynpar, tmp_l0, tmp_l2, tmp_t2-tmp_t0):
  end if:
  save MM_s, sprintf("../codeexport/%s/tmp/inertia_par%d_maple.m", robot_name, codegen_dynpar):
end if:
# Matlab Export (nur linke untere Dreiecksmatrix bei vollständigen Matrizen)
if codeexport_inertia and not(base_method_name="twist") then
  MM_s_vek := symmat2vec(MM_s):
  filename_tmp := sprintf("../codeexport/%s/tmp/inertia_floatb_%s_par%d_maple_symvec.m", robot_name, base_method_name, codegen_dynpar):
  save MM_s_vek, filename_tmp:
  read filename_tmp:
  if codegen_act then
    MatlabExport(MM_s_vek, sprintf("../codeexport/%s/tmp/inertia_floatb_%s_par%d_matlab.m", robot_name, base_method_name, codegen_dynpar), codegen_opt):
  end if:
end if:
if codegen_act and codeexport_inertia and not(base_method_name="twist") then
  MatlabExport(MM_s(7..NQ,1..6), sprintf("../codeexport/%s/tmp/inertia_joint_base_floatb_%s_par%d_matlab.m", robot_name, base_method_name, codegen_dynpar), codegen_opt):
end if:
# Gelenk-Massenmatrix.
# Eliminiere die Basis-Orientierung. Hat keinen Einfluss auf die Matrix, wird aber noch Maple teilweise nicht automatisch entfernt, da die Ausdrücke zu kompliziert zum Optimieren sind.
MMjj_s := MM_s(7..NQ,7..NQ):
for i from 1 to NQB do
  MMjj_s := subs({X_base_s[i,1]=0},MMjj_s):
end do:
if codeexport_inertia then
  MMjj_s_vek := symmat2vec(MMjj_s):
  filename_tmp := sprintf("../codeexport/%s/tmp/inertia_joint_joint_floatb_%s_par%d_maple_symvec.m", robot_name, base_method_name, codegen_dynpar):
  save MMjj_s_vek, filename_tmp:
  read filename_tmp:
  if codegen_act then
    MatlabExport(MMjj_s_vek, sprintf("../codeexport/%s/tmp/inertia_joint_joint_floatb_%s_par%d_matlab.m", robot_name, base_method_name, codegen_dynpar), codegen_opt):
  end if:
end if:
# Basis-Massenmatrix
MMbb_s := MM_s(1..6,1..6):
if codeexport_inertia and not(base_method_name="twist") then
  MMbb_s_vek := symmat2vec(MMbb_s):
  filename_tmp := sprintf("../codeexport/%s/tmp/inertia_base_base_floatb_%s_par%d_maple_symvec.m", robot_name, base_method_name, codegen_dynpar):
  save MMbb_s_vek, filename_tmp:
  read filename_tmp:
  if codegen_act then
    MatlabExport(MMbb_s_vek, sprintf("../codeexport/%s/tmp/inertia_base_base_floatb_%s_par%d_matlab.m", robot_name, base_method_name, codegen_dynpar), codegen_opt):
  end if:
end if:

# Mass Matrix Time Derivative
# Konvertiere Massenmatrix in zeitabhängige Variablen, um Zeitableitung zu berechnen

if codeexport_inertiaD then
  read sprintf("../codeexport/%s/tmp/inertia_par%d_maple.m", robot_name, codegen_dynpar):
  MM_s := MM_s: # Annahme: Massenmatrix wurde vorher generiert.
  MM_t := convert_s_t(MM_s):
  MMD_t := diff~(MM_t, t):
  MMD_s := convert_t_s(MMD_t):
  # Terme vereinfachen
  # Terme vereinfachen
  if use_simplify>=3 then
    tmp_t0:=time():
    tmp_l0 := length(MMD_s):
    printf("%s: Beginne Elementweise Vereinfachung Massenmatrix-Zeitableitung (Param. %d). Länge: %d.\n", \
      FormatTime("%Y-%m-%d %H:%M:%S"), codegen_dynpar, tmp_l0):
    for i to NQ do
      for j to NQ do
    	   if i > j then # linke untere Dreiecksmatrix
    	     MMD_s[i, j] := MMD_s[j,i]: # nehme symmetrischen Teil (wurde schon vereinfacht)
    	     next:
    	   end if:
        tmp_t1:=time():
        tmp_l1 := length(MMD_s[i,j]):
        printf("%s: Beginne Vereinfachung Massenmatrix-Zeitableitung (Param. %d; Elem. %d,%d). Länge: %d.\n", \
          FormatTime("%Y-%m-%d %H:%M:%S"), codegen_dynpar, i, j, tmp_l1):
        MMD_s[i,j] := simplify2(MMD_s[i,j]):
        tmp_t2:=time():
        tmp_l2 := length(MMD_s[i,j]):
        printf("%s: Massenmatrix-Zeitableitung (Param. %d; Elem. %d,%d) vereinfacht. Länge: %d->%d. Rechenzeit %1.1fs.\n", \
          FormatTime("%Y-%m-%d %H:%M:%S"), codegen_dynpar, i, j, tmp_l1, tmp_l2, tmp_t2-tmp_t1):
      end do:
    end do:
    tmp_l2 := length(MMD_s):
    printf("%s: Massenmatrix-Zeitableitung (Param. %d) vereinfacht. Länge: %d->%d. Rechenzeit %1.1fs.\n", \
      FormatTime("%Y-%m-%d %H:%M:%S"), codegen_dynpar, tmp_l0, tmp_l2, tmp_t2-tmp_t0):
  end if:
end if:
# Matlab Export
if codeexport_inertiaD and not(base_method_name="twist") then
  MMD_s_vek := symmat2vec(MMD_s):
  filename_tmp := sprintf("../codeexport/%s/tmp/inertia_time_derivative_floatb_%s_par%d_maple_symvec.m", \
    robot_name, base_method_name, codegen_dynpar):
  save MMD_s_vek, filename_tmp:
  read filename_tmp:
  if codegen_act then
    MatlabExport(MMD_s_vek, sprintf("../codeexport/%s/tmp/inertia_time_derivative_floatb_%s_par%d_matlab.m", \
      robot_name, base_method_name, codegen_dynpar), codegen_opt):
  end if:
end if:

if codegen_act and codeexport_inertiaD and not(base_method_name="twist") then
  MatlabExport(MMD_s[7..NQ,1..6], sprintf("../codeexport/%s/tmp/inertia_joint_base_time_derivative_floatb_%s_par%d_matlab.m", robot_name, base_method_name, codegen_dynpar), codegen_opt):
end if:
MMDjj_s := MMD_s(7..NQ,7..NQ):
for i from 1 to NQB do
  MMDjj_s := subs({X_base_s[i,1]=0},MMDjj_s):
end do:
for i from 1 to 6 do
  MMDjj_s := subs({V_base_s[i,1]=0},MMDjj_s):
end do:
if codeexport_inertiaD then
  # Vektor der unteren linken Dreiecksmatrix generieren, speichern und wieder laden. Ohne Neuladen hängt sich maple durch einen Bug auf. TODO: Klären.
  MMDjj_s_vek := symmat2vec(MMDjj_s):
  filename_tmp := sprintf("../codeexport/%s/tmp/inertia_joint_joint_time_derivative_floatb_%s_par%d_maple_symvec.m", robot_name, base_method_name, codegen_dynpar):
  save MMDjj_s_vek, filename_tmp:
  read filename_tmp:
  if codegen_act then
    MatlabExport(MMDjj_s_vek, sprintf("../codeexport/%s/tmp/inertia_joint_joint_time_derivative_floatb_%s_par%d_matlab.m", robot_name, base_method_name, codegen_dynpar), codegen_opt):
  end if:
end if:

# Coriolis Vector
# Generiere Coriolis-Vektor aus dem Lagrange-Momenten
tauCC_s := Matrix(NQ,1): # Platzhalter
;

if codeexport_corvec then
  tauCC_s := dTdqDdt_s-dTdq_s:
  for i to NQ do
    tauCC_s := subs({qDD_s(i, 1) = 0}, tauCC_s):
  end do:
  # Terme vereinfachen
  if use_simplify>=3 then
    for i to NQ do
      tmp_t1:=time():
      tmp_l1 := length(tauCC_s[i,1]):
      printf("%s: Beginne Vereinfachung Coriolis-Vektor (Param. %d; Elem. %d). Länge: %d.\n", \
        FormatTime("%Y-%m-%d %H:%M:%S"), codegen_dynpar, i, tmp_l1):
      tauCC_s[i,1] := simplify2(tauCC_s[i,1]):
      tmp_t2:=time():
      tmp_l2 := length(tauCC_s[i,1]):
      printf("%s: Coriolis-Vektor (Param %d; Elem. %d) vereinfacht. Länge: %d->%d. Rechenzeit %1.1fs.\n", \
        FormatTime("%Y-%m-%d %H:%M:%S"), codegen_dynpar, i, tmp_l1, tmp_l2, tmp_t2-tmp_t1):
   end do:
  end if:
  save tauCC_s, sprintf("../codeexport/%s/tmp/coriolisvec_par%d_maple.m", robot_name, codegen_dynpar):
end if:

# Einzelne Komponenten exportieren (falls der ganze Vektor zu lange dauert).
if codegen_act and codeexport_corvec and not(base_method_name="twist") and codegen_debug then
  for i to NQ do
    MatlabExport(tauCC_s(i), sprintf("../codeexport/%s/tmp/coriolisvec_floatb_%s_%d_par%d_matlab.m", robot_name, base_method_name, i, codegen_dynpar), codegen_opt):
  end do:
end if:
# Matlab Export: Floating base
if codegen_act and codeexport_corvec and not(base_method_name="twist") then
  MatlabExport(tauCC_s[1..6], sprintf("../codeexport/%s/tmp/coriolisvec_base_floatb_%s_par%d_matlab.m", robot_name, base_method_name, codegen_dynpar), codegen_opt):
end if:
if codegen_act and codeexport_corvec and not(base_method_name="twist") then
  MatlabExport(tauCC_s[7..NQ], sprintf("../codeexport/%s/tmp/coriolisvec_joint_floatb_%s_par%d_matlab.m", robot_name, base_method_name, codegen_dynpar), codegen_opt):
end if:
if codegen_act and codeexport_corvec and not(base_method_name="twist") then
  MatlabExport(tauCC_s[1..NQ], sprintf("../codeexport/%s/tmp/coriolisvec_floatb_%s_par%d_matlab.m", robot_name, base_method_name, codegen_dynpar), codegen_opt):
end if:
# Matlab Export: Fixed base
tauCC_s_fixb:=tauCC_s:
for i from 1 to NQB do
  tauCC_s_fixb := subs({X_base_s[i,1]=0},tauCC_s_fixb):
end do:
for i from 1 to 6 do
  tauCC_s_fixb := subs({V_base_s[i,1]=0},tauCC_s_fixb):
end do:
if codegen_act and codeexport_corvec then
  MatlabExport(tauCC_s_fixb[7..NQ], sprintf("../codeexport/%s/tmp/coriolisvec_joint_fixb_par%d_matlab.m", robot_name, codegen_dynpar), codegen_opt):
end if:

# Coriolis Matrix
# Calculation with Christoffel Symbol approach
# [KhalilDombre2002], equ. (9.7) (p. 195)

cijk := proc (i::integer, j::integer, k::integer, A, qs)
  local c:
  c := (1/2)*(diff(A[i, j], qs(k, 1)))+(1/2)*(diff(A[i, k], qs(j, 1)))-(1/2)*(diff(A[j, k], qs(i, 1))):
  return c:
end proc:

if codeexport_cormat then
  read sprintf("../codeexport/%s/tmp/inertia_par%d_maple.m", robot_name, codegen_dynpar):
  MM_s := MM_s: # Annahme: Massenmatrix wurde vorher generiert.
  Cqs:=Matrix(NQ,NQ):
  for i  from 1 to NQ do
    for j from 1 to NQ do
      Cqs[i,j]:=0:
      for k from 1 to NQ do
        Cqs[i,j]:=Cqs[i,j]+cijk(i,j,k,MM_s,q_s)*qD_s[k,1]:
      end do:
    end do:
  end do:
  # Terme vereinfachen
  if use_simplify>=2 then
    tmp_t1:=time():
    tmp_l1 := length(Cqs):
    printf("%s: Beginne Vereinfachung Coriols-Matrix (Param. %d). Länge: %d.\n", \
      FormatTime("%Y-%m-%d %H:%M:%S"), codegen_dynpar, tmp_l1):
    Cqs := simplify2(Cqs):
    tmp_t2:=time():
    tmp_l2 := length(Cqs):
    printf("%s: Coriolis-Matrix vereinfacht (Param. %d). Länge: %d->%d. Rechenzeit %1.1fs.\n", \
      FormatTime("%Y-%m-%d %H:%M:%S"), codegen_dynpar, tmp_l1, tmp_l2, tmp_t2-tmp_t1):
  end if:
end if:

# Matlab Export: Floating base
if codegen_act and codeexport_cormat and not(base_method_name="twist") then
  MatlabExport(Cqs[1..NQ,1..NQ], sprintf("../codeexport/%s/tmp/coriolismat_floatb_%s_par%d_matlab.m", robot_name, base_method_name, codegen_dynpar), codegen_opt):
end if:
if codegen_act and codeexport_cormat and not(base_method_name="twist") then
  MatlabExport(Cqs[7..NQ,1..NQ], sprintf("../codeexport/%s/tmp/coriolismat_joint_floatb_%s_par%d_matlab.m", robot_name, base_method_name, codegen_dynpar), codegen_opt):
end if:
# Matlab Export: Fixed base
Cqs_fixb:=Cqs:
if codeexport_cormat then
  for i from 1 to NQB do
    Cqs_fixb := subs({X_base_s[i,1]=0},Cqs_fixb):
  end do:
  for i from 1 to 6 do
    Cqs_fixb := subs({V_base_s[i,1]=0},Cqs_fixb):
  end do:
end if:
if codegen_act and codeexport_cormat then
  MatlabExport(Cqs_fixb[7..NQ,7..NQ], sprintf("../codeexport/%s/tmp/coriolismat_joint_fixb_par%d_matlab.m", robot_name, codegen_dynpar), codegen_opt):
end if:

# Joint Torques

if codeexport_invdyn then
  tau := dTdqDdt_s-dTdq_s+dUdq_s:
  # Terme vereinfachen
  if use_simplify>=3 then
    tmp_t1:=time():
    tmp_l1 := length(tau):
    printf("%s: Beginne Vereinfachung Invers-Dynamik (Param. %d). Länge: %d.\n", \
      FormatTime("%Y-%m-%d %H:%M:%S"), codegen_dynpar, tmp_l1):
    tau := simplify2(tau):
    tmp_t2:=time():
    tmp_l2 := length(tau):
    printf("%s: Invers-Dynamik (Param. %d) vereinfacht. Länge: %d->%d. Rechenzeit %1.1fs.\n", \
      FormatTime("%Y-%m-%d %H:%M:%S"), codegen_dynpar, tmp_l1, tmp_l2, tmp_t2-tmp_t1):
  end if:
end if:
# Matlab Export: Floating base
# Berechnung der Basis-Belastung ist für manche Basis-Darstellungen falsch (siehe oben unter Gravitationslast).
if codegen_act and codeexport_invdyn and not(base_method_name="twist") then
  MatlabExport(tau, sprintf("../codeexport/%s/tmp/invdyn_floatb_%s_par%d_matlab.m", robot_name, base_method_name, codegen_dynpar), codegen_opt):
end if:
if codegen_act and codeexport_invdyn and not(base_method_name="twist") then
  MatlabExport(tau[1..6], sprintf("../codeexport/%s/tmp/invdyn_base_floatb_%s_par%d_matlab.m", robot_name, base_method_name, codegen_dynpar), codegen_opt):
end if:
if codegen_act and codeexport_invdyn and not(base_method_name="twist") then
  MatlabExport(tau[7..NQ], sprintf("../codeexport/%s/tmp/invdyn_joint_floatb_%s_par%d_matlab.m", robot_name, base_method_name, codegen_dynpar), codegen_opt):
end if:
# Matlab Export: Fixed base
if codegen_act and codeexport_invdyn then
  taus_fixb:=tau:
  for i from 1 to NQB do
    taus_fixb := subs({X_base_s[i,1]=0},taus_fixb):
  end do:
  for i from 1 to 6 do
    taus_fixb := subs({V_base_s[i,1]=0},taus_fixb):
    taus_fixb := subs({VD_base_s[i,1]=0},taus_fixb):
  end do:
  save taus_fixb, sprintf("../codeexport/%s/tmp/invdyn_fixb_par%d_maple.m", robot_name, codegen_dynpar):
end if:
if codegen_act and codeexport_invdyn then
  MatlabExport(taus_fixb[7..NQ], sprintf("../codeexport/%s/tmp/invdyn_fixb_par%d_matlab.m", robot_name, codegen_dynpar), codegen_opt):
end if:


cdir := "/home/scott/Documents/research/software/hqca/hqca/maple/":
with(LinearAlgebra):
with(QuantumChemistry):
loaddata := readdata(cat(cdir, "h4_qc_spatial.rdm"), 6):
Flatten := proc(x) local n, a, i, j, k, l; `local`(a, n, i, j, k, l); `description`("convert chemists to numpy and flatten an array to form a matrix"); n := Size(x); a := Matrix(1 .. n[1]*n[2], 1 .. n[3]*n[4], datatype = float[8]); for i to n[1] do for j to n[2] do for k to n[3] do for l to n[4] do a[(i - 1)*n[1] + j, (k - 1)*n[3] + l] := x[i, k, j, l]; end do; end do; end do; end do; return a; end proc:
New := Array(1 .. 4, 1 .. 4, 1 .. 4, 1 .. 4, datatype = float[8]):
for i in loaddata[3 .. ()] do
    New[round(i[1]), round(i[3]), round(i[2]), round(i[4])] := i[5];
end do;
pure := Purify2RDM(New, spin_free = true, electron_number = 4, conv_tol = 0.10000000):
ExportMatrix(cat(cdir, "h4_qc_spatial_pure.csv"), Flatten(pure[rdm2]));


# Parameter Regressor Inverse Dynamics for Robot-Base
# Einleitung
# Berechnung der inversen Dynamik der Roboter-Plattform in Regressorform
#
# Dateiname:
# robot -> Berechnung für allgemeinen Roboter
# para -> Berechnung für eine parallelen Roboter
# rotmat -> Kinematik wird mit Rotationsmatrizen berechnet
# dynamics -> Berechnung der Dynamik
# regressor -> Regressorform (parameterlinear)
# Autor
# Tim Job (Studienarbeit bei Moritz Schappler), 2018-12
# Moritz Schappler, moritz.schappler@imes.uni-hannover.de
# (C) Institut für Mechatronische Systeme, Universität Hannover
# Sources
# [GautierKhalil1990] Direct Calculation of Minimum Set of Inertial Parameters of Serial Robots
# [KhalilDombre2002] Modeling, Identification and Control of Robots
# [Ortmaier2014] Vorlesungsskript Robotik I
# [Abdel2007] Modellierung, Identifikation und robuste Regelung von Robotern mit parallelkinematischen Strukturen
# Initialization
interface(warnlevel=0): # Unterdrücke die folgende Warnung.
restart: # Gibt eine Warnung, wenn über Terminal-Maple mit read gestartet wird.
interface(warnlevel=3):
with(LinearAlgebra):
#with(ArrayTools):
with(codegen):
with(CodeGeneration):
with(StringTools):
with(VectorCalculus):
# Einstellungen für Code-Export: Optimierungsgrad (2=höchster) und Aktivierung jedes Terms.
codegen_opt := 2:
codeexport_invdyn := false:
codeexport_grav := false:
codeexport_corvec := false:
codeexport_inertia := false:
read "../helper/proc_convert_s_t":
read "../helper/proc_convert_t_s":
read "../helper/proc_MatlabExport":
read "../helper/proc_index_symmat2vector":
read "../helper/proc_symmat2vector":
read "../helper/proc_skew2vec":
read "../helper/proc_vec2skew":
read "../helper/proc_combine2":
read "../transformation/proc_rotx":
read "../transformation/proc_roty":
read "../transformation/proc_rotz":
read "../transformation/proc_trotx":
read "../transformation/proc_troty":
read "../transformation/proc_trotz":
read "../transformation/proc_transl":
read "../transformation/proc_trafo_mdh":
read "../robot_codegen_definitions/robot_env_par":
read sprintf("../codeexport/%s/tmp/tree_floatb_definitions", leg_name):
read "../robot_codegen_definitions/robot_env_par":
# Definitionen für parallelen Roboter laden
read sprintf("../codeexport/%s/tmp/para_definitions", robot_name):
r_P_sP_P := -r_P_sP:
s_P_P_sP := s_P_sP:
# Ergebnisse der Kinematik für parallen Roboter laden

kinematicsfile := sprintf("../codeexport/%s/tmp/kinematics_%s_platform_maple.m", robot_name, base_method_name):
if FileTools[Exists](kinematicsfile) then
  read kinematicsfile:
else
  printf("%s. PKM-Kinematik konnte nicht geladen werden. Abbruch der Berechnung.\n", FormatTime("%Y-%m-%d %H:%M:%S")):
  quit: # Funktioniert in GUI nicht richtig...
  robot_name := "": # ...Daher auch Löschung des Roboternamens.
end if:
P_i := P_i: # nur diese Variable wird benötigt. Alle anderen zur Übersichtlichkeit löschen. Enthält Informationen zu den Koppelpunkten.
unassign('pivotMat', 'pivotMatMas', 'Jinv', 'JB_i', 'JBD_i', 'JBinv_i', 'JBDinv_i', 'U_i', 'UD_i'):

# Lade "robotics_repo_path"-File mit Link zum "imes-robotics-matlab"-Repo
read("../robotics_repo_path"):
# Lade die Funktionen aus dem "imes-robotics-matlab"-Repo
read(sprintf("%s/transformation/maple/proc_eul%s2r", robotics_repo_path, angleConvLeg)):
read(sprintf("%s/transformation/maple/proc_eul%sjac", robotics_repo_path, angleConvLeg)):

# Additional Kinematics
# Berechnung der Rotationsmatrizen
R_0_0_E_t := parse(sprintf("eul%s2r",angleConvLeg))(xE_t(4..6)):
R_0_0_E_s := parse(sprintf("eul%s2r",angleConvLeg))(xE_s(4..6)):
RPYjac_0_t := parse(sprintf("eul%sjac",angleConvLeg))(xE_t(4..6)):
RPYjac_0_s := parse(sprintf("eul%sjac",angleConvLeg))(xE_s(4..6)):
r_0_sP_P := R_0_0_E_s.r_P_sP_P:
J_P_P := J_P_P:
J_0_P := R_0_0_E_s.J_P_P.Transpose(R_0_0_E_s):
J_0_P_raute := Matrix(6,1,[J_0_P(1,1),J_0_P(1,2),J_0_P(1,3),J_0_P(2,2),J_0_P(2,3),J_0_P(3,3)]):
J_P_P_raute := Matrix(6,1,[J_P_P(1,1),J_P_P(1,2),J_P_P(1,3),J_P_P(2,2),J_P_P(2,3),J_P_P(3,3)]):

r_0_sP_P := R_0_0_E_t.r_P_sP_P:
r_0_P_sP := -r_0_sP_P:
#rD_0_sP_P := diff~(r_0_sP_P,t):
#for i to 3 do
  # for j to 6 do
      #rD_0_sP_P(i) := subs({xED_t(j)=xED_s(j),xE_t(j)=xE_s(j)},rD_0_sP_P(i)):
   #end do:
#end do:
#rD_0_sP_P:
r_0_sP_P := R_0_0_E_s.r_P_sP_P:
r_0_P_sP := -r_0_sP_P:
r_P_P_sP := -r_P_sP_P:
#sE := Matrix(mE*r_0_P_sP):
#sE := Matrix(mE*r_P_P_sP):
sE := s_P_P_sP:

# Berechnung der H-Matrix und deren Ableitung nach [Abdel2007] S.20
RPYjac_E_t := simplify(Multiply(Transpose(R_0_0_E_t),RPYjac_0_t)):
RPYjac_E_s := simplify(Multiply(Transpose(R_0_0_E_s),RPYjac_0_s)):
w_E_0_E_s := RPYjac_E_s.xED_s(4..6,1):
w_0_0_E_s := RPYjac_0_s.xED_s(4..6,1):
w_E_0_E_t := RPYjac_E_t.xED_t(4..6,1):
dRPYjac_E_t := diff~(RPYjac_E_t,t):
dRPYjac_0_t := diff~(RPYjac_0_t,t):
dRPYjac_E_s := Copy(dRPYjac_E_t):
dRPYjac_0_s := Copy(dRPYjac_0_t):
# Substituiere die zeitabhängigen Koordinaten in der H-Matrix mit zeitunabhängigen Koordinaten
for i to 3 do
  for j to 3 do
    for k from 4 to 6 do
      dRPYjac_E_s(i,j) := subs({xEDD_t(k)=xEDD_s(k),xED_t(k)=xED_s(k),xE_t(k)=xE_s(k)},dRPYjac_E_s(i,j)):
      dRPYjac_0_s(i,j) := subs({xEDD_t(k)=xEDD_s(k),xED_t(k)=xED_s(k),xE_t(k)=xE_s(k)},dRPYjac_0_s(i,j)):
    end do:
  end do:
end do:

omegaE := <omega1;omega2;omega3>:
xED_dummy := <xE,yE,zE>:
omega0 := Transpose(R_0_0_E_s).omegaE:
<xED_dummy;omegaE>:
JR := Jacobian(convert(omega0,list),convert(<xED_dummy;omegaE>,list)):
JR_T := Transpose(JR):
vel0 := Transpose(R_0_0_E_s).xED_dummy:
JT := Jacobian(convert(vel0,list),convert(<xED_dummy;omegaE>,list)):
JT_T := Transpose(JT):

wD_E_0_E_s := dRPYjac_E_s.xED_s(4..6,1)+RPYjac_E_s.xEDD_s(4..6,1):
wD_0_0_E_s := dRPYjac_0_s.xED_s(4..6,1)+RPYjac_0_s.xEDD_s(4..6,1):
# Trigonometrische Ausdrücke zusammenfassen.
w_E_0_E_s := combine2(w_E_0_E_s):
wD_E_0_E_s := combine2(wD_E_0_E_s):

H := <IdentityMatrix(3,3),ZeroMatrix(3);
      ZeroMatrix(3),RPYjac_0_s>:

Hinv := MatrixInverse(H):
dH := <ZeroMatrix(3),ZeroMatrix(3);
      ZeroMatrix(3),dRPYjac_0_s>:

a_E := Matrix(xEDD_s(1..3,1)) - g_world:
a_E := Transpose(R_0_0_E_s).(Matrix(xEDD_s(1..3,1)) - g_world):
w_0_0_E_s_stern := Matrix(3,6,[w_0_0_E_s(1),w_0_0_E_s(2),w_0_0_E_s(3),0,0,0, \
                               0,w_0_0_E_s(1),0,w_0_0_E_s(2),w_0_0_E_s(3),0, \
                               0,0,w_0_0_E_s(1),0,w_0_0_E_s(2),w_0_0_E_s(3)]):
wD_0_0_E_s_stern := Matrix(3,6,[wD_0_0_E_s(1),wD_0_0_E_s(2),wD_0_0_E_s(3),0,0,0,\
                                0,wD_0_0_E_s(1),0,wD_0_0_E_s(2),wD_0_0_E_s(3),0,\
                                0,0,wD_0_0_E_s(1),0,wD_0_0_E_s(2),wD_0_0_E_s(3)]):
w_E_0_E_s_stern := Matrix(3,6,[w_E_0_E_s(1),w_E_0_E_s(2),w_E_0_E_s(3),0,0,0,\
                               0,w_E_0_E_s(1),0,w_E_0_E_s(2),w_E_0_E_s(3),0,\
                               0,0,w_E_0_E_s(1),0,w_E_0_E_s(2),w_E_0_E_s(3)]):
wD_E_0_E_s_stern := Matrix(3,6,[wD_E_0_E_s(1),wD_E_0_E_s(2),wD_E_0_E_s(3),0,0,0,\
                                0,wD_E_0_E_s(1),0,wD_E_0_E_s(2),wD_E_0_E_s(3),0,\
                                0,0,wD_E_0_E_s(1),0,wD_E_0_E_s(2),wD_E_0_E_s(3)]):
paramVecP_old := <J_0_P_raute;sE;mE>:
paramVecP_old := <J_P_P_raute;sE;mE>:
paramVecP := <J_0_P_raute;sE;mE>:
paramVecP := <J_P_P_raute;sE;mE>:
paramVecP_M := Copy(paramVecP):
# Hierdurch (u.a.?) werden die Massen am Ende der Beinkette mit den Dynamikparametern der Plattform zusammengefasst
tmp := Matrix(10,1):
for i to N_LEGS do
  tmpSt := M(NQJ_parallel+1,1)*Multiply(Transpose(vec2skew(P_i(1..3,i))),vec2skew(P_i(1..3,i))):
  tmpSt_raute := Matrix(6,1,[tmpSt(1,1),tmpSt(1,2),tmpSt(1,3),tmpSt(2,2),tmpSt(2,3),tmpSt(3,3)]):
  tmp := <tmpSt_raute,M(NQJ_parallel+1,1)*P_i(1..3,i),M(NQJ_parallel+1,1)>:
  paramVecP_M := paramVecP_M + tmp:
end do:
# Trigonometrische Ausdrücke zusammenfassen.
wD_E_0_E_s_stern := combine2(wD_E_0_E_s_stern):

A_E := <JT_T|JR_T>.<ZeroMatrix(3,6),vec2skew(wD_E_0_E_s)+Multiply(vec2skew(w_E_0_E_s),vec2skew(w_E_0_E_s)),a_E;
        wD_E_0_E_s_stern + Multiply(vec2skew(w_E_0_E_s),w_E_0_E_s_stern),-vec2skew(a_E),ZeroMatrix(3,1)>:
# Term-Vereinfachungen vornehmen: Im planaren Fall kommt sin²+cos² vor.
# Im räumlichen Fall ist die Vereinfachung nachteilig. Dann wird der Term so gelassen.
A_E := combine2(A_E):

# Mass Matrix
#
M_regmin := <JT_T|JR_T>.<ZeroMatrix(3,6),vec2skew(wD_E_0_E_s),Transpose(R_0_0_E_s).Matrix(xEDD_s(1..3,1));
             wD_E_0_E_s_stern,-vec2skew(Transpose(R_0_0_E_s).Matrix(xEDD_s(1..3,1))),ZeroMatrix(3,1)>:
MM_regmin := Matrix(6*6, RowDimension(paramVecP)):

i_rr := 0:
for i to 6 do # Zeilenindex der Massenmatrix
  for j to 6 do  # Spaltenindex der Massenmatrix
    i_rr := i_rr + 1: # Gehe zeilenweise durch den unteren linken Teil der Massenmatrix (inkl. Diagonale)
    for k to RowDimension(paramVecP) do # Spaltenindex der Regressormatrix
    	 if not(xEDD_s[j, 1] = 0) then
        MM_regmin[i_rr, k] := diff(M_regmin[i, k], xEDD_s[j, 1]):
      else
        MM_regmin[i_rr, k] := 0:
      end if:
    end do:
  end do:
end do:
# Term-Vereinfachungen vornehmen: Im planaren Fall kommt sin²+cos² vor:
MM_regmin := combine2(MM_regmin):

# Coriolis Vector
# Nur Fliehkraft-Komponente aus Gesamtkraft.
c_regmin := <JT_T|JR_T>.<ZeroMatrix(3,6),Multiply(vec2skew(w_E_0_E_s),vec2skew(w_E_0_E_s)),ZeroMatrix(3,1);
             Multiply(vec2skew(w_E_0_E_s),w_E_0_E_s_stern),ZeroMatrix(3,3),ZeroMatrix(3,1)>:

# Gravitional Vector
# Gravitationskomponente aus Gesamtkraft.
g_regmin := <JT_T|JR_T>.<ZeroMatrix(3,6),ZeroMatrix(3,3),-Transpose(R_0_0_E_s).g_world;
             ZeroMatrix(3,6),-vec2skew(-Transpose(R_0_0_E_s).g_world),ZeroMatrix(3,1)>:

# Code Export
# Matlab
if codeexport_invdyn then
  MatlabExport(A_E, sprintf("../codeexport/%s/tmp/invdyn_floatb_%s_platform_matlab.m", robot_name, base_method_name), codegen_opt):
  MatlabExport(M_regmin, sprintf("../codeexport/%s/tmp/invdyn_floatb_%s_Mplatform_matlab.m", robot_name, base_method_name), codegen_opt):
  MatlabExport(MM_regmin, sprintf("../codeexport/%s/tmp/invdyn_floatb_%s_MMplatform_matlab.m", robot_name, base_method_name), codegen_opt):
  MatlabExport(c_regmin, sprintf("../codeexport/%s/tmp/invdyn_floatb_%s_cplatform_matlab.m", robot_name, base_method_name), codegen_opt):
  MatlabExport(g_regmin, sprintf("../codeexport/%s/tmp/invdyn_floatb_%s_gplatform_matlab.m", robot_name, base_method_name), codegen_opt):
end if:

# Maple-Export
save paramVecP, paramVecP_M, A_E, M_regmin, c_regmin, g_regmin, H, dH, sprintf("../codeexport/%s/tmp/floatb_%s_platform_dynamic_reg_maple.m", robot_name, base_method_name):


#@ Not autoload

# 3-stem
SpherePi(5,2) := [[2,o(eta[2],eta[3],eta[4])]]:

# NB nuprime is usually only defined at p=2.
# Presumably we get a generator from the unstable J-map?
SpherePi(6,3) := [[12,nuprime]]:

SpherePi(7,4) := [[infinity,nu[4]],[12,E(nuprime)]]:

for i from 5 to 20 do
 SpherePi(i+3,i) := [[24,nu[i]]]:
od:

# 4-stem
SpherePi(6,2) := [[12,o(eta[2],nuprime)]]:
SpherePi(7,3) := [[2,o(nuprime,eta[6])]]:
SpherePi(8,4) := [[2,o(nu[4],eta[7])],[2,o(E(nuprime),eta[7])]]:
SpherePi(9,5) := [[2,o(nu[5],eta[8])]]:

for i from 6 to 20 do
 SpherePi(i+4,i) := []:
od:

# 5-stem

SpherePi(7, 2) := [[2,o(eta[2],nuprime,eta[6])]]:
SpherePi(8, 3) := [[2,o(nuprime,eta[6],eta[7])]]:
SpherePi(9, 4) := [[2,o(nu[4],eta[7],eta[8])],
                   [2,o(E(nuprime),eta[7],eta[8])]]:
SpherePi(10, 5) := [[2,o(nu[5],eta[8],eta[9])]]:
SpherePi(11, 6) := [[infinity,w[6]]]:

for i from 7 to 20 do
 SpherePi(i+5,i) := []:
od:

# 6-stem

SpherePi(8, 2) := [[2,o(eta[2],nuprime,eta[6],eta[7])]]:
SpherePi(9, 3) := []:
SpherePi(10, 4) := [[24,o(nu[4],nu[7])],3]:

for i from 5 to 20 do
 SpherePi(i+6,i) := [[2,o(nu[i],nu[i+3])]]:
od:

# 7-stem

SpherePi( 9, 2) := [3]:
SpherePi(10, 3) := [15]:
SpherePi(11, 4) := [15]:
SpherePi(12, 5) := [30]:
SpherePi(13, 6) := [60]:
SpherePi(14, 7) := [120]:
SpherePi(15, 8) := [[infinity,sigma[8]],120]:

for i from 9 to 20 do
 SpherePi(i+7,i) := [[240,sigma[i]]]:
od:

# 8-stem

SpherePi(10, 2) := [15]:
SpherePi(11, 3) := [[2,epsilon[3]]]:
SpherePi(12, 4) := [[2,epsilon[4]]]:
SpherePi(13, 5) := [[2,epsilon[5]]]:
SpherePi(14, 6) := [[2,epsilon[6]],[24,nubar[6]]]:
SpherePi(15, 7) := [[2,epsilon[7]],[2,nubar[7]],[2,o(sigmaprime,eta[14])]]:
SpherePi(16, 8) := [[2,epsilon[8]],
                    [2,nubar[8]],
                    [2,o(E(sigmaprime),eta[15])],
                    [2,o(sigma[8],eta[15])]]:
SpherePi(17, 9) := [[2,epsilon[9]],
                    [2,nubar[9]],
                    [2,o(sigma[9],eta[16])]]:


make_ico_oct_complex := proc()
 global ico_oct_complex, ico_oct_complex1;
 local n,T,T0,T1,m,v,w,vo,vi,i,j,k,E,F,e,f,nm;

 n := 102;
 T["vertices"] := [seq(i,i=1..n)];
 T["embedding_dim"] := 3;

 m := (i,j) -> 3 + 20 * i + modp(j,20):
 v := table():

 v[1] := [0,0,1]:
 v[2] := [0,0,-1]:

 for i from 0 to 19 do
  v[m(0,i)] := evalf([cos(Pi*i/10)  ,sin(Pi*i/10)  , 2] /~ sqrt(5));
  v[m(2,i)] := evalf([cos(Pi*i/10)  ,sin(Pi*i/10)  , 0]);
  v[m(4,i)] := evalf([cos(Pi*i/10)  ,sin(Pi*i/10)  ,-2] /~ sqrt(5));
 od:

 for i from 0 to 4 do
  v[m(1,4*i)]   := evalf([cos(Pi*4*i/10)*2,sin(Pi*4*i/10)*2, 1] /~ sqrt(5));
  v[m(3,4*i+2)] := evalf([cos(Pi*(4*i+2)/10)*2,sin(Pi*(4*i+2)/10)*2, -1] /~ sqrt(5));
 od:

 nm := (x) -> x /~ sqrt(add(x[i]^2,i=1..3));

 for i from 0 to 4 do
  v[m(1,4*i+2)] := nm(v[m(1,4*i  )] +~ v[m(1,4*i+4)]);
  v[m(3,4*i  )] := nm(v[m(3,4*i-2)] +~ v[m(3,4*i+2)]);
 od:

 for i from 0 to 4 do
  v[m(1,4*i+1)] := nm(v[m(1,4*i  )] +~ v[m(1,4*i+2)]);
  v[m(1,4*i+3)] := nm(v[m(1,4*i+2)] +~ v[m(1,4*i+4)]);
  v[m(3,4*i+1)] := nm(v[m(3,4*i  )] +~ v[m(3,4*i+2)]);
  v[m(3,4*i+3)] := nm(v[m(3,4*i+2)] +~ v[m(3,4*i+4)]);
 od:

 vo := table():
 for i from 1 to n do
  vo[i] := v[i] /~ `+`(op(map(abs,v[i])));
 od:

 T["embedding"] := eval(v);
 T["sphere_embedding"] := eval(v);
 T["octahedron_embedding"] := eval(vo);
 T["normal"] := copy(v);

 E := [
  seq([ 1  , m(0,i)],i=0..19),
  seq(seq([m(j,i),m(j+1,i)],i=0..19),j=0..3),
  seq([m(4,i),2],i=0..19),
  seq(seq([m(j,i),m(j,i+1)],i=0..19),j=0..4),
  seq([m(0,i),m(1,i+1)],i=1..19,2),
  seq([m(0,i),m(1,i-1)],i=1..19,2),
  seq([m(1,i+1),m(2,i)],i=1..19,2),
  seq([m(1,i-1),m(2,i)],i=1..19,2),
  seq([m(2,i),m(3,i+1)],i=1..19,2),
  seq([m(2,i),m(3,i-1)],i=1..19,2),
  seq([m(3,i+1),m(4,i)],i=1..19,2),
  seq([m(3,i-1),m(4,i)],i=1..19,2),
  NULL
 ]:

 E := sort([op(map(e -> sort([op(e)]),E))]);

 T["edges"] := E;

 F := NULL;
 for e in T["edges"] do
  i,j := op(e);
  for k from max(i,j)+1 to n do
   if k <> i and k <> j and member([i,k],E) and member([j,k],E) then
    F := F,[i,j,k];
   fi;
  od:
 od:

 F := [F];
 T["faces"] := F;
 T["max_simplices"] := T["faces"];

 T["octahedron_edges"] := [
  seq([ 1  , m(0,i)],i=0..19,5),
  seq(seq([m(j,i),m(j+1,i)],i=0..19,5),j=0..3),
  seq([m(4,i),2],i=0..19,5),
  seq([m(2,i),m(2,i+1)],i=0..19),
  NULL
 ]:

 T["octahedron_edges"] := sort(map(sort,T["octahedron_edges"])):

 T["octahedron_basic_vertices"] :=
  sort([1,2,m(2,0),m(2,5),m(2,10),m(2,15)]);

 T["octahedron_basic_edges"] := [
  seq([1,m(2,5*i)],i=0..3),
  seq([m(2,5*i),m(2,5*(i+1))],i=0..3),
  seq([m(2,5*i),2],i=0..3)
 ]:

 T["octahedron_basic_edges"] := sort(map(sort,T["octahedron_basic_edges"])):

 T["octahedron_basic_faces"] := [
  seq([1,m(2,5*i),m(2,5*(i+1))],i=0..3),
  seq([2,m(2,5*i),m(2,5*(i+1))],i=0..3)
 ]:

 T["octahedron_basic_faces"] := sort(map(sort,T["octahedron_basic_faces"])):

 T["icosahedron_edges"] := [
  seq([ 1  , m(0,4*i)],i=0..4),
  seq([m(0,4*i),m(1,4*i)],i=0..4),
  seq([m(1,4*i),m(2,4*i+1)],i=0..4),
  seq([m(1,4*i),m(2,4*i-1)],i=0..4),
  seq([m(2,4*i+1),m(3,4*i+2)],i=0..4),
  seq([m(2,4*i-1),m(3,4*i-2)],i=0..4),
  seq([m(3,4*i+2),m(4,4*i+2)],i=0..4),
  seq([m(4,4*i+2),2],i=0..4),
  seq([m(1,i),m(1,i+1)],i=0..19),
  seq([m(3,i),m(3,i+1)],i=0..19),
  NULL
 ]:

 T["icosahedron_edges"] := sort(map(sort,T["icosahedron_edges"])):

 T["icosahedron_basic_vertices"] :=
  sort([1,2,seq(m(1,4*i),i=0..4),seq(m(3,4*i+2),i=0..4)]);

 T["icosahedron_basic_edges"] := [
  seq([ 1  , m(1,4*i)],i=0..4),
  seq([m(1,4*i),m(3,4*i+2)],i=0..4),
  seq([m(1,4*i),m(3,4*i-2)],i=0..4),
  seq([m(3,4*i+2),2],i=0..4),
  seq([m(1,4*i),m(1,4*(i+1))],i=0..4),
  seq([m(3,4*i+2),m(3,4*i-2)],i=0..4),
  NULL
 ]:

 T["icosahedron_basic_edges"] := sort(map(sort,T["icosahedron_basic_edges"])):

 T["icosahedron_basic_faces"] := [
  seq([1,m(1,4*i),m(1,4*(i+1))],i=0..4),
  seq([m(1,4*i),m(3,4*i+2),m(1,4*i+4)],i=0..4),
  seq([m(3,4*i-2),m(1,4*i),m(3,4*i+2)],i=0..4),
  seq([2,m(3,4*i+2),m(3,4*i-2)],i=0..4),
  NULL
 ]:

 T["icosahedron_basic_faces"] := sort(map(sort,T["icosahedron_basic_faces"])):

 w := table():

 for k from 1 to 20 do
  f := T["icosahedron_basic_faces"][k];
  w[k] := (v[f[1]] +~ v[f[2]] +~ v[f[3]]) /~ 3;
  w[k] := w[k] /~ add(w[k][i]^2,i=1..3);
 od:

 T["icosahedron_forms"] := eval(w):

 T["icosahedron_norm"] := (x) ->
  max(seq(add(x[i]*w[j][i],i=1..3),j=1..20));

 vi := table():

 for i from 1 to n do
  vi[i] := v[i] /~ T["icosahedron_norm"](v[i]);
 od:

 T["icosahedron_embedding"] := eval(vi);

 ico_oct_complex := eval(T):

 return eval(T);
end:

`subdivide/ico_oct_complex` := proc(T)
 local T0,T1,ix,n0,V0,i,x;

 T0 := `triangular_subdivision/simplicial_complex`(T):
 `normalise_embedding/simplicial_complex`(T0):
 T0["normal"] := copy(T0["embedding"]):

 T0["octahedron_edges"]  := select(e -> nops(e[1]) = 1 and member(e[2],T["octahedron_edges"] ),T0["edges"]):
 T0["icosahedron_edges"] := select(e -> nops(e[1]) = 1 and member(e[2],T["icosahedron_edges"]),T0["edges"]):
 ix := table():
 V0 := T0["vertices"]:
 n0 := nops(V0):
 for i from 1 to n0 do ix[V0[i]] := i; od:
 T1 := table():
 T1["vertices"] := [seq(i,i=1..n0)]:
 T1["edges"] := map(e -> [ix[e[1]],ix[e[2]]],T0["edges"]):
 T1["faces"] := map(f -> [ix[f[1]],ix[f[2]],ix[f[3]]],T0["faces"]):
 T1["max_simplices"] := T1["faces"]:
 T1["octahedron_edges"]  := map(e -> [ix[e[1]],ix[e[2]]],T0["octahedron_edges"]):
 T1["icosahedron_edges"] := map(e -> [ix[e[1]],ix[e[2]]],T0["icosahedron_edges"]):
 T1["octahedron_basic_vertices"]  := T["octahedron_basic_vertices"]:
 T1["octahedron_basic_edges"]     := T["octahedron_basic_edges"]:
 T1["octahedron_basic_faces"]     := T["octahedron_basic_faces"]:
 T1["icosahedron_basic_vertices"] := T["icosahedron_basic_vertices"]:
 T1["icosahedron_basic_edges"]    := T["icosahedron_basic_edges"]:
 T1["icosahedron_basic_faces"]    := T["icosahedron_basic_faces"]:

 T1["icosahedron_forms"] := copy(T["icosahedron_forms"]):
 T1["icosahedron_norm"]  := copy(T["icosahedron_norm"]):

 T1["embedding_dim"] := 3:
 T1["embedding"] := table():
 T1["normal"] := table():
 T1["sphere_embedding"]      := table():
 T1["octahedron_embedding" ] := table():
 T1["icosahedron_embedding"] := table():

 for i from 1 to n0 do
  x := T0["embedding"][V0[i]];
  x := x /~ evalf(sqrt(add(x[i]^2,i=1..3)));
  T1["embedding"][i]        := x;
  T1["normal"][i]           := x;
  T1["sphere_embedding"][i] := x;
  x := x /~ add(abs(x[i]),i=1..3);
  T1["octahedron_embedding"][i] := x;
  x := x /~ T1["icosahedron_norm"](x);
  T1["icosahedron_embedding"][i] := x;
 od:

 return eval(T1):
end:

`find_paths/ico_oct_complex` := proc(T)
 local k,E,P,E0,e,e0;

 for k in ["octahedron","icosahedron"] do
  E := {op(T[cat(k,"_edges")])};
  P := NULL:

  while E <> {} do
   e := E[1];
   E := E minus {e};
   E0 := select(e0 -> member(e[-1],e0),E);
   while E0 <> {} do
    e0 := E0[1];
    if e0[1] = e[-1] then
     e := [op(e),e0[2]];
    else
     e := [op(e),e0[1]];
    fi;
    E := E minus {e0};
    E0 := select(e0 -> member(e[-1],e0),E);
   od;
   P := P,e;
  od:
  T[cat(k,"_paths")] := [P];
 od:
end:

`make_plots/ico_oct_complex` := proc(T)
 local v;

 T["embedding"] := copy(T["octahedron_embedding"]);
 `plot/simplicial_complex`(T):
 `surface_plot/simplicial_complex`(T):
 T["octahedron_plot"] := T["plot"];
 T["octahedron_surface_plot"] := T["surface_plot"];

 T["embedding"] := copy(T["icosahedron_embedding"]);
 `plot/simplicial_complex`(T):
 `surface_plot/simplicial_complex`(T):
 T["icosahedron_plot"] := T["plot"];
 T["icosahedron_surface_plot"] := T["surface_plot"];

 T["embedding"] := copy(T["sphere_embedding"]);
 `plot/simplicial_complex`(T):
 `surface_plot/simplicial_complex`(T):
 T["sphere_plot"] := T["plot"];
 T["sphere_surface_plot"] := T["surface_plot"];

 v := eval(T["embedding"]);

 T["octahedron_arc_plot"] :=
  display(
   seq(line(v[e[1]],v[e[2]],colour=blue,thickness=4),e in T1["octahedron_edges"]),
   scaling=constrained,axes=none);

 T["icosahedron_arc_plot"] :=
  display(
   seq(line(v[e[1]],v[e[2]],colour=green,thickness=4),e in T1["icosahedron_edges"]),
   scaling=constrained,axes=none);

 T["octahedron_line_plot"] :=
  display(
   seq(line(v[e[1]],v[e[2]],colour=blue,thickness=4),e in T1["octahedron_basic_edges"]),
   scaling=constrained,axes=none);

 T["icosahedron_line_plot"] :=
  display(
   seq(line(v[e[1]],v[e[2]],colour=green,thickness=4),e in T1["icosahedron_basic_edges"]),
   scaling=constrained,axes=none);

end:

`make_javascript/ico_oct_complex` := proc(T)
 local shift,s,E,i,n;

 shift := proc(u)
  if type(u,list) then
   return map(shift,u);
  else
   return u - 1;
  fi;
 end:

 n := nops(T["vertices"]);

 s := [
  ["vertices",                    sprintf("%A",[seq(i,i=0..n-1)])],
  ["edges",                       sprintf("%A",shift(T["edges"]))],
  ["faces",                       sprintf("%A",shift(T["faces"]))],
  ["octahedron_edges",            sprintf("%A",shift(T["octahedron_edges"]))],
  ["icosahedron_edges",           sprintf("%A",shift(T["icosahedron_edges"]))],
  ["octahedron_paths",            sprintf("%A",shift(T["octahedron_paths"]))],
  ["icosahedron_paths",           sprintf("%A",shift(T["icosahedron_paths"]))],
  ["octahedron_basic_vertices",   sprintf("%A",shift(T["octahedron_basic_vertices"]))],
  ["octahedron_basic_edges",      sprintf("%A",shift(T["octahedron_basic_edges"]))],
  ["octahedron_basic_faces",      sprintf("%A",shift(T["octahedron_basic_faces"]))],
  ["icosahedron_basic_vertices",  sprintf("%A",shift(T["icosahedron_basic_vertices"]))],
  ["icosahedron_basic_edges",     sprintf("%A",shift(T["icosahedron_basic_edges"]))],
  ["icosahedron_basic_faces",     sprintf("%A",shift(T["icosahedron_basic_faces"]))]
 ]:

 E := copy(T["sphere_embedding"]):
 for i from 1 to n do E[i] := evalf[3](E[i]): od:
 s := [op(s),["sphere_embedding",sprintf("%A",[seq(E[i],i=1..n)])]]:
 E := copy(T["octahedron_embedding"]):
 for i from 1 to n do E[i] := evalf[3](E[i]): od:
 s := [op(s),["octahedron_embedding",sprintf("%A",[seq(E[i],i=1..n)])]]:
 E := copy(T["icosahedron_embedding"]):
 for i from 1 to n do E[i] := evalf[3](E[i]): od:
 s := [op(s),["icosahedron_embedding",sprintf("%A",[seq(E[i],i=1..n)])]]:

 s := map(u -> sprintf("\"%s\" : %s,\n",u[1],u[2]),s);
 s := cat("{\n",op(s),sprintf("\"num_vertices\" : %d\n}\n",n));

 T["javascript"] := s;
 return s;
end:

ico_oct_complex := make_ico_oct_complex():