IK Function in DoF Arm

Question

I'm working on a 6-DOF robotic arm project, but the inverse kinematics (IK) code in my `control.ino` file doesn't work as expected for controlling the arm in the 3D x-y-z plane. The code is designed for a 3R planar arm with three joints (link lengths l1 = 100 mm, l2 = 170 mm, l3 = 130 mm) and takes x, y, and phi (end effector orientation) as inputs. However, my physical 6-DOF arm requires positioning in 3D space (x, y, z) with six joint angles, and the current IK function only computes three angles, causing joints 3 and 6 to move unpredictably (from 0 to ~0.9 and ~0.3 radians, respectively) when set to a home position of 0. Additionally, I can only control a linear joint (if present), while the other joints don’t respond correctly, likely because the IK function doesn't account for the full 6-DOF kinematics or the arm’s 3D configuration. I tried increasing servo force, but it didn’t help, as the issue seems to be with the IK algorithm not matching my arm’s setup. Can someone review my code and help fix the IK function to properly handle 6-DOF motion in the x-y-z plane, ensuring all joints move to the correct angles?

My code is:

#include <Servo.h>
#include <math.h>
Servo joint1Servo;
Servo joint2Servo;
Servo joint3Servo;
const float l1 = 100.0;
const float l2 = 170.0;
const float l3 = 130.0;
float joint1 = 0.0;
float joint2 = 0.0;
float joint3 = 0.0;
void setup() {
 
 joint1Servo.attach(9); // Base rotation
 joint2Servo.attach(10); // Second joint
 joint3Servo.attach(11); // Third joint
 
 // Initialize serial communication
 Serial.begin(9600);
 Serial.println("Enter target x,y,phi (in degrees):");
 
 // Set initial servo positions
 joint1Servo.write(joint1);
 joint2Servo.write(joint2);
 joint3Servo.write(joint3);
}
bool inverseKinematics(float x, float y, float phi, float &theta1, float &theta2, float &theta3, bool elbowDown = true) {
 float phi_rad = phi * PI / 180.0;
 float x_w = x - l3 * cos(phi_rad);
 float y_w = y - l3 * sin(phi_rad);
 float d2 = x_w * x_w + y_w * y_w;
 if (d2 == 0) return false;
 float d = sqrt(d2);
 float cos_theta2 = (d2 - l1 * l1 - l2 * l2) / (2.0 * l1 * l2);
 if (abs(cos_theta2) > 1) return false;
 
 float theta2_rad = acos(cos_theta2);
 if (!elbowDown) theta2_rad = -theta2_rad;
 
 
 float sin_theta2 = sin(theta2_rad);
 float cos_theta2 = cos(theta2_rad);
 float theta1_rad = atan2(y_w, x_w) - atan2(l2 * sin_theta2, l1 + l2 * cos_theta2);
 
 
 float theta3_rad = phi_rad - theta1_rad - theta2_rad;
 
 
 theta1 = theta1_rad * 180.0 / PI;
 theta2 = theta2_rad * 180.0 / PI;
 theta3 = theta3_rad * 180.0 / PI;
 
 
 if (theta1 < 0 || theta1 > 180 || theta2 < 0 || theta2 > 180 || theta3 < 0 || theta3 > 180) {
 return false;
 }
 
 return true;
}
void loop() {
 
 if (Serial.available() > 0) {
 
 String input = Serial.readStringUntil('\n');
 float x, y, phi;
 
 
 int comma1 = input.indexOf(',');
 int comma2 = input.indexOf(',', comma1 + 1);
 if (comma1 != -1 && comma2 != -1) {
 x = input.substring(0, comma1).toFloat();
 y = input.substring(comma1 + 1, comma2).toFloat();
 phi = input.substring(comma2 + 1).toFloat();
 
 
 float newJoint1, newJoint2, newJoint3;
 if (inverseKinematics(x, y, phi, newJoint1, newJoint2, newJoint3)) {
 
 joint1 = newJoint1;
 joint2 = newJoint2;
 joint3 = newJoint3;
 
 
 joint1Servo.write(joint1);
 joint2Servo.write(joint2);
 joint3Servo.write(joint3);
 
 
 Serial.print("Moving to x=");
 Serial.print(x);
 Serial.print(", y=");
 Serial.print(y);
 Serial.print(", phi=");
 Serial.print(phi);
 Serial.print(" | Joint1=");
 Serial.print(joint1);
 Serial.print(", Joint2=");
 Serial.print(joint2);
 Serial.print(", Joint3=");
 Serial.println(joint3);
 } else {
 Serial.println("No valid solution for given position and orientation!");
 }
 } else {
 Serial.println("Invalid input format! Use: x,y,phi");
 }
 }
}

• pptaszni: "likely because the IK function doesn't account for the full 6-DOF kinematics or the arm’s 3D configuration" - well obviously. You are controlling only 3 joints, so you can't control more than 3 cartesian coordinates independently. Drive your 6dof arm to the "flat" position, i.e. everything aligned to resemble your previous 3dof robot, then pretend it's a 3 dof arm. Otherwise write new inverse kinematics function for your 6 dof. You can use Denavit-Hartenberg