Modeling and Symbolic Computation

Master 1 International in Computer Science - Introduction to Research 1
Introduction to Robotics : Wire-driven parallel robot

Yves Papegay - INRIA / COPRIN - January 2013, Monday 14.

Modeling a (robotic) system is the first and mandatory step to achieve whenever one is interested in the mechanical or dynamical behaviours of such system.
Defining the pertinent physical entities, selecting a formalism to attach variables to them, then writing equations expressing the physical laws produce a first kind of symbolic model.
It then has to be made explicit using numerical schemes or symbolic solvers to become useful for simulation and visualization of results.
All these activities will be explained and demonstrated in the particular and practical case of a 3RPR parallel robot.

Modeling and Simulation Process

Goal of this process is understanding and predicting some behaviors of a physical system.

Needs

Modeling and simulation process is particularly useful for replacing experiments, namely whenever experimental conditions are difficult to obtain, very costly, ore not yet existing.

img1=;
img2=;
img3=;
TabView[{"Access"->img1,"Cost"->img2,"Limit"->img3}]

Outline

img=;
Panel[img]

Exercice : Trivial exemple

img1=;sim=;
Row[{Panel[img1],sim}]

Step 1: From Physical System to Symbolic Implicit Model

Panel[Column[{OpenerView[{Text[Style["Which behavior am I interested in ?",Medium]],Text[Style["Motion of the body.",Medium,Italic]]}],OpenerView[{Text[Style["Select a theoretical framework and correspondant formalism...",Medium]],Column[{Text[Style["between several possibilities:",Medium]],Text[Style["• classical mechanics, cartesian coordinates, forces",Medium]],Text[Style["• lagrangian mecanics, generalized coordinates, energies",Medium]]}]}],OpenerView[{Text[Style["What are the physical quantities involved ?",Medium]],Column[{Text[Style["Length at rest",Medium,Italic]],Text@Style["Spring stiffness",Medium,Italic],Text@Style["Damping factor",Medium,Italic],Text@Style["Initial position",Medium,Italic],Text@Style["Initial speed",Medium,Italic],Text@Style["Mass",Medium,Italic],Text@Style["External force",Medium,Italic],Text@Style["Position",Medium,Italic],Text@Style["Speed",Medium,Italic],Text@Style["Weight",Medium,Italic],Text@Style["Global Force",Medium,Italic]}]}],OpenerView[{Text[Style["How to represent them ?",Medium]],Column[{Text[Style["• Define an inertial reference frame.\n• Attach a variable to each of the quanities:",Medium,Italic]],Grid[{{l,Text[Style["Length at rest",Medium,Italic]]},{k,Text@Style["Spring stiffness",Medium,Italic]},{b,Text@Style["Damping factor",Medium,Italic]},{(Subscript[x, 0]),Text@Style["Initial position",Medium,Italic]},{(Subscript[v, 0]),Text@Style["Initial speed",Medium,Italic]},{m,Text@Style["Mass",Medium,Italic]},{(Subscript[F, mot]),Text@Style["External force",Medium,Italic]},{x,Text@Style["Position",Medium,Italic]},{v,Text@Style["Speed",Medium,Italic]},{P,Text@Style["Weight",Medium,Italic]},{F,Text@Style["Global Force",Medium,Italic]}}]}]}],OpenerView[{Text[Style["How are these quantities linked together ?",Medium]],Column[{Text[Style["Express Newton's second law of mechanics:",Medium]],Style[F==m.((\[DifferentialD]v)/(\[DifferentialD]t)),Medium]}]}]}]]

Step 2: Getting an explicit Model

Defining inputs and outputs

Defining a model to compute outputs in terms of inputs

Step 3: Implementing a simulation code

Defining inputs

m=1.;
k=10.;
l=1.;
b=0.2;
x0=1.;
v0=0.1;
Fmot[t_]:=5 Sin[3 (Sqrt[k/m]) t]

Defining constants

