RR dual synthesis

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

1. Synthesis

RR is not an orientation-limited chain. We can define up to three task positions:  nR=5, nmax=3.

1. Definition of the RR chain in dual quaternions

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}

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

Qrr = 
      Simplify[Expand[quatmult[G, W]] /. ϵ^2 -> 0]

Qrr3 = 
      Collect[Qrr, {ϵ, Cos[(θ/2)],  Sin[(θ/2)], 
          Cos[(ϕ/2)], Sin[(ϕ/2)] }]

MatrixForm[Qrr3]

2. Separation  into 8 equations

QrrROT = {Coefficient[Qrr3[[1]], ϵ, 0], 
        Coefficient[Qrr3[[2]], ϵ, 0], 
        Coefficient[Qrr3[[3]], ϵ, 0], 
        Coefficient[Qrr3[[4]], ϵ, 0]}

QrrROT2 = 
      Collect[Expand[QrrROT], {Cos[(θ/2)] Sin[(ϕ/2)], 
          Sin[(θ/2)] Cos[(ϕ/2)], Sin[(θ/2)] Sin[(ϕ/2)], 
          Cos[(θ/2)] Cos[(ϕ/2)]}]

QrrMOM = {Coefficient[Qrr3[[1]], ϵ, 1], 
        Coefficient[Qrr3[[2]], ϵ, 1], 
        Coefficient[Qrr3[[3]], ϵ, 1], 
        Coefficient[Qrr3[[4]], ϵ, 1]}

QrrMOM2 = 
      Collect[Expand[QrrMOM], {Cos[(θ/2)] Sin[(ϕ/2)], 
          Sin[(θ/2)] Cos[(ϕ/2)], Sin[(θ/2)] Sin[(ϕ/2)], 
          Cos[(θ/2)] Cos[(ϕ/2)]}]

3.Solving for θ and ϕ in the rotation equations

MatrixForm[QrrROT2]

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

col2 = 
      Coefficient[QrrROT2, Cos[(θ/2)] Sin[(ϕ/2)]]

col3 = 
      Coefficient[QrrROT2,  Sin[(θ/2)] Sin[(ϕ/2)]]

col4 = 
      Coefficient[QrrROT2, Cos[(θ/2)] Cos[(ϕ/2)]]

matrot = Transpose[{col1, col2, col3, col4}]
MatrixForm[matrot]

P = {px, py, pz, pw} + ϵ {px0, py0, pz0, pw0}

invmat = Simplify[Inverse[matrot]]

In vector form,

g =. 
w =. 
p =.

myinvmat = ({Flatten[{-(g\[Cross]w)\[Cross]w, 
                    0} /. {g -> {gx, gy, gz}, w -> {wx, wy, wz}, 
                    p -> {px, py, pz}}], 
              Flatten[{-g\[Cross](g\[Cross]w), 
                    0} /. {g -> {gx, gy, gz}, w -> {wx, wy, wz}, 
                    p -> {px, py, pz}}], 
              Flatten[{g\[Cross]w, 0} /. {g -> {gx, gy, gz}, 
                    w -> {wx, wy, wz}, p -> {px, py, pz}}], 
              Flatten[{(g . w) 
                      g\[Cross]w, (g\[Cross]w) . (g\[Cross]
                          w)} /. {g -> {gx, gy, gz}, w -> {wx, wy, wz}, 
                    p -> {px, py, pz}}]}/((g\[Cross]w) . (g\[Cross]
                    w))) /. {g -> {gx, gy, 
              gz}, 
w -> {wx, wy, 
              wz}, 
p -> {px, py, pz}}

Simplify[(myinvmat - 
            invmat) /. {g -> {gx, gy, gz}, 
w -> {wx, wy, 
              wz}, 
p -> {px, py, pz}}]

solthetaphi = invmat . {px, py, pz, pw}

stcp = Simplify[solthetaphi[[1]]]
ctsp = Simplify[solthetaphi[[2]]]
stsp = Simplify[solthetaphi[[3]]]
ctcp = Simplify[solthetaphi[[4]]]

ctspV = (p . (-g\[Cross](g\[Cross]w)))/((g\[Cross]
                w) . (g\[Cross]w))

stcpV = (p . ((-g\[Cross]w)\[Cross]w))/((g\[Cross]
                w) . (g\[Cross]w))

stspV = (p . (g\[Cross]w))/((g\[Cross]w) . (g\[Cross]
                w))

ctcpV = ((g . w) p . (g\[Cross]w) + 
            pw ((g\[Cross]w) . (g\[Cross]w)))/((g\[Cross]
                w) . (g\[Cross]w))

Simplify[(stcp - stcpV) /. {g -> {gx, gy, gz}, w -> {wx, wy, wz}, 
          p -> {px, py, pz}}]
Simplify[(ctsp - ctspV) /. {g -> {gx, gy, gz}, w -> {wx, wy, wz}, 
          p -> {px, py, pz}}]
Simplify[(stsp - stspV) /. {g -> {gx, gy, gz}, w -> {wx, wy, wz}, 
          p -> {px, py, pz}}]
