cinematic.java

一个由c转成java的3D robot 仿真平台

/************************************************************************/
/* File: ~/sopra/RoboPackII/Cinematic.java                                */
/* This file consists of the methods the robot needs to calculate his	*/
/* new values								*/
/************************************************************************/

package RoboPackII;


final class CinematicCalculations
{
	/* This class provides the 'main class' with the needed        */
	/* calculation methods.					       */

	public static void TO6(AngleDates[] angles, double[] linkl, 
			       double[][] T)
	{
	   /* The 'angles'- and 'linkl'-arrays have to consist of 6    */
	   /* elements, the 'T'-array is a 3x4-array.		       */

	   double c12, s12;
 	   double[] s = new double[6];
	   double[] c = new double[6];
 	   double h1,h2;
 	   int i;

 	   for ( i=0 ; i<6 ; i++ )
  	     {
               c[i] = Math.cos( angles[i].act );
               s[i] = Math.sin( angles[i].act );
             }
 	   c12 = c[1] * c[2] - s[1] * s[2];
 	   s12 = c[1] * s[2] + s[1] * c[2];

           h1 = c[3] * c[4] * c[5] - s[3] * s[5];
 	   h2 = s[4] * c[5];
 	   T[2][0] = (-s12) * h1 - c12 * h2;
 
           h1 = c12 * h1 - s12 * h2;
	   h2 = s[3] * c[4] * c[5] + c[3] * s[5];
	   T[0][0] = c[0] * h1 + s[0] * h2;
	   T[1][0] = s[0] * h1 - c[0] * h2;

 	   h1 = (-c[3]) * c[4] * s[5] - s[3] * c[5];
 	   h2 = s[4] * s[5];
 	   T[2][1] = (-s12) * h1 + c12 * h2;

 	   h1 = c12 * h1 + s12 * h2;
	   h2 = c[3] * c[5] - s[3] * c[4] * s[5];
 	   T[0][1] = c[0] * h1 + s[0] * h2;
 	   T[1][1] = s[0] * h1 - c[0] * h2;


 	   h1 = c[3] * s[4];
 	   T[2][2] = s12 * h1 - c12 * c[4]; 
 
	   h1 = c12 * h1 + s12 * c[4];
 	   h2 = s[3] * s[4];
           T[0][2] = (-c[0]) * h1 - s[0] * h2;
	   T[1][2] = (-s[0]) * h1 + c[0] * h2;


 	   h1 = linkl[1] * c[1] - linkl[2] * s12;
 	   T[0][3] = c[0] * h1;
 	   T[1][3] = s[0] * h1;
 	   T[2][3] = (-linkl[1]) * s[1] - linkl[2] * c12;
	}

	public static void calculatePos(AngleDates[] angles, double[] pos,
					RoboProto rob)
	{
	   /* 'Angles has to be a 6-elements- and 'pos' a 3-elements-    */
	   /* array.						         */

           double[][] A = new double[3][4];
	   double[] position = new double[3];
	   int i,h;

           TO6(angles,rob.linkLengths,A);
              position[0] = A[0][3] + A[0][2] * rob.linkLengths[3];
              position[1] = A[2][2] * rob.linkLengths[3] + A[2][3] 
				+ rob.linkLengths[0];
              position[2] = -A[1][2] * rob.linkLengths[3] - A[1][3];    
            for ( i=0 ; i<3 ; i++ ) 
              {
                 h = (int)(position[i] * 1000 + 0.5);
                 position[i] = h / 1000.0;  
                 pos[i] = position[i];
              }
	}

