Implementation of Inverse Kinematics in Java

import java.io.*;

class IK
{
	publicstaticvoid main(String args[]) throwsIOException{
	
	BufferedReader in = newBufferedReader(newInputStreamReader(System.in));
	double w[] = newdouble [6]; 
	double a[] = newdouble[4];
	double d[] = newdouble[4];
	double q[] = newdouble[4];
		
	System.out.println("Enter values for W");
	for(inti =0;i<6;i++)
	  {
		System.out.print("w"+(i+1) + ":");
		w[i] = Double.parseDouble(in.readLine());
		System.out.println();
	  }
	
	System.out.println("Enter values for joint distance");
	for(inti = 0;i<4;i++)
	  {
		System.out.print("d"+(i+1)+ ":");
		d[i] = Double.parseDouble(in.readLine());
		System.out.println();
	  }
	
	System.out.println("Enter values for link length");
	for(inti = 0;i<4;i++)
	  {
		System.out.print("a"+(i+1)+ ":");
		a[i] = Double.parseDouble(in.readLine());
		System.out.println();
	  }
	
q[2] = d[0]-w[2]-d[3];
double t11 = Math.pow(w[0],2)+Math.pow(w[1],2)-Math.pow(a[0], 2)-Math.pow(a[1], 2);
double t12 = 2*a[0]*a[1];
q[1] = Math.toDegrees(Math.acos(t11/t12));

double t21 = a[1]*Math.sin(Math.toRadians(q[1]))*w[0];
double t22 = (a[0]+(a[1]*Math.sin(Math.toRadians(q[1]))))*w[1];

double t31 = (a[0]+a[1]*Math.cos(Math.toRadians(q[1])))*w[0];
double t32 = (a[1]*Math.sin(Math.toRadians(q[1])))*w[1];

q[0] = Math.toDegrees(Math.atan((t21+t22)/(t31-t32)));
q[3] = Math.PI*Math.log(Math.abs(w[5]));

System.out.println("The I.K/ of SCARA is " +q[0]+" " +q[1]+" "+q[2]+" "+q[3]); 



	}
}


/*OUTPUT:
Enter values for W
w1:203.4

w2:662.7

w3:557

w4:0

w5:0

w6:-1.649

Enter values for joint distance
d1:877

d2:0

d3:0

d4:200

Enter values for link length
a1:425

a2:375

a3:0

a4:0

The I.K/ of SCARA is -80.84850082995715 60.01704951685887 120.0 1.5713273928559872
*/

Leave a Reply

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