This function used a various of other mathematical functions to estimate the coordinate position of the end-effector with a trilateration method in 3D-Space. The gist of the method comes down to using 3 of the systems rope lengths and their initial position to create 3 spheres from that radius and initial point, and from there the method finds the intersection point between the 3 spheres. The implementation of the code is a translation of the code found at stackoverflow.
What positions did we use?
We tried different motor initial positions of this method of estimating the position of the end-effector. We found the best results in motors 1,2 and 5. Trilateration gives best results when the initial positions are in the same XZ-plane, and with a separated triangle. Description of the system can be found 24_7 System Description if there is any confusion in what motors are where. As can be seen in the code, the motors 1,2 and 5s initial positions are predefined, so these are the rope lengths that has to be used.
Things we had to keep in mind
As we use this method for our forward kinematic, we have to run all 8 motors at all times, at least motor 1,2 and 5 needs to run to estimate of the position of the end-effector. We also get two possible solutions, most of the time this is not a problem because we counteract this by choosing the solution that fits our bounds, and are mostly the same. The reason two solutions are found is because three spheres have two points they intersect, if the lengths are not perfect.
Concept taken from wikipedia, code taken from stackoverflow.
FUNCTION trilaterate : ARRAY [0..1] OF ARRAY [0..2] OF LREAL (* Takes 3 rope inputs, for motor 5,6,8 and returns assumed x,y,z*)
VAR_INPUT
r1 : LREAL;
r2 : LREAL;
r3 : LREAL;
END_VAR
VAR
P1 : ARRAY [0..2] OF LREAL := [0,0,0];
P2 : ARRAY [0..2] OF LREAL := [138,0,0];
P3 : ARRAY [0..2] OF LREAL := [0,0,95];
temp1 : ARRAY [0..2] OF LREAL;
temp2 : ARRAY [0..2] OF LREAL;
temp3 : ARRAY [0..2] OF LREAL;
temp4 : LREAL;
e_x : ARRAY [0..2] OF LREAL;
e_y : ARRAY [0..2] OF LREAL;
e_z : ARRAY [0..2] OF LREAL;
i : LREAL;
d : LREAL;
j : LREAL;
x : LREAL;
y : LREAL;
z : LREAL;
error : INT;
myScalar : LREAL;
END_VAR
temp1 := vector_subtract(v1:=P2, v2:=P1);
e_x := scalar_multiply(scalar:=(1/norm(deltas:=temp1)), vector:=temp1);
temp2 := vector_subtract(v1:=P3, v2:=P1);
i := dot_func(v1:=e_x,v2:=temp2);
temp3 := vector_subtract(v1:=temp2, v2:=scalar_multiply(scalar:=i,vector:=e_x));
//e_y := scalar_multiply(scalar:=(1/norm(deltas:=temp3)), vector:=temp3);
(* HOTFIX on e_y*)
myScalar := 1/SQRT(temp3[0] * temp3[0] + temp3[1] * temp3[1] + temp3[2] * temp3[2]);
e_y := scalar_multiply(scalar:=myScalar, vector:=temp3);
e_z := cross(v1:=e_x,v2:=e_y);
d := norm(deltas:=vector_subtract(v1:=P2,v2:=P1));
j := dot_func(v1:=e_y, v2:=temp2);
x :=(r1*r1 - r2*r2 + d*d)/(2*d);
y := (r1*r1 - r3*r3 - 2*i*x + i*i + j*j)/(2*j);
temp4 := (r1*r1 - x*x - y*y);
z := SQRT(temp4);
trilaterate[0][0] := P1[0] + x * e_x[0] + y * e_y[0] + z * e_z[0];
trilaterate[0][1] := P1[1] + x * e_x[1] + y * e_y[1] + z * e_z[1];
trilaterate[0][2] := P1[2] + x * e_x[2] + y * e_y[2] + z * e_z[2];
trilaterate[1][0] := P1[0] + x * e_x[0] + y * e_y[0] - z * e_z[0];
trilaterate[1][1] := P1[1] + x * e_x[1] + y * e_y[1] - z * e_z[1];
trilaterate[1][2] := P1[2] + x * e_x[2] + y * e_y[2] - z * e_z[2];
