Avaldis 13: "E" Subscript, 1 , Baseline equals 1 half left parenthesis, "x" Subscript, 1 , Baseline plus "x" Subscript, 2 , Baseline , right parenthesis plus StartFraction, left parenthesis, "r" squared minus "r" squared , right parenthesis Over 2 "R" squared , EndFraction left parenthesis, "x" Subscript, 2 , Baseline minus "x" Subscript, 1 , Baseline , right parenthesis plus 1 half StartRoot, 2 StartFraction, left parenthesis, "r" squared plus "r" squared , right parenthesis Over "R" squared , EndFraction minus StartFraction, left parenthesis, "r" squared minus "r" squared , right parenthesis squared Over "R" to the 4th power , EndFraction minus 1 , EndRoot left parenthesis, "y" Subscript, 2 , Baseline minus "y" Subscript, 1 , Baseline , right parenthesisE1=12x1+x2+r21−r222R2x2−x1+122r21+r22R2−r21−r222R4−1y2−y1
13
Avaldis 14: "E" Subscript, 2 , Baseline equals 1 half left parenthesis, "y" Subscript, 1 , Baseline plus "y" Subscript, 2 , Baseline , right parenthesis plus StartFraction, left parenthesis, "r" squared minus "r" squared , right parenthesis Over 2 "R" squared , EndFraction left parenthesis, "y" Subscript, 2 , Baseline minus "y" Subscript, 1 , Baseline , right parenthesis plus 1 half StartRoot, 2 StartFraction, left parenthesis, "r" squared plus "r" squared , right parenthesis Over "R" squared , EndFraction minus StartFraction, left parenthesis, "r" squared minus "r" squared , right parenthesis squared Over "R" to the 4th power , EndFraction minus 1 , EndRoot left parenthesis, "x" Subscript, 1 , Baseline minus "x" Subscript, 2 , Baseline , right parenthesisE2=12y1+y2+r21−r222R2y2−y1+122r21+r22R2−r21−r222R4−1x1−x2
14
Avaldis 15: "E" Subscript, 3 , Baseline equals 1 half left parenthesis, "x" Subscript, 1 , Baseline plus "x" Subscript, 2 , Baseline , right parenthesis plus StartFraction, left parenthesis, "r" squared minus "r" squared , right parenthesis Over 2 "R" squared , EndFraction left parenthesis, "x" Subscript, 2 , Baseline minus "x" Subscript, 1 , Baseline , right parenthesis minus 1 half StartRoot, 2 StartFraction, left parenthesis, "r" squared plus "r" squared , right parenthesis Over "R" squared , EndFraction minus StartFraction, left parenthesis, "r" squared minus "r" squared , right parenthesis squared Over "R" to the 4th power , EndFraction minus 1 , EndRoot left parenthesis, "y" Subscript, 2 , Baseline minus "y" Subscript, 1 , Baseline , right parenthesisE3=12x1+x2+r21−r222R2x2−x1−122r21+r22R2−r21−r222R4−1y2−y1
15
Avaldis 16: "E" Subscript, 4 , Baseline equals 1 half left parenthesis, "y" Subscript, 1 , Baseline plus "y" Subscript, 2 , Baseline , right parenthesis plus StartFraction, left parenthesis, "r" squared minus "r" squared , right parenthesis Over 2 "R" squared , EndFraction left parenthesis, "y" Subscript, 2 , Baseline minus "y" Subscript, 1 , Baseline , right parenthesis minus 1 half StartRoot, 2 StartFraction, left parenthesis, "r" squared plus "r" squared , right parenthesis Over "R" squared , EndFraction minus StartFraction, left parenthesis, "r" squared minus "r" squared , right parenthesis squared Over "R" to the 4th power , EndFraction minus 1 , EndRoot left parenthesis, "x" Subscript, 1 , Baseline minus "x" Subscript, 2 , Baseline , right parenthesisE4=12y1+y2+r21−r222R2y2−y1−122r21+r22R2−r21−r222R4−1x1−x2
16
"x" Subscript, 3 , Baselinex3
"y" Subscript, 3 , Baseliney3
"x" Subscript, 1 , Baselinex1
"y" Subscript, 1 , Baseliney1
r1c3:
"E" Subscript, 1 , BaselineE1
"E" Subscript, 2 , BaselineE2
r2c3:
"x" Subscript, 2 , Baselinex2
"y" Subscript, 2 , Baseliney2
r3c3:
r4c3:
17
"x" Subscript, 4 , Baselinex4
"y" Subscript, 4 , Baseliney4
"x" Subscript, 1 , Baselinex1
"y" Subscript, 1 , Baseliney1
r1c3:
"E" Subscript, 3 , BaselineE3
"E" Subscript, 4 , BaselineE4
r2c3:
"x" Subscript, 2 , Baselinex2
"y" Subscript, 2 , Baseliney2
r3c3:
r4c3:
18
Avaldis 19: "a" Subscript, 1 , Baseline equals left parenthesis, "E" Subscript, 1 , Baseline , "E" Subscript, 2 , Baseline , right parenthesisa1=E1,E2
19
this is the shoulder angle
20
Avaldis 21: arc tangent left parenthesis, "E" Subscript, 2 , Baseline , "E" Subscript, 1 , Baseline , right parenthesisarctanE2,E1
21
this is the elbow angle (relative to robot frame, not shoulder)
22
Avaldis 23: arc tangent left parenthesis, left parenthesis, "c" Subscript, 2 , Baseline . "y" minus "a" Subscript, 1 , Baseline . "y" , right parenthesis , left parenthesis, "c" Subscript, 2 , Baseline . "x" minus "a" Subscript, 1 , Baseline . "x" , right parenthesis , right parenthesisarctanc2.y−a1.y,c2.x−a1.x
23
Avaldis 24: "a" Subscript, 2 , Baseline equals left parenthesis, "E" Subscript, 3 , Baseline , "E" Subscript, 4 , Baseline , right parenthesisa2=E3,E4
24
Avaldis 25: arc tangent left parenthesis, "E" Subscript, 4 , Baseline , "E" Subscript, 3 , Baseline , right parenthesisarctanE4,E3
25
Avaldis 26: arc tangent left parenthesis, left parenthesis, "c" Subscript, 2 , Baseline . "y" minus "a" Subscript, 2 , Baseline . "y" , right parenthesis , left parenthesis, "c" Subscript, 2 , Baseline . "x" minus "a" Subscript, 2 , Baseline . "x" , right parenthesis , right parenthesisarctanc2.y−a2.y,c2.x−a2.x
