text:
# Berechnung und Projektion der Dynamikgleichungen
# Einleitung
# Berechnung und Projektion der Dynamikgleichungen
#
# Dateiname:
# robot -> Berechnung für allgemeinen Roboter
# para -> Berechnung für einen parallelen Roboter
# rotmat -> Kinematik wird mit Rotationsmatrizen berechnet
# projection -> Die Dynamikgleichungen werden auf EE-Koordinaten projiziert
# 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
# [Job2018_S759] Job, T. (Studienarbeit; Betreuer Moritz Schappler): Implementierung einer strukturunabhängigen Dynamikmodellierung für parallelkinematische Maschinen (2018)
# 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): # Für Zeitausgabe
;
# Einstellungen für Code-Export: Optimierungsgrad (2=höchster).
#codegen_act := true: # noch nicht implementiert
codegen_debug := false:
codegen_opt := 2:
codeexport_invdyn := true:
codeexport_actcoord := false: # Generierung der Dynamik in Antriebskoordinaten nicht standardmäßig (hoher Rechenaufwand)
;
read "../helper/proc_MatlabExport":
read "../helper/proc_simplify2":
read "../robot_codegen_definitions/robot_env_par":
read sprintf("../codeexport/%s/tmp/tree_floatb_definitions", leg_name):
# 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 der zusätzlichen Definitionen für parallele Roboter laden
read "../robot_codegen_definitions/robot_env_par":
read sprintf("../codeexport/%s/tmp/para_definitions", robot_name):
# Ergebnisse der Plattform-Dynamik laden (aus robot_para_plattform_rotmat_dynamics.mw)

read "../robot_codegen_definitions/robot_env_par":
dynamicsfile := sprintf("../codeexport/%s/tmp/floatb_platform_dynamic_maple.m", robot_name):
if FileTools[Exists](dynamicsfile) then
  read dynamicsfile:
else
  printf("%s. PKM-Dynamik 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:
# Neu-Definition der geladenen Variablen
MME:=MME:
cvecE:=cvecE:
gE:=gE:
tauE:=tauE:
H:=H:
dH:=dH:

# Ergebnisse der Kinematik für parallelen Roboter laden

read "../robot_codegen_definitions/robot_env_par":
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:
read "../robot_codegen_definitions/robot_env_par": # Nochmal laden, um Standard-Einstellungen überschreiben zu können.
# Neu-Definition der von dieser Datei gelesenen Variablen, damit sie im Workspace erscheinen
pivotMat := pivotMat:
pivotMatMas := pivotMatMas:
Jinv := Jinv:
JBinv_i := JBinv_i:
U_i := U_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, "zyx")): # TODO: Muss hier die Winkelkonvention eingesetzt werden? Wird das hier gebraucht?
# TODO: Euler-Funktion mit "parse"-Befehl hier definieren
# Alle Basisgeschwindigkeiten und -winkel aus Berechnung der seriellen Kette zu null setzen.
omegaxs_base := 0:
omegays_base := 0:
omegazs_base := 0:
alphaxs_base := 0:
betays_base := 0:
gammazs_base := 0:
vxs_base := 0:
vys_base := 0:
vzs_base := 0:
# Startzeit messen zur Beurteilung der Zeitdauer einzelner Schritte
st := time():
# Physikalische Parameter der durch Koppelgelenke bewegten Körper zu Null setzen.
NQ := NQ - (NQJ-NQJ_parallel):
for i from NQJ_parallel+1 to NQJ do
	XXC||i := 0:
	XYC||i := 0:
	XZC||i := 0:
	YYC||i := 0:
	YZC||i := 0:
	ZZC||i := 0:
	XX||i := 0:
	XY||i := 0:
	XZ||i := 0:
	YY||i := 0:
	YZ||i := 0:
	ZZ||i := 0:
	SX||i := 0:
	SY||i := 0:
	SZ||i := 0:
	MX||i := 0:
	MY||i := 0:
	MZ||i := 0:
	M||i := 0:
end do:
# Ergebnisse G-Vektor der Beinkette  laden.
# Die Rotation der Basis wird nur in der Jacobi-Matrix der inverse Kinematik berücksichtigt. Deshalb muss der Gravitationsvektor ebenfalls an die Rotation angepasst werden.

g1 := gtmp1:
g2 := gtmp2:
g3 := gtmp3:

dynamicsfile_leg := sprintf("../codeexport/%s/tmp/gravload_par%d_maple.m", leg_name, codegen_dynpar):
if FileTools[Exists](dynamicsfile_leg) then
  read dynamicsfile_leg:
