Issues with angles of elbow joint.

[Pachalicioso]

I'm having problems using the "Solve2" method for a 3 DOF arm (rotating base, shoulder, elbow and wrist joints).
I wrote a code that allows me to manually input a set of XYZ coordinates, which then are inputs for the "Solve2" function, along with a tool angle, and the lengths of the links. The output angles, however, are incorrect, and I believe that the issue is my Elbow servo, which is basically upside down, so the angles are reversed, and it's also rotated 45°.

I tried calculating the final angle for the servo as "180-elbowAngle", but it doesn't seem to work.
This is the code I'm using:

#include <FABRIK2D.h>
#include <ESP32Servo.h>


///Lenght of links in mm (shoulder to elbow: 123, elbow to wrist: 121, wrist to end effector: 125///

int lengths[]={123, 121, 125};

Fabrik2D fabrik2D(4,lengths);

Servo base, shoulder, elbow, wristTilt, wrist, claw;

//Coordinates for end effector, obtained from serial monitor 
int x, y, z;

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

  //servo pins
  base.attach(25);
  shoulder.attach(26);
  elbow.attach(27);
  wristTilt.attach(14);
  wrist.attach(12);
  claw.attach(13);
  

  fabrik2D.setTolerance(0.5);
  Serial.print("Enter 3 coordinates (in mm), evenly spaced:");
}

void loop() 
{
  
  if (Serial.available() > 0 )
  {
    //Get coordinates from serial monitor, and store in "x", "y" and "z" respectively
    String xyz = Serial.readStringUntil ('\n');
    xyz.trim();
    int coords = sscanf(xyz.c_str(), "%d %d %d", &x, &y, &z);
    if (coords == 3 )
    {
      //Print stored coordinates 
      Serial.print("\n");
      Serial.print("coordinates: ");
      Serial.print(x);
      Serial.print("\t");
      Serial.print(y);
      Serial.print("\t");
      Serial.print(z);
      Serial.print("\t");

      //Solve using "Solve2" method, with 3 coordinates, tool degree (in radians), and Lengths
      fabrik2D.solve2(x, y, z, 90*DEG_TO_RAD, lengths);

      ///Calculate angles for each servo in degrees
      int degB = fabrik2D.getBaseAngle() * RAD_TO_DEG; //Base servo
      int degS = fabrik2D.getAngle(0) * RAD_TO_DEG; //Shoulder servo
      int degE = 180-fabrik2D.getAngle(1) * RAD_TO_DEG; //Elbow servo is mirrored respect to shoulder servo. 
      int degW = fabrik2D.getAngle(2) * RAD_TO_DEG; //Wrist servo
      
      ///Print results in serial monitor///
      Serial.print("\n");
      Serial.print("Results: "); 
      Serial.print(min(180, max(0,degB)));
      Serial.print("\t");
      Serial.print(min(180, max(0,degS)));
      Serial.print("\t");
      Serial.print(min(180, max(0,degE - 45))); //Elbow is rotated 45°
      Serial.print("\t");
      Serial.print(min(180, max(0,degW)));
      Serial.print("\t");
      Serial.print("\n");


      ///Write angles to servos, restricting values between 0 - 180

      shoulder.write(min(180, max(0,degS)));
      elbow.write(min(180, max(0,degE - 45)));  //Elbow is rotated 45°
      wrist.write(min(180, max(0,degW))); 
      base.write(min(180, max(0, degB)));
        
    }
    else
    {
      
      Serial.print("Error, input not recognized. Please enter 3 coordinates (in mm), separated by a blank space: ");
      Serial.print("\n");
    }
  }
  
}

For reference, this is the arm I'm currently using https://www.printables.com/model/1008951-brazo-robotico-kuka-arduino
Is there a way to work around the inverted elbow servo? or is the issue elsewhere? please help.

[henriksod]

Hi, what does the robot arm look like when you apply the angles calculated by the solve2 function? What is incorrect about them?

Keep in mind that the angles are calculated based on the line along the previous link which intersects with the joint you want to rotate.

[Pachalicioso]

Hi, thanks for the reply.
After reading the comment you made on how the angle of the servos are calculated, I realized I was subtracting the wrong angles for the rotation of the elbow, so that's fixed!. Thanks for the help!!.
The servos act weird when you give a set of coordinates that's out of range, and the joints tend to get stuck in those cases, but I assume that setting some boundaries for the coordinates and the position of the joints should help with that.

Now I have a problem with the position of the end effector, because the part of the arm that reaches the coordinates is the wrist servo, and not the tip of the gripper (3rd link). Shouldn't the Solve2 function use the tip of the 3rd link for the position of the end effector?
For reference, this is the gripper of the arm I'm using:

[henriksod]

Hi, it depends how you set it up and which solve2 was called. Have you tried using the grip offset value?

[Pachalicioso]

I tried adding the gripping offset, but so far it only seems to affect the position in the Y axis.
This is how I set the Solve2 function, and the output I get for a set of coordinates.
The offset I used is the length from the wrist to the end of the claw, and I included a line to print the XYZ position of the elbow joint:

fabrik2D.solve2(x, y, z, 90*DEG_TO_RAD, 100, lengths);

//////////////Serial Monitor///////////////////

coordinates: 
X: 150	Y: 150	Z: 0	
Degrees: 
Base: 0	Shoulder: 30	Elbow: 142	Wrist: 157	

Elbow position: 
X:150.01	Y: -50.03	Z: 0.00

The X and Z coordinates are still being reached by the elbow joint, not the end of the claw.
I tried using a negative offset, and again the coordinates are reached by the joint, and not the claw.