RPP dual synthesis

Off[General::spell]
Off[General::spell1]

1. Synthesis

Task positions:  nR=2, nmax=3+2/3. Fix one prismatic axis to obtain nmax=3.
After solving for the rotations, nT=5.

1. Definition of the RPP chain in dual quaternions:  (θ,0), (0,d),(0,b)

quatmult[r_, 
        t_] := {r[[4]]*t[[1]] + t[[4]]*r[[1]] + 
          r[[2]]*t[[3]] - t[[2]]*r[[3]], 
        r[[4]]*t[[2]] + t[[4]]*r[[2]] + 
          r[[3]]*t[[1]] - t[[3]]*r[[1]], 
        r[[4]]*t[[3]] + t[[4]]*r[[3]] + 
          r[[1]]*t[[2]] - 
          t[[1]]*r[[2]], -r[[1]]*t[[1]] - 
          t[[2]]*r[[2]] - r[[3]]*t[[3]] + 
          t[[4]]*r[[4]]}

G = {Sin[θ/2] gx, Sin[θ/2] gy, Sin[θ/2] gz, 
          Cos[θ/2]} + ϵ {Sin[θ/2] gx0, 
            Sin[θ/2] gy0, Sin[θ/2] gz0, 0}

H = {0, 0, 0, 
          1} + ϵ {d/2 hx, d/2  hy, d/2 hz, 0}

F = {0, 0, 0, 
          1} + ϵ {b/2 fx, b/2  fy, b/2 fz, 0}

Qrpp = 
      Collect[(Collect[
              quatmult[quatmult[G, H], 
                F], ϵ] /. {ϵ^2 -> 
                0, ϵ^3 -> 0}), {ϵ,  
          Cos[(θ/2)],  Sin[(θ/2)], d, b}, Simplify]

MatrixForm[Qrpp]

2. Separating the 8 equations

Qrppsep = 
      Flatten[{Coefficient[Qrpp, ϵ, 0], 
          Coefficient[Qrpp, ϵ, 1]}]

3. Solving for the rotations

firstRPP = {Qrppsep[[1]], Qrppsep[[2]], Qrppsep[[3]], 
        Qrppsep[[4]]}

col1 = Coefficient[firstRPP, Sin[(θ/2)] ]

col2 = Coefficient[firstRPP, Cos[(θ/2)] ]

firstmat = Transpose[{col1, col2}]

MatrixForm[firstmat]

Solve all the systems,

systems = {firstmat[[{1, 4}, {1, 2}]], 
        firstmat[[{2, 4}, {1, 2}]], 
        firstmat[[{3, 4}, {1, 2}]]}
indep = {{px, pw}, {py, pw}, {pz, pw}}

solstheta = 
      Table[Inverse[systems[[i]]] . indep[[i]], {i, 1, 
          Length[systems]}]

equating all values for sin theta,

eq1 = 
      solstheta[[1, 1]] - 
        solstheta[[2, 1]]
eq2 = solstheta[[1, 1]] - solstheta[[3, 1]]

solsg = 
      Solve[{eq1 == 0, eq2 == 0, 
          gx^2 + gy^2 + gz^2 == 1}, {gx, gy, gz}]

So the axis g must be equal to the direction of the relative displacement p.

4. Eliminating d , b from the translations and create design equation

Notice that f and h appear always as their cross product; we define the plane of translations f-h by its normal direction, n. We use this so that we avoid the degenerate solution f||h.

secRPP = 
Collect[{Qrppsep[[5]], 
            Qrppsep[[6]], Qrppsep[[7]], Qrppsep[[8]]} - {px0, 
            py0, pz0, pw0}, {Cos[(θ/2)], 
          Sin[(θ/2)] }]

We choose one of the solutions for sin(θ/2); any of them can be used.

secRPP2 = 
      Collect[{secRPP[[1]] /. {Cos[(θ/2)]  -> pw,  
              Sin[(θ/2)] -> px/gx}, 
          secRPP[[2]] /. {Cos[(θ/2)]  -> pw,  
              Sin[(θ/2)] -> py/gy}, 
          secRPP[[3]] /. {Cos[(θ/2)]  -> pw,  
              Sin[(θ/2)] -> pz/gz}, 
          secRPP[[4]] /. {Cos[(θ/2)]  -> pw,  
              Sin[(θ/2)] -> px/gx}}, {d, b}, Simplify]

Substitute the solutions for g,

secRPP3 = Simplify[secRPP2 /. solsg[[2]]]