Simplify[(ctcp - ctcpV) /. {g -> {gx, gy, gz}, w -> {wx, wy, wz}, 
          p -> {px, py, pz}}]

With the values for the angles, we can define the reduced design equation for the directions,

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

wihich simplifies to

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. Substituting the values in the moment equations

Three independent moment equations are directly the design equations.

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

5. Set of final design equations

We will consider as final equations the extra equation making θ and ϕ angles and the three first equations of Qeqs equated to the moment goal quaternion.

designs = {extraeq, Qeqs[[1]] - px0, Qeqs[[2]] - py0, 
        Qeqs[[3]] - pz0}

designs2 = 
      Append[designs,  {g . g - 1, g . g0, w . w - 1, 
            w . w0}] /. {g -> {gx, gy, gz}, w -> {wx, wy, wz}, 
          g0 -> {gx0, gy0, gz0}, w0 -> {wx0, wy0, wz0}}

6. Closed algebraic solution

Reduce the number of variables by using the Plucker conditions,

designs3 = 
      Simplify[(designs /. {gz0 -> (-gx0 gx - gy0 gy)/gz, 
                wz0 -> (-wx0 wx - wy0 wy)/
                    wz}) /. {gz -> 1, wz -> 1}]

Length[designs3]

The task quaternions,

qS12 = {{-0.3606886049201249`, -0.6284626323834859`, 
          0.4131048376829656`, 
          0.5516183853546983`}, {-0.0914311803387711`, -1.6809802664612628`, -1.1926576472064931`, -1.0817602172256202`}}
qS13 = {{0.032998456600341404`, -0.754959713961203`, -0.5709599464244332`, 0.3208608292338319`}, {1.5876842333519499`, 
          1.3094201799088454`, -1.3097748506944338`, 
          0.5869815180583473`}}

designsNum = 
      Flatten[Simplify[{designs3 /. {px -> qS12[[1, 1]], 
                py -> qS12[[1, 2]], pz -> qS12[[1, 3]], 
                pw -> qS12[[1, 4]], px0 -> qS12[[2, 1]], 
                py0 -> qS12[[2, 2]], pz0 -> qS12[[2, 3]], 
                pw0 -> qS12[[2, 4]]}, 
            designs3 /. {px -> qS13[[1, 1]], 
                py -> qS13[[1, 2]], pz -> qS13[[1, 3]], 
                pw -> qS13[[1, 4]], px0 -> qS13[[2, 1]], 
                py0 -> qS13[[2, 2]], pz0 -> qS13[[2, 3]], 
                pw0 -> qS13[[2, 4]]}}]]

Solving this system takes about 10 seconds,

sols = 
      NSolve[designsNum == {0, 0, 0, 0, 0, 0, 0, 0}, {gx, gy, wx, wy, 
          gx0, gy0, wx0, wy0}]

solsb = 
      Table[{gx, gy, wx, wy, gx0, gy0, wx0, wy0} /. sols[[i]], {i, 1, 
          Length[sols]}]

We take only the real solutions,

solsc = 
      Table[If[(Im[solsb[[i, 1]]] == 0  ) , 
          solsb[[i]],  ], {i, 1, Length[solsb]}]

solsd = Select[solsc, # =!= Null &]

There are two real solutions. Construct the final axes as {{g},{g0},{w}{w0}},

pointsCg = 
      Table[({solsd[[i, 1]], solsd[[i, 2]], 
                1}\[Cross]{solsd[[i, 5]], 
                solsd[[i, 
                    6]], (-solsd[[i, 5]] solsd[[i, 1]] - 
                    solsd[[i, 6]] solsd[[i, 2]])})/{solsd[[i, 
                  1]], solsd[[i, 2]], 1} . {solsd[[i, 1]], 
              solsd[[i, 2]], 1}, {i, 1, 2}]

pointsCw = 
      Table[({solsd[[i, 3]], solsd[[i, 4]], 
                1}\[Cross]{solsd[[i, 7]], 
                solsd[[i, 
                    8]], (-solsd[[i, 7]] solsd[[i, 3]] - 
                    solsd[[i, 8]] solsd[[i, 4]])})/{solsd[[i, 
                  3]], solsd[[i, 4]], 1} . {solsd[[i, 3]], 
              solsd[[i, 4]], 1}, {i, 1, 2}]

solsFinal2 = 
      Table[{{solsd[[i, 1]], solsd[[i, 2]], 1}/
            Sqrt[{solsd[[i, 1]], solsd[[i, 2]], 
                  1} . {solsd[[i, 1]], solsd[[i, 2]], 
                  1}], (pointsCg[[i]]\[Cross]{solsd[[i, 1]], 
                  solsd[[i, 2]], 1})/
            Sqrt[{solsd[[i, 1]], solsd[[i, 2]], 
                  1} . {solsd[[i, 1]], solsd[[i, 2]], 
                  1}], {solsd[[i, 3]], solsd[[i, 4]], 1}/
            Sqrt[{solsd[[i, 3]], solsd[[i, 4]], 
                  1} . {solsd[[i, 3]], solsd[[i, 4]], 
                  1}], (pointsCw[[i]]\[Cross]{solsd[[i, 3]], 
                  solsd[[i, 4]], 1})/
            Sqrt[{solsd[[i, 3]], solsd[[i, 4]], 
                  1} . {solsd[[i, 3]], solsd[[i, 4]], 1}]}, {i, 1, 
          Length[solsd]}]

2. Analysis

1. The goal relative positions in quaternion form

These are the dual quaternions for the parameterized solution:

qS12
qS13

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]

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

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

