Unexpected solution for the 2DOF arm

[xchg-dot-ca]

Including modified example running on Leonardo
Putting 2DOF arm in Г position ( no actuators, just printing result data on Serial Monitor )
Shoulder To Elbow pointing straight up and Elbow to Actuator completely horizontal.
I expect solution angles to be 0, 90 not 90,-90
Either I'm not understanding something or there is an error in the code.

`/********************************************************

• FABRIK2D 2DOF example
• Creating the FABRIK object and moving the end effector up and down.
• You can use whichever unit you want for coordinates, lengths and tolerances as long as you are consistent.
• Default unit is millimeters.
  ********************************************************/

#include <FABRIK2D.h>

int lengths[] = {225, 150}; // 2DOF arm where shoulder to elbow is 225mm and elbow to end effector is 150mm.
Fabrik2D fabrik2D(3, lengths); // This arm has 3 joints; one in the origin, the elbow and the end effector.

float y = 0;
int toggle_y = 0;

void setup() {
Serial.begin(9600);

Serial.print("ang0");
Serial.print("\t");
Serial.print("ang1");
Serial.print("\t");
Serial.print("x0");
Serial.print("\t");
Serial.print("y0");
Serial.print("\t");
Serial.print("x1");
Serial.print("\t");
Serial.print("y1");
Serial.print("\t");
Serial.print("x2");
Serial.print("\t");
Serial.println("y2");

// Set tolerance to 0.5mm. If reachable, the end effector will approach
// the target with this tolerance
fabrik2D.setTolerance(0.5);
}

void loop() {

// Solve inverse kinematics given the coordinates x and y and the list of lengths for the arm.
fabrik2D.solve(150,225,lengths);

// Angles are printed in degrees.
// The function calls below shows how easy it is to get the results from the inverse kinematics solution.
Serial.print(fabrik2D.getAngle(0)* 57296 / 1000);
Serial.print("\t");
Serial.print(fabrik2D.getAngle(1)* 57296 / 1000);
Serial.print("\t");
Serial.print(fabrik2D.getX(0));
Serial.print("\t");
Serial.print(fabrik2D.getY(0));
Serial.print("\t");
Serial.print(fabrik2D.getX(1));
Serial.print("\t");
Serial.print(fabrik2D.getY(1));
Serial.print("\t");
Serial.print(fabrik2D.getX(2));
Serial.print("\t");
Serial.println(fabrik2D.getY(2));

delay(500);
}`

[henriksod]

Hello, the second angle, which is -90 degrees, is in fact correct for this configuration. Please refer to the following image.

As for the first angle, I am not sure. Could you try subtracting 90 from fabrik2D.getAngle(0)* 57296 / 1000? Does this fix the problem?

By the way, this is how the angles should be used:

// Get the angles (in radians [-pi,pi]) and convert them to degrees [-180,180]
  int shoulderAngle = fabrik2D.getAngle(0)* 57296 / 1000; // In degrees
  int elbowAngle = fabrik2D.getAngle(1)* 57296 / 1000; // In degrees
  
  // Write to the servos with limits, these will probably not be the same
  // for your manipulator and will have to be changed depending on your
  // setup. Since the library may output negative angles, it is important
  // to apply limits before sending the angles to the servos!
  shoulder.write(min(180, max(0, shoulderAngle + 180/2)));
  elbow.write(min(180, max(0, elbowAngle + 180/2))); 

[xchg-dot-ca]

Thank you for the reply.

I'm afraid I do no see how this image explains that -90 is correct. If I would ask to solve
(-150, 225) may be. But judging by how position of the joints in the image - all phi angles are positive, hence I assume that 90 deg is what I would get when solving (150, 225 )

Or lets do different example:
lets solve ( 0, 375 ) which mean my virtual actuator pointing straight up. I assume both joints have to be at 0 degrees right ? Well I getting solution 90, 0 degrees.

Oh, looks like solving ( 375, 0 ) confusing the library.... probably it does not like Y to be 0.

Thank you for the example with servo angles!
In my case I will not send angles to servos which are outside the servo boundary, I would deem them as failed solutions.

Curios if you have plans to add boundaries to the solver, to make sure it operates withing the desired range?

[henriksod]

Hello, so I realized now that this image may be a bit confusing, since the joint can be facing both directions. The intention is that the joint faces towards you, which means that from the line that intersects the joint, parallel to the previous link, the angle increases when you rotate left and decreases when you rotate right.

The example with servo angles is in the Usage Example in the README.md file.

I will think about adding possibility to add boundaries in the solver, but keep in mind that I will not add something that limits the usage of the library. Servos ar not all 0 to 180 and I do not want to limit my library to this.

Negative angles are unavoidable in mathematics! You will have to do the conversion yourself, surely you understand this. This library is not intended only for servos, so therefore I will not adapt it to output angles within 0 to 180 degrees. -pi to pi is the universal boundary for robotic arms.

And again, as for the first angle, it was a long time ago I used this library and I have added to the documentation since then. Of course, the correct solution is that when pointing straight up, the first joint should be 0 degrees. If you can do the analysis, I can help you much faster. I have very limited time so I do not have time to investigate this right now.

Remember that you can always contribute if you feel that something has to be fixed or updated.

[henriksod]

Hi, please see the README.md, I added a new chapter called "Servo Orientation".

[xchg-dot-ca]

Hi Henrik,

Thank you for the update. Getting back to my robotic project.
I hope this time I will finish to some level :)

[github-actions]

Stale issue message

[henriksod]

Closing the issue due to no activity, please try with the latest version and reopen if needed.