use only the three independent equations to solve for b, d:

secRPP4 = Drop[secRPP3, -1]

col1 = Coefficient[secRPP4, d]

col2 = Coefficient[secRPP4, b]

col3 = Simplify[Coefficient[secRPP4*b*d, b*d]]

mathg0 = Transpose[{col1, col2, col3}]

MatrixForm[mathg0]

system solvable for d,b : the determinant must be zero:

Des1 = Simplify[Det[mathg0]]

the equation has an h×f factor that multiplies everywhere. To eliminate the possible solution of h and f parallel, we express them as the normal to the plane that they define.

Des1b = Des1 /. (Sqrt[(px^2) + (py^2) + (pz^2)]) -> SQ

Des1c = 
      Collect[Des1b, {px, py, pz, pw, px0, py0, pz0, pw0}, 
        Simplify]

Des1d = 
      Des1c /. {(fy hx - fx hy)  -> 
            Cz, (-fy hx + fx hy) -> -Cz, (-fz hx + 
                fx hz) -> 
            Cy, (fz hx - fx hz) -> -Cy, (fz hy - 
                fy hz) -> 
            Cx, (-fz hy + fy hz) -> -Cx}

Des1e = Collect[Des1d, {gx0, gy0, gz0, SQ, pw, py, pz, px}]

Des1f = 
      Des1e /. {(((fy hx)/4) - ((fx hy)/4))  -> 
            Cz/4, (-((fy hx)/4) + ((fx hy)/4)) -> -Cz/4, (-((fz hx)/4) + ((fx hz)/4)) -> 
            Cy/4, (((fz hx)/4) - ((fx hz)/4)) -> -Cy/
              4, (((fz hy)/4) - ((fy hz)/4)) -> 
            Cx/4, (-((fz hy)/4) + ((fy hz)/4)) -> -Cx/4}

Collect[Des1f, {Cx, Cy, Cz, px, py, pz, pw}, Simplify]

p0 = {px0, py0, pz0}

g0 = {gx0, gy0, gz0}

vector expression:

Simplify[
      Des1f - 1/
            4 {Cx, Cy, 
              Cz} . ((SQ g0 . p - p . p0) p + 
                pw^2 (SQ g0 - p0) + 
                pw (SQ g0\[Cross]p - p0\[Cross]p))]

Desfinal = 
      Des1f /. {Cx -> nx, Cy -> ny, Cz -> nz, 
          SQ -> (Sqrt[(px^2) + (py^2) + (pz^2)])}

5. Closed algebraic solution adding constraints.

We solve for g first, then create four extra quaternions and solve for g0 and the normal direction, n. We use this so that we avoid the degenerate solution f||h.

SOLVE FOR THE REVOLUTE JOINT ANGLE

EvalQuat[expr_, q_] := 
      expr /. {px -> q[[1, 1]], py -> q[[1, 2]], 
          pz -> q[[1, 3]], pw -> q[[1, 4]], 
          px0 -> q[[2, 1]], py0 -> q[[2, 2]], 
          pz0 -> q[[2, 3]], pw0 -> q[[2, 4]]}

solsg

The complete dual quaternion:

qS12={{0.7508114675922651,  -0.10654941504058091,  
              0.12486135396067718,  
              0.6397960648312598},  {0.29824734722269053,  -1.492876598025916,  1.278520848755025,  -0.8481304134104388}}

gsol = {gx, gy, gz} /. EvalQuat[solsg[[2]], qS12]

SOLUTION FOR THE REST OF VARIABLES: we fix one of the prismatic directions and solve for 4 relative translations

moment equations:

Create task positions with orientation within the workspace. We need two extra position if we set some constraints in h, otherwise three.

theta13 = Random[Real, {-6.24, 6.24}]
qS13R = 
      firstRPP /. {gx -> gsol[[1]], gy -> gsol[[2]], 
          gz -> gsol[[3]], θ -> 
            theta13}
s0 = {Random[Real, {-3, 3}], Random[Real, {-3, 3}], 
        Random[Real, {-3, 3}]}
d0 = Random[Real, {-4, 4}]
axis3 = {gsol, s0\[Cross]gsol}

qS13 =  {qS13R, 
        Flatten[{Sin[theta13/2]*axis3[[2]] + 
              d0/2*Cos[theta13/2]*axis3[[1]], -d0/2*
              Sin[theta13/2]}]}

