By: Alba Perez Gracia

Institut de Robotica i Informatica Industrial, UPC-CSIC

1.1. Preliminaries and loading data

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

Matrix.m has some useful functions for line and screw geometry, as well as for quaternion operations.

<<"/Users/alba/ParallelSynthesis/PreliminaryWork/Matrix.m"

1.1.3. Functions to generate linear combinations of screws

screwSystemLines[q1_,q2_]:=Module[{l1,l2,l,q},
l1=makeLine[q1];
l2=makeLine[q2];
q=Table[RandomReal[{-10,10}]*l1+RandomReal[{-10,10}]*l2,{i,1,100}];
l=Table[makeLine2[q[[i]]],{i,1,Length[q]}]
]

screwSystemLines2[q1_, q2_] := Module[{l1, l2, l, q},
l1 = makeLine2[q1];
l2 = makeLine2[q2];
q =Table[RandomReal[{-10, 10}]*l1 + RandomReal[{-10, 10}]*l2, {i, 1, 100}];
  l = Table[makeLine2[q[[i]]], {i, 1, Length[q]}]
  ]

screwSystem combines two displacements, but uses their screw axes only.

screwSystem first makes them lines, then it combines them, then it subtracts the pitch and normalizes them.

screwSystem[q1_, q2_] := Module[{ q1u,q2u,Q1,Q2,r1, r2, s, su, c1, c2, c, pi1, pi2, pi},r1 = Table[RandomReal[{-10, 10}], {j, 1, 100}]; r2 = Table[RandomReal[{-10, 10}], {j, 1, 100}];
q1u=q1[[1]]/Sqrt[q1[[1]].q1[[1]]];
q2u=q2[[1]]/Sqrt[q2[[1]].q2[[1]]];
c1 = q1[[1]]\[Cross]q1[[2]]/(q1[[1]].q1[[1]]);
c2 = q2[[1]]\[Cross]q2[[2]]/(q2[[1]].q2[[1]]);
Q1={q1u,c1\[Cross]q1u};
Q2={q2u,c2\[Cross]q2u};
s = Table[r1[[j]]*Q1+ r2[[j]]*Q2, {j, 1, Length[r1]}];
pi=Table[s[[i, 1]].s[[i, 2]]/(s[[i, 1]].s[[i, 1]]), {i, 1, Length[r1]}];Table[ {s[[j,1]]/Sqrt[s[[j, 1]].s[[j, 1]]], (s[[j,2]]-pi[[j]]*s[[j,1]])/Sqrt[s[[j, 1]].s[[j, 1]]]}, {j, 1,  Length[r1]}]
    ]

screwSystemPitch gives both the screws and the pitch,

screwSystemPitch[q1_, q2_] := Module[{ q1u,q2u,Q1,Q2,r1, r2, s, su, c1, c2, c, pi1, pi2, pi},r1 = Table[RandomReal[{-10, 10}], {j, 1, 100}]; r2 = Table[RandomReal[{-10, 10}], {j, 1, 100}];
q1u=q1[[1]]/Sqrt[q1[[1]].q1[[1]]];
q2u=q2[[1]]/Sqrt[q2[[1]].q2[[1]]];
c1 = q1[[1]]\[Cross]q1[[2]]/(q1[[1]].q1[[1]]);
c2 = q2[[1]]\[Cross]q2[[2]]/(q2[[1]].q2[[1]]);
Q1={q1u,c1\[Cross]q1u};
Q2={q2u,c2\[Cross]q2u};
s = Table[r1[[j]]*Q1+ r2[[j]]*Q2, {j, 1, Length[r1]}];
pi=Table[s[[i, 1]].s[[i, 2]]/(s[[i, 1]].s[[i, 1]]), {i, 1, Length[r1]}];Table[{ {s[[j,1]]/Sqrt[s[[j, 1]].s[[j, 1]]], (s[[j,2]]-pi[[j]]*s[[j,1]])/Sqrt[s[[j, 1]].s[[j, 1]]]},{pi[[j]]}}, {j, 1,  Length[r1]}]
    ]

screwSystem2 takes the generating screws with their pitches,

screwSystem2[q1_, q2_] := Module[{ q1u,q2u,r1, r2, s, su,  c, pi1, pi2, pi},r1 = Table[RandomReal[{-10, 10}], {j, 1, 100}]; r2 = Table[RandomReal[{-10, 10}], {j, 1, 100}];
q1u=q1/Sqrt[q1[[1]].q1[[1]]];
q2u=q2/Sqrt[q2[[1]].q2[[1]]];
s = Table[r1[[j]]*q1u+ r2[[j]]*q2u, {j, 1, Length[r1]}];
pi=Table[s[[i, 1]].s[[i, 2]]/(s[[i, 1]].s[[i, 1]]), {i, 1, Length[r1]}];Table[ {s[[j,1]]/Sqrt[s[[j, 1]].s[[j, 1]]], (s[[j,2]]-pi[[j]]*s[[j,1]])/Sqrt[s[[j, 1]].s[[j, 1]]]}, {j, 1,  Length[r1]}]
    ]

screwSystemPitch2[q1_, q2_] := Module[{ q1u,q2u,r1, r2, s, su,  c, pi1, pi2, pi},r1 = Table[RandomReal[{-10, 10}], {j, 1, 100}]; r2 = Table[RandomReal[{-10, 10}], {j, 1, 100}];
q1u=q1/Sqrt[q1[[1]].q1[[1]]];
q2u=q2/Sqrt[q2[[1]].q2[[1]]];
s = Table[r1[[j]]*q1u+ r2[[j]]*q2u, {j, 1, Length[r1]}];
pi=Table[s[[i, 1]].s[[i, 2]]/(s[[i, 1]].s[[i, 1]]), {i, 1, Length[r1]}];Table[{ {s[[j,1]]/Sqrt[s[[j, 1]].s[[j, 1]]], (s[[j,2]]-pi[[j]]*s[[j,1]])/Sqrt[s[[j, 1]].s[[j, 1]]]},{pi[[j]]}}, {j, 1,  Length[r1]}]
    ]

