Robotics

/*
 * To change this license header, choose License Headers in Project Properties.
 * To change this template file, choose Tools | Templates
 * and open the template in the editor.
 */
package robotkinematics;

/**
 *
 * @author Wolf
 */
import java.util.Scanner;

public class forwardKin {
    static int x;
    public static double theta = 0;
    public static double alpha = 0;
    public static double a = 0;
    public static double d = 0;

    public static String vT;
    public static String vA;
    public static String vR;
    public static String vD;

    public static double product[][] = new double[4][4];
    public static String varPro[][] = new String[4][4];


   public static double[][] transform(double theta, double d, double a, double alpha){

     double Tran[][] = {
            {Math.cos(theta), -Math.sin(theta) * Math.cos(alpha), Math.sin(theta)* Math.sin(alpha), a*Math.cos(theta)},
            {Math.sin(theta), Math.cos(theta)*Math.cos(alpha), - Math.cos(theta)*Math.sin(alpha), Math.sin(theta)*a},
            {0, Math.sin(alpha), Math.cos(alpha), d},
            {0, 0, 0, 1}

        };
     return Tran;
}


    public static double[][] dhMatrix(int y){
    double dh[][] = new double[x][4];
        Scanner sc=new Scanner(System.in);
    System.out.println("Enter the values of the DH Matrix!");
        for (int i = 0; i<y; i++){
        for(int j = 0; j<4; j++){
            dh[i][j]=sc.next.Double();
        }
      }
        return dh;
    }

    public static double[][] fk(int y, double dh[][]){
        double arr[][] = new double[4][4];
        double temp[][] = new double[4][4];
        for(int i = 0; i < y; i++){

        theta = dh[i][0];
        d = dh[i][1];
        a = dh[i][2];
        alpha = dh[i][3];
        arr = transform(theta, d, a, alpha);
        temp = arr;
        if(i != 0){
        arr = multiply(temp, arr);
        }
       }


        return arr;
    }
    public static double[][] multiply(double arr1[][], double arr2[][]){
    for (int i = 0; i < 4; i++)
        {
            for (int j = 0; j < 4; j++)
            {
                for (int k = 0; k < 4; k++)
                {
                    product[i][j] = product[i][j] + arr1[i][k] * arr2[k][j];
                }
            }
        }
       return product;
    }
    public static void main(String args[]){
       Scanner sc1=new Scanner(System.in);
       System.out.println("Enter the number of joints!");
       System.out.println("==========================================================================");
       x = sc1.nextInt();
       Scanner sc=new Scanner(System.in);
       System.out.println("Press 1 for Variable Values and 2 for Numerical Values!");
       System.out.println("==========================================================================");

        if(sc.nextInt() == 2){
            //dhMatrix(x);
            double T[][] =  fk(x, dhMatrix(x));
             System.out.println("The Transformation Matrix for variable Joint Angles is: ");
              System.out.println("==========================================================================");
            for(int i = 0; i < 4; i++){
            for(int j = 0; j < 4; j++){
            System.out.print(T[i][j] + ", ");
        }
            System.out.println("");
        }


        }
        else{
            //varDHMatrix(x);
            String VT[][] = varFK(x, varDHMatrix(x));
             for(int i = 0; i < 4; i++){
            for(int j = 0; j < 4; j++){
            System.out.print(VT[i][j] + ", ");
        }
            System.out.println("");
        }
             System.out.println("INVERSE KINEMATICS");
             System.out.println("==========================================================================");

             System.out.println("Insert The Position Vector of The end-effector to calculate the joint angles!");

             Scanner sc4=new Scanner(System.in);
             double x=0,y=0,z=0;
             for(int i = 0; i<3; i++){
               if(i == 0){
                  System.out.println("The value of x: ");
                  x = sc4.nextDouble();
               }
               else if(i == 1){
                  System.out.println("The value of y: ");
                  y = sc4.nextDouble();
               }
               else if(i == 2){
                  System.out.println("The value of z: ");
                  z = sc4.nextDouble();
               }

             }
             String pos[][]=setPosition(x,y,z);
              for(int i = 0; i<4; i++){
                   for(int j = 0; j<4; j++){
                       System.out.print(pos[i][j] + ", ");
                   }
                   System.out.println("");
              }
              String arra[]= equalize(pos, VT);
                for(int j = 0; j<4; j++){
                       System.out.print(arra[j] + ", ");
                       System.out.println("");
                }
         System.out.println("For a SCARA robot: ");
         SCARA();
        }

    }
    public static String[][] varFK(int y, String varDh[][]){

        String arr[][] = new String[4][4];
        String temp[][] = new String[4][4];
        for(int i = 0; i < y; i++){

        vT = varDh[i][0];
        vD = varDh[i][1];
        vR = varDh[i][2];
        vA = varDh[i][3];
        arr = var_transform(vT, vD, vR, vA);
        temp = arr;
        if(i>0){

        arr = varMultiply(temp, arr);
        }
       }

        return arr;

    }

