Equations

Original Equations in the Text Format

A3*CTH3*CTH1*CTH2-A3*CTH3*STH1*CAL1*STH2-A3*STH3*CTH1*STH2*CAL2
-A3*STH3*STH1*CAL1*CTH2*CAL2+A3*STH3*STH1*SAL1*SAL2+D3*CTH1*STH2*SAL2
+D3*STH1*CAL1*CTH2*SAL2+D3*STH1*SAL1*CAL2+CTH1*A2*CTH2-STH1*CAL1*A2*STH2
+STH1*SAL1*D2+A1*CTH1 = X

A3*CTH3*STH1*CTH2+A3*CTH3*CTH1*CAL1*STH2-A3*STH3*STH1*STH2*CAL2
+A3*STH3*CTH1*CAL1*CTH2*CAL2-A3*STH3*CTH1*SAL1*SAL2+D3*STH1*STH2*SAL2
-D3*CTH1*CAL1*CTH2*SAL2-D3*CTH1*SAL1*CAL2+STH1*A2*CTH2+CTH1*CAL1*A2*STH2
-CTH1*SAL1*D2+A1*STH1 = Y

SAL1*STH2*A3*CTH3+A3*STH3*SAL1*CTH2*CAL2+A3*STH3*CAL1*SAL2
-D3*SAL1*CTH2*SAL2+D3*CAL1*CAL2+SAL1*A2*STH2+CAL1*D2+D1 = Z

Where S and C stand for Sine and Cosine while TH and AL stand for theta and alpha.

RRR Configuration

Unknowns: {A1, A2, A3, D1, D2, D3}

Simplified:

A3*(CTH3*CTH1*CTH2-CTH3*STH1*CAL1*STH2-STH3*CTH1*STH2*CAL2
-STH3*STH1*CAL1*CTH2*CAL2+STH3*STH1*SAL1*SAL2)
+D3*(CTH1*STH2*SAL2+STH1*CAL1*CTH2*SAL2+STH1*SAL1*CAL2)
+A2*(CTH1*CTH2-STH1*CAL1*STH2)+D2*(STH1*SAL1)+A1*CTH1 = X

A3*(CTH3*STH1*CTH2+CTH3*CTH1*CAL1*STH2-STH3*STH1*STH2*CAL2
+STH3*CTH1*CAL1*CTH2*CAL2-STH3*CTH1*SAL1*SAL2)
+D3*(STH1*STH2*SAL2-CTH1*CAL1*CTH2*SAL2-CTH1*SAL1*CAL2)
+A2*(STH1*CTH2+CTH1*CAL1*STH2)+D2*(-CTH1*SAL1)+A1*STH1 = Y

A3*(SAL1*STH2*CTH3+STH3*SAL1*CTH2*CAL2+STH3*CAL1*SAL2)
+D3*(-SAL1*CTH2*SAL2+CAL1*CAL2)+A2*(SAL1*STH2)+D2*CAL1+D1 = Z

Introducing the temporary variables:

B1 = C1 = CTH3*CTH1*CTH2-CTH3*STH1*CAL1*STH2-STH3*CTH1*STH2*CAL2
-STH3*STH1*CAL1*CTH2*CAL2+STH3*STH1*SAL1*SAL2
B2 = C2 = CTH1*STH2*SAL2+STH1*CAL1*CTH2*SAL2+STH1*SAL1*CAL2
B3 = C3 = CTH1*CTH2-STH1*CAL1*STH2
B4 = C4 = STH1*SAL1
B5 = C5 = CTH1
B6 = C6 = CTH3*STH1*CTH2+CTH3*CTH1*CAL1*STH2-STH3*STH1*STH2*CAL2
+STH3*CTH1*CAL1*CTH2*CAL2-STH3*CTH1*SAL1*SAL2
B7 = C7 = STH1*STH2*SAL2-CTH1*CAL1*CTH2*SAL2-CTH1*SAL1*CAL2
B8 = C8 = STH1*CTH2+CTH1*CAL1*STH2
B9 = C9 = -CTH1*SAL1
B10 = C10 = STH1
B11 = C11 = SAL1*STH2*CTH3+STH3*SAL1*CTH2*CAL2+STH3*CAL1*SAL2
B12 = C12 = -SAL1*CTH2*SAL2+CAL1*CAL2
B13 = C13 = SAL1*STH2
B14 = C14 = CAL1
B15 = C15 = 1