theta14 = Random[Real, {-6.24, 6.24}]
qS14R = 
      firstRPP /. {gx -> gsol[[1]], gy -> gsol[[2]], 
          gz -> gsol[[3]], θ -> 
            theta14}
s0 = {Random[Real, {-3, 3}], Random[Real, {-3, 3}], 
        Random[Real, {-3, 3}]}
d4 = Random[Real, {-4, 4}]
axis4 = {gsol, s0\[Cross]gsol}

qS14 =  {qS14R, 
        Flatten[{Sin[theta14/2]*axis4[[2]] + 
              d4/2*Cos[theta14/2]*axis4[[1]], -d4/2*
              Sin[theta14/2]}]}

theta15 = Random[Real, {-6.24, 6.24}]
qS15R = 
      firstRPP /. {gx -> gsol[[1]], gy -> gsol[[2]], 
          gz -> gsol[[3]], θ -> 
            theta15}
s0 = {Random[Real, {-3, 3}], Random[Real, {-3, 3}], 
        Random[Real, {-3, 3}]}
d5 = Random[Real, {-4, 4}]
axis5 = {gsol, s0\[Cross]gsol}

qS15 =  {qS15R, 
        Flatten[{Sin[theta15/2]*axis5[[2]] + 
              d5/2*Cos[theta15/2]*axis5[[1]], -d5/2*
              Sin[theta15/2]}]}

qS12={{0.7508114675922651,  -0.10654941504058091,  
              0.12486135396067718,  
              0.6397960648312598},  {0.29824734722269053,  -1.492876598025916,  1.278520848755025,  -0.8481304134104388}}
qS13 = {{0.8215319650090522`, -0.11658552657641279`, 
          0.13662230519994084`, -0.5411352799283374`}, {-0.9488756326290012`, 
          0.16618954943686334`, -0.7597296432786349`, -1.6681650741169665`}}
qS14 = {{0.4509682887373329`, -0.06399796678772736`, 0.07499687145915016`, 
          0.8870779740652442`}, {0.34964967269742897`, 
          0.751039887245947`, -0.07756496976621373`, -0.11701198965715673`}}
qS15 = {{0.40485744814484337`, -0.057454269329409724`, 
          0.0673285522643109`, -0.9100858858742186`}, {-1.3874217656460472`, 
          0.2687177919489258`, -0.00928177056296356`, -0.6348543110870924`}}

define moment equations imposing 2 constraints: define the direction of h

Des2 = {EvalQuat[Desfinal, qS12], EvalQuat[Desfinal, qS13], 
          EvalQuat[Desfinal, qS14], 
          EvalQuat[Desfinal, qS15]} /. {nz -> 1}

Des3 = 
      Collect[Des2 /. 
          gz0 -> (-gsol[[1]] gx0 - gsol[[2]] gy0)/
              gsol[[3]], {nx, ny, gx0, gy0}]

solshg0 = 
      Solve[Des3 == {0, 0, 0, 0}, {nx, ny, gx0, gy0}]

sols2 = {gsol, {gx0, gy0, gz0} /. 
            gz0 -> (-gsol[[1]] gx0 - gsol[[2]] gy0)/
                gsol[[3]], {nx, ny, 1}/Sqrt[{nx, ny, 1} . {nx, ny, 1}]} /. 
        solshg0

sols3 = 
      Table[If[(Im[sols2[[i, 2, 1]]] == 0  &&  
              Im[sols2[[i, 3, 1]]] == 0  ) , 
          sols2[[i]],  ], {i, 1, Length[sols2]}]

sols4 = Select[sols3, # =!= Null &]

Create the directions h and f from the normal to the plane. Any direction is fine, so we will make h=n×x, f=n×h.

f = {fx, fy, fz}
h = {hx, hy, hz}

hFromn = 
      Table[sols4[[i, 3]]\[Cross]{1, 0, 0}/
          Sqrt[sols4[[i, 3]]\[Cross]{1, 0, 0} . 
              sols4[[i, 3]]\[Cross]{1, 0, 0}], {i, 1, 
          Length[sols4]}]

fFromn = 
      Table[sols4[[i, 3]]\[Cross]hFromn[[i]]/
          Sqrt[sols4[[i, 3]]\[Cross]hFromn[[i]] . 
              sols4[[i, 3]]\[Cross]hFromn[[i]]], {i, 1, 
          Length[sols4]}]

solsFinal = 
      Table[{sols4[[i, 1]], sols4[[i, 2]], hFromn[[i]], 
          fFromn[[i]]}, {i, 1, Length[sols4]}]

2. Analysis

1. The task relative positions in quaternion form

qS12
qS13
qS14
qS15

The screw axes and the rotation and translation:

b12 = {qS12[[1, 1]], qS12[[1, 2]], 
        qS12[[1, 3]]}
point12 = (b12/
            Sqrt[b12 . b12])\[Cross]({qS12[[2, 1]], qS12[[2, 2]], 
              qS12[[2, 3]]}/Sqrt[b12 . b12])
s12 = {b12/Sqrt[b12 . b12], 
        point12\[Cross]b12/Sqrt[b12 . b12]}
psi12 = 
      2 ArcTan[qS12[[1, 4]], Sqrt[b12 . b12]]
t12 = -2 qS12[[2, 4]]/Sqrt[b12 . b12]

b13 = {qS13[[1, 1]], qS13[[1, 2]], 
        qS13[[1, 3]]}
point13 = (b13/
            Sqrt[b13 . b13])\[Cross]({qS13[[2, 1]], qS13[[2, 2]], 
              qS13[[2, 3]]}/Sqrt[b13 . b13])
s13 = {b13/Sqrt[b13 . b13], 
        point13\[Cross]b13/Sqrt[b13 . b13]}
psi13 = 
      2 ArcTan[qS13[[1, 4]], Sqrt[b13 . b13]]
t13 = -2 qS13[[2, 4]]/Sqrt[b13 . b13]

b14 = {qS14[[1, 1]], qS14[[1, 2]], 
        qS14[[1, 3]]}
point14 = (b14/
            Sqrt[b14 . b14])\[Cross]({qS14[[2, 1]], qS14[[2, 2]], 
              qS14[[2, 3]]}/Sqrt[b14 . b14])
s14 = {b14/Sqrt[b14 . b14], 
        point14\[Cross]b14/Sqrt[b14 . b14]}
psi14 = 
      2 ArcTan[qS14[[1, 4]], Sqrt[b14 . b14]]
t14 = -2 qS14[[2, 4]]/Sqrt[b14 . b14]

b15 = {qS15[[1, 1]], qS15[[1, 2]], 
        qS15[[1, 3]]}
point15 = (b15/
            Sqrt[b15 . b15])\[Cross]({qS15[[2, 1]], qS15[[2, 2]], 
              qS15[[2, 3]]}/Sqrt[b15 . b15])
s15 = {b15/Sqrt[b15 . b15], 
        point15\[Cross]b15/Sqrt[b15 . b15]}
psi15 = 
      2 ArcTan[qS15[[1, 4]], Sqrt[b15 . b15]]
t15 = -2 qS15[[2, 4]]/Sqrt[b15 . b15]

quat12 = 
      N[{Flatten[{Sin[psi12/2]*s12[[1]], Cos[psi12/2]}], 
          Flatten[{Sin[psi12/2]*s12[[2]], -t12/2 Sin[psi12/2]}] + 
            Flatten[{t12/2 Cos[psi12/2]*s12[[1]], 0}]}]

qS12

quat13 = 
      N[{Flatten[{Sin[psi13/2]*s13[[1]], Cos[psi13/2]}], 
          Flatten[{Sin[psi13/2]*s13[[2]], -t13/2 Sin[psi13/2]}] + 
            Flatten[{t13/2 Cos[psi13/2]*s13[[1]], 0}]}]

qS13

quat14 = 
      N[{Flatten[{Sin[psi14/2]*s14[[1]], Cos[psi14/2]}], 
          Flatten[{Sin[psi14/2]*s14[[2]], -t14/2 Sin[psi14/2]}] + 
            Flatten[{t14/2 Cos[psi14/2]*s14[[1]], 0}]}]

qS14

quat15 = 
      N[{Flatten[{Sin[psi15/2]*s15[[1]], Cos[psi15/2]}], 
          Flatten[{Sin[psi15/2]*s15[[2]], -t15/2 Sin[psi15/2]}] + 
            Flatten[{t15/2 Cos[psi15/2]*s15[[1]], 0}]}]

qS15

2. Transform to homogeneous matrices

If there is no more information, we assume the first absolute transformation is the identity

Pos1 = {{1, 0, 0, 0}, {0, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 
          1}}

The relative rotations are found by using Cayley's formula

Id = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}

makeB[b_, psi_] := 
      Tan[psi/2]*{{0, -b[[3]], b[[2]]}, {b[[3]], 
            0, -b[[1]]}, {-b[[2]], b[[1]], 0}}