    public static String[][] var_transform(String vT, String vD, String vR, String vA){
//        String var_tra[][] = new String[4][4];
        String var_tra[][] = {
                       {"cos("+vT+")", "-cos("+vA+ ")*sin(" +vT +")","sin("+vT+")*sin("+vA+")","cos("+vT+")*" +vR},
                       {"sin("+vT+")", "cos("+vT+")*cos("+vA+")", "-cos("+vT+")*sin("+vA+")" ,"sin("+vT+")*"+vR},
                       {"0", "sin{"+vA+")", "cos("+vA+")", vD},
                       {"0", "0", "0", "1"}
                       };

        return var_tra;
    }
    public static String[][] varMultiply(String arr1[][], String arr2[][]){


       for (int i = 0; i < 4; i++)
        {
            for (int j = 0; j < 4; j++)
            {
                for (int k = 0; k < 4; k++)
                {

                    varPro[i][j] = varPro[i][j] + "+" + arr1[i][k] + "*" + arr2[k][j];

                }


            }

        }
       return varPro;

    }

    public static String[][] varDHMatrix(int y){
        String [][] varDh = new String[y][4];
        Scanner sc=new Scanner(System.in);
        System.out.println("Enter the values of the DH Matrix with variable joint angles!");
        for (int i = 0; i< y; i++){
        for(int j = 0; j<4; j++){
            varDh[i][j]=sc.next();
        }
      }
        return varDh;
    }
    public static String[][] setPosition(double x, double y, double z){
    String position[][] = {{"1","0","0","x"},{"0","1","0","y"},{"0","0","1","z"},{"0","0","0","1"}};
    return position;
    }
    public static String[] equalize(String arr[][],String tra[][]){
        String xa[] = new String[4];
        for(int j = 0; j<4; j++){
            xa[j] = arr[j][3] + "=" + tra[j][3];
        }
        return xa;
    }
    public static void SCARA(){
    double theta[] = new double [6];
    double alpha[] = new double[4];
    double d[] = new double[4];
    double a[] = new double[4];
    Scanner sc7 = new Scanner(System.in);
    System.out.println("Enter values for W");
    for(int i =0;i<6;i++)
      {
        System.out.print("w"+(i+1) + ":");
        theta[i] = sc7.nextDouble();
        System.out.println();
      }

    System.out.println("Enter values for joint distance");
    for(int i = 0;i<4;i++)
      {
        System.out.print("d"+(i+1)+ ":");
        d[i] = sc7.nextDouble();
        System.out.println();
      }

    System.out.println("Enter values for link length");
    for(int i = 0;i<4;i++)
      {
        System.out.print("a"+(i+1)+ ":");
        a[i] = sc7.nextDouble();
        System.out.println();
      }

alpha[2] = d[0]-theta[2]-d[3];
double t11 = Math.pow(theta[0],2)+Math.pow(theta[1],2)-Math.pow(a[0], 2)-Math.pow(a[1], 2);
double t12 = 2*a[0]*a[1];
alpha[1] = Math.toDegrees(Math.acos(t11/t12));

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

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

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

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