Where B variables are assosiated with the first position and joint readings and C variables with the second.

New equation is obtained:

A3*B1+D3*B2+A2*B3+D2*B4+A1*B5 = XB
A3*B6+D3*B7+A2*B8+D2*B9+A1*B10 = YB
A3*B11+D3*B12+A2*B13+D2*B14+D1 = ZB
A3*C1+D3*C2+A2*C3+D2*C4+A1*C5 = XC
A3*C6+D3*C7+A2*C8+D2*C9+A1*C10 = YC
A3*C11+D3*C12+A2*C13+D2*C14+D1 = ZC

Maple Format:

solve({A3*B1+D3*B2+A2*B3+D2*B4+A1*B5 = XB,
A3*B6+D3*B7+A2*B8+D2*B9+A1*B10 = YB,
A3*B11+D3*B12+A2*B13+D2*B14+D1 = ZB,
A3*C1+D3*C2+A2*C3+D2*C4+A1*C5 = XC,
A3*C6+D3*C7+A2*C8+D2*C9+A1*C10 = YC,
A3*C11+D3*C12+A2*C13+D2*C14+D1 = ZC},{A1,A2,A3,D1,D2,D3});

RRP Configuration

Unknowns: {A1, A2, A3, D1, D2}

Adding the last two rows together to yield a five by five matrix:

A3*B1+DB3*B2+A2*B3+D2*B4+A1*B5 = XB
A3*B6+DB3*B7+A2*B8+D2*B9+A1*B10 = YB
A3*B11+DB3*B12+A2*B13+D2*B14+D1 = ZB
A3*C1+DC3*C2+A2*C3+D2*C4+A1*C5 = XC
A3*C6+DC3*C7+A2*C8+D2*C9+A1*C10+A3*C11+DC3*C12+A2*C13+D2*C14+D1 = ZC+YC

Which is then equal to:

A3*B1+DB3*B2+A2*B3+D2*B4+A1*B5 = XB
A3*B6+DB3*B7+A2*B8+D2*B9+A1*B10 = YB
A3*B11+DB3*B12+A2*B13+D2*B14+D1 = ZB
A3*C1+DC3*C2+A2*C3+D2*C4+A1*C5 = XC
A3*K1+DC3*K2+A2*K3+D2*K4+A1*C10+D1 = K5

Where:

K1 = C6+C11; K2 = C7+C12; K3 = C8+C13; K4 = C9+C14; K5 = ZC+YC;

DB3 and DC3 are equal to the first and second reading of the third joint value.

Maple Format:

solve({A3*B1+DB3*B2+A2*B3+D2*B4+A1*B5 = XB,
A3*B6+DB3*B7+A2*B8+D2*B9+A1*B10 = YB,
A3*B11+DB3*B12+A2*B13+D2*B14+D1 = ZB,
A3*C1+DC3*C2+A2*C3+D2*C4+A1*C5 = XC,
A3*K1+DC3*K2+A2*K3+D2*K4+A1*C10+D1 = K5},{A1,A2,A3,D1,D2});

RPR Configuration

Unknowns: {A1, A2, A3, D1, D3}

Maple Format:

solve({A3*B1+D3*B2+A2*B3+DB2*B4+A1*B5 = XB,
A3*B6+D3*B7+A2*B8+DB2*B9+A1*B10 = YB,
A3*B11+D3*B12+A2*B13+DB2*B14+D1 = ZB,
A3*C1+D3*C2+A2*C3+DC2*C4+A1*C5 = XC,
A3*K1+D3*K2+A2*K3+DC2*K4+A1*C10+D1 = K5},{A1,A2,A3,D1,D3});