else
  printf("%s. Beinketten-Dynamik (g) 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:
G := simplify2(Matrix(taug_s(7..NQ,1))):
unassign('g1','g2','g3'):
g := <g1;g2;g3>:
Rmat := Transpose(parse(sprintf("eul%s2r",angleConvLeg))(frame_A_i(1..3,1))):
gtmp1 := (Rmat.g)(1):
gtmp2 := (Rmat.g)(2):
gtmp3 := (Rmat.g)(3):
G := G:

# Ergebnisse C-Vektor der Beinkette laden
dynamicsfile_leg := sprintf("../codeexport/%s/tmp/coriolisvec_par%d_maple.m", leg_name, codegen_dynpar):
if FileTools[Exists](dynamicsfile_leg) then
  read dynamicsfile_leg:
else
  printf("%s. Beinketten-Dynamik (c) 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:
Cvec := simplify2(Matrix(tauCC_s(7..NQ,1))):
Cvec := Cvec:

# Ergebnisse M-Matrix der Beinkette laden

dynamicsfile_leg := sprintf("../codeexport/%s/tmp/inertia_par%d_maple.m", leg_name, codegen_dynpar):
if FileTools[Exists](dynamicsfile_leg) then
  read dynamicsfile_leg:
else
  printf("%s. Beinketten-Dynamik (M) 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:
MM := simplify(MM_s(7..NQ,7..NQ)):
MM := simplify(MM):
MME := simplify(MME):
# Ausdruck für Gravitationsterme der Plattform nochmal vereinfachen.
gE := simplify2(gE):
# Ergebnisse der Kinematik für parallen Roboter laden (wurde oben schon gemacht.
#read sprintf("../codeexport/%s/tmp/kinematics_%s_platform_maple.m", robot_name, base_method_name):
printf("%s. Alle Daten geladen. Generiere Dynamik für PKM %s mit Parametersatz %d\n", FormatTime("%Y-%m-%d %H:%M:%S"), robot_name, codegen_dynpar, base_method_name):

# Berechne Dynamik-Matrizen für alle Beine
# Dupliziere alle berechneten Matrizen. i steht für den Index des jeweiligen Beines

for i to N_LEGS do
  MM||i := Copy(MM):
  Cvec||i := Copy(Cvec):
  G||i := Copy(G):
end do:
# Substituiere in jeder Matrix den Winkel Alpha (Verdrehung in der Basis) und die Gelenkkoordinaten und -geschwindigkeiten
for k from 1 by 1 to N_LEGS do
  	for i to NQJ_parallel do
  		for l to 3 do
  	 		Cvec||k(i,1):=subs({frame_A_i(l,1)=frame_A_i(l,k)},Cvec||k(i,1)):
  	  		G||k(i,1):=subs({frame_A_i(l,1)=frame_A_i(l,k)},G||k(i,1)):
  		end do:
    		for m to NQJ_parallel do #alpha
      		n := (m + (k-1)*NQJ_parallel):
     		Cvec||k(i,1):=subs({qJD||m||s=qJ||D||n||s,qJ||m||s=qJ||n||s},Cvec||k(i,1)):
      		G||k(i,1):=subs({qJ||m||s=qJ||n||s},G||k(i,1)):
    		end do:
    		for j to NQJ_parallel do
    			for l to 3 do
    	 			MM||k(i,j):=subs({rame_A_i(l,1)=frame_A_i(l,k)},MM||k(i,j)):
    	 		end do:
      		for m to NQJ_parallel do #alpha
        			n := m + (k-1)*NQJ_parallel:
        			MM||k(i,j):=subs({qJ||m||s=qJ||n||s},MM||k(i,j)):
      		end do:
    		end do:
  	end do:
end do:

# Berechnung, Projektion und Addition der Dynamikgleichungen
# Berechnung der Kräfte/Momente an den Gelenken der jeweiligen Beine und Projektion auf EE-Plattform
# Abdellatif2007 S.38 (3.27); [Job2018_S759], S. 29

for i to N_LEGS do

  Jtmp := Multiply(Transpose(U_i(..,..,i)),Transpose(JBinv_i(..,..,i))):
  qDtmp := Multiply(JBinv_i(..,..,i),U_i(..,..,i).H.xED_s):
  A||i := simplify(Multiply(JBinv_i(..,..,i),JBD_i(..,..,i))):
  B||i := Multiply(-MM||i,Multiply(A||i,qDtmp)):

  # [Job2018_S759], Term in der Summe in Gl. (3.50)
  MMs||i := Jtmp . MM||i . Transpose(Jtmp) . H:
  # [Job2018_S759], Term in der Summe in Gl. (3.51)
  cvecs||i := Jtmp.MM||i.JBinv_i(..,..,i).(U_i(..,..,i).dH + UD_i(..,..,i).H).xED_s + Jtmp.B||i + Jtmp.Cvec||i:
  # [Job2018_S759], Term in der Summe in Gl. (3.52)
  gvecs||i := Jtmp.G||i:

  tau||i := Jtmp.MM||i.JBinv_i(..,..,i).(U_i(..,..,i).H.xEDD_s+U_i(..,..,i).dH.xED_s+UD_i(..,..,i).H.xED_s) + Multiply(Jtmp,(B||i+Cvec||i+G||i)):

  taus||i := MMs||i.xEDD_s + cvecs||i + gvecs||i:
end do:

# Abdellatif2007 S.40 (3.33); [Job2018_S759], (3.49)
# Aufsummieren aller Kräfte, projiziert auf EE-Plattform
Tmp := 0:
for i to N_LEGS do
  Tmp := Tmp + tau||i:
end do:
# Addiere Inverse Dynamik der Plattform
tauGes := Tmp + tauE:
# Aufsummieren aller Massenmatrizen, projiziert auf EE-Plattform
# [Job2018_S759], (3.50)
Tmp := 0:
for i to N_LEGS do
  Tmp := Tmp + MMs||i:
end do:
# Addiere Massenmatrix der Plattform
MMGes := Tmp + MME:
# Aufsummieren aller Coriolisvektoren, projiziert auf EE-Plattform
# [Job2018_S759], (3.51)
Tmp := 0:
for i to N_LEGS do
  Tmp := Tmp + cvecs||i:
end do:
# Addiere Coriolisvektor der Plattform
cvecGes := Tmp + cvecE:
# Aufsummieren aller Gravitiationsvektoren, projiziert auf EE-Plattform
# [Job2018_S759], (3.52)
Tmp := 0:
for i to N_LEGS do
  Tmp := Tmp + gvecs||i:
end do:
# Addiere Gravitiationsvektor der Plattform
gGes := Tmp - gE:
#tauGes := MMGes.xEDD_s + cvecGes + gGes:
# Replace Joint Velocities
# Substituiere die Gelenkgeschwindigkeiten über H-, Ui- und JBi-Matrix mit EE-Geschwindikeiten
Tmp := 0:
for i to N_LEGS do
  Tmp := Multiply(H,xED_s):
  Tmp := Multiply(U_i(..,..,i),Tmp):
  z||i := Multiply(JBinv_i(..,..,i),Tmp):
end do:
for i to 6 do
  for j to N_LEGS do
    for l to NQJ_parallel do
      tauGes(i,1) := subs({qJD_i_s(l,j)=z||j(l)},tauGes(i,1)):
      cvecGes(i,1) := subs({qJD_i_s(l,j)=z||j(l)},cvecGes(i,1)):
      gGes(i,1) := subs({qJD_i_s(l,j)=z||j(l)},gGes(i,1)):
      for k to 6 do
        MMGes(i,k) := subs({qJD_i_s(l,j)=z||j(l)},MMGes(i,k)):
      end do:
    end do:
  end do:
end do:
# Export
# Wähle die Einträge aus Dynamikgleichungen, die für Freiheitsgrade des Roboters relevant sind.
# (über die Auswahl-Matrix "pivotMat").
#Jtestinv := Matrix(6,6,symbol=Jentry):
#Jtest := MatrixInverse(Jtestinv):
#Jtest := simplify(Jtest):
#for i to RowDimension(Jtest) do
#  for j to ColumnDimension(Jtest) do
#    for k to RowDimension(Jtest) do
#      for l to ColumnDimension(Jtest) do
#        Jtest(i,j) := subs(Jentry[k,l]=simplify(Jinv(k,l)),Jtest(i,j)):
#      end do:
#    end do:
#  end do:
#end do:
# Dynamik-Terme in Plattform-Koordinaten
tau_x := pivotMat.tauGes:
MMGes_x := pivotMat.MMGes.Transpose(pivotMatMas):
cvecGes_x := pivotMat.cvecGes:
gGes_x := pivotMat.gGes:
# Maple-Export (zur eventuellen späteren Verarbeitung in Maple)
save tau_x,     sprintf("../codeexport/%s/tmp/invdyn_para_plfcoord_par%d_maple.m",      robot_name, codegen_dynpar):
save MMGes_x,   sprintf("../codeexport/%s/tmp/inertia_para_plfcoord_par%d_maple.m",     robot_name, codegen_dynpar):
save cvecGes_x, sprintf("../codeexport/%s/tmp/coriolisvec_para_plfcoord_par%d_maple.m", robot_name, codegen_dynpar):
save gGes_x,    sprintf("../codeexport/%s/tmp/gravvec_para_plfcoord_par%d_maple.m",     robot_name, codegen_dynpar):
# Dynamik in Antriebs-Koordinaten umrechnen. Nur machen, wenn die Jacobi-Matrix einfach genug ist. Sonst ist die symbolische Invertierung zu teuer und sollte numerisch gemacht werden
# [Job2018_S759], S. 30; Gl. 3.53, 3.54
if RowDimension(Jinv) < 5 and codeexport_actcoord then
  printf("%s. Beginn der Matrix-Invertierung.  CPU-Zeit bis hier: %1.2fs.\n", FormatTime("%Y-%m-%d %H:%M:%S"), time()-st):
  J:=MatrixInverse(Jinv): # TODO: Matrix-Invertierung in eigenem Skript (bei der Kinematik; dort mit Platzhalter-Matrix invertieren)
  save J, sprintf("../codeexport/%s/tmp/jacobian_maple.m", robot_name): # TODO: Besseren Namen wählen und dies im Kinematik-Skript machen.
  printf("%s. Matrix-Invertierung beendet. CPU-Zeit bis hier: %1.2fs.\n", FormatTime("%Y-%m-%d %H:%M:%S"), time()-st):
  # J:=simplify(J):
  # printf("%s. Optimierung beendet. CPU-Zeit bis hier: %1.2fs.\n", FormatTime("%Y-%m-%d %H:%M:%S"), time()-st):
  tau_qa     := Transpose(J) . tau_x:
  MMGes_qa   := Transpose(J) . MMGes_x:
  cvecGes_qa := Transpose(J) . cvecGes_x:
  gGes_qa    := Transpose(J) . gGes_x:
  # Maple-Export (zur eventuellen späteren Verarbeitung in Maple)
  save tau_qa,     sprintf("../codeexport/%s/tmp/invdyn_para_actcoord_par%d_maple.m",      robot_name, codegen_dynpar):
  save MMGes_qa,   sprintf("../codeexport/%s/tmp/inertia_para_actcoord_par%d_maple.m",     robot_name, codegen_dynpar):
  save cvecGes_qa, sprintf("../codeexport/%s/tmp/coriolisvec_para_actcoord_par%d_maple.m", robot_name, codegen_dynpar):
  save gGes_qa,    sprintf("../codeexport/%s/tmp/gravvec_para_actcoord_par%d_maple.m",     robot_name, codegen_dynpar):
end if:
printf("%s. Speicherung der Dynamik-Terme in symbolischer Form beendet. Starte Code-Export in Matlab\n", FormatTime("%Y-%m-%d %H:%M:%S")):
# Matlab Export
if codeexport_invdyn then
  printf("%s. Beginne Code-Export Inverse Dynamik in Plattform-Koordinaten.\n", FormatTime("%Y-%m-%d %H:%M:%S")):
  MatlabExport(tau_x,     sprintf("../codeexport/%s/tmp/invdyn_para_plfcoord_par%d_matlab.m", robot_name, codegen_dynpar), codegen_opt);
  printf("%s. Beginne Code-Export Massenmatrix in Plattform-Koordinaten.\n", FormatTime("%Y-%m-%d %H:%M:%S")):
  MatlabExport(MMGes_x,   sprintf("../codeexport/%s/tmp/inertia_para_plfcoord_par%d_matlab.m", robot_name, codegen_dynpar), codegen_opt);
  printf("%s. Beginne Code-Export Coriolis-Vektor in Plattform-Koordinaten.\n", FormatTime("%Y-%m-%d %H:%M:%S")):
  MatlabExport(cvecGes_x, sprintf("../codeexport/%s/tmp/coriolisvec_para_plfcoord_par%d_matlab.m", robot_name, codegen_dynpar), codegen_opt);
  printf("%s. Beginne Code-Export Gravitations-Vektor in Plattform-Koordinaten.\n", FormatTime("%Y-%m-%d %H:%M:%S")):
  MatlabExport(gGes_x,    sprintf("../codeexport/%s/tmp/gravvec_para_plfcoord_par%d_matlab.m", robot_name, codegen_dynpar), codegen_opt);
end if:
if codeexport_invdyn and RowDimension(Jinv) < 5 and codeexport_actcoord then
  printf("%s. Beginne Code-Export Inverse Dynamik in Antriebs-Koordinaten.\n", FormatTime("%Y-%m-%d %H:%M:%S")):
  MatlabExport(tau_qa,     sprintf("../codeexport/%s/tmp/invdyn_para_actcoord_par%d_matlab.m", robot_name, codegen_dynpar), codegen_opt);
  printf("%s. Beginne Code-Export Massenmatrix in Antriebs-Koordinaten.\n", FormatTime("%Y-%m-%d %H:%M:%S")):
  MatlabExport(MMGes_qa,   sprintf("../codeexport/%s/tmp/inertia_para_actcoord_par%d_matlab.m", robot_name, codegen_dynpar), codegen_opt);
  printf("%s. Beginne Code-Export Coriolis-Vektor in Antriebs-Koordinaten.\n", FormatTime("%Y-%m-%d %H:%M:%S")):
  MatlabExport(cvecGes_qa, sprintf("../codeexport/%s/tmp/coriolisvec_para_actcoord_par%d_matlab.m", robot_name, codegen_dynpar), codegen_opt);
  printf("%s. Beginne Code-Export Gravitations-Vektor in Antriebs-Koordinaten.\n", FormatTime("%Y-%m-%d %H:%M:%S")):
  MatlabExport(gGes_qa,    sprintf("../codeexport/%s/tmp/gravvec_para_actcoord_par%d_matlab.m", robot_name, codegen_dynpar), codegen_opt);
end if:


text:
(*
 Copyright (C) 2017 M.A.L. Marques

 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: gga_exc *)

ol1_f  := x -> 1 + (x^2/72 + 0.00677*2^(1/3)*x)/K_FACTOR_C:

f := (rs, zeta, xt, xs0, xs1) -> gga_kinetic(ol1_f, rs, zeta, xs0, xs1):

ext: mpl


text:
program whilest;
var n, i, sum : integer;
begin
    readln(n);
    i := n;
    sum := 0;
    while i > 0 do begin
        sum := sum + i;
        i := i - 1
    end;
    writeln('Summention of 1 - ', n, '  is ', sum)
end.

max_stars_repo_name: Hiroya-W/lang-processing

max_issues_repo_name: Hiroya-W/lang-processing

max_forks_repo_name: Hiroya-W/lang-processing


text:
x:=1:y:=2:z:=3:w:=4:
fun:=proc()
    uses Grid;
    global x,y,z;
    local w:=1;
    printf("%d --> %a \n",MyNode(),[x,y,z,w]);
    x:=233+MyNode();
    y:=666+MyNode();
    z:=888+MyNode();
    w:=999+MyNode();
    Barrier();
end proc:
Grid:-Launch(fun,imports={'x','y'},exports={'x','z','w'});
[x,y,z,w];


text:
# This file is about Zagier's proof that every prime congruent to 1 mod 4
# is the sum of two squares.

# The following matrices generate a dihedral group of order 12 that
# acts on Z ^ 3 preserving the quadratic form Q

S := <<-1| 2| 0>,< 0| 1| 0>,< 1|-1| 1>>;
R := << 1| 0| 2>,< 0| 0| 1>,<-1| 1|-1>>;

Q := << 1| 0| 0>,< 0| 0| 2>,< 0| 2| 0>>;
I3 := IdentityMatrix(3);

G := [seq(seq(R ^ i . S ^ j, i = 0..5),j = 0..1)];

_ASSERT("Dihedral relations",
  Equal(S . S, I3) and Equal(R . R . R, -I3) and Equal(S . R . S, 1/R));

_ASSERT("Quadratic form relations",
  Equal(Transpose(S) . Q . S, Q) and Equal(Transpose(R) . Q . R, Q));

# The following map F gives an involution on the set X

F := proc(x)
  if (R . x)[3] > 0 then
    return R . x;
  elif ((1/R) . x)[1] > 0 then
    return (1/R) . x;
  else
    return S . x;
  fi;
end:

p := ithprime(10);

X := select(u -> type(u[3],posint),
  {seq(seq(<i,j,(p - i ^ 2)/(4 * j)>,j=1..p-1),i=1..p-1)}):

_ASSERT("F preserves X",
  evalb(map(convert,map(F,X),list) = map(convert,X,list)));

_ASSERT("F is an involution on X",
  `and`(seq(Equal(F(F(x)),x),x in X)));

_ASSERT("F has a unique fixed point on X",
  evalb(map(convert,select(x -> Equal(F(x),x),X),list) = {[1,1,(p - 1)/4]}));

fix := (T) -> subs(solve(convert(T . <x,y,z> - <x,y,z>,list),{x,y,z}),[x,y,z]);


text:
 func $fooi64 (
  var %i i64, var %j i64
  ) i64 {
   return (
     add i64(dread i64 %i, dread i64 %j))}

 func $addi64I (
  var %i i64
  ) i64 {
   return (
#add i64(dread i64 %i, constval i64 0x123456789))}
     add i64(dread i64 %i, constval i64 0x800000115))}
 # EXEC: %irbuild Main.mpl
 # EXEC: %irbuild Main.irb.mpl
 # EXEC: %cmp Main.irb.mpl Main.irb.irb.mpl

max_stars_repo_name: harmonyos-mirror/OpenArkCompiler-test

max_issues_repo_name: harmonyos-mirror/OpenArkCompiler-test

max_forks_repo_name: harmonyos-mirror/OpenArkCompiler-test


text:
######################################################################

`is_element/stasheff_star` := (A::set) -> proc(Rt)
 local R,t,n,JJ,TT,J,u,i,j,k;
 global reason;

 if not(type(Rt,list) and nops(Rt) = 2) then
  reason := [convert(procname,string),"Rt cannot be split as [R,t]",Rt];
  return false;
 fi;

 R,t := op(Rt);

 if not(`is_element/ord`(A)(R)) then
  reason := [convert(procname,string),"R is not an order on A",R,A,reason];
  return false;
 fi;

 n := nops(A);
 JJ := {seq(seq({seq(R[k],k=i..j)},j=i..n),i=1..n)};

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

 if map(op,{indices(t)}) <> JJ then
  reason := [convert(procname,string),"t is not indexed by the set of R-intervals",t,R,JJ];
  return false;
 fi;

 TT := NULL;

 for J in JJ do
  u := t[J];

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

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

  if (nops(J) = 1 or nops(J) = n) and u <> 1 then
   reason := [convert(procname,string),"t[A] <> 1",A,t[J]];
   return false;
  fi;

  if u > 0 then TT := TT,J; fi;
 od;

 TT := {TT};

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

 return true;
end;

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

`is_equal/stasheff_star` := (A::set) -> proc(Rt1,Rt2)
 local R1,R2,t1,t2,n,JJ,J,i,j,k;

 R1,t1 := op(Rt1);
 R2,t2 := op(Rt2);

 if not(`is_equal/ord`(A)(R1,R2)) then
  return false;
 fi;

 n := nops(A);
 JJ := {seq(seq({seq(R1[k],k=i..j)},j=i..n),i=1..n)};

 for J in JJ do
  if t1[J] <> t2[J] then return false; fi;
 od;

 return true;
end:

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

`is_leq/stasheff_star` := NULL;

######################################################################
# Covariant functoriality for bijections p : A -> B.

`act/stasheff_star` := (A::set,B::set) -> (p) -> proc(Rt)
 local R,t,S,u,JJ,J,J1;

 R,t := op(Rt);
 S := map(a -> p[a],R);
 u := table();
 JJ := map(op,[indices(t)]);
 for J in JJ do
  J1 := map(a -> p[a],J);
  u[J1] := t[J];
 od:
 return [S,eval(u)];
end:

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

`random_element/stasheff_star` := (A::set) -> proc()
 local R,t0,JJ0,J0,t,J;

 R := `random_element/ord`(A)();
 t0 := `random_element/standard_stasheff_star`(nops(A))();

 JJ0 := map(op,[indices(t0)]);
 t := table();
 for J0 in JJ0 do
  J := map(i -> R[i],J0);
  t[J] := t0[J0];
 od:

 return [R,eval(t)];
end:

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

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

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

`phi/stasheff_trees/stasheff_star` := (A::set) -> proc(RTT)
 local R,TT,n,JJ,J,t,i,j,k;

 R,TT := op(RTT);

 n := nops(R);
 JJ := {seq(seq({seq(R[k],k=i..j)},j=i..n),i=1..n)};
 t := table();

 for J in JJ do t[J] := 0; od;
 for J in TT do t[J] := 1; od;

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

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

`phi/realisation/stasheff_trees/stasheff_star` := (A::set) -> proc(x)
 local y,C,R,n,JJ,J,RTT,t,TT,z,i,j,k;

 y := table();
 C := map(op,[indices(x)]);
 R := C[1][1];
 n := nops(A);
 JJ := {seq(seq({seq(R[k],k=i..j)},j=i..n),i=1..n)};

 for J in JJ do y[J] := 0; od;

 for RTT in C do
  t := x[RTT];
  TT := RTT[2];
  z := `phi/stasheff_trees/stasheff_star`(A)(TT);
  for J in JJ do y[J] := y[J] + t * z[J]; od;
 od;

 return eval(y);
end:

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

`phi/stasheff_star/ord_simplex_interior` := (A::set) -> proc(Rt)
 local R,t,JJ,JS,Ja,J,K,lambda,mu,a,children;

 R,t := op(Rt);
 JJ := select(J -> t[J] > 0, map(op,{indices(t)}));
 children := `children_map`(A)(JJ);
 JS := sort([op(JJ),(U,V) -> nops(U) < nops(V)]);

 lambda := table();
 for J in JS do
  if nops(J) = 1 then
   lambda[J] := 1;
  else
   lambda[J] := add(lambda[K]^(1-t[K]),K in children[J]);
  fi;
 od;

 mu := table();
 for a in A do
  Ja := select(J -> member(a,J),JJ);
  mu[a] := mul(lambda[J]^(-t[J]),J in Ja);
 od;

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

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

`phi/stasheff_star/one_cubes_prime` := (A::set) -> proc(Rt)
 `phi/ord_simplex_interior/one_cubes_prime`(A)(
  `phi/stasheff_star/ord_simplex_interior`(A)(Rt));
end;

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

`proj/stasheff_star/ord` := (A::set) -> (Rt) -> Rt[1];

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

`describe/stasheff_star` := (A::set) -> proc(Rt)
 local R,t,n,r,i,TT;
 R,t := op(Rt);
 n := nops(A);
 r := table();
 for i from 1 to n do r[R[i]] := i; od:

 TT := map(op,[indices(t)]);
 TT := select(J -> (t[J] > 0 and nops(J) > 1),TT);
 TT := map(J -> [op(J)],TT);
 TT := map(sort,TT,(a,b) -> (r[a] < r[b]));
 TT := sort(TT,(J0,J1) -> (
        (nops(J0) < nops(J1)) or
        ((nops(J0) = nops(J1)) and (r[J0[1]] < r[J1[1]]))));
 return sprintf("%A",map(J -> (J=t[{op(J)}]),TT));
end;


text:
# The groupoid of sets of size 6 is equivalent to a certain groupoid of graphs which
# we have called Holder graphs.  Specifically, if A is a set of size 6, then we write
# DA for the set of subsets of size two, and we draw an edge between such subsets iff
# they are disjoint; this gives a Holder graph.  The code below sets up the
# relevant definitions.

`is_element/holder_graphs` := (V::set) -> proc(E)
 local non_edges,pentads,E0,e;

 if nops(V) <> 15 then
  return false;
 fi;

 if not `is_element/graphs`(V)(E) then
  return false;
 fi;

 if map(v -> nops(select(e -> e[1] = v,E)),V) <> {6} then
  return false;
 fi;

 non_edges := `top/autorel`(V) minus `id/autorel`(V) minus E;
 non_edges := map(e -> {op(e)},non_edges);

 pentads := combinat[choose](V,5);
 E0 := map(e -> {op(e)},E);

 for e in E0 do
  pentads := select(p -> e minus p <> {},pentads);
 od:

 for e in non_edges do
  if nops(select(p -> e minus p = {},pentads)) <> 1 then
   return false;
  fi;
 od;

 return true;
end:

`D/holder_graph` := proc(A::set)
 local V,E,v,w;

 if nops(A) <> 6 then
  error("A does not have size six");
 fi;

 V := combinat[choose](A,2);
 E := {seq(seq([v,w],w in V),v in V)};
 E := select(e -> e[1] intersect e[2] = {},E);
 return [V,E];
end:

`T/holder_graph` := (V::set) -> proc(E)
 local T,F,t,u;

 T := combinat[choose](V,3);
 T := select(t -> member([t[1],t[2]],E) and
                  member([t[2],t[3]],E) and
		  member([t[3],t[1]],E),T);

 F := {seq(seq([t,u],u in T),t in T)};
 F := select(e -> e[1] <> e[2],F);
 F := select(e -> e[1] intersect e[2] <> {},F);
 return [T,F];
end:

`P/holder_graph` := (V) -> proc(E)
 local P,E0,e;

 P := combinat[choose](V,5);
 E0 := map(e -> {op(e)},E);

 for e in E0 do
  P := select(p -> e minus p <> {},P);
 od:

 return P;
end:

`alpha/holder_graph` := (A) -> proc(a)
 local b;
 {seq({a,b},b in A minus {a})};
end:

`beta/holder_graph` := (V) -> (E) -> proc(v)
 local P;

 P := `P/holder_graph`(V)(E);

 return select(p -> member(v,p),P);
end:

`gamma/holder_graph` := (V) -> (E) -> proc(v)
 local T,F;

 T,F := op(`T/holder_graph`(V)(E));
 return select(t -> member(v,t),T);
end:

check_holder_graph := proc()
 local A0,A1,V0,V1,V2,V3,E0,E1,E2,E3,v,beta_table,gamma_table;

 A0 := {1,2,3,4,5,6};
 V0,E0 := op(`D/holder_graph`(A0)):
 _ASSERT(
  `is_element/holder_graphs`(V0)(E0),
  "D sends sets of size six to Holder graphs"
 );

 A1 := `P/holder_graph`(V0)(E0):
 _ASSERT(
  evalb(nops(A1) = 6),
  "P sends Holder graphs to sets of size six"
 );

 _ASSERT(
  evalb(map(`alpha/holder_graph`(A0),A0) = A1),
  "alpha : A -> PDA is bijective"
 );

 V1,E1 := op(`D/holder_graph`(A1)):

 _ASSERT(
  `is_element/holder_graphs`(V1)(E1),
  "DP sends Holder graphs to Holder graphs"
 );

 beta_table := table():
 for v in V0 do beta_table[v] := `beta/holder_graph`(V0)(E0)(v); od:
 _ASSERT(
  evalb(map(v -> beta_table[v],V0) = V1) and
  evalb(map(e -> [beta_table[e[1]],beta_table[e[2]]],E0) = E1),
  "beta : G -> DPG is an isomorphism"
 );

 V2,E2 := op(`T/holder_graph`(V0)(E0)):

 _ASSERT(
  `is_element/holder_graphs`(V2)(E2),
  "T sends Holder graphs to Holder graphs"
 );

 V3,E3 := op(`T/holder_graph`(V2)(E2)):
 _ASSERT(
  `is_element/holder_graphs`(V3)(E3),
  "T^2 sends Holder graphs to Holder graphs"
 );

 gamma_table := table():
 for v in V0 do gamma_table[v] := `gamma/holder_graph`(V0)(E0)(v); od:
 _ASSERT(
  evalb(map(v -> gamma_table[v],V0) = V3) and
  evalb(map(e -> [gamma_table[e[1]],gamma_table[e[2]]],E0) = E3),
  "gamma : G -> T^2G is an isomorphism"
 );
end:

# The code below is older.  It partially duplicates the code above and then
# sets up an embedding of a Holder graph in R^3 u {infinity}

make_holder_graph := proc()
 local i,j,k,l,ii,jj,kk,
  v,v_index,num_vertices,
  e,e_index,num_edges,
  t,t_index,num_triangles,
  col,vv,vi,ww,a,b,c,d,
  pol,draw_edge,sub_graph,full_sub_graph,G;

 i := 0:
 for j from 0 to 4 do
  for k from j+1 to 5 do
   v[i] := {j,k}:
   v_index[{j,k}] := i:
   i := i+1:
  od:
 od:
 num_vertices := i;

 i := 0:
 for j from 0 to num_vertices-2 do
  for k from j+1 to num_vertices-1 do
   if v[j] intersect v[k] = {} then
    e[i] := {j,k}:
    e_index[{j,k}] := i:
    i := i+1:
   fi:
  od:
 od:
 num_edges := i;

 i := 0:
 for j from 0 to num_vertices-1 do
  for k from j+1 to num_vertices-1 do
   for l from k+1 to num_vertices-1 do
    if nops({op(v[j]),op(v[k]),op(v[l])}) = 6 then
     ii := e_index[{j,k}]:
     jj := e_index[{j,l}]:
     kk := e_index[{k,l}]:
     t[i] := {ii,jj,kk}:
     t_index[{ii,jj,kk}] := i:
     col[ii] := i;
     col[jj] := i;
     col[kk] := i;
     i := i+1:
    fi:
   od:
  od:
 od:
 num_triangles := i;

 a := 0.3: b := 2: c := sqrt(3*b^2 - a^2): d := 1.5:

 t[0] := {};

 # Coordinates for a 3D picture
 vv[ 0] := infinity:
 vv[ 1] := [-b,-b, b]:
 vv[ 2] := [ b,-b,-b]:
 vv[ 3] := [-b, b,-b]:
 vv[ 4] := [ b, b, b]:
 vv[ 5] := [ b, b,-b]:
 vv[ 6] := [-b, b, b]:
 vv[ 7] := [ b,-b, b]:
 vv[ 8] := [-b,-b,-b]:
 vv[ 9] := [ 0,-c,-a]:
 vv[10] := [-c,-a, 0]:
 vv[11] := [-a, 0, c]:
 vv[12] := [-a, 0,-c]:
 vv[13] := [ c,-a, 0]:
 vv[14] := [ 0, c,-a]:

 vi[ 9] := [ 0, 0,-d]:
 vi[10] := [ 0,-d, 0]:
 vi[11] := [-d, 0, 0]:
 vi[12] := [-d, 0, 0]:
 vi[13] := [ 0,-d, 0]:
 vi[14] := [ 0, 0,-d]:

 # Coordinates for a planar picture
 pol := (r,t) -> simplify([r * cos(Pi/6*t), r * sin(Pi/6*t)]);

 ww[ 0] := pol(1, 0):
 ww[10] := pol(1, 1):
 ww[ 4] := pol(1, 2):
 ww[ 6] := pol(1, 3):
 ww[14] := pol(1, 4):
 ww[ 1] := pol(1, 5):
 ww[12] := pol(1, 6):
 ww[ 8] := pol(1, 7):
 ww[ 9] := pol(1, 8):
 ww[ 3] := pol(1, 9):
 ww[ 5] := pol(1,10):
 ww[13] := pol(1,11):
 ww[11] := pol(3, 0):
 ww[ 2] := pol(3, 4):
 ww[ 7] := pol(3, 8):

 G := table():

 G["v"] := eval(v);
 G["e"] := eval(e);
 G["t"] := eval(t);
 G["v_index"] := eval(v_index);
 G["e_index"] := eval(e_index);
 G["t_index"] := eval(t_index);
 G["num_vertices"] := num_vertices;
 G["num_edges"] := num_edges;
 G["num_triangles"] := num_triangles;
 G["col"] := eval(col);
 G["vv"] := eval(vv);
 G["vi"] := eval(vi);
 G["ww"] := eval(ww);

 draw_edge := proc(i)
  local u,p,q,r;
  u := e[i];
  p := vv[u[1]];
  q := vv[u[2]];
  if (p = infinity) then
   r := vi[u[2]];
   return(CURVES([q,[d*q[1],d*q[2],d*q[3]]],COLOR(HUE,col[i] * 0.07)));
  elif (p[1]*q[1] + p[2] * q[2] + p[3] * q[3] > 0) then
   return(great_arc(p,q,col[i] * 0.07));
  else
   return(CURVES([p,q],COLOR(HUE,col[i] * 0.07)));
  fi;
 end:

 sub_graph := proc(edge_test)
  local ee,i;
  ee := [];
  for i from 0 to num_edges-1 do
   if (edge_test(i)) then
    ee := [op(ee),i];
   fi;
  od:
  plots[display](PLOT3D(op(map(draw_edge,ee)),THICKNESS(3)),axes=none):
 end:

 full_sub_graph := proc(vertex_test)
  sub_graph(i -> vertex_test(e[i][1]) and vertex_test(e[i][2]));
 end:

 G["full_graph"] :=
  plots[display](PLOT3D(seq(draw_edge(i),i=0..num_edges-1),THICKNESS(3)),axes=none):

 G["outer_shell"] := full_sub_graph(i -> evalb(i < 9)):
 G["inner_shell"] := sub_graph(i -> evalb(e[i][1] > 8 and e[i][2] > 8)):
 G["joins"]       := sub_graph(i -> evalb(e[i][1] > 0 and e[i][1] < 9 and e[i][2] > 8)):

 G["plane_plot"] :=
  plots[display](
   seq(line(ww[e[i][1]],ww[e[i][2]]),i=0..num_edges-1),
   axes = none
  );

 return eval(G);
end:


text:
# Kinematik für parallelen Roboter
# Einleitung
# Berechnung der Jacobi-Matrix des parallelen Roboters
#
# Dateiname:
# robot -> Berechnung für allgemeinen Roboter
# para -> Berechnung für eine parallelen Roboter
# rotmat -> Kinematik wird mit Rotationsmatrizen berechnet
# kinematics -> Berechnung der Kinematik
# 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
# [Job2018_S759] Job, T. (Studienarbeit; Betreuer Moritz Schappler): Implementierung einer strukturunabhängigen Dynamikmodellierung für parallelkinematische Maschinen (2018)
# 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): # Für Zeitausgabe
;
# Einstellungen für Code-Export: Optimierungsgrad (2=höchster) und Aktivierung jedes Terms.
#codegen_act := true: # noch nicht implementiert
codegen_debug := false:
codegen_opt := 2:
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_vec2skew":
read "../transformation/proc_rotx":
read "../transformation/proc_roty":
read "../transformation/proc_rotz":
read "../robot_codegen_definitions/robot_env_par":
read sprintf("../codeexport/%s/tmp/tree_floatb_definitions", leg_name):
# Definitionen für parallel Roboter laden
read "../robot_codegen_definitions/robot_env_par":
read sprintf("../codeexport/%s/tmp/para_definitions", robot_name):
# 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)):
# 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 := 2:
# Link-Index, für den die Jacobi-Matrix aufgestellt wird. Hier wird angenommen, dass der Endeffektor das letzte Segment (=Link) ist. Die Jacobi-Matrix kann hier aber für beliebige Segmente aufgestellt werden. (0=Basis)
LIJAC:=NL-1:
# Ergebnisse der analytischen Jacobi-Matrix (Translatorisch)
read sprintf("../codeexport/%s/tmp/jacobia_transl_%d_maple.m", leg_name, LIJAC):
read "../robot_codegen_definitions/robot_env_par":
b_transl := b_transl:
# Setze Parameter aus serieller Berechnung zu null.
px, py, pz := 0, 0, 0:
alphaxs_base, betays_base, gammazs_base := 0, 0, 0:
rxs_base, rys_base, rzs_base := 0, 0, 0:
# Merke Startzeit für Debug-Ausgabe
st := time():
# Additional Kinematics
# Definition der Koppelpunkte für jedes Bein und der EE-Koordinaten/-Geschwindigkeiten/-Beschleunigungen
tmp := Matrix(3,1,[xP[1],yP[1],zP[1]]):
for j to 3 do:
    if (xE_s(j) = 0) then
      tmp(j,1) := 0:
    end if:
  end do:
for i to N_LEGS do
  P||i := <xP[i];yP[i];zP[i]>;
  for j to 3 do:
    if (xE_s(j) = 0) then
      P||i(j,1) := 0:
    end if:
  end do:
  j := i+1:
  if i <> 1 then
     tmp := <tmp | P||i>:
  end:
end do:
P_i := tmp:
#rhs||i := parse(sprintf("eul%s2r",angleConvLeg))(xE_s(4..6)).P||i+Matrix(xE_s(1..3,1)):
# Jacobi Matrices (JB1/U1) + Derivates
# Berechnung der Jacobi-Matrix der inversen Kinematik: Koppelpunktgeschwindigkeiten P -> Gelenkgeschwindigkeiten
# Abdellatif2007 S.37 unten

transDOF := nops(indets(xE_s(1..3,1))):
JB1 := (parse(sprintf("eul%s2r",angleConvLeg))(frame_A_i(1..3,1)).b_transl)(1..transDOF,1..NQJ_parallel):
try:
  JB1inv := MatrixInverse(JB1):
catch:
  printf("%s. Jacobi-Matrix JB1 ist singulär. Rang ist %d/%d. Abbruch der Berechnungen. Keine Berechnung der PKM-Dynamik möglich.\n", \
    FormatTime("%Y-%m-%d %H:%M:%S"), Rank(JB1), RowDimension(JB1)):
  quit: # Funktioniert in GUI nicht richtig...
  robot_name := "": # ...Daher auch Löschung des Roboternamens.
end try:
for i from 1 to 3-transDOF do
  JB1inv := <JB1inv|ZeroMatrix(NQJ_parallel,1)>;
end do:
for i from 1 to 3-transDOF do
  JB1 := <JB1;ZeroMatrix(1,NQJ_parallel)>;
end do:
JB1inv := simplify2(JB1inv):
JB1 := simplify2(JB1):

# Berechnung der Matrix Ui: EE-Geschwindigkeiten -> Koppelpunktgeschwindigkeiten P. i steht für den Index des jeweiligen Beines
# Abdellatif2007 S.21 (2.21)
for i to N_LEGS do
  r||i := P||i:
  if angleConv = X_Y_Z then
     r||i := vec2skew(rotx(xE_t(4)).roty(xE_t(5)).rotz(xE_t(6)).r||i):
  elif angleConv = Z_Y_X then
     r||i := vec2skew(rotz(xE_t(4)).roty(xE_t(5)).rotx(xE_t(6)).r||i):
  end:

  r||i := vec2skew(parse(sprintf("eul%s2r",angleConvLeg))(xE_t(4..6)).P||i);
  ones := Matrix(3,3,shape=identity):
  U||i := simplify(<ones|-r||i>):
  U||i||D := simplify(diff~(U||i,t)):      #dU berechnen
  #U||i||D := convert_t_s(U||i||D): #dU berechnen
  #U||i := convert_t_s(U||i):
end do:

# Ermittlung des Robotertyps, um später die Gesamt-Jacobi-Matrix reduzieren zu können
robotType := 1:
counter := 0:
for i from 4 to 6 do
  if xE_t(i) = 0 then
    counter := counter + 1:
  end if:
end do:
if counter = 2 then
  robotType := 2:
  for i from 1 to 3 do
    if not(xE_t(i+3) = 0) then
      rotPlanar := substring(angleConvLeg,i);
    end if:
  end do:
elif counter = 3 then
  robotType := 3:
end if:
rotPlanar:

# Erstelle die Matrizen für jedes Bein.
U_i := Copy(U1):
UD_i := Copy(U1D):
ROW := RowDimension(U1):
COLUMN := ColumnDimension(U1):
# Substituiere die zeitabhängigen EE-Koordinaten mit den oben definierten zeitunabhängigen Koordinaten
for i to N_LEGS do
  for j to 3 do
    for k to 6 do
      for l from 4 to 6 do
        U||i(j,k) := subs(xE_t(l)=xE_s(l),U||i(j,k)):
        U||i||D(j,k) := subs({xED_t(l)=xED_s(l),xE_t(l)=xE_s(l)},U||i||D(j,k)):
      end do:
    end do:
  end do:
  U_i(1..ROW,1..COLUMN,i) := U||i:
  UD_i(1..ROW,1..COLUMN,i) := U||i||D:
end do:
printf("[%s] Koppelpunkt-Jacobi-Matrizen (bezogen auf B) der Beinketten berechnet. CPU-Zeit bis hier: %1.2fs.\n", FormatTime("%Y-%m-%d %H:%M:%S"), time()-st):
# Berechnung von JB1D
JB1 := convert_s_t(JB1):
JB1D := diff~(JB1,t):
JB1D := simplify(JB1D):
JB1D := simplify(convert_t_s(JB1D)):
JB1 := simplify(convert_t_s(JB1)):
printf("[%s] Zeitableitung der Koppelpunkt-Jacobi-Matrizen (bezogen auf B) der Beinketten berechnet. CPU-Zeit bis hier: %1.2fs.\n", FormatTime("%Y-%m-%d %H:%M:%S"), time()-st):
JB1inv := convert_s_t(JB1inv):
JB1Dinv := diff~(JB1inv,t):
JB1Dinv := convert_t_s(JB1Dinv):
JB1inv := convert_t_s(JB1inv):
printf("[%s] Zeitableitung der inversen Koppelpunkt-Jacobi-Matrizen (bezogen auf B) der Beinketten berechnet. CPU-Zeit bis hier: %1.2fs.\n", FormatTime("%Y-%m-%d %H:%M:%S"), time()-st):

# Berechne Jacobi-Matrizen für jedes Bein
# Dupliziere alle berechneten Matrizen. i steht für den Index des jeweiligen Beines

JBinv_i := Copy(JB1inv):
JBD_i := Copy(JB1D):
JBDinv_i := Copy(JB1Dinv):
JB_i := Copy(JB1):
#Jinv_DoThanh_i := Copy(Jinv_DoThanh||1):
ROW := RowDimension(JB1):
COLUMN := ColumnDimension(JB1):
for i to N_LEGS do
  JB||i||D := Copy(JB1D):
  JB||i||Dinv := Copy(JB1Dinv):
  JB||i||inv := Copy(JB1inv):
  JB||i := Copy(JB1):
end do:

# Substituiere in jeder Matrix den Winkel Alpha (Verdrehung in der Basis) und die Gelenkkoordinaten und -geschwindigkeiten
for k from 1 by 1 to N_LEGS do
	for i to ROW do
		for j to COLUMN do
			for l to 3 do
				JB||k||D(i,j):=subs({frame_A_i(l,1)=frame_A_i(l,k)},JB||k||D(i,j)):
				JB||k||inv(j,i):=subs({frame_A_i(l,1)=frame_A_i(l,k)},JB||k||inv(j,i)):
				JB||k||Dinv(j,i):=subs({frame_A_i(l,1)=frame_A_i(l,k)},JB||k||Dinv(j,i)):
				JB||k(i,j):=subs({frame_A_i(l,1)=frame_A_i(l,k)},JB||k(i,j)):
			end do:
      		for m to NQJ_parallel do #alpha
				#tmp := VARS(m):
        			n := m + (k-1)*NQJ_parallel:
        			JB||k||D(i,j):=subs({qJD_i_s(m,1)=qJD_i_s(m,k),qJ_i_s(m,1)=qJ_i_s(m,k)},JB||k||D(i,j)):
       			JB||k||inv(j,i):=subs({qJ_i_s(m,1)=qJ_i_s(m,k)},JB||k||inv(j,i)):
       			JB||k||Dinv(j,i):=subs({qJD_i_s(m,1)=qJD_i_s(m,k),qJ_i_s(m,1)=qJ_i_s(m,k)},JB||k||Dinv(j,i)):
        			JB||k(i,j):=subs({qJ_i_s(m,1)=qJ_i_s(m,k)},JB||k(i,j)):
     		end do:
    		end do:
  	end do:
  	JB_i(1..ROW,1..COLUMN,k) := JB||k:
  	JBD_i(1..ROW,1..COLUMN,k) := JB||k||D:
  	JBinv_i(1..COLUMN,1..ROW,k) := JB||k||inv:
  	JBDinv_i(1..COLUMN,1..ROW,k) := JB||k||Dinv:
  	for i to N_LEGS do
		for j to 6 do
			#Jinv_DoThanh||k(i,j):=subs({frame_A_i(l,1)=frame_A_i(l,k)},Jinv_DoThanh||k(i,j)):
		end do:
		for m to NQJ_parallel do
			n := m + (k-1)*NQJ_parallel:
			#Jinv_DoThanh||k(i,j):=subs({qJ_i_s(m,1)=qJ_i_s(m,k)},Jinv_DoThanh||k(i,j)):
		end do:
	end do:
	#Jinv_DoThanh_i(1..COLUMN,1..ROW,k) := Jinv_DoThanh||k:
end do:
printf("[%s] Jacobi-Matrizen der Beinketten berechnet. CPU-Zeit bis hier: %1.2fs.\n", FormatTime("%Y-%m-%d %H:%M:%S"), time()-st):
# Gesamt Jacobi-Matrix
# Berechnung der inv. Jacobi-Matrix: EE-Geschwindigkeiten -> aktive Gelenkgeschwindigkeiten
# Abdellatif2007 S.21 (2.22)
if type(AKTIV,scalar) then
  AKTIV := Matrix(N_LEGS,1,AKTIV);
end if:
Tmp := simplify(JB1inv[AKTIV(1,1),1..3].U1):
for i from 2 to N_LEGS do
  Tmp := <Tmp;JB||i||inv[AKTIV(i,1),1..3].U||i>:
end do:
Jinv := Tmp:

# Reduziere die Gesamt-Jacobi-Matrix auf Freiheitsgrade des Roboters
printf("[%s] Starte Reduzierung der Gesamt-Jacobi-Matrix. CPU-Zeit bis hier: %1.2fs.\n", FormatTime("%Y-%m-%d %H:%M:%S"), time()-st):
IdentMat := IdentityMatrix(6,6):
IdentMatMas := IdentityMatrix(6,6):
counter := 0:
for i to 3 do
  if not(xE_t(i) = 0) then
    if counter = 0 then
      counter := 1;
      JinvRed := Column(Jinv, i);
      pivotMat := IdentMat(i,..);
    else
      JinvRed := <JinvRed|Column(Jinv, i)>;
      pivotMat := <pivotMat;IdentMat(i,..)>;
    end if;
  end if:
end do:
if robotType = 2 then
  if rotPlanar = z then
    JinvRed := <JinvRed|Column(Jinv, 6)>;
    pivotMat := <pivotMat;IdentMat(6,..)>;
  elif rotPlanar = y then
    JinvRed := <JinvRed|Column(Jinv, 5)>;
    pivotMat := <pivotMat;IdentMat(5,..)>;
  elif rotPlanar = x then
    JinvRed := <JinvRed|Column(Jinv, 4)>;
    pivotMat := <pivotMat;IdentMat(4,..)>;
  end if;
elif robotType = 1 then
  if counter = 0 then
    JinvRed := Jinv(..,1..3);
    pivotMat := IdentMat(4..6,..);
  else
    JinvRed := <JinvRed|Jinv(..,4..6)>;
    pivotMat := <pivotMat;IdentMat(4..6,..)>;
  end if;
end if;
counter := 0:
for i from 1 to 6 do
  if not(xE_t(i) = 0) then
    if counter = 0 then
      counter := 1;
      pivotMatMas := IdentMat(i,..);
    else
      pivotMatMas := <pivotMatMas;IdentMat(i,..)>;
    end if;
  end if:
end do:
Jinv := JinvRed:
Jinv := simplify(Jinv):
# Export
printf("[%s] Beginne mit Code-Export. CPU-Zeit bis hier: %1.2fs.\n", FormatTime("%Y-%m-%d %H:%M:%S"), time()-st):
# Maple-Export
save pivotMat, pivotMatMas, P_i, Jinv, JB_i, JBD_i, JBinv_i, JBDinv_i, U_i, UD_i, sprintf("../codeexport/%s/tmp/kinematics_%s_platform_maple.m", robot_name, base_method_name):
MatlabExport(Jinv, sprintf("../codeexport/%s/tmp/Jinv_para_matlab.m", robot_name), codegen_opt):

max_stars_repo_name: SchapplM/robsynth-modelgen

max_issues_repo_name: SchapplM/robsynth-modelgen

max_forks_repo_name: SchapplM/robsynth-modelgen


text:
# Proof by PDF that X <~ standardChiSq(2) == Y <~ exponential(1/2)

PDF_chisq := (n, x) -> x^(1/2*n-1)*exp(-1/2*x)/(2^(1/2*n))/GAMMA(1/2*n);
simplify(PDF_chisq(2,x));

PDF_exponential := (l, x) -> l*exp(-l*x);
simplify(PDF_exponential(1/2,x));

evalb(PDF_chisq(2,x) = PDF_exponential(1/2,x));

max_stars_repo_name: vmchale/hakaru

max_issues_repo_name: vmchale/hakaru

max_forks_repo_name: vmchale/hakaru


text:
(*
 Copyright (C) 2017 M.A.L. Marques

 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: work_mgga_x *)

malpha  := 0.00186726:
coeff_d := [-9.800683e-01, -3.556788e-03, 6.250326e-03, -2.354518e-05, -1.282732e-04, 3.574822e-04]:

$include "gvt4.mpl"

f := (rs, x, t, u) -> -gtv4(malpha, coeff_d, x, 2*(t - K_FACTOR_C))/X_FACTOR_C:

max_stars_repo_name: rdietric/lsms

max_issues_repo_name: rdietric/lsms

max_forks_repo_name: rdietric/lsms


text:
# The block below either calculates or reads from file the alternative
# Boys immersion, calibrated to have special behaviour at the points
# [+/- 1, +/- 1, +/- 1]/sqrt(3).

if false then
 make_boys_embedding_alt():
 save(boys_M0,boys_M1,boys_a1,boys_embedding_alt,cat(data_dir,"/boys_embedding_alt.m")):
else
 read(cat(data_dir,"/boys_embedding_alt.m")):
fi:

bef := (x) -> evalf(boys_embedding_alt(x)):

check_bef := proc()
 _ASSERT(
  max(evalf([
  dd(bef(u[0]),u[0]),
  dd(bef(u[1]),u[6]),
  dd(bef(u[2]),u[5]),
  dd(bef(u[3]),u[4]),
  dd(bef(u[4]),u[4]),
  dd(bef(u[5]),u[5]),
  dd(bef(u[6]),u[6]),
  dd(bef(u[7]),u[0])
  ])) < 10^(-6),
  "bef(u[i])=u[j] for expected pairs (i,j)"
 );
end:

# Miscellaneous functions
dp := `dot/R`(3):
dd := `d_2/R`(3):
`mmu/H` := apply_assoc(`mu/H`,[0,0,0,1]):
rot := (x) -> [x[2],x[3],x[1]]:

# Unit quaternions.
ii[1] := [1,0,0,0]:
ii[2] := [0,1,0,0]:
ii[3] := [0,0,1,0]:
ii[4] := [0,0,0,1]:

# Misccellaneous quaternions
zt[1] := [1,1+sqrt(3),1-sqrt(3),0] /~ 3:
zt[2] := [1-sqrt(3),1,1+sqrt(3),0] /~ 3:
zt[3] := [1+sqrt(3),1-sqrt(3),1,0] /~ 3:
al := [ 1, 1, 1, 0] /~ sqrt(3):
om := [ 1, 1, 1, 1] /~ 2:
ob := [-1,-1,-1, 1] /~ 2:

# Various unit vectors in R3.  For each i, the vectors u[i], v[i] and w[i]
# form an orthonormal frame.  z[i] is in the plane spanned by u[0] and u[i]
# and is orthogonal to u[0].

u := table(): v := table(): w := table(): z := table():
u[ 0] := [ 1, 1, 1] /~ sqrt(3): v[ 0] := [ 2,-1,-1] /~ sqrt(6): w[ 0] := [ 0, 1,-1] /~ sqrt(2):
u[ 1] := [ 1, 1,-1] /~ sqrt(3): v[ 1] := [ 2,-1, 1] /~ sqrt(6): w[ 1] := [ 0,-1,-1] /~ sqrt(2): z[ 1] := [ 1, 1,-2] /~ sqrt(6):
u[ 2] := [ 1,-1, 1] /~ sqrt(3): v[ 2] := [-1, 1, 2] /~ sqrt(6): w[ 2] := [-1,-1, 0] /~ sqrt(2): z[ 2] := [ 1,-2, 1] /~ sqrt(6):
u[ 3] := [-1, 1, 1] /~ sqrt(3): v[ 3] := [ 1, 2,-1] /~ sqrt(6): w[ 3] := [-1, 0,-1] /~ sqrt(2): z[ 3] := [-2, 1, 1] /~ sqrt(6):
u[ 4] := [ 1,-1,-1] /~ sqrt(3): v[ 4] := [-1,-2, 1] /~ sqrt(6): w[ 4] := [-1, 0,-1] /~ sqrt(2): z[ 4] := [ 2,-1,-1] /~ sqrt(6):
u[ 5] := [-1, 1,-1] /~ sqrt(3): v[ 5] := [ 1,-1,-2] /~ sqrt(6): w[ 5] := [-1,-1, 0] /~ sqrt(2): z[ 5] := [-1, 2,-1] /~ sqrt(6):
u[ 6] := [-1,-1, 1] /~ sqrt(3): v[ 6] := [-2, 1,-1] /~ sqrt(6): w[ 6] := [ 0,-1,-1] /~ sqrt(2): z[ 6] := [-1,-1, 2] /~ sqrt(6):
u[ 7] := [-1,-1,-1] /~ sqrt(3): v[ 7] := [-2, 1, 1] /~ sqrt(6): w[ 7] := [ 0, 1,-1] /~ sqrt(2):

pol  := unapply(expand(cos(s) *~ u[0] +~ sin(s) *~ (cos(t) *~ v[0] +~ sin(t) *~ w[0])),s,t):
pol0 := (s,t) -> [cos(s),sin(s)*cos(t),sin(s)*sin(t)]:

# The function wave(t) has trigonometric polynomial entries and lies on
# the unit sphere, passing through all the points u[1],...,u[6].
wave := unapply(
(sin(3*t)/3) *~ u[0] +~
((sqrt(2)/3-1/2) * sin(5*t)) *~ v[0] +~
((sqrt(2)/3-1/2) * cos(5*t) * (-1)) *~ w[0] +~
((sqrt(2)/3+1/2) * sin(t)) *~ v[0] +~
((sqrt(2)/3+1/2) * cos(t)) *~ w[0],
t):

# wband(t,s) parametrises a band with wave(t) = wband(t,0) at the centre
wave_normal := unapply(combine(expand(simplify(expand(cross_product(wave(t),map(diff,wave(t),t)))))),t):
wband := unapply(sin(s) *~ wave_normal(t) +~ cos(s) *~ wave(t),t,s):

check_wband := proc()
 _ASSERT(
  simplify(expand(dp(wave(t),wave(t)) - 1)) = 0,
  "wave(t) is a unit vector"
 );

 _ASSERT(
  simplify(expand(wave(t - Pi/3) +~ rot(wave(t)))) = [0$3],
  "wave(t) is Z/3-equivariant"
 );

 _ASSERT(
  {seq(simplify(wave((2 * i - 1) * Pi/6) -~ u[[1,4,2,6,3,5][i]]),i=1..6)} = {[0$3]},
  "wave(t) passes through u[1],...,u[6]"
 );
end:

# A band in S2 with central circle perpendicular to u[0]
hband := (t,s) ->
 sin(s) *~ u[0] +~ cos(s) *~ (cos(t) *~ v[0] +~ sin(t) *~ w[0]):

# A family of bands in S2 whose central circles are lines of longitude
# (with u[0] as the North pole).

vband := (p) -> unapply(combine(
                 cos(s) *~ ( cos(t) *~ (cos(p) *~ v[0] +~ sin(p) *~ w[0]) +~ sin(t) *~ u[0]) +~
                 sin(s) *~ ( -sin(p) *~ v[0] +~ cos(p) *~ w[0] )),
                t,s):

check_bands := proc()
 _ASSERT(simplify(hband(0,0) -~ vband(0)(0,0)) = [0$3],
  "Intersection of hband and vband(0)");

 _ASSERT(simplify(hband(Pi/6,0) -~ vband(Pi/6)(0,0)) = [0$3],
  "Intersection of hband and vband(Pi/6)");

 _ASSERT(vband(0)(Pi/2,0) -~ u[0] = [0$3],"u[0] on vband(0)");

 _ASSERT(simplify(vband(0)(Pi-arctan(sqrt(2)/4),0) -~ u[3]) = [0$3],"u[3] on vband(0)");

 _ASSERT(simplify(vband(0)(-arctan(sqrt(2)/4),0) -~ u[4]) = [0$3],"u[4] on vband(0)");

 _ASSERT(vband(Pi/6)(Pi/2,0) -~ u[0] = [0$3],"u[0] on vband(Pi/6)");

 _ASSERT({
  simplify(wband(    Pi/6,0) -~ u[1]),
  simplify(wband(3 * Pi/6,0) -~ u[4]),
  simplify(wband(5 * Pi/6,0) -~ u[2]),
  simplify(wband(7 * Pi/6,0) -~ u[6]),
  simplify(wband(9 * Pi/6,0) -~ u[3]),
  simplify(wband(11* Pi/6,0) -~ u[5])} = {[0$3]},
  "u[1],...,u[6] on wband");

 _ASSERT({
  simplify(wband(        0,0) -~ hband(15*Pi/6,0)),
  simplify(wband(     Pi/3,0) -~ hband(13*Pi/6,0)),
  simplify(wband( 2 * Pi/3,0) -~ hband(11*Pi/6,0)),
  simplify(wband( 3 * Pi/3,0) -~ hband( 9*Pi/6,0)),
  simplify(wband( 4 * Pi/3,0) -~ hband( 7*Pi/6,0)),
  simplify(wband( 5 * Pi/3,0) -~ hband( 5*Pi/6,0))} = {[0$3]},
  "Intersections of wband with hband");

 _ASSERT(
  simplify(expand(hband(t-2*Pi/3,s) -~ rot(hband(t,s)))) = [0$3],
  "hband is Z/3-equivariant");

 _ASSERT(
  simplify(expand(hband(t+Pi/3,-s) +~ rot(hband(t,s)))) = [0$3],
  "hband is Z/6-equivariant");

 _ASSERT(vband(0)(t+Pi,-s) +~ vband(0)(t,s) = [0$3],
  "vband(0) is Z/2-equivariant");

 _ASSERT(vband(Pi/6)(t+Pi,-s) +~ vband(Pi/6)(t,s) = [0$3],
  "vband(Pi/6) is Z/2-equivariant");

 _ASSERT(simplify(expand(wband(t-Pi/3,0) +~ rot(wband(t,0)))) = [0$3],
  "wband is Z/6-equivariant");

 _ASSERT(wband(t+Pi,0) +~ wband(t,0) = [0$3],
  "wband is Z/2-equivariant");

end:

# An embedding of the Mobius band in R^3 whose centra curve is
# a circle of radius sqrt(8/9) on S2 passing through u[1],u[2] and u[3],
# and perpendicular to u[0].

triple_mobius := unapply(
(1 + s * cos(3*t)) *~ ((1/3) *~ u[0] +~ sqrt(8/9) *~ (cos(2*t) *~ v[0] +~ sin(2*t) *~ w[0])) +~
 (s * sin(3*t) * sqrt(2/27)) *~ [cos(2*t)-2,cos(2*t-2*Pi/3)-2,cos(2*t+2*Pi/3)-2],
t,s):

check_triple_mobius := proc()
 _ASSERT(
  simplify(expand(dp(triple_mobius(t,0),triple_mobius(t,0)) - 1)) = 0,
  "triple_mobius(t,0) is a unit vector");

 _ASSERT(
  expand(dp(triple_mobius(t,0),u[0]) - 1/3) = 0,
  "triple_mobius(t,0) lies in a plane perpendicular to u[0]");

 _ASSERT(
  simplify(expand(dp(map(coeff,expand(triple_mobius(t,s)),s),map(diff,triple_mobius(t,0),t)))) = 0,
  "The triple_mobius() offset vector is perpendicular to the line of the central circle"
 );

 _ASSERT({
  simplify(triple_mobius(  Pi/6,0) -~ u[1]),
  simplify(triple_mobius(5*Pi/6,0) -~ u[2]),
  simplify(triple_mobius(3*Pi/6,0) -~ u[3])} = {[0$3]},
  "u[1],u[2],u[3] on triple_mobius()");

 _ASSERT(
  abs(twist_number(triple_mobius) - 3) < 10^(-6),
  "triple_mobius() twist number is 3"
 );
end:

# This is a smooth map R -> R^3 whose image is close to an equilateral
# triangle
smooth_triangle := unapply([
 3 * cos(2*t-  Pi/3) + cos(4*t-5*Pi/3),
 3 * cos(2*t-5*Pi/3) + cos(4*t-  Pi/3),
 3 * cos(2*t-3*Pi/3) + cos(4*t-3*Pi/3)
] /~ (sqrt(27)) +~ u[0]/~3,t):

smooth_triangle_p := Pi/4:

smooth_triangle_offset :=
 unapply(( sin(3*(t-Pi/4))*sqrt(2/3)) *~ [cos(2*t-  Pi/3),cos(2*t-5*Pi/3),cos(2*t-3*Pi/3)] +~
         (-cos(3*(t-Pi/4))) *~ u[0],t):

triangle_mobius := unapply(
 smooth_triangle(t) +~ s *~ smooth_triangle_offset(t),
 t,s
):

check_smooth_triangle := proc()
 local t;

 _ASSERT(
  simplify(expand(smooth_triangle(t-2*Pi/3) -~ rot(smooth_triangle(t)))) = [0$3],
  "smooth_triangle is Z/3-equivariant"
 );

 _ASSERT({
  simplify(expand(smooth_triangle(     0) -~ u[1])),
  simplify(expand(smooth_triangle(  Pi/3) -~ u[2])),
  simplify(expand(smooth_triangle(2*Pi/3) -~ u[3]))} = {[0$3]},
  "u[1],u[2],u[3] on smooth_triangle"
 );

 _ASSERT(
  abs(twist_number(triangle_mobius) - 3) < 10^(-6),
  "triangle_mobius() twist number is 3"
 );
end:

make_band_plots := proc()
 global hband_plot,vband_plot,wband_plot,triangle_mobius_plot,usphere_plot,frame_plot;
 local s,t,opts;

 opts := t=0..2*Pi,s=-0.2..0.2,style=patchnogrid,scaling=constrained,axes=none;

 vband_plot := table():

 hband_plot       :=  plot3d(hband(t,s),opts);
 vband_plot[0]    :=  plot3d(vband(0)(t,s),opts);
 vband_plot[Pi/6] :=  plot3d(vband(Pi/6)(t,s),opts);
 wband_plot       :=  plot3d(wband(t,s),opts);

 triangle_mobius_plot :=  plot3d(triangle_mobius(t,s),opts);

 usphere_plot := plot3d(
  cos(s) *~ u[0] +~ sin(s) *~ (cos(t) *~ v[0] +~ sin(t) *~ w[0]),
  s=0..Pi,t=0..2*Pi,colour=grey,style=wireframe,
  scaling=constrained,axes=none
 ):

 frame_plot := display(
  usphere_plot,
  point(u[0],colour=red,symbolsize=20),
  seq(point(u[i],colour=blue,symbolsize=20),i=1..3),
  seq(point(u[i],colour=cyan,symbolsize=20),i=4..6),
  point(u[7],colour=magenta,symbolsize=20),
  seq(spacecurve(cos(t) *~ u[0] +~ sin(t) *~ z[i],t=0..arccos(1/3),colour=orange),i=1..3),
  seq(spacecurve(cos(t) *~ u[0] +~ sin(t) *~ z[i],t=arccos(1/3)..Pi,colour=green),i=1..3),
  seq(spacecurve(cos(t) *~ u[7] +~ sin(t) *~ z[i],t=0..arccos(1/3),colour=orange),i=4..6),
  seq(spacecurve(cos(t) *~ u[7] +~ sin(t) *~ z[i],t=arccos(1/3)..Pi,colour=green),i=4..6),
  spacecurve(cos(t) *~ v[0] +~ sin(t) *~ w[0],t=0..2*Pi,colour=cyan),
  seq(point(cos(k*Pi/6) *~ v[0] +~ sin(k*Pi/6) *~ w[0],colour=black,symbolsize=20),k=0..11),
  spacecurve(wave(t),t=0..2*Pi,colour=black),
  scaling=constrained,axes=none
 ):

 save(hband_plot,vband_plot,wband_plot,triangle_mobius_plot,frame_plot,usphere_plot,
      cat(data_dir,"/boys_band_plots.m"));
end:

load_band_plots := proc()
 read(cat(data_dir,"/boys_band_plots.m"));
end:

make_band_be_plots := proc()
 global hband_be_plot,vband_be_plot,wband_be_plot;
 local s,t,opts;

 opts := t=0..Pi,s=-0.2..0.2,numpoints=10000,style=patchnogrid,scaling=constrained,axes=none;

 vband_be_plot := table():

 hband_be_plot       :=  plot3d(bef(hband(t,s)),opts);
 vband_be_plot[0]    :=  plot3d(bef(vband(0)(t,s)),opts);
 vband_be_plot[Pi/6] :=  plot3d(bef(vband(Pi/6)(t,s)),opts);
 wband_be_plot       :=  plot3d(bef(wband(t,s)),opts);

 save(hband_be_plot,vband_be_plot,wband_be_plot,
      cat(data_dir,"/boys_band_be_plots.m"));
end:

load_band_be_plots := proc()
 read(cat(data_dir,"/boys_band_be_plots.m"));
end:

# If f : R2 -> R3 with f(t+2*Pi=f(t) this measures the amount of twisting,
# so a standard Mobius band gives 1 and the maps triple_mobius and
# triangle_mobius give 3.

twist_number := proc(f)
 local N,e,a,b,c,z,i;
 N := 24;
 e := 0.001;
 a := table():
 b := table():
 c := table():
 z := table():
 for i from 0 to 2*N-1 do
  a[i] := evalf(f(i*Pi/N,0));
  b[i] := evalf((f(i*Pi/N,e) -~ f(i*Pi/N,-e))/~e);
  b[i] := b[i] /~ sqrt(dp(b[i],b[i]));
 od:
 a[2*N] := a[0]; a[2*N+1] := a[1];
 b[2*N] := b[0]; b[2*N+1] := b[1];

 for i from 0 to 2*N do
  c[i] := cross_product(u[0],a[i+1] -~ a[i]);
  c[i] := c[i] /~ sqrt(dp(c[i],c[i]));
  z[i] := dp(b[i],c[i]) + I * dp(b[i],u[0]);
 od:

 return evalf(add(argument(z[i+1]/z[i]),i=0..2*N-1) / (2*Pi));
end:

# For a homogeneous quadratic map q : R4 -> R, return the matrix M
# such that q(x) = x^T M x.

quadratic_coeffs := proc(u)
 local M,i,j,c;

 M := Matrix(4,4,shape=symmetric):
 for i from 1 to 4 do M[i,i] := coeff(u,x[i],2); od:

 for i from 1 to 3 do
  for j from i+1 to 4 do
   c := coeff(coeff(u,x[i],1),x[j],1)/2;
   M[i,j] := c;
   M[j,i] := c;
  od:
 od:

 return M;
end:

# Construct a matrix of coefficients for a discrete Fourier transform.
# This will use 2*N sample points and return a trigonometric polynomial
# of degree d.

set_fourier_matrix := proc(N,d)
 global fourier_N,fourier_d,fourier_matrix;
 local Pi0,T,i,j,k;

 Pi0 := evalf(Pi);
 T := Matrix(2*d+1,2*N):
 for i from 1 to 2*N do
  T[1,i] := 1/(2*N);
  for k from 1 to d do
   T[2*k  ,i] := sin(i*k*Pi0/N)/N;
   T[2*k+1,i] := cos(i*k*Pi0/N)/N;
  od:
 od:

 fourier_N := N;
 fourier_d := d;
 fourier_matrix := T;
 return T;
end:

set_fourier_matrix(480,12):

# Calculate an approximate Fourier series for bef o b, for a function
# b : R2 -> S2 with b(t + 2*Pi,s) = b(t,s).  The result is returned as
# a table with many different entries.

fourier_approx := proc(b)
 local t,c,Pi0,d,N,U,F,m,k,e,cxyz,cuvw;

 c := table():
 Pi0 := evalf(Pi);
 d := fourier_d;
 N := fourier_N;
 U := map(evalf,Transpose(Matrix([u[0],v[0],w[0]])));
 e := 10.^(-3);

 c["vals"]    := [seq(evalf(b(i*Pi0/N, 0)),i=1..2*N)];
 c["offset0"] := [seq(evalf(b(i*Pi0/N, e)),i=1..2*N)];
 c["offset1"] := [seq(evalf(b(i*Pi0/N,-e)),i=1..2*N)];

 c["vals_be"]    := map(bef,c["vals"]);
 c["offset0_be"] := map(bef,c["offset0"]);
 c["offset1_be"] := map(bef,c["offset1"]);
 c["vals_dbe"]   :=
  [seq((c["offset0_be"][i] -~ c["offset1_be"][i]) /~ (2*e),i=1..2*N)];

 c["coeffs_be"]      := fourier_matrix . Matrix(c["vals_be"]);
 c["uvw_coeffs_be"]  := c["coeffs_be"] . U;
 c["coeffs_dbe"]     := fourier_matrix . Matrix(c["vals_dbe"]);
 c["uvw_coeffs_dbe"] := c["coeffs_dbe"] . U;

 for k in ["coeffs_be","uvw_coeffs_be","coeffs_dbe","uvw_coeffs_dbe"] do
  c[k] := trim(c[k],10.^(-6));
 od:

 F := [1,seq(op([sin(k*t),cos(k*t)]),k=1..d)];
 c["approx_be"]  := unapply(convert(Transpose(Vector(F)) . c["coeffs_be" ],list),t);
 c["approx_dbe"] := unapply(convert(Transpose(Vector(F)) . c["coeffs_dbe"],list),t);
 c["approx"] := unapply(c["approx_be"](t) +~ c["approx_dbe"](t) *~ s,t,s);

 for k in ["coeffs_be","uvw_coeffs_be","coeffs_dbe","uvw_coeffs_dbe"] do
  c[k] := convert(c[k],listlist);
 od:

 return eval(c):
end:

if false then
 vband_approx := table():
 printf("hband\n");
 hband_approx := fourier_approx(hband):
 printf("vband 0\n");
 vband_approx[0] := fourier_approx(vband(0)):
 printf("vband 1\n");
 vband_approx[Pi/6] := fourier_approx(vband(Pi/6)):
 printf("wband\n");
 wband_approx := fourier_approx(wband):
 save(hband_approx,vband_approx,wband_approx,
      cat(data_dir,"/boys_approx.m")):
else
 read(cat(data_dir,"/boys_approx.m")):
fi:

make_approx_plot := proc(a)
 a["plot"] :=
  plot3d(a["approx"](t,s),t=0..2*Pi,s=-0.1..0.1,
         style=patchnogrid,scaling=constrained,axes=none,args[2..-1]):
 return a["plot"];
end:

make_approx_plots := proc()
 global ribbon_plot;
 local opts;

 make_approx_plot(hband_approx,numpoints=5000):
 make_approx_plot(vband_approx[0],numpoints=8000):
 make_approx_plot(vband_approx[Pi/6],numpoints=5000):
 make_approx_plot(wband_approx,numpoints=5000):

 opts := t=0..2*Pi,s=-0.02..0.02,numpoints=6000,style=patchnogrid:

 ribbon_plot :=
  display(
   plot3d(vband_approx[0]["approx"](t,s),             opts,colour=red),
   plot3d(rot(vband_approx[0]["approx"](t,s)),        opts,colour=red),
   plot3d(rot(rot(vband_approx[0]["approx"](t,s))),   opts,colour=red),
   plot3d(vband_approx[Pi/6]["approx"](t,s),          opts,colour=blue),
   plot3d(rot(vband_approx[Pi/6]["approx"](t,s)),     opts,colour=blue),
   plot3d(rot(rot(vband_approx[Pi/6]["approx"](t,s))),opts,colour=blue),
   plot3d(hband_approx["approx"](t,s),                opts,colour=green),
   plot3d(wband_approx["approx"](t,s),                opts,colour=magenta),
   scaling=constrained,axes=none
  );

end:

# Given asome approximate Fourier transforms, try to work out the general
# form.  For coefficients of small absolute value, we assume that they
# are really supposed to be zero.  For coefficients that are sufficiently
# cloe, we assume that they should actually be the same.  Signs are
# inserted to ensure that the values of all parameters should be positive.

reset_outline := proc()
 global outline_k,outline_a;
 outline_k := 0;
 outline_a := table():
end:

reset_outline():

make_outline := proc(b)
 global outline_k,outline_a;
 local p,q,i,j,k,m,vp,d,x,F,found;

 p := [op(map(op,b["uvw_coeffs_be"])),
       op(map(op,b["uvw_coeffs_dbe"]))];

 q := NULL:

 for i from 1 to nops(p) do
  vp := p[i];
  if abs(vp) < 0.05 then
   q := q,0;
  else
   found := false;
   for j from 1 to outline_k do
    if abs(vp - outline_a[j]) < 0.001 then
     q := q,a[j];
     found := true;
     break;
    fi;
    if abs(vp + outline_a[j]) < 0.001 then
     q := q,-a[j];
     found := true;
     break;
    fi;
   od:
   if not(found) then
    outline_k := outline_k + 1;
    outline_a[outline_k] := abs(vp);
    q := q,signum(vp) * a[outline_k];
   fi;
  fi;
 od:

 q := [q];

 m := nops(q)/6;
 d := (m-1)/2;
 q := [[seq([q[3*i-2],q[3*i-1],q[3*i]],i=1..m)],
       [seq([q[3*i-2],q[3*i-1],q[3*i]],i=m+1..2*m)]];

 b["uvw_coeffs_be_outline"]  := q[1];
 b["uvw_coeffs_dbe_outline"] := q[2];

 x := [0,0,0];
 F := [1,seq(op([sin(k*t),cos(k*t)]),k=1..d)];

 b["outline_be"] := unapply(
 add(F[i] * q[1][i][1],i=1..m) *~ u[0] +~
 add(F[i] * q[1][i][2],i=1..m) *~ v[0] +~
 add(F[i] * q[1][i][3],i=1..m) *~ w[0],t);

 b["outline_dbe"] := unapply(
 add(F[i] * q[2][i][1],i=1..m) *~ u[0] +~
 add(F[i] * q[2][i][2],i=1..m) *~ v[0] +~
 add(F[i] * q[2][i][3],i=1..m) *~ w[0],t);
end:

# Here we find the general form of the Fourier coefficients for bef o f
# with f in {hband, vband(0), vband(Pi/6), wband}.  Then we construct a list
# of relations that must be imposed to ensure that the approximations fit
# together correctly and have the expected behaviour at the points u[i].

find_approx_form := proc()
 global approx_form_rels,approx_form_sols;
 local t;

 reset_outline():
 make_outline(hband_approx):
 make_outline(vband_approx[0]):
 make_outline(vband_approx[Pi/6]):
 make_outline(wband_approx):

 approx_form_rels := expand(simplify(map(op,expand([
 hband_approx["outline_be"](0) -~ vband_approx[0]["outline_be"](0),
 hband_approx["outline_be"](Pi/6) -~ vband_approx[Pi/6]["outline_be"](0),
 vband_approx[0]["outline_be"](Pi/2) -~ u[0],
 vband_approx[Pi/6]["outline_be"](Pi/2) -~ u[0],
 vband_approx[0]["outline_be"](Pi - arctan(sqrt(2)/4)) -~ u[4],
 vband_approx[0]["outline_be"](   - arctan(sqrt(2)/4)) -~ u[4],
 wband_approx["outline_be"](     Pi/6,0) -~ u[6],
 wband_approx["outline_be"]( 3 * Pi/6,0) -~ u[4],
 wband_approx["outline_be"]( 5 * Pi/6,0) -~ u[5],
 wband_approx["outline_be"]( 7 * Pi/6,0) -~ u[6],
 wband_approx["outline_be"]( 9 * Pi/6,0) -~ u[4],
 wband_approx["outline_be"](11 * Pi/6,0) -~ u[5],
 wband_approx["outline_be"](        0,0) -~ hband_approx["outline_be"](15*Pi/6,0),
 wband_approx["outline_be"](     Pi/3,0) -~ hband_approx["outline_be"](13*Pi/6,0),
 wband_approx["outline_be"]( 2 * Pi/3,0) -~ hband_approx["outline_be"](11*Pi/6,0),
 wband_approx["outline_be"]( 3 * Pi/3,0) -~ hband_approx["outline_be"]( 9*Pi/6,0),
 wband_approx["outline_be"]( 4 * Pi/3,0) -~ hband_approx["outline_be"]( 7*Pi/6,0),
 wband_approx["outline_be"]( 5 * Pi/3,0) -~ hband_approx["outline_be"]( 5*Pi/6,0),
 map(coeffs,expand(hband_approx["outline_be"](t-2/3*Pi)   -~   rot(hband_approx["outline_be" ](t))),{sin(t),cos(t)}),
 map(coeffs,expand(hband_approx["outline_dbe"](t-2/3*Pi)  -~   rot(hband_approx["outline_dbe"](t))),{sin(t),cos(t)}),
 map(coeffs,expand(hband_approx["outline_be"](t+Pi/3)     -~   rot(hband_approx["outline_be" ](t))),{sin(t),cos(t)}),
 map(coeffs,expand(hband_approx["outline_dbe"](t+Pi/3)    +~   rot(hband_approx["outline_dbe"](t))),{sin(t),cos(t)}),
 map(coeffs,expand(vband_approx[0]["outline_be"](t+Pi)    -~ vband_approx[   0]["outline_be" ](t)) ,{sin(t),cos(t)}),
 map(coeffs,expand(vband_approx[0]["outline_dbe"](t+Pi)   +~ vband_approx[   0]["outline_dbe"](t)) ,{sin(t),cos(t)}),
 map(coeffs,expand(vband_approx[Pi/6]["outline_be"](t+Pi) -~ vband_approx[Pi/6]["outline_be" ](t)) ,{sin(t),cos(t)}),
 map(coeffs,expand(vband_approx[Pi/6]["outline_dbe"](t+Pi)-~ vband_approx[Pi/6]["outline_dbe"](t)) ,{sin(t),cos(t)}),
 map(coeffs,expand(wband_approx["outline_be"](t-Pi/3)     -~  rot(wband_approx["outline_be"  ](t))),{sin(t),cos(t)}),
 simplify(expand(dp(hband_approx["outline_dbe"](Pi/12),u[0]))),
 #simplify(expand(dp(wband_approx["outline_dbe"](Pi/12),u[0]))),
 simplify(expand(dp(vband_approx[0]["outline_dbe"](Pi/6),u[0]))),
 simplify(expand(dp(vband_approx[Pi/6]["outline_dbe"](Pi/2),u[0]))),
 NULL])))):

 approx_form_sols := solve(approx_form_rels);
end:

make_boys_cube_complex := proc()
 global boys_cube_complex;
 local N,T,V,P,E,F,C,i,j,k,e,a;

 T := table():
 N := 50;

 V := [seq(i,i=0..N-1)];
 T["vertices"] := V;

 P := table():

 P[ 0] := [ 2, 2, 2]; P[ 1] := [ 2, 2,-2]; P[ 2] := [ 2,-2, 2]; P[ 3] := [-2, 2, 2];
 P[ 4] := [ 2,-2,-2]; P[ 5] := [-2, 2,-2]; P[ 6] := [-2,-2, 2]; P[ 7] := [-2,-2,-2];
 P[ 8] := [ 2, 2, 0]; P[ 9] := [ 2, 0, 2]; P[10] := [ 0, 2, 2]; P[11] := [ 0,-2,-2];
 P[12] := [-2, 0,-2]; P[13] := [-2,-2, 0]; P[14] := [ 2, 0,-2]; P[15] := [ 2,-2, 0];
 P[16] := [ 0,-2, 2]; P[17] := [-2, 0, 2]; P[18] := [-2, 2, 0]; P[19] := [ 0, 2,-2];
 P[20] := [ 2, 0, 0]; P[21] := [ 0, 0, 2]; P[22] := [ 0, 2, 0];
 P[23] := [-2, 0, 0]; P[24] := [ 0, 0,-2]; P[25] := [ 0,-2, 0];
 P[26] := [ 2, 1, 1]; P[27] := [ 2,-1, 1]; P[28] := [ 2,-1,-1]; P[29] := [ 2, 1,-1];
 P[30] := [ 1, 1, 2]; P[31] := [-1, 1, 2]; P[32] := [-1,-1, 2]; P[33] := [ 1,-1, 2];
 P[34] := [ 1, 2, 1]; P[35] := [ 1, 2,-1]; P[36] := [-1, 2,-1]; P[37] := [-1, 2, 1];
 P[38] := [-2,-1,-1]; P[39] := [-2, 1,-1]; P[40] := [-2, 1, 1]; P[41] := [-2,-1, 1];
 P[42] := [-1,-1,-2]; P[43] := [ 1,-1,-2]; P[44] := [ 1, 1,-2]; P[45] := [-1, 1,-2];
 P[46] := [-1,-2,-1]; P[47] := [-1,-2, 1]; P[48] := [ 1,-2, 1]; P[49] := [ 1,-2,-1];

 T["embedding_dim"] := 3;
 T["embedding"] := eval(P);
 T["cube_embedding"] := eval(P);

 T["sphere_embedding"] := table():
 for i in V do
  a := P[i];
  a := a /~ sqrt(add(a[i]^2,i=1..3));
  T["sphere_embedding"][i] := a;
 od:

 E := NULL:
 for i from 0 to N-1 do
  for j from i + 1 to N-1 do
   a := sort(map(abs,P[j] -~ P[i]));
   if modp(P[i],2) *~ modp(P[j],2) = [0,0,0] and
      (a = [0,1,1] or a = [0,0,2]) then
    E := E,[i,j];
   fi;
  od:
 od:

 E := [E];
 T["edges"] := E;

 F := NULL;
 for e in E do
  for k from e[2] + 1 to N-1 do
   if member([e[1],k],E) and member([e[2],k],E) then
    F := F,[op(e),k];
   fi;
  od:
 od:

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

 T["vertex_index"] := table():

 for i in V do
  T["vertex_index"][T["cube_embedding"][i]] := i;
  T["vertex_index"][T["sphere_embedding"][i]] := i;
 od:

 T["hedges"] := [14,28,15,48,16,32,17,40,18,36,19,44,14]:
 T["hedges"] := map(sort,[seq([T["hedges"][i],T["hedges"][i+1]],i=1..nops(T["hedges"])-1)]):

 T["wedges"] := [1,14,4,15,2,16,6,17,3,18,5,19,1]:
 T["wedges"] := map(sort,[seq([T["wedges"][i],T["wedges"][i+1]],i=1..nops(T["wedges"])-1)]):

 C := table():

 for e in T[ "edges"] do C[e] := grey;  od:
 for e in T["hedges"] do C[e] := cyan;  od:
 for e in T["wedges"] do C[e] := black; od:

 C[[ 0, 8]] := orange: C[[ 0, 9]] := orange: C[[ 0,10]] := orange: C[[ 1, 8]] := orange:
 C[[ 2, 9]] := orange: C[[ 3,10]] := orange: C[[ 7,11]] := orange: C[[ 7,12]] := orange:
 C[[ 7,13]] := orange: C[[ 4,11]] := orange: C[[ 5,12]] := orange: C[[ 6,13]] := orange:
 C[[ 0,26]] := green:  C[[ 0,34]] := green:  C[[ 0,20]] := green:  C[[20,26]] := green:
 C[[22,34]] := green:  C[[21,30]] := green:  C[[20,28]] := green:  C[[22,36]] := green:
 C[[21,32]] := green:  C[[ 4,28]] := green:  C[[ 5,36]] := green:  C[[ 6,32]] := green:
 C[[ 1,44]] := green:  C[[ 2,48]] := green:  C[[ 3,40]] := green:  C[[24,44]] := green:
 C[[25,48]] := green:  C[[23,40]] := green:  C[[24,42]] := green:  C[[25,46]] := green:
 C[[23,38]] := green:  C[[ 7,42]] := green:  C[[ 7,46]] := green:  C[[ 7,38]] := green:

 T["edge_colour"] := eval(C):

 T["cube_plot"] :=
 display(
  plot3d([ s, t,-1] *~ 1.99,s=-1..1,t=-1..1,style=patchnogrid,colour=grey),
  plot3d([ s, t, 1] *~ 1.99,s=-1..1,t=-1..1,style=patchnogrid,colour=grey),
  plot3d([ s,-1, t] *~ 1.99,s=-1..1,t=-1..1,style=patchnogrid,colour=grey),
  plot3d([ s, 1, t] *~ 1.99,s=-1..1,t=-1..1,style=patchnogrid,colour=grey),
  plot3d([-1, s, t] *~ 1.99,s=-1..1,t=-1..1,style=patchnogrid,colour=grey),
  plot3d([ 1, s, t] *~ 1.99,s=-1..1,t=-1..1,style=patchnogrid,colour=grey),
  seq(line(P[e[1]],P[e[2]],colour=T["edge_colour"][e]),e in T["edges"]),
  axes=none
 );

 boys_cube_complex := eval(T):
 return eval(T);
end:

refine_boys_cube_complex := proc()
 global refined_boys_cube_complex;
 local T0,TT,S,P,p;
 T0 := `condense/simplicial_complex`(make_boys_cube_complex()):
  `set_edges/simplicial_complex`(T0):
  `set_faces/simplicial_complex`(T0):
  `normalise_embedding/simplicial_complex`(T0):
 T0["boys_embedding"] := map(bef,eval(T0["embedding"])):
 TT := [eval(T0)]:

# P := [0.8,0.6,0.4,0.3]:
 P := [0.8]:

 for p in P do
  S := select(e -> boys_edge_length(T0)(e) > p,T0["edges"]):
  T0 := `partial_triangular_subdivision/simplicial_complex`(T0,S):
  `normalise_embedding/simplicial_complex`(T0):
  T0["sphere_embedding"] := eval(T0["embedding"]):
  T0["boys_embedding"] := map(evalf,map(bef,T0["embedding"])):
  TT := [op(TT),eval(T0)]:
 od:

 refined_boys_cube_complex := eval(T0):

# save(refined_boys_cube_complex,cat(data_dir,"/refined_boys_cube_complex.m")):

 return eval(T0):
end:

edge_length      := (T) -> e -> `d_2/R`(3)(T["embedding"][e[1]],T["embedding"][e[2]]):
boys_edge_length := (T) -> e -> `d_2/R`(3)(T["boys_embedding"][e[1]],T["boys_embedding"][e[2]]):

edge_length_plot      := (T) ->
 listplot(sort(map(edge_length(T),[op(T["edges"])]))):
boys_edge_length_plot := (T) ->
 listplot(sort(map(boys_edge_length(T),[op(T["edges"])]))):

homogeneous_basis := proc(d,x)
 return [seq(seq(x[1]^i*x[2]^j*x[3]^(d-i-j),j=0..d-i),i=0..d)];
end:

extend_cyclic := proc(p)
 local x;

 return unapply([p([x[1],x[2],x[3]]),
                 p([x[2],x[3],x[1]]),
                 p([x[3],x[1],x[2]])],x)
end:

maybe_extend_cyclic := proc(p)
 if type(p(x),list) then
  return eval(p);
 else
  return extend_cyclic(p);
 fi;
end:

make_quadric := proc()
 global general_quadric0,general_quadric,quadric_point_rels,quadric_point_sol,
  special_quadric0,special_quadric,hband_quadric,vband_quadric;
 local B,x,rels,sol;

 B := homogeneous_basis(4,x);
 general_quadric0 := unapply(add(a[i] * B[i],i=1..nops(B)),x):
 general_quadric := extend_cyclic(eval(general_quadric0));

 quadric_point_rels := [
  op(general_quadric(u[0]) -~ u[0]),
  op(general_quadric(u[1]) +~ u[1])
 ]:

 hband_quadric :=
  map(collect,combine(simplify(expand(general_quadric(hband(t,0))))),{seq(cos(k*t),k=1..10),seq(sin(k*t),k=1..10)}):

 vband_quadric[0] :=
  map(collect,combine(simplify(expand(general_quadric(vband(0)(t,0))))),{seq(cos(k*t),k=1..10),seq(sin(k*t),k=1..10)}):

 vband_quadric[Pi/6] :=
  map(collect,combine(simplify(expand(general_quadric(vband(Pi/6)(t,0))))),{seq(cos(k*t),k=1..10),seq(sin(k*t),k=1..10)}):

 quadric_point_sol := solve(quadric_point_rels);

 # We now construct a 6-parameter family of quadrics such that
 # - u[0] maps to u[0], with specified behaviour on the tangent space
 # - u[4] maps to u[4] (which forces u[5] and u[6] to also be fixed, by equivariance)
 # - pol(Pi/3,Pi/6) maps to zero.
 special_quadric := eval(general_quadric):
 rels := simplify(expand({
  op(special_quadric(u[0]) -~ u[0] /~ sqrt(2)),
  op(map(coeffs,simplify(map(rem,expand(
     special_quadric(u[0] +~ t *~ v[0]) -~ special_quadric(u[0]) -~ t *~ ([-1,2,-1]/~sqrt(2))),t^2,t)),t)),
  op(map(coeffs,simplify(map(rem,expand(
     special_quadric(u[0] +~ t *~ w[0]) -~ special_quadric(u[0]) -~ t *~ ([-1,0,1]*~sqrt(3/2))),t^2,t)),t)),
  op(special_quadric(u[4]) -~ u[4] /~ sqrt(2)),
  op(special_quadric(pol(Pi/3,Pi/6)))
 })):
 sol := solve(rels,{seq(a[i],i=7..15)}):
 special_quadric := unapply(expand(subs(sol,special_quadric(x))),x):
 special_quadric0 := unapply(special_quadric(x)[1],x);
end:

make_sextic := proc()
 global general_sextic0,general_sextic,sextic_point_rels,sextic_point_sol,
  hband_sextic,vband_sextic;
 local B,x;

 B := homogeneous_basis(6,x);
 general_sextic0 := unapply(add(a[i] * B[i],i=1..nops(B)),x):
 general_sextic := extend_cyclic(general_sextic0);

 sextic_point_rels := [
  op(general_sextic(u[0]) -~ u[0]),
  op(general_sextic(u[1]) +~ u[1])
 ]:

 hband_sextic :=
  map(collect,combine(simplify(expand(general_sextic(hband(t,0))))),{seq(cos(k*t),k=1..10),seq(sin(k*t),k=1..10)}):

 vband_sextic[0] :=
  map(collect,combine(simplify(expand(general_sextic(vband(0)(t,0))))),{seq(cos(k*t),k=1..10),seq(sin(k*t),k=1..10)}):

 vband_sextic[Pi/6] :=
  map(collect,combine(simplify(expand(general_sextic(vband(Pi/6)(t,0))))),{seq(cos(k*t),k=1..10),seq(sin(k*t),k=1..10)}):

 sextic_point_sol := solve(sextic_point_rels);
end:

check_sextic := proc()
 local x,xx;
 xx := [x[1],x[2],x[3]];

 _ASSERT(
  general_sextic(-~xx) -~ general_sextic(xx) = [0$3],
  "general_sextic is Z/2-equivariant"
 );

 _ASSERT(
  general_sextic(rot(xx)) -~ rot(general_sextic(xx)) = [0$3],
  "general_sextic is Z/3-equivariant"
 );
end:

make_quadric():
make_sextic():

refine_sextic_a := proc()
 global sextic_a0,sextic_a;
 local sextic_rels,sextic_sol;

 sextic_rels := [op(sextic_point_rels),
   seq(coeff(combine(expand(dp(u[0],hband_sextic))),f,1), f in [cos(2*t),sin(4*t),cos(4*t),sin(6*t),cos(6*t)]),
   seq(coeff(combine(expand(dp(v[0],hband_sextic))),f,1), f in [cos(4*t),sin(6*t),cos(6*t)]),
   seq(coeff(combine(expand(dp(w[0],hband_sextic))),f,1), f in [sin(4*t),sin(6*t),cos(6*t)]),
   seq(coeff(combine(expand(dp(u[0],vband_sextic[0]))),f,1), f in [sin(4*t),sin(6*t),cos(6*t)]),
   seq(coeff(combine(expand(dp(v[0],vband_sextic[0]))),f,1), f in [sin(4*t),sin(6*t),cos(6*t)]),
   seq(coeff(combine(expand(dp(w[0],vband_sextic[0]))),f,1), f in [cos(2*t),sin(4*t),cos(4*t),sin(6*t),cos(6*t)]),
   seq(coeff(combine(expand(dp(u[0],vband_sextic[Pi/6]))),f,1), f in [sin(4*t),sin(6*t),cos(6*t)]),
   seq(coeff(combine(expand(dp(v[0],vband_sextic[Pi/6]))),f,1), f in [sin(4*t),sin(6*t),cos(6*t)]),
   seq(coeff(combine(expand(dp(w[0],vband_sextic[Pi/6]))),f,1), f in [cos(2*t),sin(4*t),cos(4*t),sin(6*t),cos(6*t)])
 ]:

 sextic_sol := solve(sextic_rels);
 sextic_a0 := unapply(simplify(expand(subs(sextic_sol,general_sextic0(x)))),x);
 sextic_a  := unapply(simplify(expand(subs(sextic_sol,general_sextic(x)))) ,x);
end:

refine_sextic_b := proc()
 global sextic_b0,sextic_b;
 local sextic_rels,sextic_sol;

 sextic_rels := [op(sextic_point_rels),
  op(map(coeffs,expand(simplify(expand(map(convert,map(series,expand(general_sextic(hband(t,s)) -~ triangle_mobius(t,s)),s=0,2),polynom,s)))),{sin(t),cos(t),s}))
 ]:

 sextic_sol := solve(sextic_rels);
 sextic_b0 := unapply(simplify(expand(subs(sextic_sol,general_sextic0(x)))),x);
 sextic_b  := unapply(simplify(expand(subs(sextic_sol,general_sextic(x)))) ,x);
end:

compare_terms := (u,v) ->
 abs(subs({x[1]=1,x[2]=1,x[3]=1},u)) < abs(subs({x[1]=1,x[2]=1,x[3]=1},v));

sort_terms := proc(p)
 local T;
 T := `if`(type(p,`+`),[op(p)],[p]);
 return sort(T,compare_terms);
end:

# Precompute coefficients for approximate integration over S2 using the
# triangulation T.
int_setup := proc(T)
 local a,b,c,d,f;
 T["face_area"] := table():
 T["face_centre"] := table():
 T["face_centre_be"] := table():
 for f in T["faces"] do
  a,b,c := op(map(i -> T["embedding"][i],f));
  d := a +~ b +~ c;
  d := d /~ `norm_2/R`(3)(d);
  T["face_centre"][f] := d;
  T["face_centre_be"][f] := bef(d);
  T["face_area"][f] := spherical_triangle_area(a,b,c);
 od:
end:

# Integration over S2, normalised to have total area 1.
sphere_int := proc(g,T)
 local u,f;
 u := 0;
 for f in T["faces"] do
  u := u + (g(T["face_centre"][f]) * T["face_area"][f]);
 od:
 u := evalf(u / (4*Pi));
 return u;
end:

sphere_int_be := proc(g,T)
 local u,f;
 u := [0,0,0];
 for f in T["faces"] do
  u := u +~ (g(T["face_centre"][f]) * T["face_area"][f]) *~ T["face_centre_be"][f];
 od:
 u := evalf(u / (4*Pi));
 return u;
end:

# Symbolic integration over the sphere, again normalised to have area 1
sint0 := proc(u)
 option remember;
 local v;
 v := subs({x[1]=sin(s)*cos(t),x[2]=sin(s)*sin(t),x[3]=cos(s)},u) * sin(s)/(4*Pi);
 v := expand(convert(v,exp));
 v := int(v,t=0..2*Pi);
 v := int(v,s=0..Pi);
 return v;
end:

sint := apply_linear(sint0,realcons):

ips := (u,v) -> evalf(sint(expand(u*v))):

# Faster symbolic integration for polynomials
sintp0 := proc(u)
 local c,d,i;
 c := 1;
 for i from 1 to 3 do
  d[i] := degree(u,x[i]);
 od:
 if u <> x[1]^d[1] * x[2]^d[2] * x[3]^d[3] then
  error("Argument is not monomial");
 fi:
 if modp(d[1],2) = 1 or modp(d[2],2) = 1 or modp(d[3],2) = 1 then
  return 0;
 fi;

 return doublefactorial(d[1]-1) *
        doublefactorial(d[2]-1) *
        doublefactorial(d[3]-1) /
        doublefactorial(d[1]+d[2]+d[3]+1);
end:

sintp := apply_linear(sintp0,realcons):

ipsp := (u,v) -> evalf(sintp(expand(u*v))):

make_orthonormal := proc(B,T)
 local Y,i,j;

 Y := table():
 for i from 1 to nops(B) do
  Y[i] := B[i]:
  for j from 1 to i-1 do
   Y[i] := expand(Y[i] - sphere_int(unapply(Y[i]*Y[j],x),T) * Y[j]);
  od:
  Y[i] := expand(Y[i] / sqrt(sphere_int(unapply(Y[i]^2,x),T0))):
 od:

 return eval(Y):
end:

make_immersion := proc(p,T_)
 local pp,T,s,t;

 pp := p;
 if not(type(pp,function)) then pp := unapply(pp,x); fi;
 if not(type(pp(x),list)) then
  pp := unapply([pp([x[1],x[2],x[3]]),
                 pp([x[2],x[3],x[1]]),
                 pp([x[3],x[1],x[2]])],x);
 fi;

 if nargs > 1 then T := eval(T_) else T := table(): fi;
 T["map"] := eval(pp);
 T["map0"] := unapply(pp(x)[1],x);

 pp := T["map0"](pol(s,t));
 pp := combine(expand(simplify(evalf(expand(pp)))));
 T["pol_map0"] := unapply(pp,s,t);
 T["pol_map"] :=
  unapply(evalf([pp(s,t),pp(s+2*Pi/3,t),pp(s,t-2*Pi/3)]),s,t);

 return eval(T);
end:

make_plots := proc(T)
 local opts,p;
 opts := style=patchnogrid,scaling=constrained,axes=none;
 T["plot"] := plot3d(T["pol_map"](s,t),t=0..2*Pi,s=0..Pi,opts,numpoints=8000);
 T["hband_plot"] := plot3d(T["map"](hband(t,s)),t=0..2*Pi,s=-0.2..0.2,opts,numpoints=8000);
 T["vband_plot"] := table():
 for p in [0,Pi/6] do
  T["vband_plot"][p] := plot3d(T["map"](vband(p)(t,s)),t=0..2*Pi,s=-0.2..0.2,opts,numpoints=8000);
 od:

 return T["plot"];
end:

orth_proj := (x) -> <
 <x[2]^2+x[3]^2|-x[1]*x[2]|-x[1]*x[3]>,
 <-x[1]*x[2]|x[1]^2+x[3]^2|-x[2]*x[3]>,
 <-x[1]*x[3]|-x[2]*x[3]|x[1]^2+x[2]^2>
>;

segment_max := proc(f)
 local T,N,s_step,t_step,m0,s0,t0,i,j,sol;
 T := f(pol(s,t));
 N := 6;
 s_step := evalf(Pi/(2*N)):
 t_step := evalf(2*Pi/(3*N)):
 m0 := 0;
 s0 := 0;
 t0 := 0;
 for i from 0 to N-1 do
  for j from 0 to N-1 do
   try
    sol :=
     NLPSolve(T,[],s=i*s_step..(i+1)*s_step,t=j*t_step..(j+1)*t_step,
              maximize=true,method=sqp);
    if sol[1] > m0 then
     m0 := sol[1];
     s0 := subs(sol[2],s);
     t0 := subs(sol[2],t);
    fi;
   catch:
   end try:
  od:
 od:

 return [m0,s0,t0]
end:

# If p : R^3 -> R^3 then jac(p)(x) is a 3x3 positive semidefinite
# matrix with 0 as one eigenvalue.  The map p is immersive at a
# point x in S2 iff the other two eigenvalues of jac(p)(x) are
# strictly positive.  If p(x) is a homogeneous polynomial of degree
# d in the variables x[i], then jac(p)(x) is homogeneous of degree
# 2 in the coefficients of p, and homogeneous of degree 2d in the
# variables x[i].

jac := proc(p)
 local J0,P,J,x;
 J0 := Matrix([seq([seq(diff(p(x)[i],x[j]),j=1..3)],i=1..3)]):
 P := orth_proj(x);
 J := map(expand,P . Transpose(J0) . J0 . P);
 return unapply(J,x);
end:

jac_det := proc(p)
 local J,f,t,x;
 J := jac(p)(x);
 f := Determinant(t * IdentityMatrix(3) - J);
 return unapply(coeff(f,t,1),x);
end:

jac_det_min := proc(p)
 local f,m0,s0,t0;
 f := unapply(-jac_det(p)(x),x);
 m0,s0,t0 := op(segment_max(f));
 return [-m0,s0,t0];
end:

jac_det_plot := proc(T)
 local p;
 p := T["map"];

 T["jac_det_plot"] :=
  display(
   plot3d(0,s=0..Pi/2,t=0..2*Pi/3,colour=grey,style=patchnogrid),
   plot3d(jac_det(p)(pol(s,t)),s=0..Pi/2,t=0..2*Pi/3)
  );
 return T["jac_det_plot"];
end:

# This returns a function of x measuring the failure of p to be
# locally isometric at x.  It is easy to evaluate and integrate,
# but does not strongly penalise points where the Jacobian becomes
# singular.  If p is homogeneous quadric, then jac_dev_a(p)
jac_dev_a := proc(p)
 local J,P,E;
 J := jac(p)(x);
 P := orth_proj(x);
 E := add(add((J[i,j] - P[i,j])^2,j=1..3),i=1..3);
 return unapply(E,x);
end:

# This is a different measure of the failure of p to be a local
# isometry.  It is less easy to compute but strongly penalises
# points where the Jacobian becomes singular.
jac_dev_b := proc(p)
 local J,f,c1,c2,x;
 J := jac(p)(x);
 f := Determinant(t * IdentityMatrix(3) - J);
 c1 := - coeff(f,t,2);
 c2 := coeff(f,t,1);
 return unapply(c1*(1+1/c2),x);
end:

jac_dev_a_max := (p) -> segment_max(jac_dev_a(p));

jac_dev_b_max := (p) -> segment_max(jac_dev_b(p));

# This takes a polynomial map p : R3 -> R3 (which may depend linearly
# on some parameters) and another polynomial map p_start : R3 -> R3
# (with no parameters).  It specialises the parameters in p to make it
# as close as possible (as measured by coefficients) to p_start.
# Starting from there, it adjusts the parameters to minimize the
# value of jac_dev_b_max.
optimise_immersion := proc(p,p_start)
 global F,F_n,F_vars,F_best,F_vals;
 local pp,pp_start,p0,err,sol,aa0;

 pp := maybe_extend_cyclic(p);
 pp_start := maybe_extend_cyclic(p_start);

 F_vars := indets(pp(x)) minus {x[1],x[2],x[3]};
 F_n := nops(F_vars);
 p0 := unapply(subs({seq(F_vars[i] = a[i],i=1..F_n)},pp(x)),x);
 F_vals := table():
 F_best := NULL:

 err := evalf(expand(p0(x)[1] - pp_start(x)[1]));
 err := [coeffs(err,{x[1],x[2],x[3]})];
 err := expand(add(t^2,t in err));
 sol := solve({seq(diff(err,a[i]),i=1..F_n)}):
 aa0 := subs(sol,[seq(a[i],i=1..F_n)]);

 F := proc(aa)
  local p1,m;
  global F_vals,F_best;
  p1 := unapply(evalf(subs({seq(a[i]=aa[i],i=1..F_n)},p0(x))),x);
  m := jac_dev_b_max(p1);
  F_vals := [op(F_vals),[convert(aa,list),m]];
  if F_best = NULL or m[1] < F_best[2][1] then
   F_best := [convert(aa,list),m];
  fi;
  print(m[1]);
  return m[1];
 end:

 NLPSolve(F_n,F,initialpoint=Vector(aa0),method=nonlinearsimplex);
 return unapply(evalf(subs({seq(a[i] = F_best[1][i],i=1..F_n)},p0(x))),x);
end:

sextics := table():

sextics[1] := make_immersion(
 0.264790817514485*x[2]*x[3]^5 +
 0.310873678789934*x[2]^5*x[3] +
 0.314356886190530*x[1]^6 +
(-0.367461017219878)*x[1]^3*x[2]^3 +
 0.520593122199589*x[1]^3*x[3]^3 +
 0.576919957376212*x[3]^6 +
 0.916070813636660*x[1]^4*x[2]*x[3] +
 1.08011357291557*x[1]*x[2]^4*x[3] +
(-1.29574375446066)*x[1]^2*x[2]^4 +
(-1.50136425744714)*x[2]^4*x[3]^2 +
 1.75322964015665*x[2]^3*x[3]^3 +
 1.83297848992403*x[1]^4*x[3]^2 +
 1.97193642230680*x[1]^3*x[2]^2*x[3] +
 3.00614990897844*x[1]^2*x[2]*x[3]^3 +
 3.14671754019206*x[2]^2*x[3]^4 +
(-3.31538823843401)*x[1]^3*x[2]*x[3]^2 +
 3.47953011385635*x[1]*x[2]^3*x[3]^2 +
(-3.48258924201893)*x[1]^2*x[2]^2*x[3]^2 +
 3.68883801947675*x[1]^2*x[3]^4 +
(-3.70117135607353)*x[1]*x[2]^2*x[3]^3 +
 5.13825301199981*x[1]^2*x[2]^3*x[3]):

quadrics := table():

# This map is nice and simple and is equivariant for the full symmetry
# group of the tetrahedron.  There are double points on the intersections
# of the coordinate planes with S2, and the points +/- e[i] are all
# sent to the origin.  The map is not an immersion because the Jacobian
# is zero at [+/-1,+/-1,0]/sqrt(2) and permutations of that.
quadrics[0] := make_immersion(
 sqrt(2)*x[2]*x[3]*(-2*x[1]^2+x[2]^2+x[3]^2)+(3*sqrt(3))*x[1]^2*x[2]*x[3]
);

quadrics["singular"] := eval(quadrics[0]);

# This is fitted to the original Boys embedding.  The pictures are nice but
# the minimal Jacobian determinant is very small.
quadrics[1] := make_immersion(
 2.22504534947546*x[1]^2*x[2]*x[3]+
 0.458129331010717*x[2]*x[3]^3+
 0.189460510525367*x[1]*x[2]*x[3]^2+
 0.707682203241314*x[2]^3*x[3]+
 0.188439848721650*x[1]*x[2]^2*x[3]+
 0.259985777619243*x[1]^3*x[3]+
 (-0.287042815821211)*x[1]*x[3]^3+
 1.76981740175627*x[1]^2*x[3]^2+
 0.118256474175355*x[2]^2*x[3]^2+
 (-0.478109121568119)*x[1]^3*x[2]+
 0.279208660926366*x[1]*x[2]^3+
 (-1.02732331413132)*x[1]^2*x[2]^2+
 (-0.127292231262611)*x[2]^4+
 0.812016127928750*x[3]^4+
 0.348555338251230*x[1]^4):

quadrics["boys"] := eval(quadrics[1]);

# This is chosen for simplicity.  The pictures are again nice and the minimal
# Jacobian determinant is a bit bigger than for quadric1, but still small.
quadrics[2]  := make_immersion(
 x[1]^2*x[3]*(x[2]+x[3])+(1/2)*(-1+3*sqrt(3))*x[2]*x[3]*(x[2]^2+x[3]^2)+
 sqrt(3)*x[3]^4-(1+sqrt(3))*x[1]^2*x[2]^2):

quadrics["simple"] := eval(quadrics[2]);

# The map quadric3 is a special case of special_quadric0 with
# max(jac_dev_b) minimized.

quadrics_a[3] :=
 [0.439109301326211, 1.00088884489061, 0.276971768183746,
  0.804297719366554, -0.228929556357166, -0.0920206086999537]:

quadrics[3] := make_immersion(
 evalf(sqrt(2) * subs({seq(a[i] = quadrics_a[3][i],i=1..6)},special_quadric0(x))));

quadrics[3]["a"] := quadrics_a[3];

quadrics["best"] := eval(quadrics[3]);

# This is Apery's quadric, up to linear changes of variables in the
# domain and codomain.  It sends u[0] to itself, and u[1] to u[6]
# to the origin.
quadrics[4] := make_immersion((
 (2 + 20*sqrt(2)) * x[1]^4 +
 (2 - 10*sqrt(2)) * (x[2]^4 + x[3]^4) +
 (10*sqrt(2) + 13) * x[1]^2 * (x[2]^2 + x[3]^2) +
 27 * x[1] * x[2] * x[3] * (x[1]+x[2]+x[3]) +
 (13 - 20*sqrt(2)) * x[2]^2 * x[3]^2 +
 9 * x[1]^3 * (x[2] + x[3]) +
 9 * x[2] * x[3] * (x[2]^2 + x[3]^2) +
 9 * x[1] * (x[2]^3 + x[3]^3) +
 (9 + 20*sqrt(2)) * x[2] * x[3] * (x[2]^2 - x[3]^2) +
 (10 * sqrt(2) - 9) * x[1] * (x[2]^3-x[3]^3) +
 (10 * sqrt(2) - 9) * (-x[1]^3) * (x[2] - x[3])) / (20 * sqrt(3)));

quadrics["apery"] := eval(quadrics[4]);

# This is optimised in a similar way to quadric3 but with slightly
# different constraints
quadrics[5] := make_immersion(
 3.25245542513966246*x[1]^2*x[2]*x[3]+
 0.567045610094270236*x[1]^2*x[3]^2+
 0.506746342124321902*x[2]^3*x[3]+
 1.43695065673601552*x[2]*x[3]^3+
 0.506586664999783221*x[3]^4+
 (-1.07154585120271717)*x[1]^2*x[2]^2+
 (-0.137651281752026899)*x[2]^2*x[3]^2+
 (-0.0475300936088673548)*x[2]^4+
 (-0.0729799939799069125)*x[1]*x[3]^3+
 (0.600907939705055405)*x[1]*x[2]*x[3]^2+
 (-0.121589626981102178)*x[1]*x[2]^2*x[3]+
 (0.0173511518008154855)*x[1]*x[2]^3+
 0.194569620961009104*x[1]^3*x[3]+
 (-0.618259091505870884)*x[1]^3*x[2]+
 0.183094951469558120*x[1]^4);

find_double_direction := proc(T,x0,x1)
 local v0,v1,w0,w1,pv0,pv1,pw0,pw1,n0,n1,nn;
 v0 := evalf(u[0] -~ dp(u[0],x0) *~ x0);
 v0 := v0 /~ sqrt(dp(v0,v0));
 w0 := evalf(cross_product(x0,v0));
 v1 := evalf(u[0] -~ dp(u[0],x1) *~ x1);
 v1 := v1 /~ sqrt(dp(v1,v1));
 w1 := evalf(cross_product(x1,v1));
 pv0 := evalf(subs(t = 0,map(diff,T["map"](x0 +~ t *~ v0),t)));
 pw0 := evalf(subs(t = 0,map(diff,T["map"](x0 +~ t *~ w0),t)));
 pv1 := evalf(subs(t = 0,map(diff,T["map"](x1 +~ t *~ v1),t)));
 pw1 := evalf(subs(t = 0,map(diff,T["map"](x1 +~ t *~ w1),t)));
 n0 := cross_product(pv0,pw0);
 n1 := cross_product(pv1,pw1);
 nn := cross_product(n0,n1);
 nn := nn /~ sqrt(dp(nn,nn));
 return nn;
end:

find_double_point := proc(T,x0,y0,u0,d0)
 local pp,eqs,start,sol,xyz;

 pp := T["map"];
 eqs := {
  x[1]^2 + x[2]^2 + x[3]^2 - 1,
  y[1]^2 + y[2]^2 + y[3]^2 - 1,
  op(pp(x) -~ pp(y)),
  dp(pp(x),u0) - d0
 };
 start := {
  seq(x[i] = x0[i],i=1..3),
  seq(y[i] = y0[i],i=1..3)};
 sol := fsolve(eqs,start);
 xyz := evalf(subs(sol,
   [[x[1],x[2],x[3]],[y[1],y[2],y[3]],pp(x)]));
 if not(type(T["double_points"],list)) then
  T["double_points"] := []:
 fi:
 T["double_points"] := [op(T["double_points"]),xyz];
 return xyz;
end:

double_point_step := proc(T,e)
 local p,q,x0,y0,u0,d0;
 p := T["double_points"][-2];
 q := T["double_points"][-1];
 x0 := 2 *~ q[1] -~ p[1];
 y0 := 2 *~ q[2] -~ p[2];
 u0 := q[3] -~ p[3];
 u0 := evalf(u0 /~ sqrt(dp(u0,u0)));
 d0 := dp(q[3],u0) + e;
 return find_double_point(T,x0,y0,u0,d0);
end:

find_seam := proc(T,xyz0,u0,e_min,e_max)
 local pp,z0,z01,z02,r0,r1,c0,c1,dc0,dc1,N,M,F,i,aa,bb,a,b;

 pp := T["map"];

 N := 240:
 F := proc(e)
  local i;
  T["double_points"] := evalf([xyz0]):
  find_double_point(T,xyz0[1],xyz0[2],u0,e):
  for i from 1 to N do double_point_step(T,e); od:
  return table(["err" = dp(T["double_points"][N+1][3],u0)]);
 end:

 T["double_point_step_length"] :=
  brent_fsolve(F,e_min,e_max,false,false,10^(-7),10^(-7))[1];

 aa := T["double_points"][1];
 bb := T["double_points"][240];

 if evalf(`d_2/R`(3)(bb[1],aa[2])) < evalf(`d_2/R`(3)(bb[1],-~aa[2])) then
  T["double_points_alt"] := [
   seq(T["double_points"][i][1],i=1..240),
   seq(T["double_points"][i][2],i=1..240),
   seq(-~ T["double_points"][i][1],i=1..240),
   seq(-~ T["double_points"][i][2],i=1..240),
   T["double_points"][1][1]]:
 else
  T["double_points_alt"] := [
   seq(T["double_points"][i][1],i=1..240),
   seq(-~ T["double_points"][i][2],i=1..240),
   seq(-~ T["double_points"][i][1],i=1..240),
   seq(T["double_points"][i][2],i=1..240),
   T["double_points"][1][1]]:
 fi;

 M := 20:
 for i from 0 to M do
  a[i] := add(T["double_points_alt"][j+1][1] * evalf(cos(Pi * (2 * i + 1) * j/480)),j=0..959)/480;
  b[i] := add(T["double_points_alt"][j+1][1] * evalf(sin(Pi * (2 * i + 1) * j/480)),j=0..959)/480;
 od:

 T["seam0"] := unapply(
   add(a[i] * cos((2 * i + 1) * t) + b[i] * sin((2 * i + 1) * t),i=0..M),t):

 T["seam"] := unapply([T["seam0"](t),T["seam0"](t+2*Pi/3),T["seam0"](t-2*Pi/3)],t):

 return NULL:
end:

double_points_plot := proc(T)
 local L;
 L := T["double_points"];
 display(
  usphere_plot,
  seq(point(x[1],colour=red),x in L),
  seq(point(x[2],colour=blue),x in L),
  seq(point(-~ x[1],colour=magenta),x in L),
  seq(point(-~ x[2],colour=cyan),x in L),
  axes=none,scaling=constrained
 );
end:

seam_plot := proc(T)
 spacecurve(T["seam"](t),t=0..2*Pi,colour=red,scaling=constrained,axes=none)
end:


text:
# Base Parameter Properties for Robot based on MDH frames
# Einleitung
# Untersuchung von Eigenschaften der Regressorform und des Minimalparametervektors
#
# Dateiname:
# robot -> Berechnung für allgemeinen Roboter
# tree -> Berechnung für eine allgemeine Baumstruktur (aber bisher nur für serielle Strukturen getestet)
# base_parameter_transformations -> Umwandlung der Minimalparameterform in andere Darstellungen
# Authors
# Moritz Schappler, schappler@irt.uni-hannover.de, 2016-04
#
# (C) Institut fuer Regelungstechnik, Leibniz Universitaet Hannover
# Quellen
# [SousaCor2014] Sousa, C. D. and Cortesao, R.: Physical feasibility of robot base inertial parameter identification: A linear matrix inequality approach (2014)
# Initialisierung
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):
codegen_opt := true:
codegen_act := true:
read "../helper/proc_convert_s_t":
read "../helper/proc_convert_t_s":
read "../helper/proc_MatlabExport":
read "../robot_codegen_definitions/robot_env":
read sprintf("../codeexport/%s/tmp/tree_floatb_definitions", robot_name):
# Ergebnisse der Minimalparametergruppierung laden

if base_method_name="twist" then
  paramfile := sprintf("../codeexport/%s/tmp/minimal_parameter_vector_fixb_maple", robot_name):
  expstring:="fixb":
  PV2:=Matrix(PV2_vec[11..10*NL,1]):
elif base_method_name="eulxyz" then
  paramfile := sprintf("../codeexport/%s/tmp/minimal_parameter_vector_floatb_eulxyz_maple", robot_name):
  expstring:="floatb_eulxyz":
  PV2:=PV2_vec:
else
  printf("Nicht behandelte Basis-Methode: %s\n", base_method_name):
fi:
if FileTools[Exists](paramfile) then
  read paramfile:
else
  printf("%s. Minimalparametervektor wurde nicht berechnet. Abbruch der Untersuchung der Parameter-Abhängigkeiten.\n", FormatTime("%Y-%m-%d %H:%M:%S")):
  quit: # Funktioniert in GUI nicht richtig...
  robot_name := "": # ...Daher auch Löschung des Roboternamens.
end if:
MPV := Paramvec2:

printf("Generiere Minimalparameter-Transformationen für %s (%s)\n", robot_name, expstring):

# Minimalparametervektor als Matrixdarstellung
# Siehe [SousaCor2014] equ. (38)
# Beispielrechnung:
# U=dU/dPV2*PV2=dU/dMPV*MPV # Parameterlineare Form
# MPV=dMPV/dPV2 * PV2 # Minimalparametervektor MPV hängt nur linear von den normalen Parametern im Parametervektor PV2 ab
# dU/dMPV = dU/dPV2 * dMPV/dPV1
# Benutze Fixed-Base Parameter: Ignoriere Parameter der Basis
Paramvec_size := RowDimension(MPV):
K := Matrix(Paramvec_size, RowDimension(PV2)):
for i to Paramvec_size do
  for j from 1 to RowDimension(PV2) do
    K[i,j] := diff(MPV[i, 1], PV2[j,1]):
  end do:
end do:
# Export der Umwandlung von Parametersatz 2 nach Minimalparameter (Matrix)
if codegen_act then
  MatlabExport(K, sprintf("../codeexport/%s/tmp/PV2_MPV_transformation_linear_%s_matlab.m", robot_name, expstring), 2):
end:
save K, sprintf("../codeexport/%s/tmp/PV2_MPV_transformation_linear_dependant_%s_maple", robot_name, expstring):
# Aufteilung der Matrixdarstellung in einzelne Teilmatrizen
# Generate the Matrices required in [SousaCor2014] equ. (33)
# Wenn ein Inertialparameter zu einer linear unabhängigen Spalte der Regressormatrizen gehört,
# steht er in K als 1, alle anderen Spalteneinträge dieser Zeile sind Null und der Inertialparameter steht auch in keiner anderen Zeile.
# Falls er zu einer linear abhängigen Spalte der Regressormatrix gehört, steht er in mehreren Spalten von K oder nur in einer Spalte aber dort nicht alleine.
# Die Permutationsmatrizen P_b, P_d wählen die jeweiligen Parameter aus dem Gesamt-Inertialparametervektor aus
n_b := RowDimension(K):
n_d := ColumnDimension(K)-RowDimension(K):
n := n_b+n_d: # [SousaCor2014] equ. (31)
;
P_b := Matrix(n, n_b): # für [SousaCor2014] equ. (33)
P_d := Matrix(n, n_d): # für [SousaCor2014] equ. (33)
;

for i from 1 to n_b do:
  # gehe alle Zeilen durch und prüfe, ob der MPV nur aus einem Inertialparameter-Eintrag besteht
  # falls er nicht aus einem Inertialparameter-Eintrag besteht, kommt der Basis-Eintrag nur in dieser Zeile vor.
  # falls der MPV-Eintrag aus mehreren Inertialparameter-Einträgen besteht, die nur hier vorkommen, wird der kleinere genommen (der weiter unten steht)

  # gehe alle Spalten durch und prüfe, wenn dieser Eintrag 1 ist, ob er noch in einer anderen Zeile vorkommt.
  linunabh_gefunden :=false: # Merker, ob ein Basis-Inertialparameter gefunden wurde.
  for j from 1 to n do: # alle Spalten durchgehen
    if K(i,j)=1 then
      # printf("In MPV-Zeile %d kommt Inertialparameter %d (%s) direkt vor.\n", i, j, convert(PV2(j,1), string)):
      # gehe alle anderen Zeilen durch
      Index_j_lin_unabh := true: # Merker, dass aktueller Index Basis-Inertialparameter ist.
      for i2 from 1 to n_b do:
        if i = i2 then # die selbe Zeile die gegengeprüft wird ausschließen
          next
        end if:
        if not (K(i2,j) = 0) then
          # Der Inertialparameter kommt auch in einem anderen MPV-Eintrag vor, damit ist er kein Basisparameter
          # printf("Inertialparameter %d (%s) kommt sowohl in MPV-Zeile %d als auch in Zeile %d vor. Kein Basisparameter.\n", j, convert(PV2_vec(10+j,1), string), i, i2):
          Index_j_lin_unabh := false:
          break:
        end if:
      end do:
      if Index_j_lin_unabh then
        # linear unabhängigen Parameter speichern
        P_b(j,i):=1:
        # printf("Für MPV-Zeile %d ist %d (%s) der Basisparameter\n", i, j, convert(PV2(j,1), string)):
        linunabh_gefunden:=true:
      end if:
    end if:
    if linunabh_gefunden then
      # Nicht weiter nach einem Basisparameter suchen, da bereits einer gefunden wurde.
      break:
    end if
  end do: # j-Schleife über Inertialparameter
  if not linunabh_gefunden then
    # printf("Keinen Basis-Inertialparameter in MPV Zeile %d gefunden.\n", i):
  end if:
end do:

# Alle linear abhängigen Parameter (der Rest) in andere Permutationsmatrix P_d packen

i_d := 0:
for j from 1 to n do:
  # Prüfe ob der betrachtete Parameter in der Permutationsmatrix für linear unabhängige vorkommt
  # Summe über die Zeilen von P_b: Prüfe, ob linear unabhängig
  linunabh := false:
  for i_b from 1 to ColumnDimension(P_b) do:
    if P_b(j, i_b) = 1 then:
      linunabh := true: # kommt in P_b vor: Linear unabhängig!
      break:
    end if:
  end do:

  if not (linunabh) then
    i_d := i_d+1;
    P_d(j, i_d) := 1
  end if
end do:
# Transformationsmatrix K_d füllen. Setze K_d nur aus den Spalten von K für die linear abhängigen Inertialparameter zusammen
# K_d: Matrix zur Aufteilung des Minimalparametervektors in linear abhängige und unabhängige Inertialparameter
K_d:=Matrix(n_b, n_d):
j_Kd := 0:
for j from 1 to n do:
  # Prüfe, ob der Inertialparameter j linear abhängig ist.
  linabh := false:
  for j_d from 1 to ColumnDimension(P_d) do:
    # j: Index des untersuchten Inertialparameters
    # j_d: Index der untersuchten Spalte von P_d. Ein Eintrag hier bedeutet, dass der Inertialparameter abhängig ist.
    if P_d(j, j_d) = 1 then:
      linabh := true:
      break:
    end if:
  end do:

  if linabh then
    j_Kd := j_Kd + 1:
    K_d(..,j_Kd) := K(..,j):
  end if:
end do:
printf("Inertialparametervektor hat %d linear abhängige und %d linear unabhängige Einträge\n", n_d, n_b):
delta_b := Transpose(P_b) . PV2: # Zur Überprüfung
;
delta_d := Transpose(P_d) . PV2: # Zur Überprüfung
;
if codegen_act then
  MatlabExport(K_d, sprintf("../codeexport/%s/tmp/PV2_MPV_transformation_linear_dependant_%s_matlab.m", robot_name, expstring), 2):
  MatlabExport(P_b, sprintf("../codeexport/%s/tmp/PV2_permutation_linear_independant_%s_matlab.m", robot_name, expstring), 2):
  MatlabExport(P_d, sprintf("../codeexport/%s/tmp/PV2_permutation_linear_dependant_%s_matlab.m", robot_name, expstring), 2):
end if: