RRP dual synthesis

We include the RPR and the PRR, which have equivalent equations with some sign changes.

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

1. Synthesis

Task positions :  
            4 + 1/3.  Fix a variable in one rotation axis to obtain n=\(4\ and\ nR = 
          4\ also . 
\(Solutions : \ 12\ at\ \(\(most\)\(.\)\)\)\)

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

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}

W = {Sin[ϕ/2] wx, Sin[ϕ/2] wy, Sin[ϕ/2] wz, 
          Cos[ϕ/2]} + ϵ {Sin[ϕ/2] wx0, Sin[ϕ/2] wy0, 
            Sin[ϕ/2] wz0, 0}

Qrpr = 
      Simplify[Expand[
            quatmult[quatmult[G, W], H]] /. {ϵ^2 -> 
              0, ϵ^3 -> 0}]

Qrpr3 = 
      Collect[Qrpr, {ϵ, Cos[(θ/2)],  Sin[(θ/2)], 
          Cos[(ϕ/2)], Sin[(ϕ/2)], d}]

MatrixForm[Qrpr3]

2. Separating the 8 equations

Qrprsep = {Coefficient[Qrpr3[[1]], ϵ, 0], 
        Coefficient[Qrpr3[[2]], ϵ, 0], 
        Coefficient[Qrpr3[[3]], ϵ, 0], 
        Coefficient[Qrpr3[[4]], ϵ, 0], 
        Coefficient[Qrpr3[[1]], ϵ, 1], 
        Coefficient[Qrpr3[[2]], ϵ, 1], 
        Coefficient[Qrpr3[[3]], ϵ, 1], 
        Coefficient[Qrpr3[[4]], ϵ, 1]}

Qrprsep2 = 
      Collect[Expand[
          Qrprsep], {Cos[(θ/2)]^(_ : 1) Sin[(ϕ/2)]^(_ : 1) , Sin[(θ/2)]^(_ : 1) Cos[(ϕ/2)]^(_ : 1) , 
          Sin[(θ/2)]^(_ : 1) Sin[(ϕ/2)]^(_ : 1) , 
          Cos[(θ/2)]^(_ : 1) Cos[(ϕ/2)]^(_ : 1) }]

3. Solving for the rotations

firstRPR = {Qrprsep2[[1]], Qrprsep2[[2]], Qrprsep2[[3]], 
        Qrprsep2[[4]]}

col1 = 
      Coefficient[firstRPR, Sin[(θ/2)] Cos[(ϕ/2)]]

col2 = 
      Coefficient[firstRPR, 
        Cos[(θ/2)] Sin[(ϕ/2)]]
col3 = 
      Coefficient[firstRPR, 
        Sin[(θ/2)] Sin[(ϕ/2)]]
col4 = 
      Coefficient[firstRPR, Cos[(θ/2)] Cos[(ϕ/2)]]

firstmat = Transpose[{col1, col2, col3, col4}]

MatrixForm[firstmat]

invmat = Inverse[firstmat]

den = 
      Simplify[-(gy^2) (wx^2) - (gz^2) (wx^2) + 2 gx gy wx wy - 
          (gx^2) (wy^2) - (gz^2) (wy^2) + 2 gx gz wx wz + 
          2 gy gz wy wz - (gx^2) (wz^2) - (gy^2) (wz^2)]

Notice this is -(G×W).(G×W)=-((gz wx-gx wz)^2+(gz wy-gy wz)^2+(gy wx-gx wy)^2)  , it is different from zero if G and W are not parallel!!

Simplify[
      den + ((gz wx - gx wz)^2 + (gz wy - 
                  gy wz)^2 + (gy wx - gx wy)^2)]

sols = Simplify[invmat . {px, py, pz, pw}]

stcp = sols[[1]]
ctsp = sols[[2]]
stsp = sols[[3]]
ctcp = sols[[4]]

Extra condition for the values to be angles: equating tangents

extraeq = 
      Simplify[Numerator[stsp]*Numerator[ctcp] - 
          Numerator[stcp]*Numerator[ctsp]]

This equation simplifies to: (See quaternion synthesisRandRR notebook for whole procedure)

extraeq = 
      gz ((-pw py + px pz) wx + (pw px + 
                    py pz) wy + (-(px^2) - (py^2)) wz) + 
        gx ((-(py^2) - (pz^2)) wx + (px py - 
                    pw pz) wy + (pw py + px pz) wz) + 
        gy ((px py + pw pz) wx + (-(px^2) - 
                    (pz^2)) wy + (-pw px + 
                    py pz) wz)

4. Eliminating d from the translations

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

secRPR2 = 
      secRPR /. {Cos[(θ/2)] Sin[(ϕ/2)] -> ctsp,  
          Cos[(ϕ/2)] Sin[(θ/2)] -> stcp, 
          Sin[(θ/2)] Sin[(ϕ/2)] -> stsp,  
          Cos[(θ/2)] Cos[(ϕ/2)] -> ctcp}

Here we clear denominators, what means that we multiply p0 times (gxw).(gxw)

secRPR3 = 
      Simplify[Together[
            secRPR2]*((gz^2) ((wx^2) + (wy^2)) - 2 gx gz wx wz - 
              2 gy wy (gx wx + gz wz) + (gy^2) ((wx^2) + (wz^2)) + 
              (gx^2) ((wy^2) + (wz^2)))]

secRPR4 = 
      Collect[Simplify[Expand[Simplify[secRPR3]]], {d, px0, py0, 
          pz0}]

Eliminate the last useless equation,

secRPR5 = {secRPR4[[1]], secRPR4[[2]], 
        secRPR4[[3]]}

All equations are of the form Ad+B. We can now eliminate d :

1. Solving for d at each equation:

sold = Solve[secRPR5[[1]] == 0, d]

myd1 = Simplify[d /. sold[[1]]]

sold = Solve[secRPR5[[2]] == 0, d];

myd2 = Simplify[d /. sold[[1]]]

sold = Solve[secRPR5[[3]] == 0, d];

myd3 = Simplify[d /. sold[[1]]]

momdes1 = Simplify[myd1 - myd2]

momdes2 = Simplify[myd1 - myd3]

4b. Moment design equations for the two-step elimination

After some numerical checking, it seems that the resultant option increases the error due to an excessive number of operations, and the vector option is not correct; it must leave some piece in the way. So we will stick to the substitution option. The final design equations:

momdes1

momdes2

Denominator[momdes1[[1]]]
Denominator[momdes1[[2]]]

Expand[Denominator[momdes1[[1]]]]

Converting from rational to polynomial equations:

momdesign1 = 
      Simplify[Numerator[myd1]*Denominator[myd2] - 
          Numerator[myd2]*Denominator[myd1]]

momdesign2 = 
      Simplify[Numerator[myd1]*Denominator[myd3] - 
          Numerator[myd3]*Denominator[myd1]]

5. Eliminating θ,ϕ,d together

col8 = 
      Simplify[Coefficient[2*Qrprsep2, 
          d Sin[(θ/2)] Sin[(ϕ/2)]]]

col7 = 
      Simplify[Coefficient[2*Qrprsep2, 
          d Cos[(θ/2)] Cos[(ϕ/2)]]]
col6 = 
      Simplify[Coefficient[2*Qrprsep2, 
          d Sin[(θ/2)] Cos[(ϕ/2)]]]
col5 = 
      Simplify[Coefficient[2*Qrprsep2, 
          d Cos[(θ/2)] Sin[(ϕ/2)]]]

col4 = 
      Simplify[Coefficient[
          Qrprsep2 - col7*d/2 Cos[(θ/2)] Cos[(ϕ/2)],  
          Cos[(θ/2)] Cos[(ϕ/2)]]]

col3 = 
      Simplify[Coefficient[
          Qrprsep2 - col8*d/2 Sin[(θ/2)] Sin[(ϕ/2)],  
          Sin[(θ/2)] Sin[(ϕ/2)]]]

col2 = 
      Simplify[Coefficient[
          Qrprsep2 - col5*d/2 Cos[(θ/2)] Sin[(ϕ/2)],  
          Cos[(θ/2)] Sin[(ϕ/2)]]]

col1 = 
      Simplify[Coefficient[
          Qrprsep2 - col6*d/2 Sin[(θ/2)] Cos[(ϕ/2)],  
          Sin[(θ/2)] Cos[(ϕ/2)]]]

matQrpc = 
      Transpose[{col1, col2, col3, col4, col5, col6, col7, col8}]

MatrixForm[matQrpc]

Inverting the matrix by parts,

h = {hx, hy, hz}
g = {gx, gy, gz}
w = {wx, wy, wz}

AmatrixV = matQrpc[[{1, 2, 3, 4}, {1, 2, 3, 4}]]

BmatrixV = matQrpc[[{1, 2, 3, 4}, {5, 6, 7, 8}]]

CmatrixV = matQrpc[[{5, 6, 7, 8}, {1, 2, 3, 4}]]

DmatrixV = matQrpc[[{5, 6, 7, 8}, {5, 6, 7, 8}]]

Simplify[
      Simplify[Det[DmatrixV]] - (h . h)^2 g\[Cross]w . 
            g\[Cross]w]

g0 = {gx0, gy0, gz0}
w0 = {wx0, wy0, wz0}

Acol1 = g
Acol2 = w
Acol3 = g\[Cross]w
Acol4 = {0, 0, 0}

Anum1 = 0
Anum2 = 0
Anum3 = -g . w
Anum4 = 1

Ccol1 = g0
Ccol2 = w0
Ccol3 = g0\[Cross]w + g\[Cross]w0
Ccol4 = {0, 0, 0}

Cnum1 = 0
Cnum2 = 0
Cnum3 = -(g0 . w + g . w0)
Cnum4 = 0

Dcol1 = w\[Cross]h
Dcol2 = g\[Cross]h
Dcol3 = h
Dcol4 = (g\[Cross]w)\[Cross]h - g . w h

Dnum1 = -h . w
Dnum2 = -h . g
Dnum3 = 0
Dnum4 = -(g\[Cross]w) . h

Avector = 
      Append[Transpose[{Acol1, Acol2, Acol3, Acol4}], {Anum1, Anum2, Anum3, 
          Anum4}]

Simplify[AmatrixV - Avector]

MatrixForm[Avector]

Cvector = 
      Append[Transpose[{Ccol1, Ccol2, Ccol3, Ccol4}], {Cnum1, Cnum2, Cnum3, 
          Cnum4}]

Simplify[CmatrixV - Cvector]

MatrixForm[Cvector]

Dvector = 
      Append[Transpose[{Dcol1, Dcol2, Dcol3, Dcol4}], {Dnum1, Dnum2, Dnum3, 
          Dnum4}]

Simplify[DmatrixV - Dvector]

MatrixForm[Dvector]

The determinant of A is (g×w).(g×w):

detAmatrixV = Simplify[Det[AmatrixV]]

Simplify[detAmatrixV - (g\[Cross]w) . (g\[Cross]w)]

The determinant of D is  ((g×w).(g×w))h.h.

detDmatrixV = ((Simplify[Det[DmatrixV]] /. 
                (wx^2) + (wy^2) + (wz^2) -> 1) /. -1 + (wx^2) + 
                (wy^2) + (wz^2) -> 0) /. ((hx^2) + (hy^2) + 
              (hz^2)) -> 1

Simplify[
      detDmatrixV - (g\[Cross]w) . (g\[Cross]w) ]

AmatrixInv = Inverse[AmatrixV]

DmatrixInv = Simplify[Inverse[DmatrixV]]

DmatrixInv2 = 
      Simplify[DmatrixInv /. {(hx^2) + (hy^2) + (hz^2) -> 1}]

In vector form,

AInvRow1 = g - (g . w) w
AInvRow2 = w - (g . w) g
AInvRow3 = g\[Cross]w
AInvRow4 = g . w g\[Cross]w

AInvNum1 = 0
AInvNum2 = 0
AInvNum3 = 0
AInvNum4 = (g\[Cross]w) . (g\[Cross]w)

And D, in an even nicer way (more compact as an expression, but the other is smaller because we cancelled some extra stuff),

DInvRow1B = -(g\[Cross](g\[Cross]w))\[Cross]
          h
DInvRow2B = -(w\[Cross](w\[Cross]g))\[Cross]
          h
DInvRow3B = 
      g . w (g\[Cross]w)\[Cross]h + 
        h g\[Cross]w . g\[Cross]w
DInvRow4B = (g\[Cross]w)\[Cross]h

DInvRow1 = (-g . h g\[Cross]w + 
          g (g\[Cross]w) . h)
DInvRow2 = (g\[Cross]h + 
          w . g h\[Cross]w)
DInvRow3 = (g . w (-g h . w + w g . h) + 
          h ((g\[Cross]w) . (g\[Cross]
                    w)))
DInvRow4 = (-g h . w + w h . g)

DInvNum1 = h . g\[Cross](g\[Cross]w)
DInvNum2 = h . w\[Cross](w\[Cross]g)
DInvNum3 = -g . w h . g\[Cross]w
DInvNum4 = -h . g\[Cross]w

Simplify[DInvRow1 - DInvRow1B]
Simplify[DInvRow2 - DInvRow2B]
Simplify[DInvRow3 - DInvRow3B]
Simplify[DInvRow4 - DInvRow4B]

AInvector = 
      1/((g\[Cross]w) . (g\[Cross]w)) {Flatten[{AInvRow1, 
              AInvNum1}], Flatten[{AInvRow2, AInvNum2}], 
          Flatten[{AInvRow3, AInvNum3}], 
          Flatten[{AInvRow4, AInvNum4}]}

MatrixForm[AInvector]

DInvector = 
      1/((g\[Cross]w) . (g\[Cross]w)) {Flatten[{DInvRow1, 
              DInvNum1}], Flatten[{DInvRow2, DInvNum2}], 
          Flatten[{DInvRow3, DInvNum3}], 
          Flatten[{DInvRow4, DInvNum4}]}

MatrixForm[DInvector]

Simplify[
        AInvector - 
          Inverse[Avector]] /. {-1 + (wx^2) + (wy^2) + (wz^2) -> 
          0, -1 + (gx^2) + (gy^2) + (gz^2) -> 0}

difference = 
      Simplify[DInvector - 
            Inverse[Dvector]] /. {-1 + (wx^2) + (wy^2) + (wz^2) -> 
            0, -1 + (gx^2) + (gy^2) + (gz^2) -> 
            0, -1 + (hx^2) + (hy^2) + (hz^2) -> 0}

Collect[
        Collect[Simplify[
              Numerator[
                  difference] /. {(-1 + (hy^2) + 
                        (hz^2)) -> -hx^2}], {wx, wy, wz}, 
            Simplify] /. (hx^2) + (hy^2) + (hz^2) -> 1, {hx, hy, hz, gx, 
          gy, gz}] /. {-1 + (wx^2) + (wy^2) + (wz^2) -> 0, 
        1 - (wx^2) - (wy^2) - (wz^2) -> 0}

A small numerical check

makedir[] := 
      Module[{v, v1, v2, v3}, v1 = Random[Real, {-1, 1}];  
        v2 = Random[Real, {-1, 1}];  v3 = Random[Real, {-1, 1}]; 
        v = {v1, v2, v3}; v/Sqrt[v . v]]

makepoint[] := 
      Module[{v1, v2, v3}, v1 = Random[Real, {-3, 3}];  
        v2 = Random[Real, {-3, 3}];  
        v3 = Random[Real, {-3, 3}]; {v1, v2, v3}]

makedvec[] := 
      Module[{dir, point}, dir = makedir[];  
        point = makepoint[]; {dir, point\[Cross]dir}]

makeph[] := Random[Real, {-6, 6}]

makede[] := Random[Real, {-3, 3}]

myg = makedir[]

myw = makedir[]

myh = myg\[Cross]myw

myhU = myh/Sqrt[myh . myh]

myg0 = makepoint[]\[Cross]myg

myw0 = makepoint[]\[Cross]myw

Simplify[
        DInvector /. {gx -> myg[[1]], gy -> myg[[2]], 
            gz -> myg[[3]], wx -> myw[[1]], 
            wy -> myw[[2]], wz -> myw[[3]], 
            hx -> myhU[[1]], hy -> myhU[[2]], 
            hz -> myhU[[3]], gx0 -> myg0[[1]], 
            gy0 -> myg0[[2]], gz0 -> myg0[[3]], 
            wx0 -> myw0[[1]], wy0 -> myw0[[2]], 
            wz0 -> myw0[[3]]}] - 
      Simplify[Inverse[DmatrixV] /. {gx -> myg[[1]], 
            gy -> myg[[2]], gz -> myg[[3]], 
            wx -> myw[[1]], wy -> myw[[2]], 
            wz -> myw[[3]], hx -> myhU[[1]], 
            hy -> myhU[[2]], hz -> myhU[[3]], 
            gx0 -> myg0[[1]], gy0 -> myg0[[2]], 
            gz0 -> myg0[[3]], wx0 -> myw0[[1]], 
            wy0 -> myw0[[2]], wz0 -> myw0[[3]]}]

Simplify[
        AInvector /. {gx -> myg[[1]], gy -> myg[[2]], 
            gz -> myg[[3]], wx -> myw[[1]], 
            wy -> myw[[2]], wz -> myw[[3]], 
            hx -> myhU[[1]], hy -> myhU[[2]], 
            hz -> myhU[[3]], gx0 -> myg0[[1]], 
            gy0 -> myg0[[2]], gz0 -> myg0[[3]], 
            wx0 -> myw0[[1]], wy0 -> myw0[[2]], 
            wz0 -> myw0[[3]]}] - 
      Simplify[Inverse[AmatrixV] /. {gx -> myg[[1]], 
            gy -> myg[[2]], gz -> myg[[3]], 
            wx -> myw[[1]], wy -> myw[[2]], 
            wz -> myw[[3]], hx -> myhU[[1]], 
            hy -> myhU[[2]], hz -> myhU[[3]], 
            gx0 -> myg0[[1]], gy0 -> myg0[[2]], 
            gz0 -> myg0[[3]], wx0 -> myw0[[1]], 
            wy0 -> myw0[[2]], wz0 -> myw0[[3]]}]

Coefficients of [D]^-1[C] (need to divide by (g×w).(g×w)):

DC11 = 
      Simplify[DInvRow1 . Ccol1 + DInvNum1*Cnum1] /. -gx gx0 - gy gy0 - 
            gz gz0 -> 0

DC12 = Simplify[DInvRow1 . Ccol2 + DInvNum1*Cnum2]

DC13 = 
      Simplify[DInvRow1 . Ccol3 + DInvNum1*Cnum3] /. 
        (gx^2) + (gy^2) + (gz^2) -> 1

DC14 = Simplify[DInvRow1 . Ccol4 + DInvNum1*Cnum4]

DC21 = Simplify[DInvRow2 . Ccol1 + DInvNum2*Cnum1]

DC22 = 
      Simplify[DInvRow2 . Ccol2 + DInvNum2*Cnum2] /. -wx wx0 - wy wy0 - 
            wz wz0 -> 0

DC23 = Simplify[DInvRow2 . Ccol3 + DInvNum2*Cnum3]

DC24 = 
      Simplify[DInvRow2 . Ccol4 + DInvNum2*Cnum4] /. 
        (wx^2) + (wy^2) + (wz^2) -> 1

DC31 = Simplify[DInvRow3 . Ccol1 + DInvNum3*Cnum1]

DC32 = Simplify[DInvRow3 . Ccol2 + DInvNum3*Cnum2]

DC33 = Simplify[DInvRow3 . Ccol3 + DInvNum3*Cnum3]

DC34 = Simplify[DInvRow3 . Ccol4 + DInvNum3*Cnum4]

DC41 = Simplify[DInvRow4 . Ccol1 + DInvNum4*Cnum1]

DC42 = Simplify[DInvRow4 . Ccol2 + DInvNum4*Cnum2]

DC43 = Simplify[DInvRow4 . Ccol3 + DInvNum4*Cnum3]

DC44 = Simplify[DInvRow4 . Ccol4 + DInvNum4*Cnum4]

Now  we assemble the intermediate matrix:

MatDC = {{DC11, DC12, DC13, DC14}, {DC21, DC22, DC23, DC24}, {DC31, 
          DC32, DC33, DC34}, {DC41, DC42, DC43, DC44}}

Now we create the C inverse matrix (divide by 1/((g×w).(g×w))^2:

CinvRow1 = 
      Simplify[-(DC11 AInvRow1 + DC12 AInvRow2 + DC13 AInvRow3 + 
              DC14 AInvRow4)]

CinvRow2 = 
      Simplify[-(DC21 AInvRow1 + DC22 AInvRow2 + DC23 AInvRow3 + 
              DC24 AInvRow4)]

CinvRow3 = 
      Simplify[-(DC31 AInvRow1 + DC32 AInvRow2 + DC33 AInvRow3 + 
              DC34 AInvRow4)]

CinvRow4 = 
      Simplify[-(DC41 AInvRow1 + DC42 AInvRow2 + DC43 AInvRow3 + 
              DC44 AInvRow4)]

CinvNum1 = 
      Simplify[-(DC11 AInvNum1 + DC12 AInvNum2 + DC13 AInvNum3 + 
              DC14 AInvNum4)]

CinvNum2 = 
      Simplify[-(DC21 AInvNum1 + DC22 AInvNum2 + DC23 AInvNum3 + 
              DC24 AInvNum4)]

CinvNum3 = 
      Simplify[-(DC31 AInvNum1 + DC32 AInvNum2 + DC33 AInvNum3 + 
              DC34 AInvNum4)]

CinvNum4 = 
      Simplify[-(DC41 AInvNum1 + DC42 AInvNum2 + DC43 AInvNum3 + 
              DC44 AInvNum4)]

CInvector = 
      1/((g\[Cross]w) . (g\[Cross]w))^2 {Flatten[{CinvRow1, 
              CinvNum1}], Flatten[{CinvRow2, CinvNum2}], 
          Flatten[{CinvRow3, CinvNum3}], 
          Flatten[{CinvRow4, CinvNum4}]}

Check whether CInvector is right:

CmatrixInv = -DInvector . CmatrixV . AInvector;

Simplify[
        CInvector /. {gx -> myg[[1]], gy -> myg[[2]], 
            gz -> myg[[3]], wx -> myw[[1]], 
            wy -> myw[[2]], wz -> myw[[3]], 
            hx -> myhU[[1]], hy -> myhU[[2]], 
            hz -> myhU[[3]], gx0 -> myg0[[1]], 
            gy0 -> myg0[[2]], gz0 -> myg0[[3]], 
            wx0 -> myw0[[1]], wy0 -> myw0[[2]], 
            wz0 -> myw0[[3]]}] - 
      Simplify[CmatrixInv /. {gx -> myg[[1]], 
            gy -> myg[[2]], gz -> myg[[3]], 
            wx -> myw[[1]], wy -> myw[[2]], 
            wz -> myw[[3]], hx -> myhU[[1]], 
            hy -> myhU[[2]], hz -> myhU[[3]], 
            gx0 -> myg0[[1]], gy0 -> myg0[[2]], 
            gz0 -> myg0[[3]], wx0 -> myw0[[1]], 
            wy0 -> myw0[[2]], wz0 -> myw0[[3]]}]

The solutions for the joint variables

Solution for the angles:

p = {px, py, pz, pw}
p0 = {px0, py0, pz0, pw0}

stcp = Simplify[(AInvector[[1]] . p)]

ctsp = Simplify[AInvector[[2]] . p]

stsp = Simplify[AInvector[[3]] . p]

ctcp = Simplify[AInvector[[4]] . p]

dctsp = CInvector[[1]] . p + DInvector[[1]] . p0

dstcp = CInvector[[2]] . p + DInvector[[2]] . p0

dctcp = CInvector[[3]] . p + DInvector[[3]] . p0

dstsp = CInvector[[4]] . p + DInvector[[4]] . p0

5b. The design equations of eliminating θ, ϕ, d together

DirEq1 = Simplify[stcp*ctsp - stsp*ctcp]

Simplified equation:

DirEqSimp = 
      gz ((-pw py + px pz) wx + (pw px + 
                    py pz) wy + (-(px^2) - (py^2)) wz) + 
        gx ((-(py^2) - (pz^2)) wx + (px py - 
                    pw pz) wy + (pw py + px pz) wz) + 
        gy ((px py + pw pz) wx + (-(px^2) - 
                    (pz^2)) wy + (-pw px + 
                    py pz) wz)

Check that this is equivalent to McCarthy's book formula

mcc = 
      pw g . ({px, py, pz}\[Cross]w) + 
        g . {px, py, pz}\[Cross]({px, py, pz}\[Cross]w)

Simplify[DirEqSimp - mcc]

Simplify[
        Simplify[
            Expand[DirEq1 - 
                mcc*g\[Cross]w . 
                      g\[Cross]w/(g\[Cross]w . g\[Cross]w)^2]] /. 
          wx^2 -> 1 - wy^2 - wz^2] /. (-1 + (gx^2) + (gy^2) + 
            (gz^2)) -> 0

Simplify[
        Simplify[
            Expand[Numerator[DirEq1] - DirEqSimp*g\[Cross]w . g\[Cross]w]] /. 
          wx^2 -> 1 - wy^2 - wz^2] /. (-1 + (gx^2) + (gy^2) + 
            (gz^2)) -> 0

the moment equations,

dstcpS = Simplify[dstcp]

dctspS = Simplify[dctsp]

dstspS = Simplify[dstsp]

dctcpS = Simplify[dctcp]

MomEq1 = dctcpS*stsp - dstspS*ctcp

MomEq2 = dctspS*stcp - dstcpS*ctsp

MomEq1S = Simplify[MomEq1]

MomEq2S = Simplify[MomEq2]

without clearing denominators,

MomEqC1 = dctcpS/ctcp - dstspS/stsp

MomEqC2 = dctspS/ctsp - dstcpS/stcp

6. Closed algebraic solution

SOLVE FOR THE REVOLUTE JOINT ANGLES SEPARATELY

We have four positions and need to impose an extra constraint. If we impose the constraint in the revolute joint direction, we can solve for the directions g, w algebraically.
The extra constraint will consist on specifying

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]]}

extraeq = DirEqSimp

extraeqs = {EvalQuat[
          extraeq, {{p1x, p1y, p1z, p1w}, {p1x0, p1y0, p1z0, p1w0}}], 
        EvalQuat[extraeq, {{p2x, p2y, p2z, p2w}, {p2x0, p2y0, p2z0, p2w0}}], 
        EvalQuat[
          extraeq, {{p3x, p3y, p3z, p3w}, {p3x0, p3y0, p3z0, 
              p3w0}}]}

col1 = Coefficient[extraeqs, wx]
col2 = Coefficient[extraeqs, wy]
col3 = Coefficient[extraeqs, wz]

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

MatrixForm[mat]

This is equated to zero; the determinant of the matrix  must be zero.

conditiong = Det[mat]

Define the task quaternions:

qS12 = {{0.38520719056437336`, 
          0.5505037723241641`, -0.3436288351333719`, -0.6561099303159871`}, {-0.4375832225900376`, 1.2110719280352176`, 
          1.3923276828596234`, 
          0.030018630159717338`}}
qS13 = {{-0.5943462993126314`, -0.48082862951843897`, -0.43412850084802396`, 0.4765382988598576`}, {0.7214772584358266`, 
          0.17250604847961576`, -1.409536288708412`, -0.21019651659451716`}}
qS14 = {{-0.0640459121452486`, 0.20998403274467953`, 
          0.10043063114350148`, 
          0.9704218234653847`}, {0.32002697571466904`, -0.9030066853563115`, -0.6179132506172702`, 0.2804665106734268`}}

extraeqsN = {EvalQuat[extraeq, qS12], EvalQuat[extraeq, qS13], 
        EvalQuat[extraeq, qS14]}

col1 = Coefficient[extraeqsN, wx]
col2 = Coefficient[extraeqsN, wy]
col3 = Coefficient[extraeqsN, wz]
mat = Transpose[{col1, col2, col3}]

As extra condition, we impose gx=0. We can also make gz=1 in general.

equg = Det[mat] /. {gx -> 0, gz -> 1}

sols = Solve[equg == 0, gy]

gsols = 
      Table[{0, gy /. sols[[i]], 1}/
          Sqrt[{0, gy /. sols[[i]], 1} . {0, gy /. sols[[i]], 
                1}], {i, 1, Length[sols]}]

mats = 
      Table[mat /. {gx -> gsols[[i, 1]], 
            gy -> gsols[[i, 2]], gz -> gsols[[i, 3]]}, {i, 
          1, Length[gsols]}]

solve for w making wx=1,

mats22 = 
      Table[mats[[i, {1, 2}, {2, 3}]], {i, 1, Length[gsols]}]

indeps22 = 
      Table[-mats[[i, {1, 2}, 1]], {i, 1, Length[gsols]}]

solswyzs = 
      Table[Inverse[mats22[[i]]] . indeps22[[i]], {i, 1, 
          Length[gsols]}]

wsols = 
      Table[{1, solswyzs[[i, 1]], solswyzs[[i, 2]]}/
          Sqrt[{1, solswyzs[[i, 1]], solswyzs[[i, 2]]} . {1, 
                solswyzs[[i, 1]], solswyzs[[i, 2]]}], {i, 1, 
          Length[gsols]}]

SOLUTION FOR THE REST OF VARIABLES : we use the equations of g0, w0 to eliminate the gy0, wy0 terms.

fu = gx gx0 + gy gy0 + gz gz0
fu2 = wx wx0 + wy wy0 + wz wz0

gy0S = 
      Table[(gy0 /. Solve[fu == 0, 
                  gy0][[1]]) /. {gx -> gsols[[i, 1]], 
            gy -> gsols[[i, 2]], gz -> gsols[[i, 3]], 
            wx -> wsols[[i, 1]], wy -> wsols[[i, 2]], 
            wz -> wsols[[i, 3]]}, {i, 1, Length[gsols]}]

wy0S = 
      Table[(wy0 /. Solve[fu2 == 0, 
                  wy0][[1]]) /. {gx -> gsols[[i, 1]], 
            gy -> gsols[[i, 2]], gz -> gsols[[i, 3]], 
            wx -> wsols[[i, 1]], wy -> wsols[[i, 2]], 
            wz -> wsols[[i, 3]]}, {i, 1, Length[gsols]}]

moms = 
      Table[Simplify[({EvalQuat[MomEq1S, qS12], EvalQuat[MomEq2S, qS12], 
                    EvalQuat[MomEq1S, qS13], EvalQuat[MomEq2S, qS13], 
                    EvalQuat[MomEq1S, qS14], 
                    EvalQuat[MomEq2S, qS14]} /. {gy0 -> gy0S[[i]], 
                    wy0 -> wy0S[[i]]}) /. {gx -> 
                  gsols[[i, 1]], gy -> gsols[[i, 2]], 
                gz -> gsols[[i, 3]], wx -> wsols[[i, 1]], 
                wy -> wsols[[i, 2]], wz -> wsols[[i, 3]]}] /. 
          hz -> 1, {i, 1, Length[gsols]}]

we collect by w0, g0 and independent term as a function of h:

col1 = Coefficient[moms, gx0]
col2 = Coefficient[moms, gy0]
col3 = Coefficient[moms, gz0]

col4 = Coefficient[moms, wx0]
col5 = Coefficient[moms, wy0]
col6 = Coefficient[moms, wz0]

col7 = 
      Chop[Simplify[
          moms - gx0 col1 - gy0 col2 - gz0 col3 - wx0 col4 - wy0 col5 - 
            wz0 col6]]

mat0 = 
      Table[Transpose[{col1[[i]], col3[[i]], col4[[i]], 
            col6[[i]], col7[[i]]}], {i, 1, Length[gsols]}]

MatrixForm[mat0[[1]]]

We compute the 6 5x5 minors.
In principle, the degree in hx and hy must be 5 at most. We could eliminate hx or hy from one column, but it is not needed in this case.

mymins0 = 
      Table[Collect[Expand[Minors[mat0[[i]], 5]], {hx, hy}, 
          Simplify], {i, 1, Length[gsols]}]

Collect by powers of hx to create a 6x6 matrix,

Coefficient[mymins0, hx^6]

row1W = 
      Table[Collect[{Coefficient[mymins0[[i, 1, 1]], hx^5], 
            Coefficient[mymins0[[i, 1, 1]], hx^4], 
            Coefficient[mymins0[[i, 1, 1]], hx^3], 
            Coefficient[mymins0[[i, 1, 1]], hx^2], 
            Coefficient[mymins0[[i, 1, 1]], hx], 
            Coefficient[mymins0[[i, 1, 1]], hx, 0]}, hy], {i, 1, 
          Length[gsols]}]
row2W = 
      Table[Collect[{Coefficient[mymins0[[i, 2, 1]], hx^5], 
            Coefficient[mymins0[[i, 2, 1]], hx^4], 
            Coefficient[mymins0[[i, 2, 1]], hx^3], 
            Coefficient[mymins0[[i, 2, 1]], hx^2], 
            Coefficient[mymins0[[i, 2, 1]], hx], 
            Coefficient[mymins0[[i, 2, 1]], hx, 0]}, hy], {i, 1, 
          Length[gsols]}]
row3W = 
      Table[Collect[{Coefficient[mymins0[[i, 3, 1]], hx^5], 
            Coefficient[mymins0[[i, 3, 1]], hx^4], 
            Coefficient[mymins0[[i, 3, 1]], hx^3], 
            Coefficient[mymins0[[i, 3, 1]], hx^2], 
            Coefficient[mymins0[[i, 3, 1]], hx], 
            Coefficient[mymins0[[i, 3, 1]], hx, 0]}, hy], {i, 1, 
          Length[gsols]}]
row4W = 
      Table[Collect[{Coefficient[mymins0[[i, 4, 1]], hx^5], 
            Coefficient[mymins0[[i, 4, 1]], hx^4], 
            Coefficient[mymins0[[i, 4, 1]], hx^3], 
            Coefficient[mymins0[[i, 4, 1]], hx^2], 
            Coefficient[mymins0[[i, 4, 1]], hx], 
            Coefficient[mymins0[[i, 4, 1]], hx, 0]}, hy], {i, 1, 
          Length[gsols]}]
row5W = 
      Table[Collect[{Coefficient[mymins0[[i, 5, 1]], hx^5], 
            Coefficient[mymins0[[i, 5, 1]], hx^4], 
            Coefficient[mymins0[[i, 5, 1]], hx^3], 
            Coefficient[mymins0[[i, 5, 1]], hx^2], 
            Coefficient[mymins0[[i, 5, 1]], hx], 
            Coefficient[mymins0[[i, 5, 1]], hx, 0]}, hy], {i, 1, 
          Length[gsols]}]
row6W = 
      Table[Collect[{Coefficient[mymins0[[i, 6, 1]], hx^5], 
            Coefficient[mymins0[[i, 6, 1]], hx^4], 
            Coefficient[mymins0[[i, 6, 1]], hx^3], 
            Coefficient[mymins0[[i, 6, 1]], hx^2], 
            Coefficient[mymins0[[i, 6, 1]], hx], 
            Coefficient[mymins0[[i, 6, 1]], hx, 0]}, hy], {i, 1, 
          Length[gsols]}]

mathyW = 
      Table[{row1W[[i]], row2W[[i]], row3W[[i]], row4W[[i]], 
          row5W[[i]], row6W[[i]]}, {i, 1, Length[gsols]}]

MatrixForm[mathyW[[1]]]

equHY = Table[Det[mathyW[[i]]], {i, 1, Length[gsols]}]

Making it monic,

equHYm = Simplify[equHY/Coefficient[equHY, hy, 15]]

solsDhy0 = 
      Table[Sort[Solve[equHY[[i]] == 0, hy]], {i, 1, 
          Length[gsols]}]

solshy0 = hy /. solsDhy0

Another way to solve it, good for implementing in non-symbolic programs, is solving matrix eigenvalue problem instead of the computation of the resultant for one variable, as in Manocha,

m5 = Coefficient[mathyW, hy^5]

m4 = Coefficient[mathyW, hy^4]

m3 = Coefficient[mathyW, hy^3]

m2 = Coefficient[mathyW, hy^2]

m1 = Coefficient[mathyW, hy, 1]

m0 = Coefficient[mathyW, hy, 0]

<< LinearAlgebra`MatrixManipulation`

MatrixForm[m0[[1]]]

construct the C-system

Id6 = IdentityMatrix[6]

Z6 = ZeroMatrix[6]

As the condition number of m0 is OK, we can do the change of variables z=1/hy, then create a matrix C

LinearAlgebra`MatrixManipulation`MatrixConditionNumber[
      m0[[1]]]
LinearAlgebra`MatrixManipulation`MatrixConditionNumber[
      m1[[1]]]
LinearAlgebra`MatrixManipulation`MatrixConditionNumber[
      m2[[1]]]
LinearAlgebra`MatrixManipulation`MatrixConditionNumber[
      m3[[1]]]
LinearAlgebra`MatrixManipulation`MatrixConditionNumber[
      m4[[1]]]
LinearAlgebra`MatrixManipulation`MatrixConditionNumber[
      m5[[1]]]

m5bar = 
      Table[Inverse[m0[[i]]] . m5[[i]], {i, 1, 
          Length[gsols]}]
m4bar = 
      Table[Inverse[m0[[i]]] . m4[[i]], {i, 1, 
          Length[gsols]}]
m3bar = 
      Table[Inverse[m0[[i]]] . m3[[i]], {i, 1, 
          Length[gsols]}]
m2bar = 
      Table[Inverse[m0[[i]]] . m2[[i]], {i, 1, 
          Length[gsols]}]
m1bar = 
      Table[Inverse[m0[[i]]] . m1[[i]], {i, 1, 
          Length[gsols]}]

Cmatrix = 
      Table[LinearAlgebra`MatrixManipulation`BlockMatrix[{{Z6, Id6, Z6, Z6, 
              Z6}, {Z6, Z6, Id6, Z6, Z6}, {Z6, Z6, Z6, Id6, Z6}, {Z6, Z6, Z6, 
              Z6, Id6}, {-m5bar[[i]], -m4bar[[i]], -m3bar[[i]], -m2bar[[i]], -m1bar[[i]]}}], {i, 1, 
          Length[gsols]}]

MatrixForm[Cmatrix[[1]]]

solsInv = 
      Table[Eigenvalues[Cmatrix[[i]]], {i, 1, Length[gsols]}]

Length[solsInv[[1]]]

solsInv2 = 
      Table[Select[solsInv[[i]], # =!= 0.  &], {i, 1, 
          Length[gsols]}]

Length[solsInv2[[1]]]

solshy = 1/solsInv2

solshy2 = 
      Table[Sort[solshy[[i]]], {i, 1, Length[gsols]}]

Simplify[solshy2 - solshy0]

the solutions are the same, but  some of them with bad precission.

SOLUTIONS FOR  h, g0, w0

mathyW[[1, {2, 3, 4, 5, 6}, {1, 2, 3, 4, 5}]]

MatsHY = 
      Table[mathyW[[j, {2, 3, 4, 5, 6}, {1, 2, 3, 4, 5}]] /. {hy -> 
              solshy0[[j, i]]}, {j, 1, Length[gsols]}, {i, 1, 
          15}]

indepsHY = 
      Table[-mathyW[[j, {2, 3, 4, 5, 6}, 6]] /. {hy -> 
              solshy0[[j, i]]}, {j, 1, Length[gsols]}, {i, 1, 
          15}]

dets = 
      Table[Det[MatsHY[[j, i]]], {j, 1, Length[gsols]}, {i, 1, 
          15}]

solshx = 
      Table[(Inverse[MatsHY[[j, i]]] . 
              indepsHY[[j, i]])[[5]], {j, 1, Length[gsols]}, {i, 1, 
          15}]

check the relation between variables,

Simplify[((Inverse[MatsHY[[1, 1]]] . 
                  indepsHY[[1, 1]])[[5]])^5 - (Inverse[
                MatsHY[[1, 1]]] . 
              indepsHY[[1, 1]])[[1]]]
Simplify[((Inverse[MatsHY[[1, 1]]] . 
                  indepsHY[[1, 1]])[[5]])^4 - (Inverse[
                MatsHY[[1, 1]]] . 
              indepsHY[[1, 1]])[[2]]]

we use the non-normalized ones for the substitution in mat0

solsH = 
      Table[{solshx[[j, i]], solshy0[[j, i]], 1}, {j, 1, 
          Length[gsols]}, {i, 1, 15}]

solsHn = 
      Table[{solshx[[j, i]], solshy0[[j, i]], 1}/
          Sqrt[{solshx[[j, i]], solshy0[[j, i]], 
                1} . {solshx[[j, i]], solshy0[[j, i]], 1}], {j, 1, 
          Length[gsols]}, {i, 1, 15}]

Here we choose a set of equations which are independent,

Mats0 = 
      Table[mat0[[j, {2, 4, 5, 6}, {1, 2, 3, 4}]] /. {hx -> 
              solsH[[j, i, 1]], hy -> solsH[[j, i, 2]], 
            hz -> solsH[[j, i, 3]]}, {j, 1, Length[gsols]}, {i, 1, 
          15}]

dets0 = 
      Table[Det[Mats0[[j, i]]], {j, 1, Length[gsols]}, {i, 1, 
          15}]

indep0 = 
      Table[-mat0[[j, {2, 4, 5, 6}, 5]] /. {hx -> 
              solsH[[j, i, 1]], hy -> solsH[[j, i, 2]], 
            hz -> solsH[[j, i, 3]]}, {j, 1, Length[gsols]}, {i, 1, 
          15}]

solsGWxz0 = 
      Table[Inverse[Mats0[[j, i]]] . indep0[[j, i]], {j, 1, 
          Length[gsols]}, {i, 1, 15}]

solsG0 = 
      Table[{gx0, gy0 /. gy0 -> gy0S[[j]], 
            gz0} /. {gx0 -> solsGWxz0[[j, i, 1]], 
            gz0 -> solsGWxz0[[j, i, 2]]}, {j, 1, Length[gsols]}, {i, 1, 
          15}]

solsW0 = 
      Table[{wx0, wy0 /. wy0 -> wy0S[[j]], 
            wz0} /. {wx0 -> solsGWxz0[[j, i, 3]], 
            wz0 -> solsGWxz0[[j, i, 4]]}, {j, 1, Length[gsols]}, {i, 1, 
          15}]

solsTOTAL = 
      Table[{solsHn[[j, i]], gsols[[j]], solsG0[[j, i]], 
          wsols[[j]], solsW0[[j, i]]}, {j, 1, Length[gsols]}, {i, 1, 
          15}]

Eliminate the complex solutions,

solsTOTALb = 
      Table[If[(Im[solsTOTAL[[j, i, 1, 1]]] == 0  &&  
              Im[solsTOTAL[[j, i, 2, 1]]] == 0  &&  
              Im[solsTOTAL[[j, i, 3, 1]]] == 0 ) , 
          solsTOTAL[[j, i]],  ], {j, 1, Length[gsols]}, {i, 1, 
          15}]

solsTOTAL1 = 
      Table[Select[solsTOTALb[[j]], # =!= Null &], {j, 1, 
          Length[gsols]}]

Length[solsTOTAL1[[1]]]

Length[solsTOTAL1[[2]]]

Length[solsTOTAL1[[3]]]

2. Analysis

1. The task relative positions in quaternion form

qS12
qS13
qS14

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]

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

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

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]

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.

solsTOTAL1

The joint axes expressed as lines

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

ngsol = 1
nsol = 10

G = {solsTOTAL1[[ngsol, nsol, 2]], 
        solsTOTAL1[[ngsol, nsol, 3]]}

W = {solsTOTAL1[[ngsol, nsol, 4]], 
        solsTOTAL1[[ngsol, nsol, 5]]}

h = solsTOTAL1[[ngsol, nsol, 1]]

Make them unit and clean

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

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

Wline = {W[[1]]/Sqrt[W[[1]] . W[[1]]], 
        W[[2]]/Sqrt[W[[1]] . W[[1]]]}

Wline[[1]] . Wline[[2]]

We put H in an arbitrary location,

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

dirPoint = pointG/Sqrt[pointG . pointG]

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

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

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-W (rotation will be added to θ)

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

pointNgw = Ngw[[1]]\[Cross]Ngw[[2]]

Ngwline = {Ngw[[1]], pointNgw\[Cross]Ngw[[1]]}

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

cosanglegh = Xaxis[[1]] . Ngwline[[1]]

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

angle1 = ArcCos[cosanglegh]

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

If[(Drop[((Gtot . Ztrans[0, 0] . Ztrans[angle1, length1]) . {1, 
                    0, 0, 0}), {4}]) . Ngwline[[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}

Ngwline[[1]]

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

Gline[[1]]

Transformation about X from  G to w along normalGW (rotation so that Z-axis is aligned with W)

cosanglegh = Gline[[1]] . Wline[[1]]

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

angle2 = ArcCos[cosanglegh]

angle2*180/Pi

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

If[(Drop[((Gtot . Ztrans[0, 0] . Ztrans[angle1, length1] . 
                      Xtrans[angle2, length2]) . {0, 0, 1, 0}), {4}]) . 
          Wline[[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}

Wline[[1]]

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

Ngwline[[1]]

check that we get the intersection point:

part = (Wline[[2, 1]] - pointNgw[[2]] Wline[[1, 3]] + 
            pointNgw[[3]] Wline[[1, 2]])/(Ngwline[[1, 
                  2]] Wline[[1, 3]] - 
            Ngwline[[1, 3]] Wline[[1, 2]])

pointInt = pointNgw + part Ngwline[[1]]

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

Transformation about Z from normalGW to normal WH (rotation will be added to ϕ)

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

pointNwh = Nwh[[1]]\[Cross]Nwh[[2]]

Nwhline = {Nwh[[1]], pointNwh\[Cross]Nwh[[1]]}

cosanglegh = Ngwline[[1]] . Nwhline[[1]]

minusasinanglegh = 
      Ngwline[[1]] . Nwhline[[2]] + 
        Ngwline[[2]] . Nwhline[[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}]) . 
          Nwhline[[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}

Nwhline[[1]]

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

cosanglegh = Wline[[1]] . Hline[[1]]

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

angle4 = ArcCos[cosanglegh]

angle4*180/Pi

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}]) . 
          Hline[[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}

Hline[[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
phi10 = 0
d10 = 0

Compose the kinematic equations,

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

Htot = Inverse[KE10] . Pos1

KE10 . Htot

4. Compute inverse kinematics and check positions

Solutions: 1(big),2(half big), 12(big),

Inverse kinematics for the joint variables

thetaSol = 2*ArcTan[ctsp, stsp]

phiSol = 2*ArcTan[stcp, stsp]

For d, any of the four possibilities is fine,

dSol = 2*dctcp/ctcp;

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]], 
          wx -> Wline[[1, 1]], wy -> Wline[[1, 2]], 
          wz -> Wline[[1, 3]], wx0 -> Wline[[2, 1]], 
          wy0 -> Wline[[2, 2]], wz0 -> Wline[[2, 3]], 
          hx -> Hline[[1, 1]], hy -> Hline[[1, 2]], 
          hz -> Hline[[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]]}

Check if the chain hits the rest of positions

Position 2:

theta12 = EvalPos[thetaSol, qS12]
phi12 = EvalPos[phiSol, qS12]

d12 = EvalPos[dSol, qS12]

Assemble kinematic equations and compare with absolute transformation

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

Pos2

Position 3:

theta13 = EvalPos[thetaSol, qS13]
phi13 = EvalPos[phiSol, qS13]

d13 = EvalPos[dSol, qS13]

Assemble kinematic equations and compare with absolute transformation

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

Pos3

Position 4:

theta14 = EvalPos[thetaSol, qS14]
phi14 = EvalPos[phiSol, qS14]

d14 = EvalPos[dSol, qS14]

Assemble kinematic equations and compare with absolute transformation

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

Pos4

Chop[Simplify[KE12 - Pos2]]
Chop[Simplify[KE13 - Pos3]]
Chop[Simplify[KE14 - Pos4]]

Table of values for Synthetica

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

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

phis = ({phi10, phi12, phi13, phi14} + {angle3, angle3, angle3, 
                angle3})*180/Pi + {0, 0, 360, 0}

des = {d10, d12, d13, d14}

Table of values for Synthetica sol2

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

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

phis = ({phi10, phi12, phi13, phi14} + {angle3, angle3, angle3, 
                angle3})*180/Pi + {0, 0, 0, 0}

des = {d10, d12, d13, d14}

5. Automated analysis

solsTOTAL2 = Flatten[solsTOTAL1, 1]

Length[solsTOTAL2]

Function definition

GetG[i_, data_] := {data[[i, 2]], 
        data[[i, 3]]}
GetW[i_, data_] := {data[[i, 4]], 
        data[[i, 5]]}
GetH[i_, data_] := 
      Module[{pointG, dirPoint, pointH,  Hline}, 
        pointG = data[[i, 2]]\[Cross]data[[i, 3]]; 
        dirPoint = pointG/Sqrt[pointG . pointG]; 
        pointH = pointG + 1*dirPoint + 2*data[[i, 2]];  
        Hline = {data[[i, 1]]/Sqrt[data[[i, 1]] . data[[i, 1]]], 
            pointH\[Cross]data[[i, 1]]/
              Sqrt[data[[i, 1]] . 
                  data[[i, 1]]]} ]

MakeUnit[S_] := {S[[1]]/Sqrt[S[[1]] . S[[1]]], 
        S[[2]]/Sqrt[S[[1]] . S[[1]]]}
MakeNormal[G_, W_] := 
      Module[{Ngw, pointNgw, 
          Ngwline}, 
Ngw = ({G[[1]]\[Cross]
                  W[[1]], 
                G[[1]]\[Cross]W[[2]] + 
                  G[[2]]\[Cross]W[[1]]})/
            Sqrt[(G[[1]]\[Cross]W[[1]]) . (G[[1]]\[Cross]
                    W[[1]])]; 
pointNgw = 
          Ngw[[1]]\[Cross]
            Ngw[[2]]; 
Ngwline = {Ngw[[1]], 
            pointNgw\[Cross]Ngw[[1]]}]

Creates [G] transformation with origin at the intersection point of the common normal of G and W with G, x-axis along common normal

MakeGtrans[G_] := 
      Module[{dirPoint, pointG, Rgtrans, Ghom, 
          Gtot}, 
pointG = G[[1]]\[Cross]G[[2]]; 
        dirPoint = 
          pointG/Sqrt[
              pointG . pointG]; 
Rgtrans = {dirPoint, 
            G[[1]]\[Cross]dirPoint, G[[1]], 
            pointG}; 
Ghom = 
          Transpose[Rgtrans]; 
Gtot = 
          Append[Ghom, {0, 0, 0, 1}]]

Transformation along Z from G origine to  the common normal Ngw

MakeDH1[Gtot_, G_, W_] := 
      Module[{Ngwline, dirPoint, pointG, cosangle, minusasinangle, angle1, 
          lenght1, dhpars}, 
Ngwline = 
          MakeNormal[G, W]; 
pointG = 
          G[[1]]\[Cross]G[[2]]; 
        dirPoint = 
          pointG/Sqrt[pointG . pointG]; 
cosangle = 
          dirPoint . Ngwline[[1]]; 
minusasinangle = 
          dirPoint . Ngwline[[2]] + (pointG\[Cross]dirPoint) . 
              Ngwline[[1]]; 
angle1 = 
          ArcCos[cosangle]; 
length1 = -minusasinangle/
            Sqrt[1 - 
                cosangle^2]; 
If[(Drop[((Gtot . 
                          Ztrans[0, 0] . Ztrans[angle1, length1]) . {1, 0, 
                        0, 0}), {4}]) . Ngwline[[1]] == 1,  , 
          angle1 = -angle1; 
          length1 = -length1]; 
dhpars = {angle1, 
            length1}]

Transformation along X from G to W along the common normal Ngw

MakeDH2[Gtot_, G_, W_, DH1_] := 
      Module[{Ngwline, cosangle, minusasinangle, angle2, lenght2, 
          dhpars}, 
Ngwline = 
          MakeNormal[G, W]; 
cosangle = 
          G[[1]] . W[[1]]; 
minusasinangle = 
          G[[1]] . W[[2]] + 
            G[[2]] . W[[1]]; 
angle2 = 
          ArcCos[cosangle]; 
length2 = -minusasinangle/
            Sqrt[1 - 
                cosangle^2]; 
If[(Drop[((Gtot . 
                          Ztrans[0, 0] . Ztrans[DH1[[1]], DH1[[2]]] . 
                          Xtrans[angle2, length2] . Ztrans[0, 0]) . {0, 0, 
                        1, 0}), {4}]) . W[[1]] == 1,  , 
          angle2 = -angle2; 
          length2 = -length2]; 
dhpars = {angle2, 
            length2}]

Transformation along Z from common normal Ngw to common normal Nwh along W.

MakeDH3[Gtot_, G_, W_, DH1_, DH2_] := 
      Module[{Ngwline, cosangle, minusasinangle, angle3, lenght3, 
          dhpars}, 
cosangle = 
          G[[1]] . W[[1]]; 
minusasinangle = 
          G[[1]] . W[[2]] + 
            G[[2]] . W[[1]]; 
angle3 = 
          ArcCos[cosangle]; 
length3 = -minusasinangle/
            Sqrt[1 - 
                cosangle^2]; 
If[(Drop[((Gtot . 
                          Ztrans[0, 0] . Ztrans[DH1[[1]], DH1[[2]]] . 
                          Xtrans[DH2[[1]], DH2[[2]]] . Ztrans[0, 0] . 
                          Ztrans[angle3, length3]) . {1, 0, 0, 
                        0}), {4}]) . W[[1]] == 1,  , 
          angle3 = -angle3; 
          length3 = -length3]; 
dhpars = {angle3, 
            length3}]

Transformation along X from W to H along Nwh

MakeDH4[Gtot_, G_, W_, DH1_, DH2_, DH3_] := 
      Module[{Ngwline, cosangle, minusasinangle, angle4, lenght4, 
          dhpars}, 
cosangle = 
          G[[1]] . W[[1]]; 
minusasinangle = 
          G[[1]] . W[[2]] + 
            G[[2]] . W[[1]]; 
angle4 = 
          ArcCos[cosangle]; 
length4 = -minusasinangle/
            Sqrt[1 - 
                cosangle^2]; 
If[(Drop[((Gtot . 
                          Ztrans[0, 0] . Ztrans[DH1[[1]], DH1[[2]]] . 
                          Xtrans[DH2[[1]], DH2[[2]]] . Ztrans[0, 0] . 
                          Ztrans[DH3[[1]], DH3[[2]]] . 
                          Xtrans[angle4, length4]) . {0, 0, 1, 
                        0}), {4}]) . W[[1]] == 1,  , 
          angle4 = -angle4; 
          length4 = -length4]; 
dhpars = {angle4, 
            length4}]

MakeHtrans[Gtot_, DH1_, DH2_, DH3_, DH4_, Pos1_] := 
      Module[{theta10, phi10, d10, KE10}, 
theta10 = 
          0; 
phi10 = 0; 
d10 = 
          0; 
KE10 = 
          Gtot . Ztrans[theta10, 0] . Ztrans[DH1[[1]], DH1[[2]]] . 
            Xtrans[DH2[[1]], DH2[[2]]] . Ztrans[phi10, 0] . 
            Ztrans[DH3[[1]], DH3[[2]]] . 
            Xtrans[DH4[[1]], DH4[[2]]] . 
            Ztrans[0, d10]; 
Htot = 
          Inverse[KE10] . Pos1]

EvalPos[expr_, Gline_, Wline_, Hline_, 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]], 
          wx -> Wline[[1, 1]], wy -> Wline[[1, 2]], 
          wz -> Wline[[1, 3]], wx0 -> Wline[[2, 1]], 
          wy0 -> Wline[[2, 2]], wz0 -> Wline[[2, 3]], 
          hx -> Hline[[1, 1]], hy -> Hline[[1, 2]], 
          hz -> Hline[[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]]}

CheckPos[pos_, i_, data_, G_, W_, H_, Gtot_, DH1_, DH2_, DH3_, 
        DH4_, Htot_, quats_] := 
      Module[{thetaSol, phiSol, dSol, KE1pos}, 
thetaSol = 
          EvalPos[2*ArcTan[ctsp, stsp], G, W, H, 
            quats[[pos - 1]]]; 
phiSol = 
          EvalPos[2*ArcTan[stcp, stsp], G, W, H, 
            quats[[pos - 1]]]; 
dSol = 
          EvalPos[2*dctcp/ctcp, G, W, H, 
            quats[[pos - 1]]]; 
KE1pos = 
          Gtot . Ztrans[thetaSol, 0] . Ztrans[DH1[[1]], DH1[[2]]] . 
            Xtrans[DH2[[1]], DH2[[2]]] . Ztrans[phiSol, 0] . 
            Ztrans[DH3[[1]], DH3[[2]]] . 
            Xtrans[DH4[[1]], DH4[[2]]] . Ztrans[0, dSol] . 
            Htot]

Apply to results of numerical solution: 19 good solutions.

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}}

Analyze[i_, data_, quats_] := 
      Module[{G, W, H, Gtot, DH1, DH2, DH3, DH4, Htot, KE12, KE13, KE14, 
          results}, 
G = 
          GetG[i, data]; 
W = 
          GetW[i, data]; 
H = 
          GetH[i, data]; 
Gtot = 
          MakeGtrans[G]; 
DH1 = 
          MakeDH1[Gtot, G, W]; 
DH2 = 
          MakeDH2[Gtot, G, W, DH1]; 
DH3 = 
          MakeDH3[Gtot, MakeNormal[G, W], MakeNormal[W, H], DH1, 
            DH2]; 
DH4 = 
          MakeDH4[Gtot, W, H, DH1, DH2, DH3]; 
Htot = 
          MakeHtrans[Gtot, DH1, DH2, DH3, 
            DH4, {{1, 0, 0, 0}, {0, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 
                1}}]; 
KE12 = 
          CheckPos[2, i, data, G, W, H, Gtot, DH1, DH2, DH3, DH4, Htot, 
            quats]; 
KE13 = 
          CheckPos[3, i, data, G, W, H, Gtot, DH1, DH2, DH3, DH4, Htot, 
            quats]; 
KE14 = 
          CheckPos[4, i, data, G, W, H, Gtot, DH1, DH2, DH3, DH4, Htot, 
            quats]; 
results = {KE12, KE13, 
            KE14}]

poses = {Pos2, Pos3, Pos4}

quats = {qS12, qS13, qS14}

AllAnalysis = 
        Table[Analyze[i, solsTOTAL2, quats], {i, 1, 
            Length[solsTOTAL2]}];

zeroAll = {{{0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 
            0}}, {{0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 
            0}}, {{0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 
            0}}}

goodResults = {}

goodRRP = 
      Flatten[Table[
          If[Chop[AllAnalysis[[i]] - poses ] ==  zeroAll, 
            Append[goodResults, i], ], {i, 1, 
            Length[solsTOTAL2]}]]

AgoodResults = {}

AlmostgoodRRP = 
      Flatten[Table[
          If[Chop[AllAnalysis[[i]] - poses, 10^(-7) ] ==  
              zeroAll, Append[AgoodResults, i], ], {i, 1, 
            Length[solsTOTAL2]}]]

Length[goodRRP]

goodRRPf = Select[goodRRP, # =!= Null &]

AgoodRRPf = Select[AlmostgoodRRP, # =!= Null &]

Length[goodRRPf]

solsRRPfinal = solsTOTAL2[[goodRRPf]]

solsRRPfinalA = solsTOTAL2[[AgoodRRPf]]

Export["solsRRPthesis.dat", solsRRPfinalA, "Lines"]

PRR, RPR dual synthesis

1. PRR

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

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}

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

W = {Sin[ϕ/2] wx, Sin[ϕ/2] wy, Sin[ϕ/2] wz, 
          Cos[ϕ/2]} + ϵ {Sin[ϕ/2] wx0, Sin[ϕ/2] wy0, 
            Sin[ϕ/2] wz0, 0}

Qrpr = 
      Simplify[Expand[
            quatmult[P, quatmult[G, W]]] /. {ϵ^2 -> 
              0, ϵ^3 -> 0}]

Qrpr3 = 
      Collect[Qrpr, {ϵ, Cos[(θ/2)],  Sin[(θ/2)], 
          Cos[(ϕ/2)], Sin[(ϕ/2)], d}]

MatrixForm[Qrpr3]

2. Separating the 8 equations

Qrprsep = {Coefficient[Qrpr3[[1]], ϵ, 0], 
        Coefficient[Qrpr3[[2]], ϵ, 0], 
        Coefficient[Qrpr3[[3]], ϵ, 0], 
        Coefficient[Qrpr3[[4]], ϵ, 0], 
        Coefficient[Qrpr3[[1]], ϵ, 1], 
        Coefficient[Qrpr3[[2]], ϵ, 1], 
        Coefficient[Qrpr3[[3]], ϵ, 1], 
        Coefficient[Qrpr3[[4]], ϵ, 1]}

Qrprsep2 = 
      Collect[Expand[
          Qrprsep], {Cos[(θ/2)]^(_ : 1) Sin[(ϕ/2)]^(_ : 1) , Sin[(θ/2)]^(_ : 1) Cos[(ϕ/2)]^(_ : 1) , 
          Sin[(θ/2)]^(_ : 1) Sin[(ϕ/2)]^(_ : 1) , 
          Cos[(θ/2)]^(_ : 1) Cos[(ϕ/2)]^(_ : 1) }]

3. Eliminating θ,ϕ,d

col8 = 
      Simplify[Coefficient[2*Qrprsep2, 
          d Sin[(θ/2)] Sin[(ϕ/2)]]]

col7 = 
      Simplify[Coefficient[2*Qrprsep2, 
          d Cos[(θ/2)] Cos[(ϕ/2)]]]
col6 = 
      Simplify[Coefficient[2*Qrprsep2, 
          d Sin[(θ/2)] Cos[(ϕ/2)]]]
col5 = 
      Simplify[Coefficient[2*Qrprsep2, 
          d Cos[(θ/2)] Sin[(ϕ/2)]]]

col4 = 
      Simplify[Coefficient[
          Qrprsep2 - col7*d/2 Cos[(θ/2)] Cos[(ϕ/2)],  
          Cos[(θ/2)] Cos[(ϕ/2)]]]

col3 = 
      Simplify[Coefficient[
          Qrprsep2 - col8*d/2 Sin[(θ/2)] Sin[(ϕ/2)],  
          Sin[(θ/2)] Sin[(ϕ/2)]]]

col2 = 
      Simplify[Coefficient[
          Qrprsep2 - col5*d/2 Cos[(θ/2)] Sin[(ϕ/2)],  
          Cos[(θ/2)] Sin[(ϕ/2)]]]

col1 = 
      Simplify[Coefficient[
          Qrprsep2 - col6*d/2 Sin[(θ/2)] Cos[(ϕ/2)],  
          Sin[(θ/2)] Cos[(ϕ/2)]]]

matQrpc = 
      Transpose[{col1, col2, col3, col4, col5, col6, col7, col8}]

MatrixForm[matQrpc]

Inverting the matrix by parts, as usual.

AmatrixV = matQrpc[[{1, 2, 3, 4}, {1, 2, 3, 4}]]

BmatrixV = matQrpc[[{1, 2, 3, 4}, {5, 6, 7, 8}]]

CmatrixV = matQrpc[[{5, 6, 7, 8}, {1, 2, 3, 4}]]

DmatrixV = matQrpc[[{5, 6, 7, 8}, {5, 6, 7, 8}]]

Simplify[
      Simplify[Det[DmatrixV]] - (h . h)^2 g\[Cross]w . 
            g\[Cross]w]

g = {gx, gy, gz}
h = {hx, hy, hz}
w = {wx, wy, wz}
g0 = {gx0, gy0, gz0}
w0 = {wx0, wy0, wz0}

Acol1 = g
Acol2 = w
Acol3 = g\[Cross]w
Acol4 = {0, 0, 0}

Anum1 = 0
Anum2 = 0
Anum3 = -g . w
Anum4 = 1

Ccol1 = g0
Ccol2 = w0
Ccol3 = g0\[Cross]w + g\[Cross]w0
Ccol4 = {0, 0, 0}

Cnum1 = 0
Cnum2 = 0
Cnum3 = -(g0 . w + g . w0)
Cnum4 = 0

Dcol1 = h\[Cross]w
Dcol2 = h\[Cross]g
Dcol3 = h
Dcol4 = h\[Cross](g\[Cross]w) - g . w h

Dnum1 = -h . w
Dnum2 = -h . g
Dnum3 = 0
Dnum4 = -(g\[Cross]w) . h

Avector = 
      Append[Transpose[{Acol1, Acol2, Acol3, Acol4}], {Anum1, Anum2, Anum3, 
          Anum4}]

Simplify[AmatrixV - Avector]

MatrixForm[Avector]

Cvector = 
      Append[Transpose[{Ccol1, Ccol2, Ccol3, Ccol4}], {Cnum1, Cnum2, Cnum3, 
          Cnum4}]

Simplify[CmatrixV - Cvector]

MatrixForm[Cvector]

Dvector = 
      Append[Transpose[{Dcol1, Dcol2, Dcol3, Dcol4}], {Dnum1, Dnum2, Dnum3, 
          Dnum4}]

Simplify[DmatrixV - Dvector]

MatrixForm[Dvector]

The determinant of A is (g×w).(g×w):

AmatrixInv = Inverse[AmatrixV]

AmatrixInvN = 
      Simplify[AmatrixInv*((gy^2) (wx^2) + (gz^2) (wx^2) - 
              2 gx gy wx wy + (gx^2) (wy^2) + (gz^2) (wy^2) - 
              2 gx gz wx wz - 2 gy gz wy wz + (gx^2) (wz^2) + 
              (gy^2) (wz^2))]

In vector form,

AInvRow1 = g - (g . w) w
AInvRow2 = w - (g . w) g
AInvRow3 = g\[Cross]w
AInvRow4 = g . w g\[Cross]w

AInvNum1 = 0
AInvNum2 = 0
AInvNum3 = 0
AInvNum4 = (g\[Cross]w) . (g\[Cross]w)

And D, in an even nicer way (more compact as an expression, but the other is smaller because we cancelled some extra stuff),

DInvRow1 = -h\[Cross](g\[Cross](g\[Cross]
                  w))
DInvRow2 = -h\[Cross](w\[Cross](w\[Cross]
                  g))
DInvRow3 = 
      g . w h\[Cross](g\[Cross]w) + 
        h g\[Cross]w . g\[Cross]w
DInvRow4 = h\[Cross](g\[Cross]w)

DInvRow1 = (g . h g\[Cross]w - 
          g (g\[Cross]w) . h)
DInvRow2 = (-g\[Cross]h - 
          w . g h\[Cross]w)
DInvRow3 = (g . w (g h . w - w g . h) + 
          h ((g\[Cross]w) . (g\[Cross]
                    w)))
DInvRow4 = (g h . w - w h . g)

DInvNum1 = h . g\[Cross](g\[Cross]w)
DInvNum2 = h . w\[Cross](w\[Cross]g)
DInvNum3 = -g . w h . g\[Cross]w
DInvNum4 = -h . g\[Cross]w

AInvector = 
      1/((g\[Cross]w) . (g\[Cross]w)) {Flatten[{AInvRow1, 
              AInvNum1}], Flatten[{AInvRow2, AInvNum2}], 
          Flatten[{AInvRow3, AInvNum3}], 
          Flatten[{AInvRow4, AInvNum4}]}

MatrixForm[AInvector]

DInvector = 
      1/((g\[Cross]w) . (g\[Cross]w)) {Flatten[{DInvRow1, 
              DInvNum1}], Flatten[{DInvRow2, DInvNum2}], 
          Flatten[{DInvRow3, DInvNum3}], 
          Flatten[{DInvRow4, DInvNum4}]}

MatrixForm[DInvector]

Simplify[
        AInvector - 
          Inverse[Avector]] /. {-1 + (wx^2) + (wy^2) + (wz^2) -> 
          0, -1 + (gx^2) + (gy^2) + (gz^2) -> 0}

difference = 
      Simplify[DInvector - 
            Inverse[Dvector]] /. {-1 + (wx^2) + (wy^2) + (wz^2) -> 
            0, -1 + (gx^2) + (gy^2) + (gz^2) -> 
            0, -1 + (hx^2) + (hy^2) + (hz^2) -> 0}

Collect[
        Collect[Simplify[
              Numerator[
                  difference] /. {(-1 + (hy^2) + 
                        (hz^2)) -> -hx^2}], {wx, wy, wz}, 
            Simplify] /. (hx^2) + (hy^2) + (hz^2) -> 1, {hx, hy, hz, gx, 
          gy, gz}] /. {-1 + (wx^2) + (wy^2) + (wz^2) -> 0, 
        1 - (wx^2) - (wy^2) - (wz^2) -> 0}

Coefficients of [D]^-1[C] (need to divide by (g×w).(g×w)):

DC11 = 
      Simplify[DInvRow1 . Ccol1 + DInvNum1*Cnum1] /. -gx gx0 - gy gy0 - 
            gz gz0 -> 0

DC12 = Simplify[DInvRow1 . Ccol2 + DInvNum1*Cnum2]

DC13 = 
      Simplify[DInvRow1 . Ccol3 + DInvNum1*Cnum3] /. 
        (gx^2) + (gy^2) + (gz^2) -> 1

DC14 = Simplify[DInvRow1 . Ccol4 + DInvNum1*Cnum4]

DC21 = Simplify[DInvRow2 . Ccol1 + DInvNum2*Cnum1]

DC22 = 
      Simplify[DInvRow2 . Ccol2 + DInvNum2*Cnum2] /. -wx wx0 - wy wy0 - 
            wz wz0 -> 0

DC23 = Simplify[DInvRow2 . Ccol3 + DInvNum2*Cnum3]

DC24 = 
      Simplify[DInvRow2 . Ccol4 + DInvNum2*Cnum4] /. 
        (wx^2) + (wy^2) + (wz^2) -> 1

DC31 = Simplify[DInvRow3 . Ccol1 + DInvNum3*Cnum1]

DC32 = Simplify[DInvRow3 . Ccol2 + DInvNum3*Cnum2]

DC33 = Simplify[DInvRow3 . Ccol3 + DInvNum3*Cnum3]

DC34 = Simplify[DInvRow3 . Ccol4 + DInvNum3*Cnum4]

DC41 = Simplify[DInvRow4 . Ccol1 + DInvNum4*Cnum1]

DC42 = Simplify[DInvRow4 . Ccol2 + DInvNum4*Cnum2]

DC43 = Simplify[DInvRow4 . Ccol3 + DInvNum4*Cnum3]

DC44 = Simplify[DInvRow4 . Ccol4 + DInvNum4*Cnum4]

Now  we assemble the intermediate matrix:

MatDC = {{DC11, DC12, DC13, DC14}, {DC21, DC22, DC23, DC24}, {DC31, 
          DC32, DC33, DC34}, {DC41, DC42, DC43, DC44}}

MatrixForm[MatDC]

Now we create the C inverse matrix (divide by 1/((g×w).(g×w))^2:

CinvRow1 = 
      Simplify[-(DC11 AInvRow1 + DC12 AInvRow2 + DC13 AInvRow3 + 
              DC14 AInvRow4)]

CinvRow2 = 
      Simplify[-(DC21 AInvRow1 + DC22 AInvRow2 + DC23 AInvRow3 + 
              DC24 AInvRow4)]

CinvRow3 = 
      Simplify[-(DC31 AInvRow1 + DC32 AInvRow2 + DC33 AInvRow3 + 
              DC34 AInvRow4)]

CinvRow4 = 
      Simplify[-(DC41 AInvRow1 + DC42 AInvRow2 + DC43 AInvRow3 + 
              DC44 AInvRow4)]

CinvNum1 = 
      Simplify[-(DC11 AInvNum1 + DC12 AInvNum2 + DC13 AInvNum3 + 
              DC14 AInvNum4)]

CinvNum2 = 
      Simplify[-(DC21 AInvNum1 + DC22 AInvNum2 + DC23 AInvNum3 + 
              DC24 AInvNum4)]

CinvNum3 = 
      Simplify[-(DC31 AInvNum1 + DC32 AInvNum2 + DC33 AInvNum3 + 
              DC34 AInvNum4)]

CinvNum4 = 
      Simplify[-(DC41 AInvNum1 + DC42 AInvNum2 + DC43 AInvNum3 + 
              DC44 AInvNum4)]

CInvector = 
      1/((g\[Cross]w) . (g\[Cross]w))^2 {Flatten[{CinvRow1, 
              CinvNum1}], Flatten[{CinvRow2, CinvNum2}], 
          Flatten[{CinvRow3, CinvNum3}], 
          Flatten[{CinvRow4, CinvNum4}]}

The solutions for the joint variables

Solution for the angles:

p = {px, py, pz, pw}
p0 = {px0, py0, pz0, pw0}

stcp = Simplify[(AInvector[[1]] . p)]

ctsp = Simplify[AInvector[[2]] . p]

stsp = Simplify[AInvector[[3]] . p]

ctcp = Simplify[AInvector[[4]] . p]

g = {gx, gy, gz}
w = {wx, wy, wz}

This solution is the same as for the RR chain.

dctsp = CInvector[[1]] . p + DInvector[[1]] . p0

dstcp = CInvector[[2]] . p + DInvector[[2]] . p0

dctcp = CInvector[[3]] . p + DInvector[[3]] . p0

dstsp = CInvector[[4]] . p + DInvector[[4]] . p0

4. The design equations

DirEq1 = Simplify[stcp*ctsp - stsp*ctcp]

The numerator of DirEq1 is equal to DirEqsimp*(g×w.g×w) (It has be seen). The denominator of DirEq1 is (g×w.g×w)^2. As g is not parallel to w in general, we use only the simplified equation:

DirEqSimp = 
      gz ((-pw py + px pz) wx + (pw px + 
                    py pz) wy + (-(px^2) - (py^2)) wz) + 
        gx ((-(py^2) - (pz^2)) wx + (px py - 
                    pw pz) wy + (pw py + px pz) wz) + 
        gy ((px py + pw pz) wx + (-(px^2) - 
                    (pz^2)) wy + (-pw px + 
                    py pz) wz)

the moment equations,

dstcpS = Simplify[dstcp]

dctspS = Simplify[dctsp]

dstspS = Simplify[dstsp]

dctcpS = Simplify[dctcp]

MomEq1 = dctcpS*stsp - dstspS*ctcp

MomEq2 = dctspS*stcp - dstcpS*ctsp

MomEq1S = Simplify[MomEq1]

MomEq2S = Simplify[MomEq2]

5. Closed algebraic solution

SOLVE FOR THE REVOLUTE JOINT ANGLES SEPARATELY

We have four positions and need to impose an extra constraint. If we impose the constraint in the revolute joint direction, we can solve for the directions g, w algebraically.
The extra constraint will consist on specifying

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]]}

extraeq = DirEqSimp

extraeqs = {EvalQuat[
          extraeq, {{p1x, p1y, p1z, p1w}, {p1x0, p1y0, p1z0, p1w0}}], 
        EvalQuat[extraeq, {{p2x, p2y, p2z, p2w}, {p2x0, p2y0, p2z0, p2w0}}], 
        EvalQuat[
          extraeq, {{p3x, p3y, p3z, p3w}, {p3x0, p3y0, p3z0, 
              p3w0}}]}

col1 = Coefficient[extraeqs, wx]
col2 = Coefficient[extraeqs, wy]
col3 = Coefficient[extraeqs, wz]

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

MatrixForm[mat]

This is equated to zero; the determinant of the matrix  must be zero.

conditiong = Det[mat]

Define the task quaternions:

qS12={{0.7508114675922651,  -0.10654941504058091,  
              0.12486135396067718,  
              0.6397960648312598},  {0.29824734722269053,  -1.492876598025916,  1.278520848755025,  -0.8481304134104388}}
qS13={{0.25733169337962214,  -0.42802623807537565,  -0.7748431982667803,  
              0.38754607106913747},  {-0.07488679789827832,  -1.9054547134431754,  1.3039529206204281,  0.552309005505367}}
qS14={{0.4396233964625992,  -0.3053336707709429,  
              0.8290658545593983,  
              0.16171712209881983},  {-2.5976796498852788,  -1.3796539299903128,  0.8037138020698826,  0.336478172047491}}

extraeqsN = {EvalQuat[extraeq, qS12], EvalQuat[extraeq, qS13], 
        EvalQuat[extraeq, qS14]}

col1 = Coefficient[extraeqsN, wx]
col2 = Coefficient[extraeqsN, wy]
col3 = Coefficient[extraeqsN, wz]
mat = Transpose[{col1, col2, col3}]

As extra condition, we impose gx=0. We can also make gz=1 in general.

equg = Det[mat] /. {gx -> 0, gz -> 1}

sols = Solve[equg == 0, gy]

gsols = 
      Table[{0, gy /. sols[[i]], 1}/
          Sqrt[{0, gy /. sols[[i]], 1} . {0, gy /. sols[[i]], 
                1}], {i, 1, Length[sols]}]

mats = 
      Table[mat /. {gx -> gsols[[i, 1]], 
            gy -> gsols[[i, 2]], gz -> gsols[[i, 3]]}, {i, 
          1, Length[gsols]}]

solve for w making wx=1,

mats22 = 
      Table[mats[[i, {1, 2}, {2, 3}]], {i, 1, Length[gsols]}]

indeps22 = 
      Table[-mats[[i, {1, 2}, 1]], {i, 1, Length[gsols]}]

solswyzs = 
      Table[Inverse[mats22[[i]]] . indeps22[[i]], {i, 1, 
          Length[gsols]}]

wsols = 
      Table[{1, solswyzs[[i, 1]], solswyzs[[i, 2]]}/
          Sqrt[{1, solswyzs[[i, 1]], solswyzs[[i, 2]]} . {1, 
                solswyzs[[i, 1]], solswyzs[[i, 2]]}], {i, 1, 
          Length[gsols]}]

SOLUTION FOR THE REST OF VARIABLES : we use the equations of g0, w0 to eliminate the gy0, wy0 terms.

fu = gx gx0 + gy gy0 + gz gz0
fu2 = wx wx0 + wy wy0 + wz wz0

gy0S = 
      Table[(gy0 /. Solve[fu == 0, 
                  gy0][[1]]) /. {gx -> gsols[[i, 1]], 
            gy -> gsols[[i, 2]], gz -> gsols[[i, 3]], 
            wx -> wsols[[i, 1]], wy -> wsols[[i, 2]], 
            wz -> wsols[[i, 3]]}, {i, 1, Length[gsols]}]

wy0S = 
      Table[(wy0 /. Solve[fu2 == 0, 
                  wy0][[1]]) /. {gx -> gsols[[i, 1]], 
            gy -> gsols[[i, 2]], gz -> gsols[[i, 3]], 
            wx -> wsols[[i, 1]], wy -> wsols[[i, 2]], 
            wz -> wsols[[i, 3]]}, {i, 1, Length[gsols]}]

moms = 
      Table[Simplify[({EvalQuat[MomEq1S, qS12], EvalQuat[MomEq2S, qS12], 
                    EvalQuat[MomEq1S, qS13], EvalQuat[MomEq2S, qS13], 
                    EvalQuat[MomEq1S, qS14], 
                    EvalQuat[MomEq2S, qS14]} /. {gy0 -> gy0S[[i]], 
                    wy0 -> wy0S[[i]]}) /. {gx -> 
                  gsols[[i, 1]], gy -> gsols[[i, 2]], 
                gz -> gsols[[i, 3]], wx -> wsols[[i, 1]], 
                wy -> wsols[[i, 2]], wz -> wsols[[i, 3]]}] /. 
          hz -> 1, {i, 1, Length[gsols]}]

we collect by w0, g0 and independent term as a function of h:

col1 = Coefficient[moms, gx0]
col2 = Coefficient[moms, gy0]
col3 = Coefficient[moms, gz0]

col4 = Coefficient[moms, wx0]
col5 = Coefficient[moms, wy0]
col6 = Coefficient[moms, wz0]

col7 = 
      Chop[Simplify[
          moms - gx0 col1 - gy0 col2 - gz0 col3 - wx0 col4 - wy0 col5 - 
            wz0 col6]]

mat0 = 
      Table[Transpose[{col1[[i]], col3[[i]], col4[[i]], 
            col6[[i]], col7[[i]]}], {i, 1, Length[gsols]}]

MatrixForm[mat0[[1]]]

We compute the 6 5x5 minors.
In principle, the degree in hx and hy must be 5 at most. We could eliminate hx or hy from one column, but it is not needed in this case.

mymins0 = 
      Table[Collect[Expand[Minors[mat0[[i]], 5]], {hx, hy}, 
          Simplify], {i, 1, Length[gsols]}]

Collect by powers of hx to create a 6x6 matrix,

Coefficient[mymins0, hx^6]

row1W = 
      Table[Collect[{Coefficient[mymins0[[i, 1, 1]], hx^5], 
            Coefficient[mymins0[[i, 1, 1]], hx^4], 
            Coefficient[mymins0[[i, 1, 1]], hx^3], 
            Coefficient[mymins0[[i, 1, 1]], hx^2], 
            Coefficient[mymins0[[i, 1, 1]], hx], 
            Coefficient[mymins0[[i, 1, 1]], hx, 0]}, hy], {i, 1, 
          Length[gsols]}]
row2W = 
      Table[Collect[{Coefficient[mymins0[[i, 2, 1]], hx^5], 
            Coefficient[mymins0[[i, 2, 1]], hx^4], 
            Coefficient[mymins0[[i, 2, 1]], hx^3], 
            Coefficient[mymins0[[i, 2, 1]], hx^2], 
            Coefficient[mymins0[[i, 2, 1]], hx], 
            Coefficient[mymins0[[i, 2, 1]], hx, 0]}, hy], {i, 1, 
          Length[gsols]}]
row3W = 
      Table[Collect[{Coefficient[mymins0[[i, 3, 1]], hx^5], 
            Coefficient[mymins0[[i, 3, 1]], hx^4], 
            Coefficient[mymins0[[i, 3, 1]], hx^3], 
            Coefficient[mymins0[[i, 3, 1]], hx^2], 
            Coefficient[mymins0[[i, 3, 1]], hx], 
            Coefficient[mymins0[[i, 3, 1]], hx, 0]}, hy], {i, 1, 
          Length[gsols]}]
row4W = 
      Table[Collect[{Coefficient[mymins0[[i, 4, 1]], hx^5], 
            Coefficient[mymins0[[i, 4, 1]], hx^4], 
            Coefficient[mymins0[[i, 4, 1]], hx^3], 
            Coefficient[mymins0[[i, 4, 1]], hx^2], 
            Coefficient[mymins0[[i, 4, 1]], hx], 
            Coefficient[mymins0[[i, 4, 1]], hx, 0]}, hy], {i, 1, 
          Length[gsols]}]
row5W = 
      Table[Collect[{Coefficient[mymins0[[i, 5, 1]], hx^5], 
            Coefficient[mymins0[[i, 5, 1]], hx^4], 
            Coefficient[mymins0[[i, 5, 1]], hx^3], 
            Coefficient[mymins0[[i, 5, 1]], hx^2], 
            Coefficient[mymins0[[i, 5, 1]], hx], 
            Coefficient[mymins0[[i, 5, 1]], hx, 0]}, hy], {i, 1, 
          Length[gsols]}]
row6W = 
      Table[Collect[{Coefficient[mymins0[[i, 6, 1]], hx^5], 
            Coefficient[mymins0[[i, 6, 1]], hx^4], 
            Coefficient[mymins0[[i, 6, 1]], hx^3], 
            Coefficient[mymins0[[i, 6, 1]], hx^2], 
            Coefficient[mymins0[[i, 6, 1]], hx], 
            Coefficient[mymins0[[i, 6, 1]], hx, 0]}, hy], {i, 1, 
          Length[gsols]}]

mathyW = 
      Table[{row1W[[i]], row2W[[i]], row3W[[i]], row4W[[i]], 
          row5W[[i]], row6W[[i]]}, {i, 1, Length[gsols]}]

MatrixForm[mathyW[[1]]]

equHY = Table[Det[mathyW[[i]]], {i, 1, Length[gsols]}]

Making it monic,

equHYm = Simplify[equHY/Coefficient[equHY, hy, 15]]

solsDhy0 = 
      Table[Sort[Solve[equHY[[i]] == 0, hy]], {i, 1, 
          Length[gsols]}]

solshy0 = hy /. solsDhy0

Another way to solve it, good for implementing in non-symbolic programs, is solving matrix eigenvalue problem instead of the computation of the resultant for one variable, as in Manocha,

m5 = Coefficient[mathyW, hy^5]

m4 = Coefficient[mathyW, hy^4]

m3 = Coefficient[mathyW, hy^3]

m2 = Coefficient[mathyW, hy^2]

m1 = Coefficient[mathyW, hy, 1]

m0 = Coefficient[mathyW, hy, 0]

<< LinearAlgebra`MatrixManipulation`

MatrixForm[m0[[1]]]

construct the C-system

Id6 = IdentityMatrix[6]

Z6 = ZeroMatrix[6]

As the condition number of m0 is OK, we can do the change of variables z=1/hy, then create a matrix C

LinearAlgebra`MatrixManipulation`MatrixConditionNumber[
      m0[[1]]]
LinearAlgebra`MatrixManipulation`MatrixConditionNumber[
      m1[[1]]]
LinearAlgebra`MatrixManipulation`MatrixConditionNumber[
      m2[[1]]]
LinearAlgebra`MatrixManipulation`MatrixConditionNumber[
      m3[[1]]]
LinearAlgebra`MatrixManipulation`MatrixConditionNumber[
      m4[[1]]]
LinearAlgebra`MatrixManipulation`MatrixConditionNumber[
      m5[[1]]]

m5bar = 
      Table[Inverse[m0[[i]]] . m5[[i]], {i, 1, 
          Length[gsols]}]
m4bar = 
      Table[Inverse[m0[[i]]] . m4[[i]], {i, 1, 
          Length[gsols]}]
m3bar = 
      Table[Inverse[m0[[i]]] . m3[[i]], {i, 1, 
          Length[gsols]}]
m2bar = 
      Table[Inverse[m0[[i]]] . m2[[i]], {i, 1, 
          Length[gsols]}]
m1bar = 
      Table[Inverse[m0[[i]]] . m1[[i]], {i, 1, 
          Length[gsols]}]

Cmatrix = 
      Table[LinearAlgebra`MatrixManipulation`BlockMatrix[{{Z6, Id6, Z6, Z6, 
              Z6}, {Z6, Z6, Id6, Z6, Z6}, {Z6, Z6, Z6, Id6, Z6}, {Z6, Z6, Z6, 
              Z6, Id6}, {-m5bar[[i]], -m4bar[[i]], -m3bar[[i]], -m2bar[[i]], -m1bar[[i]]}}], {i, 1, 
          Length[gsols]}]

MatrixForm[Cmatrix[[1]]]

solsInv = 
      Table[Eigenvalues[Cmatrix[[i]]], {i, 1, Length[gsols]}]

Length[solsInv[[1]]]

solsInv2 = 
      Table[Select[solsInv[[i]], # =!= 0.  &], {i, 1, 
          Length[gsols]}]

Length[solsInv2[[1]]]

solshy = 1/solsInv2

solshy2 = 
      Table[Sort[solshy[[i]]], {i, 1, Length[gsols]}]

Simplify[solshy2 - solshy0]

the solutions are the same, but  some of them with bad precission.

SOLUTIONS FOR  h, g0, w0

mathyW[[1, {2, 3, 4, 5, 6}, {1, 2, 3, 4, 5}]]

MatsHY = 
      Table[mathyW[[j, {2, 3, 4, 5, 6}, {1, 2, 3, 4, 5}]] /. {hy -> 
              solshy0[[j, i]]}, {j, 1, Length[gsols]}, {i, 1, 
          15}]

indepsHY = 
      Table[-mathyW[[j, {2, 3, 4, 5, 6}, 6]] /. {hy -> 
              solshy0[[j, i]]}, {j, 1, Length[gsols]}, {i, 1, 
          15}]

dets = 
      Table[Det[MatsHY[[j, i]]], {j, 1, Length[gsols]}, {i, 1, 
          15}]

solshx = 
      Table[(Inverse[MatsHY[[j, i]]] . 
              indepsHY[[j, i]])[[5]], {j, 1, Length[gsols]}, {i, 1, 
          15}]

check the relation between variables,

Simplify[((Inverse[MatsHY[[1, 1]]] . 
                  indepsHY[[1, 1]])[[5]])^5 - (Inverse[
                MatsHY[[1, 1]]] . 
              indepsHY[[1, 1]])[[1]]]
Simplify[((Inverse[MatsHY[[1, 1]]] . 
                  indepsHY[[1, 1]])[[5]])^4 - (Inverse[
                MatsHY[[1, 1]]] . 
              indepsHY[[1, 1]])[[2]]]

we use the non-normalized ones for the substitution in mat0

solsH = 
      Table[{solshx[[j, i]], solshy0[[j, i]], 1}, {j, 1, 
          Length[gsols]}, {i, 1, 15}]

solsHn = 
      Table[{solshx[[j, i]], solshy0[[j, i]], 1}/
          Sqrt[{solshx[[j, i]], solshy0[[j, i]], 
                1} . {solshx[[j, i]], solshy0[[j, i]], 1}], {j, 1, 
          Length[gsols]}, {i, 1, 15}]

Here we choose a set of equations which are independent,

Mats0 = 
      Table[mat0[[j, {2, 4, 5, 6}, {1, 2, 3, 4}]] /. {hx -> 
              solsH[[j, i, 1]], hy -> solsH[[j, i, 2]], 
            hz -> solsH[[j, i, 3]]}, {j, 1, Length[gsols]}, {i, 1, 
          15}]

dets0 = 
      Table[Det[Mats0[[j, i]]], {j, 1, Length[gsols]}, {i, 1, 
          15}]

indep0 = 
      Table[-mat0[[j, {2, 4, 5, 6}, 5]] /. {hx -> 
              solsH[[j, i, 1]], hy -> solsH[[j, i, 2]], 
            hz -> solsH[[j, i, 3]]}, {j, 1, Length[gsols]}, {i, 1, 
          15}]

solsGWxz0 = 
      Table[Inverse[Mats0[[j, i]]] . indep0[[j, i]], {j, 1, 
          Length[gsols]}, {i, 1, 15}]

solsG0 = 
      Table[{gx0, gy0 /. gy0 -> gy0S[[j]], 
            gz0} /. {gx0 -> solsGWxz0[[j, i, 1]], 
            gz0 -> solsGWxz0[[j, i, 2]]}, {j, 1, Length[gsols]}, {i, 1, 
          15}]

solsW0 = 
      Table[{wx0, wy0 /. wy0 -> wy0S[[j]], 
            wz0} /. {wx0 -> solsGWxz0[[j, i, 3]], 
            wz0 -> solsGWxz0[[j, i, 4]]}, {j, 1, Length[gsols]}, {i, 1, 
          15}]

solsTOTAL = 
      Table[{solsHn[[j, i]], gsols[[j]], solsG0[[j, i]], 
          wsols[[j]], solsW0[[j, i]]}, {j, 1, Length[gsols]}, {i, 1, 
          15}]

Eliminate the complex solutions,

solsTOTALb = 
      Table[If[(Im[solsTOTAL[[j, i, 1, 1]]] == 0  &&  
              Im[solsTOTAL[[j, i, 2, 1]]] == 0  &&  
              Im[solsTOTAL[[j, i, 3, 1]]] == 0 ) , 
          solsTOTAL[[j, i]],  ], {j, 1, Length[gsols]}, {i, 1, 
          15}]

solsTOTAL1 = 
      Table[Select[solsTOTALb[[j]], # =!= Null &], {j, 1, 
          Length[gsols]}]

Length[solsTOTAL1[[3]]]

2. RPR

Continue as in the two previous cases, beware of sign changes.

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

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}

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

W = {Sin[ϕ/2] wx, Sin[ϕ/2] wy, Sin[ϕ/2] wz, 
          Cos[ϕ/2]} + ϵ {Sin[ϕ/2] wx0, Sin[ϕ/2] wy0, 
            Sin[ϕ/2] wz0, 0}

Qrpr = 
      Simplify[Expand[
            quatmult[G, quatmult[P, W]]] /. {ϵ^2 -> 
              0, ϵ^3 -> 0}]

Qrpr3 = 
      Collect[Qrpr, {ϵ, Cos[(θ/2)],  Sin[(θ/2)], 
          Cos[(ϕ/2)], Sin[(ϕ/2)], d}]

MatrixForm[Qrpr3]