The below function, screwsystemPitch3, assumes that the screws are unit and does not divide by the magnitude for those cases in which we have a pure translation.

screwSystemPitch3[q1_, q2_] := Module[{ q1u,q2u,r1, r2, s, su,  c, pi1, pi2, pi},r1 = Table[RandomReal[{-2, 2}], {j, 1, 100}]; r2 = Table[RandomReal[{-2, 2}], {j, 1, 100}];
q1u=q1;
q2u=q2;
s = Table[r1[[j]]*q1u+ r2[[j]]*q2u, {j, 1, Length[r1]}];
pi=Table[s[[i, 1]].s[[i, 2]]/(s[[i, 1]].s[[i, 1]]), {i, 1, Length[r1]}];Table[{ {s[[j,1]]/Sqrt[s[[j, 1]].s[[j, 1]]], (s[[j,2]]-pi[[j]]*s[[j,1]])/Sqrt[s[[j, 1]].s[[j, 1]]]},{pi[[j]]}}, {j, 1,  Length[r1]}]
    ]

Function to make a gradual l.c. of screws, instead of random.

In order to keep the same direction for both screws, we check their signs and rectify if needed.

screwSystemPitch2Gradual[q1_, q2_] := Module[{ q1u, q2u, r1, r2, s, su,  c, pi1, pi2, pi, mag},
  r1 = Table[j, {j, 1, 10, 2}]; r2 = Table[j, {j, 1, 10, 2}];
  q1u = q1/Sqrt[q1[[1]].q1[[1]]];
  q2u = q2/Sqrt[q2[[1]].q2[[1]]];
If[q1u[[1]].q2u[[1]]>0, ,q2u=-q2/Sqrt[q2[[1]].q2[[1]]]];
s = Flatten[Table[ Table[r1[[j]]*q1u + r2[[i]]*q2u, {j, 1, Length[r1]}], {i, 1, Length[r2]}], 1];
  pi = Table[s[[i, 1]].s[[i, 2]]/(s[[i, 1]].s[[i, 1]]), {i, 1, Length[s]}];
  mag = Table[s[[i, 1]].s[[i, 1]], {i, 1, Length[s]}]; Table[{ {s[[j, 1]]/Sqrt[s[[j, 1]].s[[j, 1]]], (s[[j, 2]] - pi[[j]]*s[[j, 1]])/Sqrt[s[[j, 1]].s[[j, 1]]]}, {pi[[j]]}, {mag[[j]]}}, {j, 1,  Length[s]}]
      ]

1.2. Definition of the RP chain

RP, or PR, chain is an orientation-limited chain. The counting gives nR=2, nmax=2.5, nT=3. 
We define two general task positions and one extra location with orientation in the workspace of the chain.

1.2.1. Creating the forward kinematics

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/2hx,d/2 hy,d/2 hz,0}

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

Qrp2=Collect[Qrp,{ϵ,Cos[(θ/2)], Sin[(θ/2)] }]

1.2.2. Separation into 8 equations

QrpROT={Coefficient[Qrp2[[1]],ϵ,0],Coefficient[Qrp2[[2]],ϵ,0],Coefficient[Qrp2[[3]],ϵ,0],Coefficient[Qrp2[[4]],ϵ,0]}

QrpMOM=Collect[Expand[{Coefficient[Qrp2[[1]],ϵ,1],Coefficient[Qrp2[[2]],ϵ,1],Coefficient[Qrp2[[3]],ϵ,1],Coefficient[Qrp2[[4]],ϵ,1]}],{Cos[(θ/2)] ,Sin[(θ/2)],d}]

MatrixForm[QrpMOM]

1.2.3. Pitch of the RP chain:

pitchRP=Simplify[QrpROT[[{1,2,3}]].QrpMOM[[{1,2,3}]]/QrpROT[[{1,2,3}]].QrpROT[[{1,2,3}]]]/.{(gx^2)+(gy^2)+(gz^2)->1,gx gx0+gy gy0+gz gz0->0}

1.3. Definition of the PR chain

1.3.1. Creating the forward kinematics

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

Qpr2=Collect[Qpr,{ϵ,Cos[(θ/2)], Sin[(θ/2)] }]

1.3.2. Separation into 8 equations

QprROT={Coefficient[Qpr2[[1]],ϵ,0],Coefficient[Qpr2[[2]],ϵ,0],Coefficient[Qpr2[[3]],ϵ,0],Coefficient[Qpr2[[4]],ϵ,0]}

QprMOM=Collect[Expand[{Coefficient[Qpr2[[1]],ϵ,1],Coefficient[Qpr2[[2]],ϵ,1],Coefficient[Qpr2[[3]],ϵ,1],Coefficient[Qpr2[[4]],ϵ,1]}],{Cos[(θ/2)] ,Sin[(θ/2)],d}]

MatrixForm[QprMOM]

1.3.3. Pitch of the RP chain:

pitchPR=Simplify[QprROT[[{1,2,3}]].QprMOM[[{1,2,3}]]/QprROT[[{1,2,3}]].QprROT[[{1,2,3}]]]/.{(gx^2)+(gy^2)+(gz^2)->1,gx gx0+gy gy0+gz gz0->0}

1.4. Synthesis solution

1.4.1. Solution for the orientation (valid for both RP and PR chains)

The goal dual quaternion is defined as P,

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

The solution for the direction of the revolute joint and the rotation angle about that joint is

gsol={{gx->-(px/(Sqrt[(px^2)+(py^2)+(pz^2)])),gy->-(py/(Sqrt[(px^2)+(py^2)+(pz^2)])),gz->-(pz/(Sqrt[(px^2)+(py^2)+(pz^2)]))},{gx->(px/(Sqrt[(px^2)+(py^2)+(pz^2)])),gy->(py/(Sqrt[(px^2)+(py^2)+(pz^2)])),gz->(pz/(Sqrt[(px^2)+(py^2)+(pz^2)]))}}

thetasol=2 ArcTan[pw,(pz/gz)]