	public static int inverseAngle1(double[][] T, double[] angNew, 
		AngleDates[] angOld, int reminder, RoboProto rob)
	{
	   /* 'T' has to be a 3x4-array, 'angNew' consists of 2 elements  */
	   /* and 'angOld' of 6. 				 	  */
	   /* Returns the new value of reminder.			  */
	
	   double help1, help2, help3;
 	   int i;
	     
 	   if ( T[0][3] != 0.0 || T[1][3] != 0.0 )
  	     {
  	       /* atan2 is calculatable for these values,so */
      	       /* two angle values can be determined.       */

   	       help1 = Math.atan2( T[1][3] , T[0][3] );
	       help2 = Math.sqrt( T[0][3]*T[0][3] + T[1][3]*T[1][3]);
 	       help3 = Math.atan2( 0.0 , help2 );
   	       angNew[0] = help1 - help3;

  	       help3 = Math.atan2( 0.0 , -help2 );
  	       angNew[1] = help1 - help3;

   	       if ( angNew[0] == 0.0 && rob.angles[0].act <= 0.0 &&
     		    angNew[1] >= 0.0 ) 
		 angNew[1] = -Math.PI;
   	       if ( angNew[1] == 0.0 && rob.angles[0].act < 0.0 &&
       		    angNew[0] > 0.0 ) 
		 angNew[0] = -Math.PI; 

 	       for ( i=0 ; i < 2 ; i++ )
    		 {
     		   if ( angNew[i] < (-Math.PI ))
		     angNew[i] = angNew[i] + 2 * Math.PI;
                   else {
         		  if ( angNew[i] > Math.PI ) 
			    angNew[i] = angNew[i] - 2 * Math.PI;
         		}   

   
   		 }
	       return 0;
	     }
	   else 
	     {
	       /* Point is on the Y-line. In this case */
               /* atan2 is not calculatable.           */

       	      angNew[0] = angOld[0].act;
       	      if ( angOld[0].act + 1.570796327  <=  (rob.angles[0].max)  &&
            	   angOld[0].act + 1.570796327  >= (rob.angles[0].min ) )
                 angNew[1] = angOld[0].act  + Math.PI / 2.0;
              else angNew[1] = angOld[0].act - Math.PI / 2.0; 
      	      return 2;
    	     }
	}

	public static int inverseAngle3(double[][] T, double[] angNew, 
			double[] linkl, int reminder, RoboProto rob)
	{
	   /* 'T' has to be a 3x4-array, 'angNew' consists of 2 elements  */
	   /* and 'linkl' of 3. 				 	  */
	   /* Returns the new value of reminder.			  */
	
	   double help, K, help2;
 	   int i;

	   K = T[0][3] * T[0][3] + T[1][3] * T[1][3] + T[2][3] * T[2][3];
	   K = K - linkl[1] * linkl[1] - linkl[2] * linkl[2];
 	   K = K / ( 2 * linkl[1] );
 	   if ( Math.abs(K) <= 0.000001 ) 
	     K = 0.0;

 	   help = linkl[2] * linkl[2] - K * K;

 	   if ( help > 0 )
  	     {
 	       help = Math.sqrt( help );
  	       help2 = Math.atan2( 0.0 , linkl[2] );
   	       angNew[0] = help2 - Math.atan2( K , help );
   	       angNew[1] = help2 - Math.atan2( K , (-help) );

   	       for (i=0; i < 2; i++)
   		 {
  		   if ( angNew[i] > rob.angles[2].max ) 
		     angNew[i] = angNew[i] - 2 * Math.PI;
   		 }

   	       return 0;
 	     }
	   else 
	     return 1;  // Calculation not possible.
	}

	public static int inverseAngle2(double[][] T, double[] angNew, 
			double[] linkl, double angle1, double angle3, 
		        double[] s, double[] c, int reminder, RoboProto rob)
	{
	   /* 'T' has to be a 3x4-array, 'angNew' consists of 2 elements  */
	   /* and 'linkl' of 3. 's' and 'c' consist of 6 elements.	  */
	   /* Returns the new value of reminder.			  */

	   double help1,help2;

 	   help1 = T[2][3] * T[2][3];
 	   help2 = c[1] * T[0][3] + s[1] * T[1][3];
 	   help1 = help1 + help2 * help2;

 	   if ( help1 > 0.0 )
 	     {
  	       help1 = (-linkl[1] * c[3]) * T[2][3] +
                       ( c[1] * T[0][3] + s[1] * T[1][3] ) *
                       ( linkl[1] * s[3] - linkl[2] );
   	       if ( help1 == -0.0 )
		 help1 = 0.0;
 	       help2 = (linkl[1] * s[3] - linkl[2]) * T[2][3] -
            	       (-linkl[1] * c[3])*(c[1] * T[0][3] + s[1] * T[1][3]);
  	       if ( Math.abs(help1) <= 0.000001) 
		 help1 = 0.0;
  	       if ( Math.abs(help2) <= 0.000001) 
		 help2 = 0.0;

	       if ( help1 != 0.0 || help2 != 0.0 )
 	         {
   		   angNew[0] = Math.atan2( help1,help2 ) - angle3;
   	           if ( angNew[0] > rob.angles[1].max )
		     angNew[0] = angNew[0] - 2*Math.PI;
   		   if ( Math.abs(angNew[0] - Math.PI) <= 0.000001 ) 
	             angNew[0] = -Math.PI;
     		   return 0;
  	         }
  	       else return 1;
 	     }
 	   else return 1;
	}

