In this post, we implement Direct Kinematics (Forward Kinematics) for a SCARA robot in Java using the Denavit-Hartenberg (DH) convention. Direct kinematics determines the position and orientation of the robot’s end-effector (tool tip) given the joint angles and link parameters. This is the fundamental calculation in robot arm control and simulation.
What is Direct Kinematics?
For a multi-joint robot arm, each link between joints is described by four DH parameters:
- θ (theta) — Joint angle (rotation about Z-axis)
- d — Link offset (translation along Z-axis)
- a — Link length (translation along X-axis)
- α (alpha) — Link twist (rotation about X-axis)
For each link, a 4×4 General Link Coordinate Transformation Matrix (GLCTM) is computed from its DH parameters. The final end-effector pose is obtained by multiplying all the link matrices together: T04 = T01 × T12 × T23 × T34.
Java Code Implementation
// ============================================================
// Direct Kinematics for SCARA Robot — Java Implementation
// Uses Denavit-Hartenberg (DH) convention.
// Computes end-effector transformation matrix T04 = T01*T12*T23*T34
// ============================================================
import java.io.*;
public class DirectKinematics {
// Computes the 4x4 General Link Coordinate Transformation Matrix (GLCTM)
// for a single link using the DH parameters.
// Parameters:
// theta (t) - joint angle in degrees
// d - link offset
// a - link length
// alpha (al) - link twist in degrees
public static double[][] computeGLCTM(double theta, double d, double a, double alpha) {
// Convert angles from degrees to radians
double t = theta * Math.PI / 180;
double al = alpha * Math.PI / 180;
double[][] glctm = new double[4][4];
// Row 0: [cos(t), -sin(t)*cos(al), sin(t)*sin(al), a*cos(t)]
glctm[0][0] = Math.cos(t);
glctm[0][1] = -Math.sin(t) * Math.cos(al);
glctm[0][2] = Math.sin(t) * Math.sin(al);
glctm[0][3] = a * Math.cos(t);
// Row 1: [sin(t), cos(t)*cos(al), -sin(al)*cos(t), a*sin(t)]
glctm[1][0] = Math.sin(t);
glctm[1][1] = Math.cos(t) * Math.cos(al);
glctm[1][2] = -Math.sin(al) * Math.cos(t);
glctm[1][3] = a * Math.sin(t);
// Row 2: [0, sin(al), cos(al), d]
glctm[2][0] = 0;
glctm[2][1] = Math.sin(al);
glctm[2][2] = Math.cos(al);
glctm[2][3] = d;
// Row 3 (homogeneous row): [0, 0, 0, 1]
glctm[3][0] = 0;
glctm[3][1] = 0;
glctm[3][2] = 0;
glctm[3][3] = 1;
return glctm;
}
// Multiplies two 4x4 matrices and returns the result
public static double[][] multiplyMatrices(double[][] m, double[][] n) {
double[][] result = new double[4][4];
for (int i = 0; i < 4; i++) {
for (int j = 0; j < 4; j++) {
for (int k = 0; k < 4; k++) {
result[i][j] += m[i][k] * n[k][j];
}
}
}
return result;
}
public static void main(String[] args) throws IOException {
BufferedReader obj = new BufferedReader(new InputStreamReader(System.in));
// ----- INPUT: Joint angles (theta values) -----
// SCARA has 4 joints: q1, q2, q3 (fixed=0), q4
double[] q = new double[4];
System.out.println("Enter joint angle q1 (degrees):");
q[0] = Double.parseDouble(obj.readLine());
System.out.println("Enter joint angle q2 (degrees):");
q[1] = Double.parseDouble(obj.readLine());
q[2] = 0; // q3 is fixed at 0 for SCARA
System.out.println("Enter joint angle q4 (degrees):");
q[3] = Double.parseDouble(obj.readLine());
// ----- INPUT: Link offsets (d values) -----
double[] d = new double[4];
System.out.println("Enter link offset d1:");
d[0] = Double.parseDouble(obj.readLine());
d[1] = 0; // d2 = 0
System.out.println("Enter link offset d3:");
d[2] = Double.parseDouble(obj.readLine());
System.out.println("Enter link offset d4:");
d[3] = Double.parseDouble(obj.readLine());
// ----- INPUT: Link lengths (a values) -----
double[] a = new double[4];
System.out.println("Enter link length a1:");
a[0] = Double.parseDouble(obj.readLine());
System.out.println("Enter link length a2:");
a[1] = Double.parseDouble(obj.readLine());
a[2] = 0; // a3 = 0
a[3] = 0; // a4 = 0
// ----- FIXED: Link twists (alpha values) for SCARA -----
// alpha1 = 180 (links 1 and 2 are antiparallel), rest = 0
double[] al = {180, 0, 0, 0};
// ----- COMPUTE: Individual link transformation matrices -----
double[][] t01 = computeGLCTM(q[0], d[0], a[0], al[0]); // Base to Link 1
double[][] t12 = computeGLCTM(q[1], d[1], a[1], al[1]); // Link 1 to Link 2
double[][] t23 = computeGLCTM(q[2], d[2], a[2], al[2]); // Link 2 to Link 3
double[][] t34 = computeGLCTM(q[3], d[3], a[3], al[3]); // Link 3 to End-Effector
// ----- COMPUTE: Final transformation T04 = T01 * T12 * T23 * T34 -----
double[][] t04 = multiplyMatrices(t01, t12);
t04 = multiplyMatrices(t04, t23);
t04 = multiplyMatrices(t04, t34);
// ----- OUTPUT: Print the final 4x4 transformation matrix -----
System.out.println("\nEnd-Effector Transformation Matrix T04:");
for (int i = 0; i < 4; i++) {
for (int j = 0; j < 4; j++) {
System.out.printf("%-25s", t04[i][j]);
}
System.out.println();
}
}
}
Explanation of the Code
computeGLCTM()— Computes the 4×4 DH transformation matrix for one robot link. The matrix encodes both rotation (3×3 top-left block) and translation (rightmost column). The angles are converted from degrees to radians before callingMath.cos()andMath.sin().multiplyMatrices()— Standard 4×4 matrix multiplication using three nested loops. The result ofT01 × T12 × T23 × T34gives the full transformation from the robot base frame to the end-effector frame.- SCARA-specific constants — For a SCARA robot, joint 3 angle (
q3 = 0), link lengthsa3 = a4 = 0, offsetd2 = 0, and twistsal[0] = 180°(links 1 and 2 are antiparallel) andal[1..3] = 0°are fixed by the robot’s geometry. - Output Matrix — The resulting 4×4 matrix T04 encodes the complete pose of the end-effector. The top-left 3×3 block is the rotation matrix (orientation), and the top-right 3×1 column is the position vector [x, y, z] of the end-effector.
Sample Input
q1 = 45 degrees
q2 = -60 degrees
q4 = 90 degrees
d1 = 877 mm
d3 = 120 mm
d4 = 200 mm
a1 = 425 mm
a2 = 375 mm
Sample Output
End-Effector Transformation Matrix T04:
0.9659 0.2588 8.66E-17 203.46
0.2588 -0.9659 -8.66E-17 662.74
6.12E-17 1.06E-16 -1.0 557.0
0.0 0.0 0.0 1.0
Step-by-Step Output Explanation
- Position (right column, rows 0–2): The end-effector is located at approximately
x = 203.46 mm,y = 662.74 mm,z = 557.0 mmfrom the robot base. These values match the expected forward kinematics for the given joint angles and link lengths. - Rotation (top-left 3×3 block): The values in the rotation submatrix encode the orientation of the end-effector. The near-zero values (e.g.,
8.66E-17) are floating-point rounding artifacts — they are effectively zero. - Homogeneous row: The bottom row
[0, 0, 0, 1]is always preserved in a valid homogeneous transformation matrix.
See Also
Conclusion
Direct kinematics maps from joint space (angles) to Cartesian space (end-effector position and orientation). The DH convention provides a systematic way to build the transformation matrices for any serial robot arm. The output matrix T04 is the key result — its position column directly gives you where the end-effector is in 3D space. The complementary problem — given a desired end-effector position, find the required joint angles — is called Inverse Kinematics, which is covered in the next post.
This content is really interesting. I have bookmarked it.
Do you allow guest posting on your website ?
I can write high quality posts for you. Let me know.