1.4.2. Solution for the location: RP chain

Solve for g0 linearly in each task position P, Q,

solsg0A=Solve[{QrpMOM[[1]],QrpMOM[[2]],QrpMOM[[3]]}=={px0,py0,pz0},{gx0,gy0,gz0}]
solsg0B=Solve[{QrpMOM[[1]],QrpMOM[[2]],QrpMOM[[3]]}=={qx0,qy0,qz0},{gx0,gy0,gz0}]

Equate both solutions, as g0 must be the same to reach both positions,

solsg0Eq=(({gx0,gy0,gz0}/.solsg0A[[1]])/.{θ->θA,d->dA})-(({gx0,gy0,gz0}/.solsg0B[[1]])/.{θ->θB,d->dB})

Add a pitch condition,

condPitch=-2 pw0/(dA Sin[θA/2])+2 qw0/(dB Sin[θB/2])

Plucker equations,

Plucker2=gx gx0+gy gy0+gz gz0
Plucker3=hx^2+hy^2+hz^2-1

systemRP=Flatten[{solsg0Eq,Plucker3,condPitch}]/.gsol[[2]]

We need to solve systemRP for h and dA, dB.

1.4.3. Solution for the location: PR chain

Solve for g0 linearly in each task position P, Q,

solsg0prA=Solve[{QprMOM[[1]],QprMOM[[2]],QprMOM[[3]]}=={px0,py0,pz0},{gx0,gy0,gz0}]
solsg0prB=Solve[{QprMOM[[1]],QprMOM[[2]],QprMOM[[3]]}=={qx0,qy0,qz0},{gx0,gy0,gz0}]

Equate both solutions, as g0 must be the same to reach both positions,

solsg0prEq=(({gx0,gy0,gz0}/.solsg0prA[[1]])/.{θ->θA,d->dA})-(({gx0,gy0,gz0}/.solsg0prB[[1]])/.{θ->θB,d->dB})

Add a pitch condition,

condPitch=-2 pw0/(dA Sin[θA/2])+2 qw0/(dB Sin[θB/2])

systemPR=Flatten[{solsg0prEq,Plucker3,condPitch}]/.gsol[[2]]

We need to solve systemPR for h and dA, dB.

1.5. Task definition

There are several ways that the task can be defined. Here we present one of them.

1.5.1. Select a random relative displacement, qS12,

qS12=makequat[makedvec[],makeph[],makede[]]

We fix it to whatever one we like,

qS12={{-0.7660919205622986`,-0.02286392435386076`,0.6270201482659655`,-0.13937770223689366`},{0.8173367237218512`,-0.2243890022290148`,0.01357398721530062`,-4.39463037446269`}}

Take the screw only, used later for calculations,

qS12S={qS12[[1,{1,2,3}]],qS12[[2,{1,2,3}]]}

Pitch and perpendicular point on the line for the first displacement, qS12,

pitch12N=qS12S[[1]].qS12S[[2]]/(qS12S[[1]].qS12S[[1]])
c12N=qS12S[[1]]\[Cross]qS12S[[2]]/qS12S[[1]].qS12S[[1]]

1.5.2. Solve for g and theta12,

Now we solve for the direction g and angle theta12. Those will be used to create the second task position.

Notice that we may have to change the component used in solving for the angle if the selected one is zero.

gsolN=({gx,gy,gz}/.gsol[[2]])/.{px->qS12[[1,1]],py->qS12[[1,2]],pz->qS12[[1,3]]}
theta12N=thetasol/.{px->qS12[[1,1]],py->qS12[[1,2]],pz->qS12[[1,3]],pw->qS12[[1,4]],gx->gsolN[[1]],gy->gsolN[[2]],gz->gsolN[[3]]}

1.5.3. Select a random value for the angle theta13 and create the quaternion qs13,

theta13N=RandomReal[{-6.29,6.29}]

theta13N=0.167057216462684`

qs13=QrpROT/.{gx->gsolN[[1]],gy->gsolN[[2]],gz->gsolN[[3]],θ->theta13N}

1.5.4. Select the location of qS13,

This fixes the location of the plane containing the screw system.

Select a random point on the screw and compute the line of qS13. This point is not the perpendicular from the origin, that will be calculated later.

p13N=makepoint[]

p13N={0.635420072116581`,1.4472360537520501`,0.3238767128529929`}

lS13N={gsolN,p13N\[Cross]gsolN}

1.5.5. Select a value for the slope of the pitch distribution,

This fixes the shape of the triangle.

KN=0.5

KN=0.72

Now we can solve for the pitch of qS13, previous calculation of the perpendicular point,

c13N=lS13N[[1]]\[Cross]lS13N[[2]]/lS13N[[1]].lS13N[[1]]

pitch13N=KN*Norm[(c13N-c12N),2]+pitch12N

Now we can calculate the slide of the second displacement,

t13N=2*pitch13N*Tan[theta13N/2]

1.5.6. Calculate the second relative displacement, qS13,

We have all the information to build qS13,

qS13 = {qs13, Flatten[{Sin[theta13N/2]*lS13N[[2]] + t13N/2*Cos[theta13N/2]*lS13N[[1]], -t13N/2*Sin[theta13N/2]}]}

qS13S={qS13[[1,{1,2,3}]],qS13[[2,{1,2,3}]]}

1.5.7. Generate the screws of the screw system as a linear combination of qS12, qS13,

screws contain the obtained screws, pitches and "magnitudes"; screwsNorm makes them unit and arranges the orientation so that they all point in the same direction; pitches selects only the pitches from the obtained system.

The coefficients of the function to generate the screw system can be changed to obtain a different set of screws from the system. Also, the screws are not generated in order, this can be changed, too.

screws = screwSystemPitch2Gradual[qS12S, qS13S];
screwsNorm = Table[Sign[screws[[i,1,1,3]]]*screws[[i,1]]/Norm[screws[[i,1, 1]],2], {i, 1, Length[screws]}]
pitches=Table[screws[[i,2]],{i, 1, Length[screws]}]

Length[screws ]

We generate points on the screws in order to plot them,

pointsCNo=Table[CommonNormal2[screwsNorm[[i]],screwsNorm[[i+1]]][[2,1]],{i,1,Length[screws]-1}];

linesScrews=Table[Line[{pointsCNo[[i]]-  screwsNorm[[i,1]],pointsCNo[[i]]+pitches[[i,1]]*screwsNorm[[i,1]]},VertexColors->{Blue,Green}],{i,1,Length[pointsCNo]}];
Graphics3D[{Blue,Thick,Opacity[.8],linesScrews},AspectRatio->1]

Generate a nice plot for display, with the two initial screws that generate the system added in blue,

pointsCNS=CommonNormal2[qS12S,qS13S][[2]]
directionsS={Sign[qS12S[[1,3]]]*qS12S[[1]]/Sqrt[qS12S[[1]].qS12S[[1]]],Sign[qS13S[[1,3]]]*qS13S[[1]]/Sqrt[qS13S[[1]].qS13S[[1]]]}
pitchesS={qS12S[[1]].qS12S[[2]]/(qS12S[[1]].qS12S[[1]]),qS13S[[1]].qS13S[[2]]/(qS13S[[1]].qS13S[[1]])}

Adjust this variable to change the thickness of the arrows,

thAr=0.03

linesNice=Table[{Hue[1/4],Arrowheads[{{.04,1}}],Arrow[Tube[{pointsCNo[[i]],pointsCNo[[i]]+pitches[[i,1]]* screwsNorm[[i,1]]},thAr]]},{i,1,Length[pointsCNo]}];
linesNiceS={Hue[1/2],Arrow[Tube[{pointsCNS[[1]],pointsCNS[[1]]+pitchesS[[1]]*directionsS[[1]]},thAr]],Arrow[Tube[{pointsCNS[[2]],pointsCNS[[2]]+pitchesS[[2]]*directionsS[[2]]},thAr]]};
alllines=Flatten[{linesNice,linesNiceS},1];

Show[Graphics3D[{Blue,Thick,Opacity[.8],alllines},AspectRatio->1,Axes->True,Boxed->True,ViewPoint->{1,1,1},ImageSize->600],DisplayFunction->(PopupWindow[Button["Click here"],#,WindowSize->{650,650}]&)]

1.5.8. Compute several parameters of the screw system,

The distance between screws is computed at the common normal line (the one intersecting at the point normal from the origin), and counting from the screw qS12,

distanceLines=Table[Norm[pointsCNo[[i]]-pointsCNS[[1]],2],{i, 1, Length[screws]-1}]

The distribution of pitches is linear,

datum=Table[{distanceLines[[i]],pitches[[i,1]]},{i, 1, Length[screws]-1}];
ListPlot[datum]

We redefine the name of some variables in order to use them to compute the magnitude,

DeltaThAN=theta12N
DeltaThBN=theta13N

pAN=pitch12N
pBN=pitch13N

dABN = Norm[pointsCNS[[2]] - pointsCNS[[1]], 2]

KN
(pBN-pAN)/dABN

Calculate the initial angle that defines the initial configuration of the RPRP linkage. It is the angle from the ground link to the RP link.

We use the formula derived in the reference notebooks for the tangent of theta0/2,

soltan0=dAB Kslope/(pA Tan[(DeltaThA/2)]+(dAB Kslope-pA) Tan[(DeltaThB/2)])

soltan0Alt=dAB Kslope/(pA Tan[(DeltaThA/2)]-pB Tan[(DeltaThB/2)])

theta0N=2ArcTan[soltan0]/.{Kslope->KN,DeltaThA->DeltaThAN,DeltaThB->DeltaThBN,pA->pAN,dAB->dABN}
theta0NAlt=2ArcTan[soltan0Alt]/.{Kslope->KN,DeltaThA->DeltaThAN,DeltaThB->DeltaThBN,pA->pAN,pB->pBN,dAB->dABN}

We need to check out which one of these is the right one. For the second, we have changed the sign of Kslope.

1.5.9. Compute the magnitudes corresponding to the screws of the system

The formula for the magnitude, which is sin(DeltaTheta/2), is created with X as X/sqrt(1+X^2).

In XAlt, we have changed the sign of Kslope.

X=(((dAC-dAB) pA Tan[(DeltaThA/2)]+dAC (dAB Kslope-pA) Tan[(DeltaThB/2)])/(dAB (dAC Kslope-pA)))

XAlt=((dAB pA Tan[(DeltaThA/2)]+dAC (pB Tan[(DeltaThB/2)]-pA Tan[(DeltaThA/2)]))/(dAB pC))

Simplify[(X/.Kslope->-Kslope)-(XAlt/.{pB->Kslope dAB+pA,pC->Kslope dAC+pA})]

magScrews=Simplify[X/Sqrt[1+X^2]]

These two expressions are equivalent, the only difference is that the second one includes the pitches pB, pC of the second reference screw and of the screw we are calculating the magnitude of (C)

magScrewsAlt=Simplify[X/Sqrt[1+X^2]]/.Kslope->-Kslope
magScrewsAltB=Simplify[XAlt/Sqrt[1+XAlt^2]]

1.5.10. Convert to relative and absolute displacements,

Define the reference configuration: we can take any arbitrary displacement, its value will modify the shape of the end-effector only.

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

Find the magnitude of the screws, which is the sin(theta/2), as a function of parameters of the distribution,

magvalues=Simplify[magScrews/.{DeltaThA->DeltaThAN,DeltaThB->DeltaThBN,dAB->dABN,theta0->theta0N,pA->pAN,Kslope->KN}]
magvaluesAlt=Simplify[magScrewsAlt/.{DeltaThA->DeltaThAN,DeltaThB->DeltaThBN,dAB->dABN,theta0->theta0NAlt,pA->pAN,Kslope->KN}]

distanceLinesS12=Prepend[distanceLines,0]

magvaluesList=Table[magvalues/.dAC->distanceLinesS12[[i]],{i,1,Length[distanceLines]}]
thetavalues=Table[2 ArcSin[magvaluesList[[i]]],{i,1,Length[distanceLines]}]

DeltaThAN-thetavalues[[1]]

magvaluesListAlt=Table[magvaluesAlt/.dAC->distanceLinesS12[[i]],{i,1,Length[distanceLines]}]
thetavaluesAlt=Table[2 ArcSin[magvaluesListAlt[[i]]],{i,1,Length[distanceLines]}]

Create the slides,

tvalues=2*Table[pitches[[i,1]]*Tan[thetavalues[[i+1]]/2],{i,1,Length[thetavalues]-1}]
tvaluesAlt=2*Table[pitches[[i,1]]*Tan[thetavaluesAlt[[i+1]]/2],{i,1,Length[thetavalues]-1}]

Create the relative displacements in dual quaternion form,

displacements = Collect[Table[Flatten[{Sin[thetavalues[[i+1]]/2]*(1 + ϵ pitches[[i, 1]])*(screws[[i, 1, 1]] + ϵ screws[[i, 1, 2]]), Cos[thetavalues[[i+1]]/2] - ϵ tvalues[[i]]/2 Sin[thetavalues[[i+1]]/2]}], {i, 1, Length[screws] - 2}], ϵ] /. ϵ^2 -> 0

displacementsAlt = Collect[Table[Flatten[{Sin[thetavaluesAlt[[i+1]]/2]*(1 + ϵ pitches[[i, 1]])*(screws[[i, 1, 1]] + ϵ screws[[i, 1, 2]]), Cos[thetavaluesAlt[[i+1]]/2] - ϵ tvaluesAlt[[i]]/2 Sin[thetavaluesAlt[[i+1]]/2]}], {i, 1, Length[screws] - 2}], ϵ] /. ϵ^2 -> 0

Create the absolute displacements, and save to a file.

matrices=Prepend[Pos1,Table[dq2HM[displacements[[i]]],{i,1,Length[displacements]}]]

matricesAlt = Prepend[Pos1, Table[dq2HM[displacementsAlt[[i]]], {i, 1, Length[displacements]}]]

matrices>>matsfromLCEx2.txt

1.6. Apply synthesis solution to the task

1.6.1. RP synthesis

The result is solsTOTALB, which is {g,g0,h}.

Both solutions for the rotation axis are the same with different sign; we select the second one.

gsolN

theta12N
theta13N

Solve the systemRP giving values to the task positions,

solsg0AN=solsg0A/.{d->dA,gx->gsolN[[1]],gy->gsolN[[2]],gz->gsolN[[3]],θ->theta12N,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]],qx->qS13[[1,1]],qy->qS13[[1,2]],qz->qS13[[1,3]],qw->qS13[[1,4]],qx0->qS13[[2,1]],qy0->qS13[[2,2]],qz0->qS13[[2,3]],qw0->qS13[[2,4]]}

solsg0BN=solsg0B/.{d->dB,gx->gsolN[[1]],gy->gsolN[[2]],gz->gsolN[[3]],θ->theta13N,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]],qx->qS13[[1,1]],qy->qS13[[1,2]],qz->qS13[[1,3]],qw->qS13[[1,4]],qx0->qS13[[2,1]],qy0->qS13[[2,2]],qz0->qS13[[2,3]],qw0->qS13[[2,4]]}

systemRPN=systemRP/.{θA->theta12N,θB->theta13N,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]],qx->qS13[[1,1]],qy->qS13[[1,2]],qz->qS13[[1,3]],qw->qS13[[1,4]],qx0->qS13[[2,1]],qy0->qS13[[2,2]],qz0->qS13[[2,3]],qw0->qS13[[2,4]]}

solshF=Solve[systemRPN=={0,0,0,0,0},{hx,hy,hz,dA,dB}]

Check that the solutions for g0 are both the same,

solsg0=({gx0,gy0,gz0}/.solsg0AN[[1]])/.solshF
solsg0=({gx0,gy0,gz0}/.solsg0BN[[1]])/.solshF

The solutions for h are the same in different directions,

solsh=({hx,hy,hz})/.solshF

Check that they are axes,

solsh[[1]].solsh[[1]]
gsolN.solsg0[[2]]

solsTOTAL contains two solutions, both with same G and with h in different directions,

solsTOTAL=Chop[Sort[Table[{gsolN,solsg0[[i]],solsh[[i]]},{i,1,Length[solsh]}]]]

Slides and angles to reach the positions:

d12N=dA/.solshF
d13N=dB/.solshF

theta12N*180/Pi
theta13N*180/Pi

1.6.2. PR synthesis

The result is solsTOTAL, which is {g,g0,h}.

Both solutions for the rotation axis are the same with different sign; we select the second one.

gsolN

theta12N
theta13N

Solve the systemRP giving values to the task positions,

solsg0prAN=solsg0prA/.{d->dA,gx->gsolN[[1]],gy->gsolN[[2]],gz->gsolN[[3]],θ->theta12N,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]],qx->qS13[[1,1]],qy->qS13[[1,2]],qz->qS13[[1,3]],qw->qS13[[1,4]],qx0->qS13[[2,1]],qy0->qS13[[2,2]],qz0->qS13[[2,3]],qw0->qS13[[2,4]]}

solsg0prBN=solsg0prB/.{d->dB,gx->gsolN[[1]],gy->gsolN[[2]],gz->gsolN[[3]],θ->theta13N,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]],qx->qS13[[1,1]],qy->qS13[[1,2]],qz->qS13[[1,3]],qw->qS13[[1,4]],qx0->qS13[[2,1]],qy0->qS13[[2,2]],qz0->qS13[[2,3]],qw0->qS13[[2,4]]}

systemPRN=systemPR/.{θA->theta12N,θB->theta13N,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]],qx->qS13[[1,1]],qy->qS13[[1,2]],qz->qS13[[1,3]],qw->qS13[[1,4]],qx0->qS13[[2,1]],qy0->qS13[[2,2]],qz0->qS13[[2,3]],qw0->qS13[[2,4]]}

