SetDirectory[NotebookDirectory[]];
Get["PlanarLinkages.m"];

PlanarLinkages   —   Copyright © 2015   Christoph Koutschan
This program comes with absolutely no warranty; it is free software,
and you are welcome to redistribute it and/or modify it under the
terms of the GNU General Public License (http://www.gnu.org/licenses/).

Demo

Arithmetic of motion polynomials

(* Motion polynomials are input using the symbol "eta". *)
t+I+eta*(2-I)

(* When Times is used, eta is always interpreted to be on the left. *)
{I*eta,eta*I}

(* To specify the order of factors, use NonCommutativeMultiply. *)
{I**eta,eta**I}

(* In the same fashion, motion polynomials can be multiplied. *)
(t+1-eta)**(t^2+1+2I+eta*I*t)

(* See how motion polynomials are represented internally. *)
z+eta*w
FullForm[%]

(* Check the multiplication of motion polynomials (a+eta*b)*(c+eta*d). *)
MP[(Subscript[z, 1]),(Subscript[w, 1])]**MP[(Subscript[z, 2]),(Subscript[w, 2])]

(* Invert an element in SE2. Note that real numbers in K correspond to 1 in SE2. *)
FullSimplify[(z+eta*w)**(Conjugate[z]-eta*w)]

Action of (Subscript[SE, 2])

?ActR2

(* Action of SE2. *)
vec=ActR2[MP[(Subscript[x, 1])+I*(Subscript[x, 2]),(Subscript[y, 1])+I*(Subscript[y, 2])],{x,y}];
Together[Simplify[vec,Element[{(Subscript[x, 1]),(Subscript[x, 2]),(Subscript[y, 1]),(Subscript[y, 2])},Reals]]]

(* Product in SE2 is bilinear. *)
prod=ComplexExpand/@(MP[(Subscript[x, 1])+I*(Subscript[x, 2]),(Subscript[y, 1])+I*(Subscript[y, 2])]**MP[(Subscript[x, 1])'+I*(Subscript[x, 2])',(Subscript[y, 1])'+I*(Subscript[y, 2])'])

TableForm[Flatten[CoefficientList[List@@prod/.I->i,i]]]

Fixpoint of a motion

?FixPoint

(* Compute the fixpoint of a motion. *)
FixPoint[t+I-eta*0]

FixPoint[t+eta]

(* Test fixpoint formula. *)
FixPoint[t-α-eta*β]
FullSimplify[(First[%]+I*Last[%])-β/(Conjugate[α]-α)]

Factorization of motion polynomials

(* Factor a motion polynomial into linear factors. *)
FactorMP[(t+I)^6+eta*t]

(* Expand again. *)
Expand[%]

(* Now with factored coefficients. *)
Factor/@%

(* Data structure used for representing factored motion polynomials. *)
FactoredMP[eta,eta,eta]
Expand[%]

(* This polynomial admits several factorizations (actually 5! = 120). *)
FactorMP[poly=Product[t-k I,{k,5}]+eta*(t+I)]

(* This is an other factorization using a different order of the roots. *)
FactorMP[poly,Order->Table[k I,{k,5,1,-1}]]

(* Here not any permutation of the roots yields a factorization. *)
poly=(t^2+1)^2*(t+2I)+eta*(t+I)^2;
FactorMP[poly]
FactorMP[poly,Order->{-I,-I,I,I,-2I}]

(* Elliptic translational motion. *)
(* The polynomial cannot be factored, unless we multiply it by t^2+1. *)
poly=t^2+1+eta*(a*t-b*I)
fact=FactorMP[poly]

(* Do this factorization "by hand". *)
ansatz=FactoredMP@@Table[t+(-1)^Ceiling[(k-1)/2]*I+eta*w[k],{k,4}]
(* Derive a system of equations. *)
Union[CoefficientList[Last[Expand[ansatz]-(t^2+1)*poly],t]]
(* Test whether the coefficients of fact[[2]] satisfy the system. *)
Expand[%/.w[k_]:>fact[[k,2]]]

(* All 6 factorizations of a "random" degree 3 motion polynomial. *)
roots=t/.Solve[t^3+I==0];
facts=FactorMP[t^3+I+eta*(t^2-2),Order->#]&/@Permutations[roots]

(* Show their linkages. *)
AnimateMP@@Append[facts,Polygon->{{0,0},{0,1},{1,1},{1,0}}/2]

Animation of a motion

?AnimateMP

(* Animation of a simple revolution. *)
AnimateMP[t+I+0 eta,ImageSize->{200,200}]

(* A horizontal translational motion. *)
AnimateMP[1+eta*(t+1),ImageSize->{400,100},PlotRange->{{-10,10},{-2,2}},Trace->None]

(* A vertical translational motion. *)
AnimateMP[t+eta*I]

Construction of linkages

(* Circular translation. *)
fact=FactorMP[t^2+1+eta*(t-I)]

(* Show different decompositions of this motion depending on the choice of C[1]. *)
AnimateMP@@Table[fact/.C[1]->k,{k,-1/2,1,1/2}]

(* Define a linkage by constructing its link graph. *)
linkage={{1,2,fact[[2]]/.C[1]->-1/2},{1,3,fact[[2]]/.C[1]->0},{2,4,fact[[1]]/.C[1]->-1/2},{3,4,fact[[1]]/.C[1]->0}}

?ShowLinkage

ShowLinkage[linkage,1,Trace->{4,{0,0}},Thickness->{0.007,0.003},ImageSize->200]

(* A degree 6 motion polynomial that allows only a single factorization. *)
mp1=FactoredMP@@Table[t+I+eta*k*I,{k,6}]
AnimateMP[mp1,Polygon->{{0,0}}]

(* A different polynomial that produces basically the same curve. *)mp2=FactoredMP@@Table[t+I+eta*(1+(-1)^k)*I,{k,0,5}]
AnimateMP[mp2,Polygon->{{0,0}}]

?ConstructLinkage

linkage=ConstructLinkage[mp2,t+2I+0*eta]

ShowLinkage[linkage,PlotRange->{{-2,2},{-1,2.5}},Trace->{7,{0.1,0}}]

Example 4.4 from the paper

P1=t+I-3eta;
P3=t+2I+eta*(18+3I);
{P2,P4}=Flip[P1,P3];
TableForm[{P1,P3,P4,P2}]

FixPoint/@{P1,P3,P4,P2}

(* Check cycle condition (with quick-and-dirty way of inverting P1 and P3). *)
P2**P4**(2t-P3)**(2t-P1)

linkage={{1,2,P2},{3,4,P3},{1,3,P1},{2,4,P4}}

ConstructLinkage[FactoredMP[P2],P4]===linkage

ShowLinkage[linkage,Axes->True,ColorOutput->{1->Yellow,2->Red,3->Green,4->Blue},Thickness->0.1,Links->{2,1,4,3}]

ShowLinkage[linkage,ColorOutput->{1->Yellow,2->Red,3->Green,4->Blue},Thickness->0.1,Links->{2,1,4,3},Return->"animation3D"]

opts=Sequence@@{PlotRange->{{-1,6},{-2,4}},Axes->True,ImageSize->1000,LabelStyle->40,Ticks->{Automatic,{-2,0,1,2,3,4}}};
pic[l3_]:=ShowLinkage[ReplacePart[linkage,{2,1}->l3],Return->{"picture",-1.10715},ColorOutput->{1->Black,4->White},Thickness->0.04,opts];
TableForm[{{Show[pic[5],ImageSize->200,LabelStyle->10],Show[pic[3],ImageSize->200,LabelStyle->10]}}]
(*
Export["../images/weak.jpg",pic[5],"CompressionLevel"->0.1];
Export["../images/strong.jpg",pic[3],"CompressionLevel"->0.1];
*)

Example: a simple quadratic motion polynomial

(* A quadratic motion. *)
poly=(t+I)*(t-1+I)+eta*(t+2I)
AnimateMP[poly]

(* Now factor this quadratic polynomial into linear factors. *)
fact=FactorMP[poly]

(* Test whether the factorization is correct. *)
Expand[fact]-poly

(* Animation of the factored motion. *)
AnimateMP[fact]

(* Different style of animation. *)
AnimateMP[fact,Style->"wheels"]

(* Compute the second factorization. *)
facts={fact,FactorMP[poly,Order->{1-I,-I}]}

(* Show the corresponding linkage. *)
{{P1,P2},{P3,P4}}=List@@@facts;
link={{2,1,P1},{3,2,P2},{3,4,P4},{4,1,P3}};
ShowLinkage[link,3,Trace->{1,{0,0}},Thickness->{0.015,0.003},Links->{4,1,3,2}]

(* Create movie ToyExample.mp4 *)
ShowLinkage[link,3,Trace->{1,{0,0}},Thickness->{0.015,0.003},Links->{4,1,3,2},ImageSize->1000,Return->{"movie",150}]

Example: elliptic translation

(* We consider the ellipse given by the equation (x+1)^2 + 4*y^2 == 1. *)
ContourPlot[(x+1)^2+4y^2==1,{x,-2,0},{y,-1/2,1/2},AspectRatio->1/2]

(* We use the following parametrization. *)
ParametricPlot[{-2,t}/(t^2+1),{t,-100,100},PlotRange->{{-2,0},{-1/2,1/2}}]

(* This translates into the following motion polynomial, *)
(* realizing a translational motion along the ellipse. *)
poly=t^2+1+eta*(-2+I*t);
AnimateMP[poly]

(* To factor this motion polynomial, it is multiplied by t^2+1. *)
(* We obtain a 2-parameter family of factorizations. *)
fact1=FactorMP[poly]

(* This is a factorization with different order of the roots. *)
FactorMP[poly,Order->{I,-I,-I,I}]

(* Using the first factorization, we can construct a linkage which *)
(* strongly realizes the translational motion along the ellipse. *)
ShowLinkage[ConstructLinkage[fact1/.{C[1]->0,C[2]->-I/2},t+9/5I+eta*0],Trace->{5,{0,0}},Direction->{0.03,0}]

(* Multiplying with t-I from the left also enables us to find a factorization. *)
(* By doing so we change the motion (it is not any more a translational one). *)
(* However, the orbit of the origin under this new motion is still the ellipse. *)
fact2=FactorMP[(t-I)**(t^2+1+eta*(-2+I*t))]

(* Choose some value for the parameter of this one-dimensional family of factorizations. *)
fact2=fact2/.C[1]->-I

(* Use this factorization to construct a linkage. *)
(* The second argument is a linear motion polynomial that can be chosen almost randomly. *)
(* With some experiments, we found this one which gives a nice-looking linkage. *)
linkEll=ConstructLinkage[fact2,t+9/5*I+eta*18/35*I]

(* Rename the labels such that they agree with those in the paper. *)
linkEll=Map[If[IntegerQ[#],If[#≤4,5-#,13-#],#]&,linkEll,{2}];

(* Define some coloring of the links. *)
colEll={4->White,3->Yellow,2->Orange,1->Red,8->Green,7->Cyan,6->Blue,5->Magenta};

(* Now the motion produced by the end effector is not any more translational. *)
ShowLinkage[linkEll,4,Trace->{1,{0,0}},ColorOutput->colEll,Direction->{0.03,0}]

(* Define an order of the links. This produces 5 collisions at t=∞. *)
(* This is the ordering produced by our code in ConstructLinkage. *)
(* Also the pen collides only at t=∞. *)
ordEll={8,4,5,1,6,2,7,3};
ShowLinkage[linkEll,4,Trace->{1,{0,0}},Links->ordEll,Return->"collisions"]
ShowLinkage[linkEll,4,Trace->{1,{0,0}},Links->ordEll,ColorOutput->colEll]

(* Here is a different ordering, figured out by hand. *)
(* It produces only 2 collisions, both at t=∞. *)
(* Also the pen collides only at t=∞. *)
ordEll={5,1,6,2,7,8,4,3};
ShowLinkage[linkEll,4,Trace->{1,{0,0}},Links->ordEll,Return->"collisions"]
ShowLinkage[linkEll,4,Trace->{1,{0,0}},Links->ordEll,ColorOutput->colEll,ImageSize->600,Return->"animation3D"]

ellgraph[s_]:=ShowLinkage[linkEll,4,ColorOutput->colEll,Thickness->{1,0.004},Return->"graph",ImageSize->s];
ellgraph[400]
(*Export["../www/EllipseLG.jpg",ellgraph[600],"CompressionLevel"->0.01];*)

(* In order to build the linkage, compute integral lengths of the links. *)
(* For each link the positions of the joints connected to it are given, *)
(* namely their x-coordinates in the initial configuration of the linkage. *)
pos=First/@(112*(FixPoint[Last[#]]&/@linkEll));
Table[pos[[First/@Position[Most/@linkEll,i]]],{i,8}]

(* Create movie Ellipse.mp4. *)
ShowLinkage[linkEll,4,Trace->{1,{0,0}},Links->ordEll,ColorOutput->colEll,Range->{-Pi/2+10^(-6),Pi/2+1/2},ImageSize->2000,Return->{"movie",300}]

(* Create movie Ellipse3D.mp4. *)ShowLinkage[linkEll,4,Trace->{1,{0,0}},Links->ordEll,ColorOutput->colEll,Boxed->False,Thickness->{0.007,0.004},ImageSize->2000,PlotRange->{{-0.65,0.3},{-0.43,0.5}},Range->{1.3,1.8},ViewPoint->{-0.1,-3,1.5},ViewVertical->{0,0,1},Return->{"movie3D",200}]

(* Create the picture shown in the paper. *)
grpr=MapThread[First[ShowLinkage[linkEll,4,Trace->{1,{0,0}},ColorOutput->{_->#2},Links->ordEll,Return->{"picture",#1}]]&,
{ArcTan[{2,1/2,0,-1}],{White,LightGray,Gray,Black}}];
cnt=1;grpr=Flatten[grpr/.{Red->Black,RGBColor[0.99,0,0]->White}/.Line[a_List/;Length[a]>10]:>If[cnt==1,cnt++;{Thickness[0.006],Line[a],Thickness[0.002]},{}]];Clear[cnt];
Graphics[grpr,PlotRange->{{-2.05,0.05},{-0.9,0.75}},ImageSize->600]
(*
Export["../images/elliptic.jpg",Graphics[grpr,PlotRange->{{-2.05,0.05},{-0.9,0.75}},ImageSize->2000],"CompressionLevel"->0.1];
*)

(* Construct a collision-free linkage using some links of special shape. *)
cfEll=Join[linkEll,Table[{i,-i,FixPoint[Cases[linkEll,{i-4,i,a_}->a][[1]]]},{i,6,7}]];
cfEll=cfEll/.{a:(6|7),b_,c_}/;a-b==1->{-a,b,c};
cfEll=cfEll/.{a:(2|3|4),b_,c_}/;b-a==4->{-a,b,c};
cfEll=Join[cfEll,{{2,-2,{-3/5,0}},{3,-3,{-7/10,0}},{4,-4,{7/20,0}}}];
cfCol=Join[colEll,-#1->#2&@@@colEll]/.White->GrayLevel[0.8];

(* Display the collision-free linkage. *)
ShowLinkage[cfEll,4,Trace->{1,{0,0}},Links->{-4,8,7,-3,-7,6,-2,-6,5,1,2,3,4},ColorOutput->cfCol,Boxed->False,Thickness->{0.011,0.004},Return->"animation3D",ViewPoint->{-0.1,-3,1.5},ViewVertical->{0,0,1},ImageSize->600,Lighting->{{"Directional",GrayLevel[0.85],10{2,-3,3}},{"Directional",GrayLevel[0.3],10{-2,-3,3}},{"Directional",GrayLevel[0.2],10{0,-3,-3}}}]

Example: John Hancock’s “J”

(* Our parametrization {x/d, y/d} of Hancock's J. *)
With[{
x=-451-8666 t-64488 t^2-237449 t^3-436132 t^4-321880 t^5,
y=-651-11325 t-78158 t^2-270569 t^3-472949 t^4-336018 t^5,
d=170*(9+162 t+1219 t^2+4908 t^3+11187 t^4+13770 t^5+7225 t^6)},
poly=(d+eta**(x+I*y))/Coefficient[d,t^6]]

fact=FactorMP[poly]

(* Choose values for the free parameters. *)
fact=fact/.Table[C[i]->C[i]-fact[[13-2*i,2]],{i,6}]

(* Construct the linkage and show the link graph. *)
linkJ=ConstructLinkage[fact,t+1+I+eta];
ShowLinkage[linkJ,Return->"graph",Thickness->{0,0.001},ImageSize->650]

(* Some options for drawing this linkage *)
optsJ=Sequence[Trace->{13,{-7/10,7/10}},Direction->{0.025,0.03},Range->{-0.55,1.7},ImageSize->500,Thickness->{0.009,0.001},ColorOutput->Flatten[Table[Thread[{i,i+13}->GrayLevel[1/2+i/26]],{i,13}]],Links->{2,1,14,15,3,16,4,5,17,18,6,19,7,20,8,21,22,9,23,10,11,24,25,12,26,13}];

(* 2D animation *)
ShowLinkage[linkJ,Axes->True,optsJ]

(* 3D animation using a different style for the links *)
ShowLinkage[linkJ,Axes->True,ColorOutput->{},Return->"animation3D",Style->"Star",optsJ]

(* 3D animation with a different ordering of the links *)
ShowLinkage[linkJ,Axes->True,ColorOutput->{},Links->{14,1,17,4,18,5,21,8,22,9,25,12,26,13,24,11,23,10,20,7,19,6,16,3,15,2},Return->"animation3D",optsJ]

(* Produce the picture shown in the paper. *)
(* Use "/200" for the steps in TraceMP. *)
Get["PlanarLinkages.m"];
picJ=ShowLinkage[linkJ,Return->{"picture",-0.325},ImageSize->4000,PlotRange->{{-1.1,2.4},{-0.1,2.2}},optsJ];
picJ=picJ/.{Red->Black,RGBColor[0.99,0,0]->White};
Show[picJ,ImageSize->500]
(*Export["../images/JohnHancockLink.jpg",ImageResize[picJ,4000],"CompressionLevel"->0.01];*)

(* The speed of the pen is not very uniform. *)
expr=Norm[D[ActR2[poly,{0,0}],t]];
Plot[expr*If[t>0,Exp[t],1],{t,-0.55,1.7},PlotRange->{0,50}]

(* Use non-constant time steps. *)
TimeDiscretization={};t0=-0.55;speed={};
While[t0<1.7,With[{d=0.003*(2Erf[20(t0+0.1)]+3)*(Erf[-6(t0-3/2)]/2+1/2)^0},AppendTo[TimeDiscretization,t0+=d];AppendTo[speed,{t0,d*(expr/.t->t0)}]]];
Print["Length: ",Length[TimeDiscretization]];
ListPlot[speed,Joined->True,PlotRange->{0,0.2}]

(* Generate pictures for movie HancockJ.mp4. *)
ShowLinkage[linkJ,Return->{"movie",TimeDiscretization},ImageSize->1400,PlotRange->{{-1.1,3},{-0.8,2.4}},optsJ]

(* If we stack the links according to their labels, we get lots of collisions. *)
Timing[Length[ShowLinkage[linkJ,Links->Range[26],Return->"collisions"]]]

coll=ShowLinkage[linkJ,Return->"collisions",optsJ];
Print["All collisions: ",Length[coll]];
coll=Select[coll,ArcTan[-0.55]<Last[#]<ArcTan[1.7]&];
Print["Collisions in range: ",Length[coll]];
Print["  counting triangles: ",Length[Union[First/@coll]]];
{#1,N[#2],N[#3]}&@@@coll

(* Observe the 2nd and 3rd collision in the animation. *)
ShowLinkage[linkJ,Range->ArcTan[coll[[2,3]]+{-0.01,0.01}],ImageSize->500,Thickness->0.001,PlotRange->{{1.0,1.3},{0.9,1.2},{0.0,0.15}},
ColorOutput->{5->Green,18->Blue,6->Yellow,19->Red},Return->"animation3D",optsJ]

(* This ordering of the links has only 6 collisions, but the pen is not on top. *)
Timing[Union[First/@Select[ShowLinkage[linkJ,Links->{2,1,14,15,3,16,4,5,17,18,6,19,7,20,8,21,22,9,23,10,11,24,12,13,25,26},Return->"collisions",optsJ],ArcTan[-0.55]<Last[#]<ArcTan[1.7]&]]]