g=9.81`;
Δt=0.01;

Defining formulas

F[n_]:=P[n]+Fmot[n Δt]+Fspring[n]+Fdamp[n]
P[n_]:=m g
Fspring[n_]:=-k (x[n]-l)
Fdamp[n_]:=-b v[n]

Iterative numerical scheme

NumIter[0]=Module[{v,x},x=x0;v=v0;{x,v}];
NumIter[n_]:=NumIter[n]=Module[{Localv,Localx},Localx=x[n-1]+Δt v[n-1];Localv=v[n-1]+((Δt F[n-1])/m);{Localx,Localv}]

x[n_]:=NumIter[n][[1]]
v[n_]:=NumIter[n][[2]]

Table[NumIter[i],{i,1,10000}];

ListPlot[{Table[x[i],{i,1,3000}],Table[v[i],{i,1,3000}]}]

Step 3bis: Implementing visualisation code

Draw[x_]:=Graphics[{Arrowheads[0.2],Arrow[{{-0.5, 11}, {-0.5, -1}}], Line[{{-0.5, 9.5}, {-0.3, 9.5}}], Text["0", {0, 9.5}],Line[{{-0.5, 7.5}, {-0.3, 7.5}}],Text["l", {0, 7.5}],Text["x", {-1, 9.5 - x}],Line[{{-0.7, 9.5 - x}, {-0.5, 9.5 -x}}],Rectangle[{1, 9 - x}],Arrowheads[0.1],Arrow[{{1.5, 9.5 -x}, {1.5, 7.5 -x}}],Text["P", {1.3, 8.5 -x}],Arrow[{{2.5, 9.5 -x}, {2.5, 9.5 -x - 0.6 (2 -x)}}],Text["F", {2.8, 9.5 -x - 0.3 (2 -x)}],With[{n = 15},Line[Prepend[Append[Flatten[Table[{{1.2,11-(1+x)((2 j-1)/(2 n+1))},{1.8,11-((1+x)2)(j/(2 n+1))}},{j,1,n}],1],{1.5,10-x}],{1.5,11}]]]},PlotRange -> {{-2, 3}, {-2, 12}}]

ListAnimate[Table[Draw[x[i]],{i,1,3000}]]

ClearAll[m,k,l,b,g,x0,y0,F,P,Fmot,Fspring,Fdamp,NumIter,Draw]

Geometrical Models of Robots

Serial Robot

corps0={Black,Line[{{0,0,0},{0,0,11}}],Cylinder[{{0,0,0},{0,0,1}},5],Cuboid[{-4,-17,0},{4,-3,4}],Yellow,Cylinder[{{0,0,1},{0,0,3}},5]};
corps1={Black,Line[{{-5,0,6.8},{5,0,6.8}}],Cylinder[{{0,0,3},{0,0,4.5}},5],Cylinder[{{0,0,4.5},{0,0,4.8}},3.5],Yellow,Cuboid[{-1.5,-3,4.8},{1.5,3,8.8}]};
corps2={Black,Line[{{-5,0,15.8},{5,0,15.8}}],Cylinder[{{ 1.5,0, 6.8},{2.7,0,6.8}},1.5],Cylinder[{{ -1.5,0, 6.8},{-2.7,0,6.8}},1.5],Cuboid[{1.5,-1.5,6.8},{2.7,1.5,15.8}],Cylinder[{{ 1.5,0, 15.8},{2.7,0,15.8}},1.5],Cylinder[{{ -1.5,0, 15.8},{-2.7,0,15.8}},1.5],Cuboid[{-2.7,-1.5,6.8},{-1.5,1.5,15.8}]} ;
corps3={Yellow,Cuboid [{{-1.5,-3,13.8},{1.5,10,17.8}}],Black,Line[{{-5,11.2,15.8},{5,11.2,15.8}}],Cylinder[{{-1.5,11.2,15.8},{1.5,11.2,15.8}},1.2],Polygon[{{{1.5,10,17.8},{1.5,11.2,17},{-1.5,11.2,17},{-1.5,10,17.8},{1.5,10,17.8}},{{1.5,10,13.8},{1.5,11.2,14.6},{-1.5,11.2,14.6},{-1.5,10,13.8},{1.5,10,13.8}},{{1.5,10,17.8},{1.5,11.2,17},{1.5,11.2,14.6},{1.5,10,13.8},{1.5,10,17.8}},{{-1.5,10,17.8},{-1.5,11.2,17},{-1.5,11.2,14.6},{-1.5,10,13.8},{-1.5,10,17.8}}}]} ;
corps4={Black,Cuboid[{{-1.5,11.2,14.8},{1.5,15.2,16.8}}],Cylinder[{{0,13.2,16.5},{0,13.2,17.1}},2.6]};
corps5={Black,Cuboid[{{-0.6,15.2,16.5},{0,22,17.1}}],Cuboid[{{0,15.2,16.5},{0.6,22,17.1}}]};
corps5bis={Yellow,Cuboid[{{-2.6,13.2,16.5},{-2,20,17.1}}],Cuboid[{{2,13.2,16.5},{2.6,20,17.1}}]};
Manipulate[Graphics3D[{corps0,Rotate[{corps1,Rotate[{corps2,Rotate[{corps3,Rotate[{corps4,corps5bis},δ,{1,0,0},{0,11.2,15.8}]},γ,{1,0,0},{0,0,15.8}]},β,{1,0,0},{0,0,6.8}]},α,{0,0,1}]},PlotRange->{{-20,20},{-20,30},{0,40}}],{{α,0},-3Pi/4,3Pi/4},{{β,0},-Pi/2,Pi/2},{{γ,0},-Pi/2,3Pi/2},{{δ,0},-Pi/2,Pi/2}]

Parallel Robot

DrawBase[A_]:={ {Opacity[0.05,Red],Map[Disk[#,50]&,A],Opacity[1,White],Map[Disk[#,1]&,A],PointSize[Large],Pink,Map[Point[#]&,A]}}
H2D[{a_,{x_,y_}}]:=Function[l,Map[{{Cos[a],-Sin[a]},{Sin[a],Cos[a]}}.#+{x,y}&,l]]
DrawMobile[A_,B_,{θ_,{X_,Y_}}]:=
With[{newB=H2D[{θ,{X,Y}}][B]},{Map[{Opacity[0.5],Darker[Green],PointSize[0.02],Point[#]}&,A],Opacity[0.5],Blue,Line[{A[[1]],newB[[1]]}],
Blue,Line[{A[[2]],newB[[2]]}],
Blue,Line[{A[[3]],newB[[3]]}],Map[{Blue,Point[#]}&,newB],
Opacity[0.5],Red,Polygon[newB],{Opacity[1],PointSize[0.02],Red,Point[{X,Y}]}}]
Draw3RPR[A_,B_,t_List]:=With[
{x=Map[#[[1]]&,A],y=Map[#[[2]]&,A]},With[{xmin=Min[x],xmax=Max[x],ymin=Min[y],ymax=Max[y]},
Graphics[Flatten[{
DrawBase[A],Map[DrawMobile[A,B,#]&,t]
}],
Axes->True,Frame->True,GridLines->Automatic,PlotRange->{{-30+xmin,30+xmax},{-30+ymin,30+ymax}}]]]
DrawCirCons[P_]:=With[{tt=CirCons[P]},{Circle[tt[[1]],tt[[2]]],Map[{Opacity[1],PointSize[0.005],Red,Point[#]}&,P]}]
A={{0.,0.},{25.,0.},{35.,45.}};
B={{0.,0.},{10.,0.},{5.,10.}};
Manipulate[Draw3RPR[A,B,{{θ Degree,{x,y}}}],{θ,-120,120},{x,-10,30},{y,0,40}]

Direct (forward) and Inverse Models

Find the position and orientation of the end effector in term of the coordinates of the robot.

Find the coordinates of the robot in term of the position and orientation of the end effector.

Exercice : Trivial exemple

Serial case

Manipulate[Column[With[{o={0,0},a=RotationMatrix[α Degree].{20,0},b=RotationMatrix[(α+β)Degree].{10,0}},{Graphics[{{Black,Thick,Line[{o,a}],Line[{a,a+b}]},{Red,PointSize[Large],Map[Point,{o,a}],Blue,Point[a+b]}},Axes->True,Frame->True,PlotRange->{{-5,35},{-5,20}}],N[a],N[a+b]}]],{α,-10,100},{β,-120,120}]

Reduce[{Cos[α]l1+Cos[α+β]l2==x,Sin[α]l1+Sin[α+β]l2==y},{α,β}]/.{l1->20.,l2->10.,x->28.241947376594794`,y->4.762872742068995`}

Parallel case

Manipulate[Column[With[{o1={0,0},o2={30,0},a={x,y}},{Graphics[{{Black,Thick,Line[{o1,a}],Line[{a,o2}]},PointSize[Large],{Red,Map[Point,{o1,o2}]},{Blue,Point[a]}},Axes->True,Frame->True,PlotRange->{{-5,35},{-5,20}}],EuclideanDistance[a,o1],EuclideanDistance[a,o2]}]],{x,0,30},{y,0,15}]

Geometrical Model of a 3RPR Parallel Robot

Draw3RPR[A,B,{{0Degree,{10,20}}}]

Inverse Model

Find the lengthes of the bars in term of the position and orientation of the end effector.

Step 1

Clear[a1,a2,a3,b1,b2,b3,x,y,θ,R2RB]

Define a fixed reference frame R: (O, i, j) and a mobile reference frame (Subscript[R, B]): ((Subscript[O, B]), (Subscript[i, B]), (Subscript[j, B]))

Attach points ((Subscript[A, 1]),(Subscript[A, 2]),(Subscript[A, 3])) and  ((Subscript[B, 1]),(Subscript[B, 2]),(Subscript[B, 3])) to the fixed and mobile ends of each bar

Define coordinates  ((Subscript[a, 1]),(Subscript[a, 2]),(Subscript[a, 3])) of points  ((Subscript[A, 1]),(Subscript[A, 2]),(Subscript[A, 3])) in the fixed frame and coordinates  ((Subscript[b, 1]),(Subscript[b, 2]),(Subscript[b, 3])) of points  ((Subscript[B, 1]),(Subscript[B, 2]),(Subscript[B, 3])) in the mobile frame

Express position and orientation of (Subscript[R, B]) with respect to R with the help of angle θ=⟨(Subscript[iOi, B])⟩ and coordinates of vector (Subscript[OO, B]): (x,y)

R2RB[v_]:=P+RotationMatrix[θ].v

Express each vector (Subscript[A, i])(Subscript[B, i]) in terms of (Subscript[a, i]), (Subscript[b, i]),θ,x and y

AiBi=(R2RB[bi]-ai)

Express length of each bar ((Subscript[l, i])) in terms of (Subscript[a, i]), (Subscript[b, i]),θ,x and y

li^2==(#[[1]]^2+#[[2]]^2)&[AiBi/.{ai->{xai,yai},P->{x,y},bi->{xbi,ybi}}]

Manipulate[Column[{Draw3RPR[A,B,{{θ Degree,{x,y}}}],(((0.` +x))^2)+(((0.` +y))^2),(((-25.`+x+10.` Cos[θ Degree]))^2)+(((0.` +y+10.` Sin[θ Degree]))^2),(((-35.`+x+5.` Cos[θ Degree]-10.` Sin[θ Degree]))^2)+(((-45.`+y+10.` Cos[θ Degree]+5.` Sin[θ Degree]))^2)}],{θ,-120,120},{x,-10,30},{y,0,40}]

NumVal={θ->N[Pi/4],x->10.,y->25.,l1^2->725.`,l2^2->1091.4213562373097`,l3^2->902.5126265847084`,xa1->0.,ya1->0.,xa2->25.,ya2->0.,xa3->35.,ya3->45.,xb1->0.,yb1->0.,xb2->10.,yb2->0.,xb3->5.,yb3->10.};

Direct Model

Find the position and orientation of the end effector in term of the lengthes of the bars.

AiBi/.{ai->{xa1,ya1},P->{x,y},bi->{xb1,yb1}}//MatrixForm

(#[[1]]^2+#[[2]]^2)&[AiBi/.{ai->{xa1,ya1},P->{x,y},bi->{xb1,yb1}}]

eq1=l1^2-(#[[1]]^2+#[[2]]^2)&[AiBi/.{ai->{xa1,ya1},P->{x,y},bi->{xb1,yb1}}]
eq2=l2^2-(#[[1]]^2+#[[2]]^2)&[AiBi/.{ai->{xa2,ya2},P->{x,y},bi->{xb2,yb2}}]
eq3=l3^2-(#[[1]]^2+#[[2]]^2)&[AiBi/.{ai->{xa3,ya3},P->{x,y},bi->{xb3,yb3}}]

Solve[{eq1==0,eq2==0,eq3==0},{x,y,θ}]

eq12=Collect[Simplify[Expand[eq2-eq1]],{x,y}]

eq13=Collect[Simplify[Expand[eq3-eq1]],{x,y}]

resxy =Solve[{eq12==0,eq13==0},{x,y}]

WeierStrass={Cos[θ]->((1-(t^2))/(1+(t^2))),Sin[θ]->((2 t)/(1+(t^2)))};
(Solve[{Cos[θ]==(Cos[θ]/.WeierStrass),Sin[θ]==(Sin[θ]/.WeierStrass)},t]/.NumVal)[[1]]

respol=Collect[Numerator[Together[eq1/.resxy/.WeierStrass][[1]]],t]

Exponent[respol,t]

NSolve[respol/.v_^4:>(v^2/.NumVal)^2/.NumVal,t]