solshF=Solve[systemPRN=={0,0,0,0,0},{hx,hy,hz,dA,dB}]

Check that the solutions for g0 are both the same,

solsg0=({gx0,gy0,gz0}/.solsg0prAN[[1]])/.solshF
solsg0=({gx0,gy0,gz0}/.solsg0prBN[[1]])/.solshF

The solutions for h are the same in different directions,

solsh=({hx,hy,hz})/.solshF

Check that they are axes,

solsh[[1]].solsh[[1]]
gsolN.solsg0[[2]]

solsTOTALB contains two solutions, both with same G and with h in different directions,

solsTOTALB=Chop[Sort[Table[{gsolN,solsg0[[i]],solsh[[i]]},{i,1,Length[solsh]}]]]

Slides and angles to reach the positions:

d12prN=dA/.solshF
d13prN=dB/.solshF

theta12N*180/Pi
theta13N*180/Pi

1.6.3. Final solutions

solsTOTAL
solsTOTALB

d12N
d13N
d12prN
d13prN

theta12N*180/Pi
theta13N*180/Pi

2.1. Checking the solutions at the dual quaternion task positions

We check that the solutions can reach qS12 and qS13 for the given values of the axes and the joint variables,

For the first relative position, qS12,

Qrp2N=Qrp2/.{gx->solsTOTAL[[1,1,1]],gy->solsTOTAL[[1,1,2]],gz->solsTOTAL[[1,1,3]],gx0->solsTOTAL[[1,2,1]],gy0->solsTOTAL[[1,2,2]],gz0->solsTOTAL[[1,2,3]],hx->solsTOTAL[[1,3,1]],hy->solsTOTAL[[1,3,2]],hz->solsTOTAL[[1,3,3]],θ->theta12N,d->d12N[[1]]}

Qpr2N=Qpr2/.{gx->solsTOTALB[[1,1,1]],gy->solsTOTALB[[1,1,2]],gz->solsTOTALB[[1,1,3]],gx0->solsTOTALB[[1,2,1]],gy0->solsTOTALB[[1,2,2]],gz0->solsTOTALB[[1,2,3]],hx->solsTOTALB[[1,3,1]],hy->solsTOTALB[[1,3,2]],hz->solsTOTALB[[1,3,3]],θ->theta12N,d->d12prN[[1]]}

{Coefficient[Qrp2N,ϵ,0],Coefficient[Qrp2N,ϵ,1]}
{Coefficient[Qpr2N,ϵ,0],Coefficient[Qpr2N,ϵ,1]}
qS12

For the second relative position, qS13,

Qrp2N=Qrp2/.{gx->solsTOTAL[[1,1,1]],gy->solsTOTAL[[1,1,2]],gz->solsTOTAL[[1,1,3]],gx0->solsTOTAL[[1,2,1]],gy0->solsTOTAL[[1,2,2]],gz0->solsTOTAL[[1,2,3]],hx->solsTOTAL[[1,3,1]],hy->solsTOTAL[[1,3,2]],hz->solsTOTAL[[1,3,3]],θ->theta13N,d->d13N[[1]]}

Qpr2N=Qpr2/.{gx->solsTOTALB[[1,1,1]],gy->solsTOTALB[[1,1,2]],gz->solsTOTALB[[1,1,3]],gx0->solsTOTALB[[1,2,1]],gy0->solsTOTALB[[1,2,2]],gz0->solsTOTALB[[1,2,3]],hx->solsTOTALB[[1,3,1]],hy->solsTOTALB[[1,3,2]],hz->solsTOTALB[[1,3,3]],θ->theta13N,d->d13prN[[1]]}

{Coefficient[Qrp2N,ϵ,0],Coefficient[Qrp2N,ϵ,1]}
{Coefficient[Qpr2N,ϵ,0],Coefficient[Qpr2N,ϵ,1]}
qS13

2.2. Calculating the D-H parameters of the RP, PR chains

2.2.1. Transform the task positions to homogeneous matrices, using Pos1 as reference,

Pos1

qS12
qS13

Pos2=dq2HM[qS12[[1]]+ϵ qS12[[2]]]
Pos3=dq2HM[qS13[[1]]+ϵ qS13[[2]]]

2.2.2. Take only the real solutions to be analyzed

Take only the real solutions,

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

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

solutions2RP=Select[solutionsRP,#=!=Null&]

solutions2PR=Select[solutionsPR,#=!=Null&]

2.2.3. Express the joint axes as lines

Here we will call G, H the joint axes of the RP chain, and G2, H2 the joint axes of the PR chain (it is the second chain).

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

nsol=1

G={solutions2RP[[nsol,1]],solutions2RP[[nsol,2]]}
h=solutions2RP[[nsol,3]]
h.h

G2={solutions2PR[[nsol,1]],solutions2PR[[nsol,2]]}
h2=solutions2PR[[nsol,3]]
h2.h2

Make them unit and clean,

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

Gline2={G2[[1]]/Sqrt[G2[[1]].G2[[1]]],G2[[2]]/Sqrt[G2[[1]].G2[[1]]]}
Gline2[[1]].Gline2[[2]]

We put H in an arbitrary location,

pointG=Gline[[1]]\[Cross]Gline[[2]]
dirPoint=pointG/Sqrt[pointG.pointG]
pointH=pointG+1*dirPoint+1*Gline[[1]]

The pointH2 here is not the perpendicular from the origin, that is why we recalculate it.

pointG2=Gline2[[1]]\[Cross]Gline2[[2]]
dirPoint2=pointG2/Sqrt[pointG2.pointG2]
pointH2=pointG2+1*dirPoint2+1*Gline2[[1]]

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

Hline2={h2/Sqrt[h2.h2],pointH2\[Cross]h2/Sqrt[h2.h2]}

pointH2=Hline2[[1]]\[Cross]Hline2[[2]]
dirPointH2=pointH2/Sqrt[pointH2.pointH2]

2.2.4. D-H parameters and [G],[H] matrices for the RP chain

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

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

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

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

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

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

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

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

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

angle1=ArcCos[cosanglegh]
angle1*180/Pi

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

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

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

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

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

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

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

angle2=ArcCos[cosanglegh]
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}]).Hline[[1]]==1, ,angle2=-angle2;length2=-length2]

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

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

