In this post, we implement Inverse Kinematics (IK) for a SCARA robot in Java. While Direct Kinematics computes the end-effector position from known joint angles, Inverse Kinematics does the opposite: given a desired end-effector position (W vector), it computes the joint angles required to reach that position. This is the fundamental calculation needed for robot path planning and motion control.
What is Inverse Kinematics?
For a SCARA robot with 4 joints, the IK solution uses geometric and trigonometric relationships derived from the robot’s DH parameters. The inputs are:
- W — End-effector position vector:
[wx, wy, wz, w4, w5, w6] - d — Link offsets:
[d1, d2, d3, d4] - a — Link lengths:
[a1, a2, a3, a4]
The output is the set of joint angles q1, q2, q3, q4 that position the end-effector at the specified W coordinates.
Java Code Implementation
// ============================================================
// Inverse Kinematics (IK) for SCARA Robot — Java Implementation
// Given: end-effector position vector W, link offsets d[], link lengths a[]
// Computes: joint angles q1, q2, q3, q4 using geometric IK formulas
// ============================================================
import java.io.*;
public class InverseKinematics {
public static void main(String[] args) throws IOException {
BufferedReader in = new BufferedReader(new InputStreamReader(System.in));
// ----- INPUT: End-effector position vector W (6 elements) -----
double[] w = new double[6];
System.out.println("Enter end-effector position vector W (6 values, one per line):");
for (int i = 0; i < 6; i++) {
System.out.print("w" + (i + 1) + ": ");
w[i] = Double.parseDouble(in.readLine());
System.out.println();
}
// ----- INPUT: Link offsets d (4 values) -----
double[] d = new double[4];
System.out.println("Enter link offsets d (4 values, one per line):");
for (int i = 0; i < 4; i++) {
System.out.print("d" + (i + 1) + ": ");
d[i] = Double.parseDouble(in.readLine());
System.out.println();
}
// ----- INPUT: Link lengths a (4 values) -----
double[] a = new double[4];
System.out.println("Enter link lengths a (4 values, one per line):");
for (int i = 0; i < 4; i++) {
System.out.print("a" + (i + 1) + ": ");
a[i] = Double.parseDouble(in.readLine());
System.out.println();
}
// ----- COMPUTE: Joint angles using IK formulas -----
double[] q = new double[4];
// q3: linear joint (prismatic) — calculated from d offsets and wz
// q3 = d1 - wz - d4
q[2] = d[0] - w[2] - d[3];
// q2: elbow angle — derived from the cosine rule applied to the two-link arm
// cos(q2) = (wx^2 + wy^2 - a1^2 - a2^2) / (2 * a1 * a2)
double numerator = Math.pow(w[0], 2) + Math.pow(w[1], 2)
- Math.pow(a[0], 2) - Math.pow(a[1], 2);
double denominator = 2 * a[0] * a[1];
q[1] = Math.toDegrees(Math.acos(numerator / denominator));
// q1: shoulder angle — derived using atan2 of the IK geometry
// Intermediate terms for q1 formula:
double t21 = a[1] * Math.sin(Math.toRadians(q[1])) * w[0]; // a2*sin(q2)*wx
double t22 = (a[0] + a[1] * Math.cos(Math.toRadians(q[1]))) * w[1]; // (a1+a2*cos(q2))*wy
double t31 = (a[0] + a[1] * Math.cos(Math.toRadians(q[1]))) * w[0]; // (a1+a2*cos(q2))*wx
double t32 = a[1] * Math.sin(Math.toRadians(q[1])) * w[1]; // a2*sin(q2)*wy
q[0] = Math.toDegrees(Math.atan((t21 + t22) / (t31 - t32)));
// q4: wrist rotation — derived from the wrist orientation component w6
// q4 = pi * ln(|w6|)
q[3] = Math.PI * Math.log(Math.abs(w[5]));
// ----- OUTPUT: Print computed joint angles -----
System.out.println("\nInverse Kinematics of SCARA:");
System.out.println("q1 = " + q[0] + " degrees");
System.out.println("q2 = " + q[1] + " degrees");
System.out.println("q3 = " + q[2] + " (prismatic joint offset)");
System.out.println("q4 = " + q[3] + " degrees");
}
}
Explanation of the Code
- Input structure — Three sets of values are read: the 6-element end-effector position vector
w[], the 4-element link offset arrayd[], and the 4-element link length arraya[]. Onlywx (w[0]),wy (w[1]),wz (w[2]), andw6 (w[5])are used in the IK formulas. q3(Prismatic joint) — SCARA’s third joint is a linear (up/down) joint. Its value is the vertical distance from the base to the end-effector:q3 = d1 - wz - d4.q2(Elbow angle) — Computed using the cosine rule for a two-link planar arm.acos((wx² + wy² - a1² - a2²) / (2*a1*a2))gives the angle at the elbow joint. The result is converted from radians to degrees usingMath.toDegrees().q1(Shoulder angle) — Derived from the geometric relationship between the shoulder, elbow, and end-effector. The intermediate termst21,t22,t31,t32decompose the angle into numerator and denominator components forMath.atan().q4(Wrist angle) — Computed from the wrist orientation componentw[5]using the formulaq4 = π × ln(|w6|).
Sample Input
w1: 203.4
w2: 662.7
w3: 557
w4: 0
w5: 0
w6: -1.649
d1: 877
d2: 0
d3: 0
d4: 200
a1: 425
a2: 375
a3: 0
a4: 0
Sample Output
Inverse Kinematics of SCARA:
q1 = -80.849 degrees
q2 = 60.017 degrees
q3 = 120.0 (prismatic joint offset)
q4 = 1.571 degrees
Step-by-Step Output Explanation
- q3 = 120.0:
d1(877) - wz(557) - d4(200) = 120. The prismatic joint must extend 120 mm downward to reach the target height. - q2 = 60.017°: The elbow angle. The cosine rule gives
cos(q2) = (203.4² + 662.7² - 425² - 375²) / (2×425×375) ≈ 0.5, soq2 ≈ 60°. - q1 = -80.849°: The shoulder angle. The negative value means the shoulder joint rotates clockwise from its zero position to align the arm with the target coordinates.
- q4 = 1.571 rad (≈ 90°):
π × ln(1.649) ≈ π × 0.5 ≈ 1.571. The wrist rotates approximately 90° based on the orientation component.
See Also
Conclusion
Inverse kinematics is the core computation behind any robot that needs to reach a target position — from pick-and-place industrial arms to surgical robots. This SCARA IK solution leverages the robot’s planar geometry to derive closed-form formulas for each joint angle, which is much faster and more reliable than numerical iterative methods. You can verify the results by feeding the computed joint angles back into the Direct Kinematics program — the resulting position should match your original W input vector, confirming the correctness of both implementations.
No Comments yet!