	public static int inverseAngle4(double[][] T, double[] angNew, 
	    AngleDates[] angOld, double angle1, double angle2, double angle3,
	    double[] s, double[] c, int reminder, RoboProto rob) 
	{
	   /* 'T' has to be a 3x4-array, 'angNew' consists of 2 elements  */
	   /* and 'angOld' of 6. 's' and 'c' consist of 6 elements.	  */
	   /* Returns the new value of 'reminder'.			  */

	   double help1, help2, help3, help4;
	   double c23, s23;

           c23 = c[2] * c[3] - s[2] * s[3];
 	   s23 = c[2] * s[3] + s[2] * c[3];

	   help1 = (-T[0][2]) * s[1] + T[1][2] * c[1];
           help3 = (-T[0][2]) * c[1] * c23 ;
 	   help4 = (-T[1][2]) * s[1] * c23 ;
	   help2 = help3 + help4 + T[2][2] * s23;
 	   if ( Math.abs(help1) <= 0.000001 )
	     help1 = 0.0;
 	   if ( Math.abs(help2) <= 0.000001 )
	     help2 = 0.0;

 	   if ( Math.abs(help1) <= 1.000001 && Math.abs(help2) <= 1.000001 &&
     	      ( Math.abs(help1) >= 0.01 || Math.abs(help2) >= 0.01))
  	     {
  	       angNew[0] = Math.atan2( help1,help2 );
   	       if ( Math.abs(Math.abs(angNew[0]) - Math.PI) <= 0.000001 )
   	         {
   	           if ( angOld[3].act < 0.0 && angNew[0] >= 0.0 ) 
		     angNew[0] = angNew[0] * (-1);
    		   if ( angOld[3].act > 0.0 && angNew[0] < 0.0 )
		     angNew[0] = angNew[0] * ( -1);
   		 } 
  	       return 0;
  	     }
	   else 
	     {
      	       if ( Math.abs(help1) <= 0.01 && Math.abs(help2) <= 0.01 )
       	         { 
		   angNew[0] = angOld[3].act;
           	   return 5;
        	 }
      	       else return 1;
     	     } 
	}

	public static int inverseAngle5(double[][] T, double[] angNew,
			double a1, double a2, double a3, double a4, double[] s,
			double[] c, int reminder, RoboProto rob)
	{
	   /* 'T' has to be a 3x4-array, 'angNew' consists of 6 elements.  */
	   /* 's' and 'c' consist of 6 elements.	                   */
	   /* Returns the new value of 'reminder'.	       		   */

	   double help1;
 	   double help2;
 	   double c23, s23;

 	   c23 = c[2] * c[3] - s[2] * s[3];
 	   s23 = c[2] * s[3] + s[2] * c[3];

 	   help1 = -(T[0][2] * ( c[1] * c23 * c[4] + s[1] * s[4] ) 
		  + T[1][2] * 
                ( s[1] * c23 * c[4] - c[1] * s[4] ) - T[2][2] * ( s23 * c[4]));
 	   help2 = T[0][2] * ( (-c[1]) * s23 ) + T[1][2] * ( (-s[1]) * s23 ) 
	           + T[2][2] * (-c23);
	   if ( Math.abs(help1) <= 0.000001 )
	     help1 = 0.0;
 	   if ( Math.abs(help2) <= 0.000001 )
	     help2 = 0.0;

 	   if ( Math.abs(help1) <= 1.000001 && Math.abs(help2) <= 1.000001 &&
    	        ( help1 != 0.0 || help2 != 0.0))
 	     {
  	       angNew[0] = Math.atan2( help1,help2 );
   	       return 0;
  	     }
 	   else return 1;
	}

	public static int inverseAngle6(double[][] T, AngleDates[] angles,
	 	       double[] angNew, double a1, double a2, double a3,
		       double a4, double a5, double[] s, double[] c, 
		       int reminder, RoboProto rob) 
	{
	   /* 'T' has to be a 3x4-array, 'angles' consists of 6 elements  */
	   /* and 'angNew' of 6. 's' and 'c' consist of 6 elements.	  */
	   /* Returns the new value of 'reminder'.			  */

	   double help1;