check that we get the intersection point: either one can be used, in case the denominator is zero

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

pointInt=pointNgh+part Nghline[[1]]
(Gtot.Ztrans[0,0].Ztrans[angle1,length1].Xtrans[angle2,length2]).{0,0,0,1}

2.2.4.4. Relative transformation [H] from H to the gripper.

theta10=0
d10=0

Compose the kinematic equations,

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

Htot=Inverse[KE10].Pos1
Chop[Htot,10^(-7)]

Chop[KE10.Htot]

2.2.5. D-H parameters and [G],[H] matrices for the PR chain

2.2.5.1. Find transformation [G] (PR)

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

Rgtrans={dirPointH2,Hline2[[1]]\[Cross]dirPointH2,Hline2[[1]],pointH2}

Ghom=Transpose[Rgtrans]

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

Check Gtot:

rotmat=Gtot2[[{1,2,3},{1,2,3}]]
zeromat={{0,0,0},{0,0,0},{0,0,0}}
dispmat=Flatten[Gtot2[[{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]]]}

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

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

2.2.5.2. Transformation about Z from initial x-axis in H to normal H-G (slide will be added to d) (PR)

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

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

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

Xaxis={dirPointH2, pointH2\[Cross]dirPointH2}

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

angle1PR=ArcCos[cosanglegh]
angle1PR*180/Pi

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

If[(Drop[((Gtot2.Ztrans[0,0].Ztrans[angle1PR,length1PR]).{1,0,0,0}),{4}]).Nghline[[1]]==1, ,angle1PR=-angle1PR;length1PR=-length1PR]

(Drop[((Gtot2.Ztrans[0,0].Ztrans[angle1PR,length1PR]).{1,0,0,0}),{4}]).Nghline[[1]]

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

(Gtot2.Ztrans[0,0].Ztrans[angle1PR,length1PR]).{1,0,0,0}
Nghline[[1]]

2.2.5.3. Transformation about X from  H to G along normalHG (rotation so that Z-axis is aligned with G) (PR)

cosanglegh=Hline2[[1]].Gline2[[1]]
minusasinanglegh=Hline2[[1]].Gline2[[2]]+Hline2[[2]].Gline2[[1]]

angle2PR=ArcCos[cosanglegh]
angle2PR*180/Pi

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

If[(Drop[((Gtot2.Ztrans[0,0].Ztrans[angle1PR,length1PR].Xtrans[angle2PR,length2PR]).{0,0,1,0}),{4}]).Gline2[[1]]==1, ,angle2PR=-angle2PR;length2PR=-length2PR]

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

(Gtot2.Ztrans[0,0].Ztrans[angle1PR,length1PR].Xtrans[angle2PR,length2PR]).{0,0,1,0}
Gline2[[1]]

check that we get the intersection point:

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

pointInt=pointNgh+part Nghline[[1]]
(Gtot2.Ztrans[0,0].Ztrans[angle1PR,length1PR].Xtrans[angle2PR,length2PR]).{0,0,0,1}

2.2.5.4. Relative transformation [H] from H to the gripper. (PR)

theta10=0
d10=0

Compose the kinematic equations,

KE10=Gtot2.Ztrans[0,d10].Ztrans[angle1PR,length1PR].Xtrans[angle2PR,length2PR].Ztrans[theta10,0]

Htot2=Inverse[KE10].Pos1
Chop[Htot2]

Chop[KE10.Htot2]

2.3. Checking that the chains reach the three task absolute displacements

For the RP chain,

KEPos1 =Chop[ Gtot.Ztrans[theta10, 0].Ztrans[angle1, length1].Xtrans[angle2, length2].Ztrans[0, d10].Htot]
Pos1
Chop[KEPos1-Pos1]

KEPos2 = Gtot.Ztrans[theta12N, 0].Ztrans[angle1, length1].Xtrans[angle2, length2].Ztrans[0, d12N[[1]]].Htot
Pos2
Chop[KEPos2-Pos2]

KEPos3 = Gtot.Ztrans[theta13N, 0].Ztrans[angle1, length1].Xtrans[angle2, length2].Ztrans[0, d13N[[1]]].Htot
Pos3
Chop[KEPos3-Pos3]

For the PR chain,

KEPos1 =Chop[ Gtot2.Ztrans[theta10, 0].Ztrans[angle1PR, length1PR].Xtrans[angle2PR, length2PR].Ztrans[0, d10].Htot2]
Pos1
Chop[KEPos1-Pos1]

KEPos2 =  Gtot2.Ztrans[0,d12prN[[1]]].Ztrans[angle1PR, length1PR].Xtrans[angle2PR, length2PR].Ztrans[theta12N, 0].Htot2
Pos2
Chop[KEPos2-Pos2]

KEPos3 = Gtot2.Ztrans[0,d13prN[[1]]].Ztrans[angle1PR, length1PR].Xtrans[angle2PR, length2PR].Ztrans[theta13N, 0].Htot2
Pos3
Chop[KEPos3-Pos3]

Both chains reach the positions.

2.4. Calculating the joint variables to reach all displacements of the trajectory

The strategy is to calculate the angles easily from the quaternion, and then solve for the slides in the moment equations. If all four slides give the same value, then the solution is correct and that position can be reached.

There are two things we are changing here: we consider the standard and alternative solutions for the magnitude, and we consider both cases of angles that we get when computing the angle using the arctan formula, that is, with plus/minus sign in the sin.

displacementsLC=Table[{Coefficient[displacements[[i]],ϵ,0],Coefficient[displacements[[i]],ϵ,1]},{i,1,Length[displacements]}]
displacementsLCAlt=Table[{Coefficient[displacementsAlt[[i]],ϵ,0],Coefficient[displacementsAlt[[i]],ϵ,1]},{i,1,Length[displacementsAlt]}]

2.4.1. RP robot

