Hexapod Tutorial

This tutorial demonstrates the usage of the Hexapod (Stewart Platform) module in PaceRobots.

Overview

The Hexapod module provides functionality for modeling, analyzing, and simulating a Stewart Platform (6-DOF parallel manipulator) [1]. The platform consists of two rigid bodies:

the base (or ground effector) and
the mobile platform (or end effector)

connected by six extensible legs.

Stewart Platform

[Image source: Wikipedia. By utzonbike at German Wikipedia. Created with Autodesk Inventor. CC BY-SA 3.0]

By changing the lengths of the legs, the mobile platform can be configured to any desired position and orientation (called pose) with respect to the base.

For controlling the hexapod, we need to solve two problems:

Inverse Kinematics Problem (IKP): Compute the leg lengths for a given platform configuration.
Direct Kinematics Problem (DKP): Compute the platform configuration for given leg lengths.

For solving these problems in PaceRobots.jl, we use the methods described in [2].

Kinematics

Inverse Kinematics Problem (IKP)

For a given platform configuration, computing the leg lengths is immediate. It is implemented in the IKP function:

julia> prec = 14  # Precision in bits

julia> lengths = Hexapod.IKP(A, B, prec)
6-element Vector{Rational{BigInt}}:
 Rational{BigInt}(7121849//16384)
 Rational{BigInt}(7115423//16384)
 Rational{BigInt}(222739//512)
 Rational{BigInt}(7122795//16384)
 Rational{BigInt}(7120897//16384)
 Rational{BigInt}(7122497//16384)

Direct Kinematics Problem (DKP)

Solving the DKP is more complex. There exist different formulations of the algebraic equations that describe the DKP, the two main families being the position-based and the displacement-based formulations. We focus on the later.

Let's introduce the nomenclature used in this part: $\bullet$ $R_f$: the base Cartesian reference frame of center $O$.

$\bullet$ $R_m$: the Cartesian reference frame of center $C$ (end-effector) relative to the mobile platform.

$\bullet$ $A_i$: location of the center of the joint of kinematics chain $i$ on the base.

$\bullet$ $B_i$: location of the center of the joint of kinematics chain $i$ on the moving platform.

$\bullet$ $L_i$: overall effective distance between $B_i$ and $A_i$.

Let us denote by $\vec{v}_{|R}$ (resp. $M_{|R}$) the coordinates of the vector $\vec{v}$ (resp. the point $M$) with respect to the Cartesian reference frame $R$, and by $NM_{|R}$ the matrix whose columns are $M_iN_i|_R$, $i=1\ldots6$.

If there exists any mobile platform position $\overrightarrow{OB}_{i|R_f}$ which meets the constraints $L_i^2 = \|\overrightarrow{A_i B_i}\|^2$, $i = 1 \ldots 6$, then there exists a rotation $\mathcal{R}$ such that:

\[\begin{equation} \overrightarrow{OB}_{i|R_f} = \overrightarrow{OC}_{|R_f} + \mathcal{R} \cdot \overrightarrow{CB}_{i|R_m}, \quad i = 1 \ldots 6 \end{equation}\]

As used by several authors, the natural way to set an algebraic equation system from (1) is to straightforwardly use the rotation matrix parameters and the vector $\overrightarrow{OC}_{|R_f}$ coordinates as unknowns. Since $\mathcal{R}$ is a rotation (a $3 \times 3$ matrix), the following relations hold:

\[\begin{equation} \mathcal{R}^T \mathcal{R} = I_3, \quad \det(\mathcal{R}) = 1 \end{equation}\]

This first set of equations depend neither on the geometry of the platform, nor on the length of the legs and will be denoted by $S_{dis,rot}$.

The constraints $L_i^2 = \|\overrightarrow{A_i B_i}\|^2$, $i = 1 \ldots 6$ can then be expressed accordingly to equations (1):

\[\begin{equation} L_i^2 = \|\overrightarrow{OB}_{i|R_f} - \overrightarrow{OA}_{i|R_f}\|^2 = \|\overrightarrow{OC}_{|R_f} + \mathcal{R} \cdot \overrightarrow{CB}_{i|R_m} - \overrightarrow{OA}_{i|R_f}\|^2. \end{equation}\]

Let denote by $S_{dis,geom}$ the resulting system. The full modelisation of the displacement leads to an algebraic system $S_{rot} = S_{dis,rot} \cup S_{dis,geom}$ made of 13 polynomial equations depending on 12 unknowns ($[X, Y, Z, r_{ij}, j = 1 \ldots 3, i = 1 \ldots 3]$), where $[X, Y, Z]$ are the coordinates of $\overrightarrow{OC}_{|R_f}$.

Quaternion-based Equations:

Now, using quaternions to parameterize the group of rotations, we can use the Cayley transform; if $H$ is any anti-symmetric matrix:

\[\begin{equation} H = \begin{bmatrix} 0 & a & b \\ -a & 0 & c \\ -b & -c & 0 \end{bmatrix} \end{equation}\]

then, provided that 1 is not an eigenvalue of $H$, we have that

\[\begin{equation} \mathcal{R} = \frac{I + H}{I - H} = \begin{bmatrix} \frac{-1+c^2+a^2+b^2}{1+c^2+a^2+b^2} & \frac{2a+bc}{1+c^2+a^2+b^2} & \frac{2b+ac}{1+c^2+a^2+b^2} \\ \frac{-2a+bc}{1+c^2+a^2+b^2} & \frac{-1+c^2+a^2+b^2}{1+c^2+a^2+b^2} & \frac{2c+ab}{1+c^2+a^2+b^2} \\ \frac{-2b+ac}{1+c^2+a^2+b^2} & \frac{-2c+ab}{1+c^2+a^2+b^2} & \frac{-1+c^2+a^2+b^2}{1+c^2+a^2+b^2} \end{bmatrix} \end{equation}\]

is a rotation. Conversely, if $\mathcal{R}$ is a rotation then $H = \frac{\mathcal{R} - I}{\mathcal{R} + I}$ is anti-symmetric, assuming that $-1$ is not an eigenvalue of $\mathcal{R}$.

By replacing (5) in (1) and removing the denominators, one obtains a system depending on 6 variables $[X, Y, Z, a, b, c]$.

julia> sys1 = Hexapod.eqs_quat(A, B)
6-element Vector{Any}:
 94474738081//500000 - 3538250717//12500c - 253923821//5000b - 39008452997//62500a + 35961//100t3 + 1019//125t2 - 395731//500t1 + 400731626193//500000c² + 52331710957//62500bc + 157620899701//500000b² + 235358181//5000ac - 3499299517//12500ab + 464593007813//500000a² - l1² - 196794//125t3c - 6841//25t3b + t3² + 4//1t2c - 6841//25t2a + t2² + 4//1t1b + 196794//125t1a + t1² + 35561//100t3c² + 35561//100t3b² + 6841//25t3ac - 196794//125t3ab + 35961//100t3a² - 7831//5t2c² - 6841//25t2bc + 1019//125t2b² - 4//1t2ab - 7831//5t2a² - 395731//500t1c² - 196794//125t1bc - 532551//500t1b² + 4//1t1ac - 532551//500t1a² - l1²c² - l1²b² - l1²a² + t3²c² + t3²b² + t3²a² + t2²c² + t2²b² + t2²a² + t1²c² + t1²b² + t1²a²
 188608609061//1000000 + 6151885723//62500c - 8383538447//31250b + 156065902747//250000a + 44701//125t3 - 172377//250t2 - 194377//500t1 + 74584650609//1000000c² - 678712119//250000bc + 1043029230157//1000000b² + 8383538447//31250ac + 6151885723//62500ab + 185801054341//200000a² - l2² + 137623//250t3c - 187547//125t3b + t3² - 187547//125t2a + t2² - 137623//250t1a + t1² + 44701//125t3c² + 44701//125t3b² + 187547//125t3ac + 137623//250t3ab + 44701//125t3a² - 17377//125t2c² - 187547//125t2bc - 172377//250t2b² - 17377//125t2a² - 194377//500t1c² + 137623//250t1bc - 188913//100t1b² - 188913//100t1a² - l2²c² - l2²b² - l2²a² + t3²c² + t3²b² + t3²a² + t2²c² + t2²b² + t2²a² + t1²c² + t1²b² + t1²a²
 757029205837//4000000 + 2898352042//15625c - 13734663421//62500b - 78084541389//125000a + 89903//250t3 + 341139//500t2 + 402747//1000t1 + 3200213458829//4000000c² - 840321383//1000bc + 1270789204109//4000000b² + 13682018171//62500ac + 2823708167//15625ab + 3719695249101//4000000a² - l3² + 128006//125t3c - 153332//125t3b + t3² + 4//1t2c - 153332//125t2a + t2² + 4//1t1b - 128006//125t1a + t1² + 88903//250t3c² + 88903//250t3b² + 153332//125t3ac + 128006//125t3ab + 89903//250t3a² + 853163//500t2c² - 153332//125t2bc + 341139//500t2b² - 4//1t2ab + 853163//500t2a² + 402747//1000t1c² + 128006//125t1bc - 823909//1000t1b² + 4//1t1ac - 823909//1000t1a² - l3²c² - l3²b² - l3²a² + t3²c² + t3²b² + t3²a² + t2²c² + t2²b² + t2²a² + t1²c² + t1²b² + t1²a²
 755998623449//4000000 + 2859880578//15625c + 3427046866//15625b + 39048659591//62500a + 44701//125t3 + 85322//125t2 - 402747//1000t1 + 3201288965849//4000000c² + 52521210809//62500bc + 1272619517721//4000000b² - 3427046866//15625ac + 2859880578//15625ab + 3717909860121//4000000a² - l4² + 127956//125t3c + 153332//125t3b + t3² + 153332//125t2a + t2² - 127956//125t1a + t1² + 44701//125t3c² + 44701//125t3b² - 153332//125t3ac + 127956//125t3ab + 44701//125t3a² + 213278//125t2c² + 153332//125t2bc + 85322//125t2b² + 213278//125t2a² - 402747//1000t1c² + 127956//125t1bc + 823909//1000t1b² + 823909//1000t1a² - l4²c² - l4²b² - l4²a² + t3²c² + t3²b² + t3²a² + t2²c² + t2²b² + t2²a² + t1²c² + t1²b² + t1²a²
 94449473267//500000 + 12189956109//125000c + 33815342857//125000b - 1559712251//2500a + 89901//250t3 - 344481//500t2 + 194687//500t1 + 37170843771//500000c² + 179657573//62500bc + 521464820083//500000b² - 33245598857//125000ac + 12396928109//125000ab + 464901398587//500000a² - l5² + 137509//250t3c + 375057//250t3b + t3² + 4//1t2c + 375057//250t2a + t2² + 4//1t1b - 137509//250t1a + t1² + 88901//250t3c² + 88901//250t3b² - 375057//250t3ac + 137509//250t3ab + 89901//250t3a² - 69463//500t2c² + 375057//250t2bc - 344481//500t2b² - 4//1t2ab - 69463//500t2a² + 194687//500t1c² + 137509//250t1bc + 944801//500t1b² + 4//1t1ac + 944801//500t1a² - l5²c² - l5²b² - l5²a² + t3²c² + t3²b² + t3²a² + t2²c² + t2²b² + t2²a² + t1²c² + t1²b² + t1²a²
 47245964297//250000 - 879760381//3125c + 762464957//15625b + 19531436491//31250a + 44701//125t3 + 1059//125t2 + 198113//250t1 + 200515294377//250000c² - 26173159379//31250bc + 78934731809//250000b² - 762464957//15625ac - 879760381//3125ab + 232204061889//250000a² - l6² - 39362//25t3c + 34114//125t3b + t3² + 34114//125t2a + t2² + 39362//25t1a + t1² + 44701//125t3c² + 44701//125t3b² - 34114//125t3ac - 39362//25t3ab + 44701//125t3a² - 195751//125t2c² + 34114//125t2bc + 1059//125t2b² - 195751//125t2a² + 198113//250t1c² - 39362//25t1bc + 266341//250t1b² + 266341//250t1a² - l6²c² - l6²b² - l6²a² + t3²c² + t3²b² + t3²a² + t2²c² + t2²b² + t2²a² + t1²c² + t1²b² + t1²a²

julia> Hexapod.subs_lengths!(sys1, lengths)

julia> prec = 23;

julia> sols = Hexapod.DKP(sys1, prec)
8-element Vector{Vector{MPFI.BigInterval}}:
 [[-0.1028437574942424978321798, -0.1028437557055078451745865], [-42.6239276285217656877, -42.6239275803910048039], [-234.080432403831553249, -234.080432356128718886], [-0.000340107190308625897997, -0.000340107190066616989893], [0.00137054604234942669214, 0.0013705460424221672929], [-0.282291867115832281773, -0.282291867115831451843]]
 [[-37.37547855354410844195, -37.37547746333890205704], [21.491358189709236686549, 21.491358570647232989514], [-124.352611838341082333, -124.352610935540193675], [-0.000249610111367412839219, -0.00024961010694024535392], [0.243729663080638897896, 0.243729663081434218018], [-0.142181429779010196243, -0.142181429778944398548]]
 [[37.2840836926121893106, 37.28408478382531206739], [21.602525847695250331838, 21.602526220796819435733], [-124.348285793780450659, -124.348284889961947383], [-0.000263421753968787974388, -0.000263421749548475544347], [-0.244859799494984518904, -0.244859799494212783818], [-0.139989338860055602528, -0.139989338860007193769]]
 [[-5.938091924678033456177e-06, -5.938091787210034211421e-06], [-1.6436540441320195825e-05, -1.64365401342254948914e-05], [-1.462191814984850760749e-05, -1.462191779751558750044e-05], [1.22260837694345183147e-08, 1.22260837707129326464e-08], [-1.31225107352168921305e-08, -1.31225107352132592902e-08], [4.68572966605655482527e-08, 4.68572966605656011922e-08]]
 [[-9.517823399909296981471e-05, -9.517823179946267636964e-05], [0.00104996861891515878983, 0.00104996862874323306431], [-358.60799659480396942, -358.607996594713771321], [-0.000574450685249406444959, -0.000574450685249079170195], [2.17516200057028497605e-06, 2.17516200057141678268e-06], [1.20643972354613631039e-06, 1.20643972354620626516e-06]]
 [[-36.83724268062164327936, -36.83724159876591945032], [21.377953687583051235608, 21.377954310383037727625], [-234.107206945402548721, -234.107206122566568508], [-0.000333247487670878339853, -0.000333247483152095930115], [-0.245028889129714213375, -0.245028889129017188986], [0.139926385079593347173, 0.139926385079642361473]]
 [[36.91769326464470590485, 36.91769434474936361582], [21.19210136907894239316, 21.192101998037119911847], [-234.111755017934740054, -234.111754198260437775], [-0.000348361885380129128669, -0.000348361880872394502985], [0.243558805154625050526, 0.24355880515533806628], [0.142247219244667023479, 0.142247219244726018768]]
 [[0.06999189163255054096056357, 0.06999189339612874307051733], [-43.1480470488126608766, -43.1480469937123004131], [-124.379750475636906597, -124.379750434719806122], [-0.000255641448860615206275, -0.000255641448625262406605], [0.00123940655114286076553, 0.0012394065512049897604], [0.282291869200159655122, 0.282291869200160415283]]

The output shows:

The system of equations generated by eqs_quat
After substituting the leg lengths
The solutions as intervals, where each solution is a vector of 6 intervals representing [X, Y, Z, a, b, c]

Optimized Quaternion Equations:

We can suppose that $\overrightarrow{CB}_{1|R_m} = 0$ and $\overrightarrow{OA}_{1} = 0$. Then, the previous equations can be rewritten:

\[\begin{equation} L_1^2 = \|\overrightarrow{OC}_{|R_f}\|^2 \end{equation}\]

\[\begin{equation} L_i^2 - L_1^2 = \|\overrightarrow{OC}_{|R_f}\|^2 - \|\overrightarrow{OA}_{i|R_f}\|^2 = \|\mathcal{R} \cdot \overrightarrow{CB}_{i|R_m} - \overrightarrow{OA}_{i|R_f}\|^2 + 2 \langle \mathcal{R} \cdot \overrightarrow{CB}_{i|R_m} - \overrightarrow{OA}_{i|R_f}, \overrightarrow{OC}_{|R_f} \rangle \text{ for } i = 2, \ldots, 6. \end{equation}\]

julia> sys2 = Hexapod.eqs_quat2(A, B)
6-element Vector{Any}:
 l1² - t3² - t2² - t1²
 -162226804217//1000000 + 182912211//250000c - 26409171//62500b - 9560013//125000a + 1001//500t3 + 34883//50t2 - 100677//250t1 - 549757973781//1000000c² + 55942894617//125000bc - 291438903097//1000000b² - 26255829//62500ac - 181849789//250000ab - 678970080661//1000000a² + l2² - l1² - 531211//250t3c + 153342//125t3b + 4//1t2c + 153342//125t2a + 4//1t1b + 531211//250t1a - 999//500t3c² - 999//500t3b² - 153342//125t3ac - 531211//250t3ab + 1001//500t3a² - 178398//125t2c² + 153342//125t2bc + 34883//50t2b² - 4//1t2ab - 178398//125t2a² - 100677//250t1c² - 531211//250t1bc + 206007//250t1b² + 4//1t1ac + 206007//250t1a² + l2²c² + l2²b² + l2²a² - l1²c² - l1²b² - l1²a²
 -1880580999561//4000000 - 1624//625c + 119127//125000b + 234092845601//125000a - 1//500t3 - 337063//500t2 - 1194209//1000t1 - 12135561556361//4000000c² + 983560801//125000bc - 512619927129//4000000b² - 119127//125000ac - 1624//625ab - 10767600483929//4000000a² + l3² - l1² - 12992//5t3c + 119127//125t3b + 119127//125t2a + 12992//5t1a - 1//500t3c² - 1//500t3b² - 119127//125t3ac - 12992//5t3ab - 1//500t3a² - 1636263//500t2c² + 119127//125t2bc - 337063//500t2b² - 1636263//500t2a² - 1194209//1000t1c² - 12992//5t1bc - 241193//1000t1b² - 241193//1000t1a² + l3²c² + l3²b² + l3²a² - l1²c² - l1²b² - l1²a²
 -121190218201//800000 + 1974723//500c + 284903287//125000b - 122128719//125000a + 1001//500t3 - 84303//125t2 - 77743//200t1 - 2171972435801//800000c² - 369967887969//125000bc - 4023214281901//4000000b² + 284528213//125000ac - 15777//4ab - 14277125401901//4000000a² + l4² - l1² - 2598//1t3c - 187537//125t3b + 4//1t2c - 187537//125t2a + 4//1t1b + 2598//1t1a - 999//500t3c² - 999//500t3b² + 187537//125t3ac - 2598//1t3ab + 1001//500t3a² - 409053//125t2c² - 187537//125t2bc - 84303//125t2b² - 4//1t2ab - 409053//125t2a² - 77743//200t1c² - 2598//1t1bc - 1889011//1000t1b² + 4//1t1ac - 1889011//1000t1a² + l4²c² + l4²b² + l4²a² - l1²c² - l1²b² - l1²a²
 -235042698491//500000 + 1593291//250000c + 1330401//250000b + 93628551133//50000a + 3//500t3 + 348557//500t2 - 295209//250t1 - 428935591251//500000c² - 25201747521//10000bc - 1152030457081//500000b² - 1330401//250000ac + 1593291//250000ab - 1345923349841//500000a² + l5² - l1² - 531097//250t3c - 443467//250t3b - 443467//250t2a + 531097//250t1a + 3//500t3c² + 3//500t3b² + 443467//250t3ac - 531097//250t3ab + 3//500t3a² - 713637//500t2c² - 443467//250t2bc + 348557//500t2b² - 713637//500t2a² - 295209//250t1c² - 531097//250t1bc - 369338//125t1b² - 369338//125t1a² + l5²c² + l5²b² + l5²a² - l1²c² - l1²b² - l1²a²
 -12543938349//20000 + 7998//15625c + 464365819//125000b - 2950294//15625a + 1001//500t3 - 8//25t2 - 791957//500t1 - 313598448533//500000c² + 764086//15625bc - 113472236789//100000b² + 464229181//125000ac - 8002//15625ab - 567361177753//500000a² + l6² - l1² + 16//125t3c - 68319//125t3b + 4//1t2c - 68319//125t2a + 4//1t1b - 16//125t1a - 999//500t3c² - 999//500t3b² + 68319//125t3ac + 16//125t3ab + 1001//500t3a² - 24//125t2c² - 68319//125t2bc - 8//25t2b² - 4//1t2ab - 24//125t2a² - 791957//500t1c² + 16//125t1bc - 1065233//500t1b² + 4//1t1ac - 1065233//500t1a² + l6²c² + l6²b² + l6²a² - l1²c² - l1²b² - l1²a²

julia> Hexapod.subs_lengths!(sys2, lengths);

julia> sols2 = Hexapod.DKP(sys2, prec)
8-element Vector{Vector{MPFI.BigInterval}}:
 [[-395.797270535426759241, -395.797262501023393411], [-97.077872764569315944524, -97.077870888896776059489], [151.227854350740152346, 151.227856280612610131], [-0.000340107192244695196339, -0.000340107188372552666908], [0.00137054604234942669214, 0.00137054604351327630354], [-0.282291867115838402148, -0.282291867115825123301]]
 [[-415.098266633787691585, -415.098263690515611485], [14.9877037109694915657471, 14.9877039431652447263388], [128.133738271444641336, 128.133738842573919575], [-0.000249610110260563406427, -0.000249610109153771535096], [0.24372966308103655795, 0.243729663081235387984], [-0.142181429778985605379, -0.142181429778969155945]]
 [[-391.685541196904081079, -391.68553772169674751], [6.8185936628909212755229, 6.81859377663808669685822], [188.375687377479386485, 188.375687663586381927], [-0.000263421753416248920645, -0.000263421752863709866875], [-0.244859799494695118253, -0.244859799494598651358], [-0.139989338860034326551, -0.139989338860028275456]]
 [[-395.73099634040460211, -395.730996340108771708], [4.075981984391817877, 4.07598198440378305541], [179.804950288534698841, 179.804950288623381402], [1.2226083659490885844e-08, 1.22260838231279202196e-08], [-1.3122510735380458593e-08, -1.31225107349154550761e-08], [4.68572966605607836924e-08, 4.68572966605675599559e-08]]
 [[-396.183329629852611392, -396.18332962036405176], [4.15538893630998123455, 4.15538893674261018956], [-178.804242992094499162, -178.804242989256644322], [-0.000574450685251042818619, -0.000574450685245806422902], [2.17516200055947536523e-06, 2.17516200057758426924e-06], [1.20643972354565908404e-06, 1.20643972354677835919e-06]]
 [[-415.87524411717356683, -415.875242586377718026], [15.8244239964512850418436, 15.8244243771117341083183], [-125.486354140183689599, -125.48635361708477692], [-0.000333247484281967423805, -0.000333247483152271821364], [-0.245028889129714213375, -0.245028889129539957271], [0.139926385079611810798, 0.139926385079624064383]]
 [[-391.396779076780638812, -391.396778116908208162], [6.43080121879515432316312, 6.43080131488560694736139], [-188.988522591556637381, -188.988522461197682799], [-0.000348361885098395714564, -0.000348361884816662300445], [0.243558805155159812337, 0.243558805155204375824], [0.14224721924469467058, 0.142247219244698357789]]
 [[-396.100516242409162612, -396.100511557835464144], [-96.660221159321577976065, -96.660219000883231870718], [-150.700493141776278076, -150.700491474200225103], [-0.000255641450272732004151, -0.000255641446507087209797], [0.00123940655095647378102, 0.00123940655195053769839], [0.282291869200153947096, 0.282291869200166109432]]

Solution Processing

Now, given a solution to algebraic system describing DKP, i.e. $\big(\overrightarrow{OC}_{|R_f}, a, b, c\big)$, we can find the mobile platform position $\big(\overrightarrow{OB}_{|R_f}\big)$ by:

\[\begin{equation} \overrightarrow{OB}_{i|R_f} = \overrightarrow{OC}_{|R_f} + \mathcal{R} \cdot \overrightarrow{CB}_{i|R_m}, \quad i = 1 \ldots 6 \end{equation}\]

julia> sols = Hexapod.midpoints(sols) # Get midpoints of solution intervals
8-element Vector{Vector{BigFloat}}:
 [-0.1028437565998751715033831055725792671690782542626152462617028504610061645507812, -42.62392760445638524580136419217524235136806964874267578125, -234.08043237998013606782077289381049922667443752288818359375, -0.00034010719018762144394514399211898632302553124873156775720417499542236328125, 0.00137054604238579699252134334896448264462520683082402683794498443603515625, -0.282291867115831866807962313714597257785499095916748046875]
 [-37.37547800844150524949595360002518873443477787077426910400390625, 21.491358380178234838031483926179543431089769001118838787078857421875, -124.352611386940638003861803184690870693884789943695068359375, -0.0002496101091538290965696246040688051348599429957175743766129016876220703125, 0.24372966308103655795722249088886002255094354040920734405517578125, -0.14218142977897729739567012074985541403293609619140625]
 [37.28408423821875068899546479261886133826919831335544586181640625, 21.60252603424603488378535030844940223460071138106286525726318359375, -124.348285341871199020935545576094227726571261882781982421875, -0.00026342175175863175936764156031297083593312180482826079241931438446044921875, -0.24485979949459865136129010053378518563249599537812173366546630859375, -0.13998933886003139814846463195863179862499237060546875]
 [-5.938091855944033833798989729444217969787922495328302829875610768795013427734375e-06, -1.64365402877728453582240545096561780240296268829069958883337676525115966796875e-05, -1.462191797368204755396733058465140247476204971022184508910868316888809204101562e-05, 1.222608377007372548054133915918668991119335962003411566456634318456053733825684e-08, -1.312251073521507571035725500362104170378363572624369481900430400855839252471924e-08, 4.6857296660565574722458735169861032687776969396509230136871337890625e-08]
 [-9.517823289927782309217904345250438617916100003668589124572463333606719970703125e-05, 0.00104996862382919592706993211083006667383443755170446820557117462158203125, -358.60799659475887037063390749835889437235891819000244140625, -0.0005744506852492428075769805541285657757288873881407198496162891387939453125, 2.17516200057085087936392497100291970775487460088015723158605396747589111328125e-06, 1.2064397235461712877720164005523884043213911354541778564453125e-06]
 [-36.83724213969378136483874552897788134941947646439075469970703125, 21.37795399898304448161614164936050741516737616620957851409912109375, -234.107206533984558614880366889110518968664109706878662109375, -0.0003332474854114871349841104153431496637249864534169319085776805877685546875, -0.245028889129365701180537744086063156601085211150348186492919921875, 0.13992638507961785432343049251358024775981903076171875]
 [36.9176938046970347603349671405936760493204928934574127197265625, 21.192101683558031152503596528102747242883197031915187835693359375, -234.111754608097588914450426500479807145893573760986328125, -0.0003483618831262618158269447641369083445983534375045564956963062286376953125, 0.24355880515498155840321196745346110645868975552730262279510498046875, 0.14224721924469652112321682579931803047657012939453125]
 [0.0699918925143396420155404489581228537342649786978654447011649608612060546875, -43.148047021262480644858416933828948458540253341197967529296875, -124.37975045517835635984493780625825820607133209705352783203125, -0.00025564144874293880644006357130974989300131738900745403952896595001220703125, 0.00123940655117392526296482582162834795713735047684167511761188507080078125, 0.282291869200160035202618047378564369864761829376220703125]

julia> B_fix = Hexapod.quat_to_pos(sols[4], B) # Compute mobile platform position
6-element Vector{Vector{BigFloat}}:
 [68.41000365974327238059131970506560785221276347473073897731410397682338953018234, 393.587981984399729603316824196534567710666855516743467546803003642708063125611, 0.9999502885647889325414297594964268993872119270505116617187013616785407066345256]
 [375.0939906967270299269246939334241218003308674169837821921191789442673325538637, -137.6230256084005007906972342194550979320596083288474886785479611717164516448961, 8.119715971990463238979302416000802928262858326746709280996583402156829833984295e-06]
 [306.6639877756142542812109422038749453180321103210048683251898182788863778114363, -256.0120238414236013453868661851603887554050358044133162138678017072379589080826, 1.000017418546020297883460212760581690654543805807374923233510344289243221282961]
 [-306.6640121956950318655225531863531646800186602694338056096512445947155356407205, -255.9120089379399366541089803417574300312890660247244767333540949039161205291751, 1.312367334627669286262428409713139804658454945496259824722073972225189208984434e-06]
 [-375.0570093267298601735337526839541071864853874137657463450068462407216429710391, -137.5090071718688977975834699380997850164912339743228386623741243965923786163338, 0.9999884213025430550134793507093457017069959222310693292001815279945731163024981]
 [-68.22799631322913170504613552339011363312112120466101927718227670993655920028709, 393.6199852317802361257875448646072808988574838473528672011525486595928668975854, -5.330050147350082614304010444071319429430039216200043483695480972528457641601587e-05]

Path Planning

The module provides path planning capabilities using different algorithms:

The path planning algorithms support several options:

alg: Choice of algorithm
- "RS": Default algorithm
- "LACE-1": Classic Newton algorithm without intersections
- "LACE-2": Conditional intersections (similar to RS)
Precision parameters:
- input_precision: Default 52
- point_precision: Default 52
- work_precision: Default 52
- output_precision: Default 32
Other parameters:
- max_nb_loops: Maximum number of iterations (default 10)

julia> new_sol, newton_status = Hexapod.follow_path(
    sys1, sols[4], R,
    max_nb_loops=10,
    output_precision=Int32(52),
    alg="RS");

julia> new_sol, newton_status = Hexapod.follow_path(
    sys1, sols[4], R,
    output_precision=Int32(52),
    max_nb_loops=Int32(10),
    alg="LACE-1");  # Alternative using LACE-1 algorithm

Simulation Examples

Small Leg Deformations:

julia> m = 1;
julia> deformations = [Hexapod.deform_legs(lengths, m) for _ in 1:50]

julia> B_list = Hexapod.follow_path(A, B, sols[4], deformations, alg="LACE-1")

julia> Hexapod.animate_robot(A, B_list, name="robot_1", path=pwd())

Pure Rotational Movement:

julia> prec = 25;
julia> commands = Hexapod.generate_lengths(A, B, 50, prec);

julia> B_list = Hexapod.follow_path(A, B, sols[4], commands, alg="LACE-1");

julia> Hexapod.animate_robot(A, B_list, name="robot_2", path=pwd())

References

[1] V.E. Gough. Contribution to discussion of papers on research in automobile stability, control and tyre performance, 1956-1957. Proc. Auto Div. Inst. Mech. Eng.

[2] Jean-Charles Faugère, Jean-Pierre Merlet, Fabrice Rouillier. On solving the direct kinematics problem for parallel robots. [Research Report] RR-5923, INRIA. 2006. ⟨inria-00072366v2⟩