In the original sketch you were feeding acos() -6727.00 and -34436.86, neither of which is in the range -1 to 1. It looks like your calculation is going bad WAY before that.
The first mistake I see is calculating 'Theta' in degrees and passing it to the cos() function which takes an angle in radians.
What does 'd' represent and does it make sense for it to be 14699?
Thats right, the value is ridiculously extreme and that because theta was converted once in degrees and then in rad. This has now been fixed by deleting this conversion.
Theta = (50 + (atan2(X, h)));
d = abs(((h - Y) / (cos(Theta))));
Now the values of d are reasonable. But the problem now is that acosA (and also acosB) are printing nan because f2a and f2b are outside the range [-1, 1]. Is there a way to change that?Are you sure that your 50 is a value in radians? The atan2() function is going to return radians so whatever that offset of 50 represents, it must be in radians.
Continue to fix your math errors until f2a and f2b are correct.
It might help if you described what each step of the calculation was supposed to do.
Using the law of the cosine with SSS i want to determine acos(f2a) and acos(f2b).
The servo positions (for A) starts at 50 degrees and should have been added to Theta at the end, not the beginning. I have now changed this.
Theta = atan2(X, h);
...
float Astrich = (acosA * 180) / PI;
float B = (acosB * 180) / PI;
A = 50 + Astrich;
Serial.print("A=");
Serial.print(A);
Serial.print(" , B=");
Serial.println(B);
Now, some values are looking correct, but acosA still prints nan (and B also at the beginning).
The loop must start with the condition 0<= X <=60 , so i dont really know how to stop printing nan without affection X condition.
You seem to have at least two triangles where some sides or angles are known and you are calculating other angles. It would help me to help you if you could draw a diagram of the device with the known and unknown values marked.
Skizzen.pdf (48.7 KB)
a, b and h are known.
A, B, d, z and Theta need to be calculated
This is the sketch behind the code.
Could you please tell me if you are able to open the file?
I was able to open the file.
a = 204mm (Upper Leg)
b = 176mm (Lower leg)
h = 290mm (Height of Hip)
You also know Theta at X(mm forward and back)=0, Y(mm above ground)=0 is 50°. Given a, b, and h, Theta (Angle B) is -36.802 (call it 37) so straight down (BaseTheta) == 87°.
First step is calculating Y for a semicircle. I think you have that right 'r' = FinalX/2 = 30. Y = sqrt(r**2+(CurrentX-r)**2)
Now we can calculate the hip angle between vertical (87°) and the destination X,Y point. Hip angle is atan2(CurrentX, h-Y), Lets do that for CurrentX = 30 (Y = 30): 6.582°
Now we need to calculate the distance 'd', opposite the knee angle. The start point is 0,h and the destination is CurrentX,Y. so sqrt((CurrentX**2)+(h-y)**2) = 261.725 = 262mm.
Now we calculate the knee angle to get the d==262. We have three sides of a triangle so we can use the Law of Cosines:
arccos( (a2 + b2 - d2) / 2ab) = 1.51579 rad = 86.848° (call it 87°).
Set servo 0 (hip) to BaseTheta (87°) + 6° = 93°
Set servo 1 (knee) to 87°
Hi, Thanks again for the help.
Firstly why: BaseTheta (87°) + 6° = 93° ? Why add the angle of the knee to the one of the hip?
I rewrote the code according to your instructions. Since the leg has to make a semicircle Movement, this is what the code looks like:
\
void IK()
{
int a = 204.0, b = 176.0; // a= upper leg lenght, b= lower leg lenght)
float A, B;
float Theta;
int h;
float d;
float Y;
h = 290;
for (X = 0; X <= 60; X += 1)
{
float CurrentX = X;
float FinalX = 60/2;
Y = sqrt(pow(CurrentX,2) - pow((CurrentX - FinalX), 2)); // Y for semicircle
Theta = atan2(CurrentX, (h - Y)); // hip angle
d = sqrt(CurrentX + pow((h - Y), 2));
Serial.print("Y=");
Serial.print(Y);
Serial.print(", Theta=");
Serial.print(Theta);
Serial.print(", d=");
Serial.println(d);
float asq = pow(a, 2);
float bsq = pow(b, 2);
float dsq = pow(d, 2);
Serial.print("asq=");
Serial.print(asq);
Serial.print(", bsq=");
Serial.print(bsq);
Serial.print(", dsq=");
Serial.println(dsq);
float fb = (asq + bsq - dsq);
Serial.print("fb=");
Serial.println(fb);
float f2b = abs(fb / (2 * a * b));
Serial.print("f2b=");
Serial.println(f2b);
float acosB = acos(f2b);
Serial.print("acosB=");
Serial.println(acosB);
float A = ((acosB * 180) / PI) + Theta;
float B = (acosB * 180) / PI;
Serial.print("A=");
Serial.print(A);
Serial.print(" , B=");
Serial.println(B);
setServo(0, 50 + A);
setServo(1, B);
delay(500);
}
\
When running the code and opening the serial monitor prints, for Y = 30, Theta = 0.11, which does not make sense, because the calculator shows me 6.58 but the serial monitor something else.
OK, I think I got all the formulas correct now. The answers make sense.
void setup()
{
Serial.begin(115200);
delay(200);
IK();
}
void loop() {}
void IK()
{
// It is traditional to label the sides of a triangle
// 'a', 'b', and 'c' with angle 'A' opposite side 'a',
// angle 'B' opposite side 'b', and angle 'C' opposite
// side 'c'.
const float a = 204.0; // upper leg length (mm)
const float b = 176.0; // lower leg length (mm)
// 'c' will be the length of the leg from the hip joint to the toe.
// 'C' will be the knee angle
// 'B' will be the upper leg angle
const float h = 290.0; // Height of hip joint
const float asq = a * a;
const float bsq = b * b;
const float ThighAngleVertical = 86.8; // Degrees
// P1
const float InitialX = 0.0;
const float InitialY = 0.0;
// P2
const float TargetX = 60.0;
const float TargetY = 0.0;
const float RangeX = TargetX - InitialX;
const float RangeY = TargetY - InitialY;
const float RadiusOfMotion = (TargetX - InitialX) / 2.0;
// Serial.print("asq=");
// Serial.print(asq);
// Serial.print(", bsq=");
// Serial.println(bsq);
for (float X = InitialX; X <= TargetX; X += 6.0)
{
float Y = sqrt(RadiusOfMotion * RadiusOfMotion - pow(X - RadiusOfMotion, 2)) + // Y for semicircle
InitialY + RangeY * (X - InitialX) / RangeX; // linear interpolation of Y motion
float LegAngle = degrees(atan2(X, (h - Y))); // leg angle relative to vertical
float c = sqrt(X * X + pow((h - Y), 2)); // Length of leg (hip to toe)
Serial.print("X=");
Serial.print(X);
Serial.print(", Y=");
Serial.print(Y);
Serial.print(", LegAngle=");
Serial.print(LegAngle); // Display in degrees
Serial.print(", c=");
Serial.print(c);
float csq = c * c;
// Law of Cosine
// Given the lengths of three sides of a triangle, a, b, and c,
// calculate the cosine of the knee angle 'C' opposite side 'c' (leg length).
float cosC = (asq + bsq - csq) / (2.0 * a * b);
Serial.print(", cosC=");
Serial.print(cosC);
// Given the lengths of three sides of a triangle, a, b, and c,
// calculate the cosine of the thigh angle 'B' opposite side 'b' (lower leg).
float cosB = (asq + csq - bsq) / (2.0 * a * c);
Serial.print(", cosB=");
Serial.print(cosB);
float KneeAngle = degrees(acos(cosC));
float ThighAngleFromLeg = degrees(acos(cosB));
float ThighAngle = ThighAngleVertical + LegAngle - ThighAngleFromLeg;
Serial.print(", KneeAngle=");
Serial.print(KneeAngle);
Serial.print(", ThighAngleFromLeg=");
Serial.print(ThighAngleFromLeg);
Serial.print(", ThighAngle=");
Serial.println(ThighAngle);
//setServo(0, ThighAngle);
//setServo(1, KneeAngle);
delay(500);
}
}Results:
X=0.00, Y=0.00, LegAngle=0.00, c=290.00, cosC=-0.16, cosB=0.80, KneeAngle=99.22, ThighAngleFromLeg=36.80, ThighAngle=50.00
X=6.00, Y=18.00, LegAngle=1.26, c=272.07, cosC=-0.02, cosB=0.76, KneeAngle=91.14, ThighAngleFromLeg=40.30, ThighAngle=47.76
X=12.00, Y=24.00, LegAngle=2.58, c=266.27, cosC=0.02, cosB=0.75, KneeAngle=88.65, ThighAngleFromLeg=41.36, ThighAngle=48.02
X=18.00, Y=27.50, LegAngle=3.92, c=263.12, cosC=0.05, cosB=0.74, KneeAngle=87.32, ThighAngleFromLeg=41.93, ThighAngle=48.80
X=24.00, Y=29.39, LegAngle=5.26, c=261.71, cosC=0.06, cosB=0.74, KneeAngle=86.73, ThighAngleFromLeg=42.18, ThighAngle=49.89
X=30.00, Y=30.00, LegAngle=6.58, c=261.73, cosC=0.06, cosB=0.74, KneeAngle=86.73, ThighAngleFromLeg=42.17, ThighAngle=51.21
X=36.00, Y=29.39, LegAngle=7.87, c=263.08, cosC=0.05, cosB=0.74, KneeAngle=87.30, ThighAngleFromLeg=41.93, ThighAngle=52.73
X=42.00, Y=27.50, LegAngle=9.09, c=265.84, cosC=0.03, cosB=0.75, KneeAngle=88.47, ThighAngleFromLeg=41.44, ThighAngle=54.45
X=48.00, Y=24.00, LegAngle=10.23, c=270.30, cosC=-0.01, cosB=0.76, KneeAngle=90.37, ThighAngleFromLeg=40.63, ThighAngle=56.40
X=54.00, Y=18.00, LegAngle=11.23, c=277.31, cosC=-0.06, cosB=0.77, KneeAngle=93.44, ThighAngleFromLeg=39.31, ThighAngle=58.72
X=60.00, Y=0.00, LegAngle=11.69, c=296.14, cosC=-0.21, cosB=0.81, KneeAngle=102.15, ThighAngleFromLeg=35.52, ThighAngle=62.97
Hello again! Big Thanks for your help 
What exactly is ThighAngleVertical? Because to me it looks like B (ThighAngleFromLeg).
Why do you define ThighAngleVertical = 86.8° ? (is
it because the maximum of Y is reached at this point? Could you please explain this ? Or even better, could you please attach the new sketch since you changed the parameters?
Thanks a lot, !!!!
When the leg (hip to toe) is vertical, the thigh points -36.8° backward from 'vertical'. You say that is 50° so if the thigh was pointing straight down it would be at 86.8°. Knowing the X, Y position of the toe we can calculate the angle from vertical (86.8°). From that, we subtract the thigh angle needed to get the right leg length. That puts the toe in the desired place.
Alright, got it!
Thank you very much!!