makeA[Id_, B_] := (Inverse[Id - B]) . (Id + B)

B12 = N[makeB[s12[[1]], psi12]]
A12 = makeA[Id, B12]
A2 = A12

The absolute rotations will be given by A12=A2A1^(-1), but as A1 is the identity, A12=A2 and so on.

B13 = N[makeB[s13[[1]], psi13]]
A13 = makeA[Id, B13]
A3 = A13

B14 = N[makeB[s14[[1]], psi14]]
A14 = makeA[Id, B14]
A4 = A14

B15 = N[makeB[s15[[1]], psi15]]
A15 = makeA[Id, B15]
A5 = A15

For the translation, we use the following formula: (I-A)v=dxs. This gives the component perp. to s and then we need to add t.s.

d1 = {0, 0, 0}
dsol = {dx, dy, dz}

eq12 = (Id - A12) . s12[[2]] - 
        dsol\[Cross]s12[[1]]
sols = 
      Solve[{eq12[[1]] == 0, eq12[[2]] == 0, 
          dsol . s12[[1]] == 0}, {dx, dy, 
          dz}]
d12 = (dsol /. {sols[[1, 1]], sols[[1, 2]], 
              sols[[1, 3]]}) + 
        t12*s12[[1]]
d2 = d12 - A12 . d1
Pos2 = 
      Append[Transpose[Append[Transpose[A2], d2]], {0, 0, 0, 1}]

MatrixForm[Pos2]

eq13 = (Id - A13) . s13[[2]] - 
        dsol\[Cross]s13[[1]]
sols = 
      Solve[{eq13[[1]] == 0, eq13[[2]] == 0, 
          dsol . s13[[1]] == 0}, {dx, dy, 
          dz}]
d13 = (dsol /. {sols[[1, 1]], sols[[1, 2]], 
              sols[[1, 3]]}) + 
        t13*s13[[1]]
d3 = d13 - A13 . d1
Pos3 = 
      Append[Transpose[Append[Transpose[A3], d3]], {0, 0, 0, 1}]

MatrixForm[Pos3]

eq14 = (Id - A14) . s14[[2]] - 
        dsol\[Cross]s14[[1]]
sols = 
      Solve[{eq14[[1]] == 0, eq14[[2]] == 0, 
          dsol . s14[[1]] == 0}, {dx, dy, 
          dz}]
d14 = (dsol /. {sols[[1, 1]], sols[[1, 2]], 
              sols[[1, 3]]}) + 
        t14*s14[[1]]
d4 = d14 - A14 . d1
Pos4 = 
      Append[Transpose[Append[Transpose[A4], d4]], {0, 0, 0, 1}]

MatrixForm[Pos4]

eq15 = (Id - A15) . s15[[2]] - 
        dsol\[Cross]s15[[1]]
sols = 
      Solve[{eq15[[1]] == 0, eq15[[2]] == 0, 
          dsol . s15[[1]] == 0}, {dx, dy, 
          dz}]
d15 = (dsol /. {sols[[1, 1]], sols[[1, 2]], 
              sols[[1, 3]]}) + 
        t15*s15[[1]]
d5 = d15 - A15 . d1
Pos5 = 
      Append[Transpose[Append[Transpose[A5], d5]], {0, 0, 0, 1}]

MatrixForm[Pos5]

3. Compute the Denavit-Hartenberg parameters

Preliminaries

Ztrans[θ_, 
        d_] := {{Cos[θ], -Sin[θ], 0, 0}, {Sin[θ], 
          Cos[θ], 0, 0}, {0, 0, 1, d}, {0, 0, 0, 1}}

Xtrans[α_, 
        a_] := {{1, 0, 0, a}, {0, Cos[α], -Sin[α], 0}, {0, 
          Sin[α], Cos[α], 0}, {0, 0, 0, 1}}

Take only the real solutions.

sols = solsFinal

The joint axes expressed as lines

To analyze the different solutions, change the value of nsol.

nsol = 1

G = {sols[[nsol, 1]], sols[[nsol, 2]]}

h = sols[[nsol, 3]]

f = sols[[nsol, 4]]

Make them unit and clean

Gline = {G[[1]]/Sqrt[G[[1]] . G[[1]]], 
        G[[2]]/Sqrt[G[[1]] . G[[1]]]}

Gline[[1]] . Gline[[2]]

We put H in an arbitrary location,

pointG = Gline[[1]]\[Cross]Gline[[2]]