PRR Configuration

Unknowns: {A1, A2, A3, D2, D3}

Maple Format:

solve({A3*B1+D3*B2+A2*B3+D2*B4+A1*B5 = XB,
A3*B6+D3*B7+A2*B8+D2*B9+A1*B10 = YB,
A3*B11+D3*B12+A2*B13+D2*B14+DB1 = ZB,
A3*C1+D3*C2+A2*C3+D2*C4+A1*C5 = XC,
A3*K1+D3*K2+A2*K3+D2*K4+A1*C10+DC1 = K5},{A1,A2,A3,D2,D3});

PPR Configuration

Unknowns: {A1, A2, A3, D3}

Adding the second and third row together to yield a four by four matrix:

A3*B1+D3*B2+A2*B3+DB2*B4+A1*B5 = XB
A3*B6+D3*B7+A2*B8+DB2*B9+A1*B10+A3*B11+D3*B12+A2*B13+DB2*B14+DB1 = YB+ZB
A3*C1+D3*C2+A2*C3+DC2*C4+A1*C5 = XC
A3*K1+D3*K2+A2*K3+DC2*K4+A1*C10+DC1 = K5

Which is then equal to:

A3*B1+D3*B2+A2*B3+DB2*B4+A1*B5 = XB
A3*K6+D3*K7+A2*K8+DB2*K9+A1*B10+DB1 = K10
A3*C1+D3*C2+A2*C3+DC2*C4+A1*C5 = XC
A3*K1+D3*K2+A2*K3+DC2*K4+A1*C10+DB1 = K5

Where:

K6 = B6+B11; K7 = B7+B12; K8 = B8+B13; K9 = B9+B14; K10 = YB+ZB;

DB1, DB2, DC1 and DC2 are equal to the first and second reading of the first and second joint value.

Maple Format:

solve({A3*B1+D3*B2+A2*B3+DB2*B4+A1*B5 = XB,
A3*K6+D3*K7+A2*K8+DB2*K9+A1*B10+DB1 = K10,
A3*C1+D3*C2+A2*C3+DC2*C4+A1*C5 = XC,
A3*K1+D3*K2+A2*K3+DC2*K4+A1*C10+DC1 = K5},{A1,A2,A3,D3});

PRP Configuration

Unknowns: {A1, A2, A3, D2}

Maple Format:

solve({A3*B1+DB3*B2+A2*B3+D2*B4+A1*B5 = XB,
A3*K6+DB3*K7+A2*K8+D2*K9+A1*B10+DB1 = K10,
A3*C1+DC3*C2+A2*C3+D2*C4+A1*C5 = XC,
A3*K1+DC3*K2+A2*K3+D2*K4+A1*C10+DC1 = K5},{A1,A2,A3,D2});

RPP Configuration

Unknowns: {A1, A2, A3, D1}

Maple Format:

solve({A3*B1+DB3*B2+A2*B3+DB2*B4+A1*B5 = XB,
A3*K6+DB3*K7+A2*K8+DB2*K9+A1*B10+D1 = K10,
A3*C1+DC3*C2+A2*C3+DC2*C4+A1*C5 = XC,
A3*K1+DC3*K2+A2*K3+DC2*K4+A1*C10+D1 = K5},{A1,A2,A3,D1});

PPP Configuration

Unknowns: {A1, A2, A3}

New Equation:

A3*B1+D3*B2+A2*B3+D2*B4+A1*B5 = XB
A3*B6+D3*B7+A2*B8+D2*B9+A1*B10 = YB
A3*B11+D3*B12+A2*B13+D2*B14+D1 = ZB

Maple Format:

solve({A3*B1+D3*B2+A2*B3+D2*B4+A1*B5 = XB,
A3*B6+D3*B7+A2*B8+D2*B9+A1*B10 = YB,
A3*B11+D3*B12+A2*B13+D2*B14+D1 = ZB},{A1,A2,A3});