# Implementing Direct Kinematics in Java

```import java.io.*;
class DK
{
public static double[][] calculate_glctm(double t, double d, double a, double al)
{
t = t * Math.PI/180;
al = al * Math.PI/180;
double glctm[][] = new double[4][4];
glctm[0][0] = Math.cos(t);
glctm[0][1] = -1 * Math.sin(t) * Math.cos(al);
glctm[0][2] = Math.sin(t) * Math.sin(al);
glctm[0][3] = a * Math.cos(t);
glctm[1][0] = Math.sin(t);
glctm[1][1] = Math.cos(t) * Math.cos(al);
glctm[1][2] = -1 * Math.sin(al) * Math.cos(t);
glctm[1][3] = a * Math.sin(t);
glctm[2][0] = 0;
glctm[2][1] = Math.sin(al);
glctm[2][2] = Math.cos(al);
glctm[2][3] = d;
glctm[3][0] = 0;
glctm[3][1] = 0;
glctm[3][2] = 0;
glctm[3][3] = 1;
return glctm;
}

public static double[][] matrix_mul(double m[][], double n[][])
{
double p[][] = new double[4][4];
int i,j,k;
for(i=0;i<4;i++)
{
for(j=0;j<4;j++)
{
for(k=0;k<4;k++)
{
p[i][j] += m[i][k] * n[k][j];
}
}
}
return p;
}

public static void main(String Args[])throws IOException
{
int i,j,k;

//for theta k
double q[] = new double[4];
System.out.println("Enter value of q1:");
System.out.println("Enter value of q2:");
q[2] = 0;
System.out.println("Enter value of q4:");

//for dk
double d[] = new double[4];
System.out.println("Enter value of d1:");
d[1] = 0;
System.out.println("Enter value of d3:");
System.out.println("Enter value of d4:");

//for ak
double a[] = new double[4];
System.out.println("Enter value of a1:");
System.out.println("Enter value of a2:");
a[2] = 0;
a[3] = 0;

//for alpha
double al[] = new double[4];
al[0] = 180;
al[1] = 0;
al[2] = 0;
al[3] = 0;

//generate glctm
double t01[][] = new double[4][4];
double t12[][] = new double[4][4];
double t23[][] = new double[4][4];
double t34[][] = new double[4][4];

t01 = calculate_glctm(q[0],d[0],a[0],al[0]);
t12 = calculate_glctm(q[1],d[1],a[1],al[1]);
t23 = calculate_glctm(q[2],d[2],a[2],al[2]);
t34 = calculate_glctm(q[3],d[3],a[3],al[3]);

//calculate final
double t04[][] = new double[4][4];
t04 = matrix_mul(t01,t12);
t04 = matrix_mul(t04,t23);
t04 = matrix_mul(t04,t34);

//display op
for(i=0;i<4;i++)
{
for(j=0;j<4;j++)
{
System.out.print(t04[i][j]+"\t");
}
System.out.println();
}
}
}

/* OUTPUT

c:\jdk1.3\bin\java.exe   DK
Working Directory - C:\Documents and Settings\student\Desktop\
Class Path - .;c:\KawaEnt5.0\kawaclasses.zip;c:\jdk1.3\lib\tools.jar;c:\jdk1.3\jre\lib\rt.jar;c:\jdk1.3\jre\lib\i18n.jar
Enter value of q1:
45
Enter value of q2:
-60
Enter value of q4:
90
Enter value of d1:
877
Enter value of d3:
120
Enter value of d4:
200
Enter value of a1:
425
Enter value of a2:
375
0.9659258262890684	0.2588190451025207	8.659274570719354E-17	203.4632400908375
0.2588190451025207	-0.9659258262890684	-8.659274570719355E-17	662.7425668626834
6.123031769111886E-17	1.0605402120460133E-16	-1.0	557.0
0.0	0.0	0.0	1.0
Process Exit...

*/
```

## One thought on “Implementing Direct Kinematics in Java”

1. 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.

This site uses Akismet to reduce spam. Learn how your comment data is processed.