dirPoint = pointG/Sqrt[pointG . pointG]

pointH = pointG + 1*dirPoint + 1*Gline[[1]]

Hline = {h/Sqrt[h . h], pointH\[Cross]h/Sqrt[h . h]}

pointF = pointG + 2*dirPoint + 2*Gline[[1]]

Fline = {f/Sqrt[f . f], pointF\[Cross]f/Sqrt[f . f]}

Find transformation [G]

Locate the first frame in the point of the perpendicular to G from the origin, with directions { Ng , G×Ng , G}

Rgtrans = {dirPoint, Gline[[1]]\[Cross]dirPoint, Gline[[1]], 
        pointG}

Ghom = Transpose[Rgtrans]

Gtot = Append[Ghom, {0, 0, 0, 1}]
MatrixForm[Gtot]

Check Gtot:

rotmat = Gtot[[{1, 2, 3}, {1, 2, 3}]]
zeromat = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}
dispmat = 
      Flatten[Gtot[[{1, 2, 3}, {4}]]]
dmat = makeB[dispmat, Pi/2]

bigmat = {Join[rotmat[[1]], {0, 0, 0}], 
        Join[rotmat[[2]], {0, 0, 0}], Join[rotmat[[3]], {0, 0, 0}], 
        Join[(dmat . rotmat)[[1]], rotmat[[1]]], 
        Join[(dmat . rotmat)[[2]], rotmat[[2]]], 
        Join[(dmat . rotmat)[[3]], rotmat[[3]]]}

MatrixForm[bigmat]

bigmat . {0, 0, 1, 0, 0, 0}

Gline

bigmat . {1, 0, 0, 0, 0, 0}

dirPoint

Transformation about Z from initial x-axis in G to normal G-H (rotation will be added to θ)

Ngh = ({Gline[[1]]\[Cross]Hline[[1]], 
            Gline[[1]]\[Cross]Hline[[2]] + 
              Gline[[2]]\[Cross]Hline[[1]]})/
        Sqrt[(Gline[[1]]\[Cross]
                Hline[[1]]) . (Gline[[1]]\[Cross]
                Hline[[1]])]

pointNgh = Ngh[[1]]\[Cross]Ngh[[2]]

Nghline = {Ngh[[1]], pointNgh\[Cross]Ngh[[1]]}

Xaxis = {dirPoint,  pointG\[Cross]dirPoint}

cosanglegh = Xaxis[[1]] . Nghline[[1]]

minusasinanglegh = 
      Xaxis[[1]] . Nghline[[2]] + 
        Xaxis[[2]] . Nghline[[1]]

angle1 = ArcCos[cosanglegh]

length1 = -minusasinanglegh/Sqrt[1 - cosanglegh^2]

If[(Drop[((Gtot . Ztrans[0, 0] . Ztrans[angle1, length1]) . {1, 
                    0, 0, 0}), {4}]) . Nghline[[1]] == 1,  , 
      angle1 = -angle1; length1 = -length1]

check that we get the direction Ngh, when the angle θ is zero:

(Gtot . Ztrans[0, 0] . Ztrans[angle1, length1]) . {1, 0, 0, 
        0}

Nghline[[1]]

Transformation about X from  G to H along normalGH (rotation so that Z-axis is aligned with H)

cosanglegh = Gline[[1]] . Hline[[1]]

minusasinanglegh = 
      Gline[[1]] . Hline[[2]] + 
        Gline[[2]] . Hline[[1]]

angle2 = ArcCos[cosanglegh]

length2 = -minusasinanglegh/Sqrt[1 - cosanglegh^2]

If[(Drop[((Gtot . Ztrans[0, 0] . Ztrans[angle1, length1] . 
                      Xtrans[angle2, length2]) . {0, 0, 1, 0}), {4}]) . 
          Hline[[1]] == 1,  , angle2 = -angle2; 
      length2 = -length2]

check that we get the direction H, when the angle θ is zero:

(Gtot . Ztrans[0, 0] . Ztrans[angle1, length1] . 
          Xtrans[angle2, length2]) . {0, 0, 1, 0}

Hline[[1]]

check that we get the intersection point:

part = (Hline[[2, 1]] - pointNgh[[2]] Hline[[1, 3]] + 
            pointNgh[[3]] Hline[[1, 2]])/(Nghline[[1, 
                  2]] Hline[[1, 3]] - 
            Nghline[[1, 3]] Hline[[1, 2]])

pointInt = pointNgh + part Nghline[[1]]

