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
	{
		BufferedReader obj = new BufferedReader(new InputStreamReader(System.in));
		int i,j,k;
		
		//for theta k
		double q[] = new double[4];
		System.out.println("Enter value of q1:");
		q[0] = Double.parseDouble(obj.readLine());
		System.out.println("Enter value of q2:");
		q[1] = Double.parseDouble(obj.readLine());
		q[2] = 0;
		System.out.println("Enter value of q4:");
		q[3] = Double.parseDouble(obj.readLine());
		
		//for dk
		double d[] = new double[4];
		System.out.println("Enter value of d1:");
		d[0] = Double.parseDouble(obj.readLine());
		d[1] = 0;
		System.out.println("Enter value of d3:");
		d[2] = Double.parseDouble(obj.readLine());
		System.out.println("Enter value of d4:");
		d[3] = Double.parseDouble(obj.readLine());
		
		//for ak
		double a[] = new double[4];
		System.out.println("Enter value of a1:");
		a[0] = Double.parseDouble(obj.readLine());
		System.out.println("Enter value of a2:");
		a[1] = Double.parseDouble(obj.readLine());
		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
		System.out.println("Answer");
		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
Answer
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.

Leave a Reply

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