qS12

qS13

2. Transform to four 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

For the translation, let me try to use the following formula: (I-A)v=dxs. This will give 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]

check the inverse transform:

checkA = Pos2[[{1, 2, 3}, {1, 2, 3}]]

checkB = (checkA - Id) . Inverse[checkA + Id]

checkb = {checkB[[3, 2]], checkB[[1, 3]], 
        checkB[[2, 1]]}

N[Tan[psi12/2]*s12]

checks = checkb/Sqrt[checkb . checkb]

N[s12]

checkd = Flatten[Pos2[[{1, 2, 3}, {4}]] ]

checkv = 
      N[-1/(2*Tan[psi12/2]^3)*
          checkB . checkB . (Id - checkB) . checkd]

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

The joint axes G,W expressed as lines

G = {solsFinal2[[2, 1]],  solsFinal2[[2, 2]]}

W = {solsFinal2[[2, 3]],  solsFinal2[[2, 4]]}

Make them unit and clean

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

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

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

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

Find transformation [G]

at the perpendicular with the origin

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

Nline = {pointG/Sqrt[pointG . pointG], {0, 0, 0}}

transformation [G] goes from origin to the frame {N, G×N,G} at the perpendicular point with G

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

Ghom = Transpose[Rgtrans]
MatrixForm[Ghom]

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}

Nline

Transformation along Z from pointG to common normal with 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]]}

Intersection point with G: Easier way to compute the intersection point: (pointN+t.Ndir)×Gdir=G0 (they must have same moment)

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

pointInt = pointNgw + part Ngwline[[1]]

cosangle2 = Nline[[1]] . Ngwline[[1]]

minusasinangle2 = 
      Nline[[1]] . Ngwline[[2]] + 
        Nline[[2]] . Ngwline[[1]]

attention to this minus sign

angle1 = ArcCos[cosangle2]

angle1*180/Pi

length1 = (-minusasinangle2/Sqrt[1 - cosangle2^2])

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

check that this transformation is right:

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

Gline[[1]]

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

Ngwline[[1]]

checking the moment:

rotmat = (Gtot . Ztrans[0, 0] . Ztrans[angle1, length1])[[{1, 2, 
            3}, {1, 2, 3}]]

zeromat = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}

dispmat = 
      Flatten[(Gtot . Ztrans[0, 0] . Ztrans[angle1, length1])[[{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}

Ngwline

Transformation from G to W along X

Length and angle between the first R-joint (G) and the P-joint (P)

cosanglegf = Gline[[1]] . Wline[[1]]

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

angle2 = ArcCos[cosanglegf]

angle2*180/Pi

length2 = -minusasinanglegf/Sqrt[1 - cosanglegf^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 P, when the angle θ is zero:

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

Wline[[1]]

check that we get the point:

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

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

pointInt2 = pointNgw + part Ngwline[[1]]

Relative transformation [H] from W to the gripper.

Relative transformation from last R-joint to the gripper:  calculated as position 1. We need the angles theta and phi to go there to the first position, which are zero.....
Just in case, we already have the procedure to find them.

theta10 = 0
phi10 = 0

now we compose the kinematic equations

KE10 = 
      Gtot . Ztrans[theta10, 0] . Ztrans[angle1, length1] . 
        Xtrans[angle2, length2] . Ztrans[phi10, 0]

The homogeneous transform for the first position P1: it is a rotation about the z-axis only,

Htot = Inverse[KE10] . Pos1

KE10 . Htot

4. Inverse kinematics and check the task positions

Inverse kinematics

thetaSol = 2*ArcTan[ctsp, stsp]

phiSol = 2*ArcTan[stcp, stsp]

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

Assemble kinematic equations and compare with absolute transformation

KE12 = 
      Gtot . Ztrans[theta12, 0] . Ztrans[angle1, length1] . 
        Xtrans[angle2, length2] . Ztrans[phi12, 0] . Htot

Pos2

Position 3:

theta13 = EvalPos[thetaSol, qS13]

phi13 = EvalPos[phiSol, qS13]

Assemble kinematic equations and compare with absolute transformation

KE13 = 
      Gtot . Ztrans[theta13, 0] . Ztrans[angle1, length1] . 
        Xtrans[angle2, length2] . Ztrans[phi13, 0] . Htot

Pos3

Table of values for Synthetica

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

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

phiS = {phi10, phi12, phi13}*180/Pi + {0, 0, -360}