(Gtot . Ztrans[0, 0] . Ztrans[angle1, length1] . 
          Xtrans[angle2, length2]) . {0, 0, 0, 1}

Transformation about Z from normal G-H to normal H-F (translation will be added to d)

Hline

Fline

Nhf = ({Hline[[1]]\[Cross]Fline[[1]], 
            Hline[[1]]\[Cross]Fline[[2]] + 
              Hline[[2]]\[Cross]Fline[[1]]})/
        Sqrt[(Hline[[1]]\[Cross]
                Fline[[1]]) . (Hline[[1]]\[Cross]
                Fline[[1]])]

pointNhf = Nhf[[1]]\[Cross]Nhf[[2]]

Nhfline = {Nhf[[1]], pointNhf\[Cross]Nhf[[1]]}

cosanglegh = Nghline[[1]] . Nhfline[[1]]

minusasinanglegh = 
      Nghline[[1]] . Nhfline[[2]] + 
        Nghline[[2]] . Nhfline[[1]]

angle3 = ArcCos[cosanglegh]

length3 = -minusasinanglegh/Sqrt[1 - cosanglegh^2]

If[(Drop[((Gtot . Ztrans[0, 0] . Ztrans[angle1, length1] . 
                      Xtrans[angle2, length2] . 
                      Ztrans[angle3, length3]) . {1, 0, 0, 0}), {4}]) . 
          Nhfline[[1]] == 1,  , angle3 = -angle3; 
      length3 = -length3]

check that we get the direction Ngh, when the angle θ is zero:

(Gtot . Ztrans[0, 0] . Ztrans[angle1, length1] . 
          Xtrans[angle2, length2] . Ztrans[angle3, length3]) . {1, 0, 0, 
        0}

Nhfline[[1]]

Transformation about X from  H to F along normalHF (rotation so that Z-axis is aligned with F)

cosanglegh = Hline[[1]] . Fline[[1]]

minusasinanglegh = 
      Hline[[1]] . Fline[[2]] + 
        Hline[[2]] . Fline[[1]]

angle4 = ArcCos[cosanglegh]

length4 = -minusasinanglegh/Sqrt[1 - cosanglegh^2]

If[(Drop[((Gtot . Ztrans[0, 0] . Ztrans[angle1, length1] . 
                      Xtrans[angle2, length2] . Ztrans[angle3, length3] . 
                      Xtrans[angle4, length4]) . {0, 0, 1, 0}), {4}]) . 
          Fline[[1]] == 1,  , angle4 = -angle4; 
      length4 = -length4]

check that we get the direction H, when the angle θ is zero:

(Gtot . Ztrans[0, 0] . Ztrans[angle1, length1] . 
          Xtrans[angle2, length2] . Ztrans[angle3, length3] . 
          Xtrans[angle4, length4]) . {0, 0, 1, 0}

Fline[[1]]

Relative transformation [H] from H to the gripper.

Relative transformation from last joint to the gripper:  calculated as position 1. The angles theta and phi to go  to the first position are set to zero.

theta10 = 0
d10 = 0
b10 = 0

Compose the kinematic equations,

KE10 = 
      Gtot . Ztrans[theta10, 0] . Ztrans[angle1, length1] . 
        Xtrans[angle2, length2] . Ztrans[0, d10] . Ztrans[angle3, length3] . 
        Xtrans[angle4, length4] . Ztrans[0, b10]

Htot = Inverse[KE10] . Pos1

KE10 . Htot

4. Compute inverse kinematics and check positions

Inverse kinematics for the joint variables

EvalPos[expr_, q_] := 
      expr /. {gx -> Gline[[1, 1]], gy -> Gline[[1, 2]], 
          gz -> Gline[[1, 3]], gx0 -> Gline[[2, 1]], 
          gy0 -> Gline[[2, 2]], gz0 -> Gline[[2, 3]], 
          hx -> Hline[[1, 1]], hy -> Hline[[1, 2]], 
          hz -> Hline[[1, 3]], fx -> Fline[[1, 1]], 
          fy -> Fline[[1, 2]], fz -> Fline[[1, 3]], 
          px -> q[[1, 1]], py -> q[[1, 2]], 
          pz -> q[[1, 3]], pw -> q[[1, 4]], 
          px0 -> q[[2, 1]], py0 -> q[[2, 2]], 
          pz0 -> q[[2, 3]], pw0 -> q[[2, 4]]}

quat = {Append[p, pw], Append[p0, pw0]}

