I'm having a bit of trouble getting inverse kinematics (IK) working correctly on my 2DOF robotic arm. Most of the time, the arm moves perfectly to the positions I give it but it seems to struggle or completely fail when I use:
Values 100 mm or greater, or Single-digit values (like X = 5, Z = 3 0r X = 60, Z = 4)
Anything in between tends to work fine. I'm not sure if it's an issue in the math, or just a logic bug.
Project Details
Arduino Board: Uno (basic)
Servo Pins:
Base: Pin 3 (not in use because I am just using the X Z plane for now)
Shoulder: Pin 5
Elbow: Pin 6
Link Lengths:
Shoulder to elbow: 100 mm
Elbow to wrist: 100 mm
Planes Used: X-Z (ignoring Y for now)
No gripper:
Hardware Arduino uno, two 9g servos (shoulder, Elbow) both have a 5vās supplied
I have added the base servo as well not to sure if its needed or not (hight torque servo āblack oneā)
Iām using the standard Servo.H and Maths.H library and calculating IK using the Law of Cosines and Pythagoras theorem. Below is the code Iām using:
Cang = hypotenuse
ABangle = is the a and b angels in a triangle
ABang = random variable (slightly relating to ABangle)
float X = 75; //these are the current cordites
float Z = 90;
#include <Servo.h>
#include <math.h>
Servo bServo;
Servo shServo;
Servo elServo;
void setup() {
Serial.begin(9600);
bServo.attach(3);
shServo.attach(5);
elServo.attach(6);
}
//----------------------------------------------
void loop() {
kinamatics();
}
//----------------------------------------------
void kinamatics() {
float X = 75;
float Z = 90;
float C = calculateHypotenuse(X, Z);
float numerator = (100 * 100 + C * C - 100 * 100);
float denominator = (2 * 100 * C);
float angleRad = acos(numerator / denominator);
float ABang = angleRad * 180 / PI;
//float Cang = (180 - (2 * ABang));
float ABangle = (ABang * 2);
float Cang = (180 - ABangle);
Serial.print("Angle Cs disstace: ");
Serial.println(C);
Serial.print("Angle Sh in degrees: ");
Serial.println(ABang);
Serial.print("Angle El in degrees: ");
Serial.println(Cang);
shServo.write(ABang);
elServo.write(Cang);
delay(10000);
}
//----------------------------------------------
float calculateHypotenuse(float X, float Z) {
// Calculate the square of a and b
float XSquared = pow(X, 2);
float ZSquared = pow(Z, 2);
// Calculate the sum of squares
float sumOfSquares = XSquared + ZSquared;
// Calculate the square root of the sum
float c = sqrt(sumOfSquares);
return c;
}
For positions like (X=75, Z=90), the arm moves accurately.
For positions like (X=105, Z=90) or (X=3, Z=2), it fails or behaves oddly.
No compile errorsājust strange behaviour or the arm just folds in on its self
Questions
Am I missing an angle offset or servo range issue?
Could this be due to acos() receiving a value outside [-1,1]?
I'm very happy to provide more information Like test values if needed. This is my first IK project, and first forum post so any help would be really appreciated.
Please edit your first post here (DON'T add another one!) and, as the forum rules require, enclose your code inside "CODE" tags. Without it, the code is almost unreadable and not very usable for testing your code and helping you.
Use the small pencil button below your post to edit it, select the code section and press the <CODE/> button, then save it.
Thanks.
Please post your full sketch, using code tags when you do
Posting your code using code tags prevents parts of it being interpreted as HTML coding and makes it easier to copy for examination
In my experience the easiest way to tidy up the code and add the code tags is as follows
Start by tidying up your code by using Tools/Auto Format in the IDE to make it easier to read. Then use Edit/Copy for Forum and paste what was copied in a new reply. Code tags will have been added to the code to make it easy to read in the forum thus making it easier to provide help.
It is also helpful to post error messages in code tags as it makes it easier to scroll through them and copy them for examination
Hey sorry about that one mate hope I have fixed it enough but the link u provide wouldnāt let me view it so I hope what I have change has made it a little bit more useable
Are you sure that both servos can turn all the way to 0 (zero) and all the way to 180 degrees?
Here's a simple test sketch.
/*
Connect servo to pin 9 on UNO or Nano.
Try this test sketch with the Servo library to see how your
servo responds to different settings, type a position
(0 to 180) or if you type a number greater than 180 it will be
interpreted as microseconds(544 to 2400), in the top of serial
monitor and hit [ENTER], start at 90 (or 1472) and work your
way toward zero (544) 5 degrees (or 50 micros) at a time, then
toward 180 (2400).
*/
#include <Servo.h>
Servo servo;
void setup() {
// initialize serial:
Serial.begin(9600); // set serial monitor baud rate to match
// set serial monitor line ending to "NewLine"
servo.write(90);
servo.attach(9);
prntIt();
}
void loop() {
// if there's any serial available, read it:
while (Serial.available() > 0) {
// look for the next valid integer in the incoming serial stream:
int pos = Serial.parseInt();
if(Serial.read() == '\n'){} //skip 1 second delay
pos = constrain(pos, 0, 2400);
servo.write(pos);
prntIt();
}
}
void prntIt()
{
Serial.print(" degrees = ");
Serial.print(servo.read());
Serial.print("\t");
Serial.print("microseconds = ");
Serial.println(servo.readMicroseconds());
}
I am trying to find the green angels to write to the servos and am not to sure of the correct formula to do that where the
float numerator = (100 * 100 + C * C - 100 * 100);
came from witch is the Law of Cosines but looking back it was a bit dumb because as people have pointed out it just equals C
so I donāt think that pythagorizes is what the problem is but I could be missing something
But if you have a way to make the 2 reds and and blue line into 2 right angel triangles so it can find that angels. Thatās seems to be the way I've been finding how most people do it but cant find any source codeš ā¦.
Regards.
but @xfpd Made a really good point I am going to spend the day trouble shooting with that for now
You should label all sides and vertices of all triangles.
Triangle (C EL SH) has two equal sides. Find isosceles triangle theorem to find angles... AND... all three sides of the lower triangle are known (C 75 90)... you can calculate all remaining angles of (C SH EL).
below your post to edit it, select the code section and press the 