RProbot={QrpROT,QrpMOM}/.{gx->solsTOTAL[[1,1,1]],gy->solsTOTAL[[1,1,2]],gz->solsTOTAL[[1,1,3]],gx0->solsTOTAL[[1,2,1]],gy0->solsTOTAL[[1,2,2]],gz0->solsTOTAL[[1,2,3]],hx->solsTOTAL[[1,3,1]],hy->solsTOTAL[[1,3,2]],hz->solsTOTAL[[1,3,3]]}

anglesSol=Table[2*ArcTan[displacementsLC[[i,1,4]],-Norm[displacementsLC[[i,1,{1,2,3}]],2]],{i,1,Length[displacementsLC]}]

anglesSolAlt=Table[2*ArcTan[displacementsLCAlt[[i,1,4]],-Norm[displacementsLCAlt[[i,1,{1,2,3}]],2]],{i,1,Length[displacementsLCAlt]}]

desSol1=Table[d/.(Solve[(RProbot[[2,1]]/.θ->anglesSol[[i]])==displacementsLC[[i,2,1]],d])[[1]],{i,1,Length[displacementsLC]}]
desSol2=Table[d/.(Solve[(RProbot[[2,2]]/.θ->anglesSol[[i]])==displacementsLC[[i,2,2]],d])[[1]],{i,1,Length[displacementsLC]}]
desSol3=Table[d/.(Solve[(RProbot[[2,3]]/.θ->anglesSol[[i]])==displacementsLC[[i,2,3]],d])[[1]],{i,1,Length[displacementsLC]}]
desSol4=Table[d/.(Solve[(RProbot[[2,4]]/.θ->anglesSol[[i]])==displacementsLC[[i,2,4]],d])[[1]],{i,1,Length[displacementsLC]}]

Chop[desSol1-desSol3]

desSol1=Table[d/.(Solve[(RProbot[[2,1]]/.θ->anglesSolAlt[[i]])==displacementsLCAlt[[i,2,1]],d])[[1]],{i,1,Length[displacementsLCAlt]}]
desSol2=Table[d/.(Solve[(RProbot[[2,2]]/.θ->anglesSolAlt[[i]])==displacementsLCAlt[[i,2,2]],d])[[1]],{i,1,Length[displacementsLCAlt]}]
desSol3=Table[d/.(Solve[(RProbot[[2,3]]/.θ->anglesSolAlt[[i]])==displacementsLCAlt[[i,2,3]],d])[[1]],{i,1,Length[displacementsLCAlt]}]
desSol4=Table[d/.(Solve[(RProbot[[2,4]]/.θ->anglesSolAlt[[i]])==displacementsLCAlt[[i,2,4]],d])[[1]],{i,1,Length[displacementsLCAlt]}]

Chop[desSol1-desSol3]

In this case, the alternative solution and the negative sign in the sin of the angle (that is, we take the minus sign and do not leave it with the axes) are to be taken. It may not be the general case, we need to keep trying with more examples in order to see that it works.

2.4.2. PR robot

PRrobot={QprROT,QprMOM}/.{gx->solsTOTALB[[1,1,1]],gy->solsTOTALB[[1,1,2]],gz->solsTOTALB[[1,1,3]],gx0->solsTOTALB[[1,2,1]],gy0->solsTOTALB[[1,2,2]],gz0->solsTOTALB[[1,2,3]],hx->solsTOTALB[[1,3,1]],hy->solsTOTALB[[1,3,2]],hz->solsTOTALB[[1,3,3]]}

anglesSol=Table[2*ArcTan[displacementsLC[[i,1,4]],-Norm[displacementsLC[[i,1,{1,2,3}]],2]],{i,1,Length[displacementsLC]}]

anglesSolAlt=Table[2*ArcTan[displacementsLCAlt[[i,1,4]],-Norm[displacementsLCAlt[[i,1,{1,2,3}]],2]],{i,1,Length[displacementsLCAlt]}]

desSol1PR=Table[d/.(Solve[(PRrobot[[2,1]]/.θ->anglesSol[[i]])==displacementsLC[[i,2,1]],d])[[1]],{i,1,Length[displacementsLC]}]
desSol2PR=Table[d/.(Solve[(PRrobot[[2,2]]/.θ->anglesSol[[i]])==displacementsLC[[i,2,2]],d])[[1]],{i,1,Length[displacementsLC]}]
desSol3PR=Table[d/.(Solve[(PRrobot[[2,3]]/.θ->anglesSol[[i]])==displacementsLC[[i,2,3]],d])[[1]],{i,1,Length[displacementsLC]}]
desSol4PR=Table[d/.(Solve[(PRrobot[[2,4]]/.θ->anglesSol[[i]])==displacementsLC[[i,2,4]],d])[[1]],{i,1,Length[displacementsLC]}]

Chop[desSol1PR-desSol3PR]

desSol1PR=Table[d/.(Solve[(PRrobot[[2,1]]/.θ->anglesSolAlt[[i]])==displacementsLCAlt[[i,2,1]],d])[[1]],{i,1,Length[displacementsLCAlt]}]
desSol2PR=Table[d/.(Solve[(PRrobot[[2,2]]/.θ->anglesSolAlt[[i]])==displacementsLCAlt[[i,2,2]],d])[[1]],{i,1,Length[displacementsLCAlt]}]
desSol3PR=Table[d/.(Solve[(PRrobot[[2,3]]/.θ->anglesSolAlt[[i]])==displacementsLCAlt[[i,2,3]],d])[[1]],{i,1,Length[displacementsLCAlt]}]
desSol4PR=Table[d/.(Solve[(PRrobot[[2,4]]/.θ->anglesSolAlt[[i]])==displacementsLCAlt[[i,2,4]],d])[[1]],{i,1,Length[displacementsLCAlt]}]

Chop[desSol1PR-desSol3PR]

In this case also, the alternative solution and the negative sign in the sin of the angle (that is, we take the minus sign and do not leave it with the axes) are to be taken. It may not be the general case, we need to keep trying with more examples in order to see that it works.