solsthetaNum = EvalPos[solstheta, quat]

thetaSol = 
      2*ArcTan[solsthetaNum[[1, 2]], solsthetaNum[[1, 1]]]

matnum = EvalPos[mathg0, quat]

solsdb = 
      Simplify[Inverse[
            matnum[[{1, 2}, {1, 2}]]] . (-matnum[[{1, 2}, 
                  3]])]

Check if the chain hits the rest of positions

Position 2:

theta12 = EvalPos[thetaSol, qS12]

d12 = EvalPos[solsdb[[1]], qS12]

b12 = EvalPos[solsdb[[2]], qS12]

Assemble kinematic equations and compare with absolute transformation

KE12 = 
      Gtot . Ztrans[theta12, 0] . Ztrans[angle1, length1] . 
        Xtrans[angle2, length2] . Ztrans[0, d12] . Ztrans[angle3, length3] . 
        Xtrans[angle4, length4] . Ztrans[0, b12] . Htot

Pos2

Position 3:

theta13 = EvalPos[thetaSol, qS13]

d13 = EvalPos[solsdb[[1]], qS13]

b13 = EvalPos[solsdb[[2]], qS13]

Assemble kinematic equations and compare with absolute transformation

KE13 = 
      Gtot . Ztrans[theta13, 0] . Ztrans[angle1, length1] . 
        Xtrans[angle2, length2] . Ztrans[0, d13] . Ztrans[angle3, length3] . 
        Xtrans[angle4, length4] . Ztrans[0, b13] . Htot

Pos3

Position 4:

theta14 = EvalPos[thetaSol, qS14]

d14 = EvalPos[solsdb[[1]], qS14]

b14 = EvalPos[solsdb[[2]], qS14]

Assemble kinematic equations and compare with absolute transformation

KE14 = 
      Gtot . Ztrans[theta14, 0] . Ztrans[angle1, length1] . 
        Xtrans[angle2, length2] . Ztrans[0, d14] . Ztrans[angle3, length3] . 
        Xtrans[angle4, length4] . Ztrans[0, b14] . Htot

Pos4

Position 5:

theta15 = EvalPos[thetaSol, qS15]

d15 = EvalPos[solsdb[[1]], qS15]

b15 = EvalPos[solsdb[[2]], qS15]

Assemble kinematic equations and compare with absolute transformation

KE15 = 
      Gtot . Ztrans[ theta15, 0] . Ztrans[angle1, length1] . 
        Xtrans[angle2, length2] . Ztrans[0, d15] . Ztrans[angle3, length3] . 
        Xtrans[angle4, length4] . Ztrans[0, b15] . Htot

Pos5

pos5num = 
      EvalPos[{gx Sin[(θ/2)] + ϵ ((((b fx)/2) + ((d hx)/2)) Cos[(θ/2)] + (gx0 + (1/2) b (fz gy - fy gz) + 
                          (1/2) d (-gz hy + 
                                gy hz)) Sin[(θ/2)]), 
            gy Sin[(θ/2)] + ϵ ((((b fy)/2) + ((d hy)/2)) Cos[(θ/2)] + (gy0 + (1/2) b (-fz gx + fx gz) + 
                          (1/2) d (gz hx - 
                                gx hz)) Sin[(θ/2)]), 
            gz Sin[(θ/2)] + ϵ ((((b fz)/2) + ((d hz)/2)) Cos[(θ/2)] + ((1/2) b (fy gx - fx gy) + gz0 + 
                          (1/2) d (-gy hx + 
                                gx hy)) Sin[(θ/2)]), 
            Cos[(θ/2)] + ((1/2) b (-fx gx - fy gy - 
                          fz gz) + 
                    (1/2) d (-gx hx - gy hy - 
                          gz hz)) ϵ Sin[(θ/2)]}, 
          qS15] /. {θ -> theta15, d -> d15, 
          b -> b15}

Flatten[{Coefficient[pos5num, ϵ, 0], 
        Coefficient[pos5num, ϵ, 1]}]

qS15

Table of values for Synthetica

Add the joint parameters, pass to degrees + make it in the -Pi,Pi range.

thetas = ({theta10, theta12, theta13, theta14, theta15} + {angle1, 
                angle1, angle1, angle1, angle1})*180/Pi + {0, 0, -360, 
          0, -360}

des = {d10, d12, d13, d14, d15} + {length3, length3, length3, length3, 
          length3}

bes = {b10, b12, b13, b14, b15}