cinematic.java

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

 	   double 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][0])*( c[1]*c23*s[4] - s[1]*c[4]); 
	   help3 = T[1][0]*( s[1]*c23*s[4] + c[1]*c[4]);
 	   help4 = T[2][0]*( s23*s[4]);
 	   help1 = help1 - help3 + help4;

 	   help2 = T[0][0]*((c[1]*c23*c[4] + s[1]*s[4])*c[5] - c[1]*s23*s[5]) +
                   T[1][0]*((s[1]*c23*c[4]-c[1]*s[4])*c[5] - s[1]*s23*s[5]) -
                   T[2][0]*(s23*c[4]*c[5] + c23*s[5]);
           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 );

	       /* **************************************************** */
   	       if ( Math.abs(Math.abs(angNew[0]) - Math.PI ) < 0.000001 )
      		 {
      	 	   if ( angles[5].act < 0.0 && angNew[0] >= 0.0 ) 
		     angNew[0] = -angNew[0];
    		   if ( angles[5].act > 0.0 && angNew[0] < 0.0 )
		     angNew[0] = -angNew[0];
     		  }
	       /* **************************************************** */
        
   	       if ( angNew[0] < (-Math.PI ))
	         angNew[0] = angNew[0] + 2 * Math.PI;
    	       else 
		 {
	           if ( angNew[0] > Math.PI )
		     angNew[0] = angNew[0] - 2 * Math.PI;
         	 }   

  	       return 0;
 	     }
 	   else return 1;
	}

	public static int inverseCinematic(double[][] T, double[] angNew,
			AngleDates[] angOld, double[] linkl, int result,
			int f_a_g, RoboProto rob)
	{
	   /* 'T' has to be a 3x4-array and is frame 6 relatively to     */
	   /* frame 0. 'angNew' consists of 6 elements as 'angOld' does. */
	   /* 'linkl' has got 3 elements.				 */
	   /* 'angOld' are the actual angles and 'linkl' the actual link */
	   /* lengths. If result is 0 then a valid combination of angles */
	   /* has been found.						 */
	   /* Returns the new value of 'result'.             		 */

	   double[][] A = new double[3][4];
 	   double angMax,angHelp;
 	   int number;
 	   AngleDates[] w = { new AngleDates(0.0, 0.0, 0.0),
			      new AngleDates(0.0, 0.0, 0.0),  
			      new AngleDates(0.0, 0.0, 0.0),
			      new AngleDates(0.0, 0.0, 0.0),  
			      new AngleDates(0.0, 0.0, 0.0),
			      new AngleDates(0.0, 0.0, 0.0) }; 
 	   double[] s = new double[6], c = new double[6];
 	   double zw,help;
	   double[] angle = new double[2];
 	   int i,j,k, reminder;      
		      /* reminder = 0 ... result is OK.
                         reminder = 1 ... no result.
                         reminder = 2 ... the result has to be checked.
                         reminder = 5 ... set angle 5  to 0.        */
     
 	   double[][] w_tab = new double[8][7]; 
		      // w_tab[i][0] = 0 ... Angle combination is OK. 
                      // w_tab[i][0] = 1 ... Angle combination is not OK.
                      // w_tab[i][0] = 2 ... Combination has to be checked.

 	   result = 1;
	   reminder = 300; // stupid, only for the syntax needed default value

 	   reminder = inverseAngle1 ( T,angle,angOld,reminder, rob );
 	   for ( i=0 ; i < 2 ; i++ )
	     {
	       w_tab[i][0] = reminder;
               w_tab[i][1] = angle[0];
             }
 	   for ( i=2 ; i < 4 ; i++ )
	     {
	       w_tab[i][0] = reminder;
               w_tab[i][1] = angle[1];
             }
             
 	   reminder = inverseAngle3 ( T,angle,linkl,reminder, rob );
 	   if ( reminder == 0 )
 	     {   
	       /* It exists a solution for angle 3 */
   	       for ( i=0 ; i < 2 ; i++ ) 
	         {
		   w_tab[i*2][3] = angle[0];
                   w_tab[i*2+1][3] = angle[1];
                 }

  	       for ( i=0 ; i < 4 ; i++ )
 	         {
	           s[1] = Math.sin ( w_tab[i][1] );
 	           c[1] = Math.cos ( w_tab[i][1] );
   	  	   s[3] = Math.sin ( w_tab[i][3] );
   		   c[3] = Math.cos ( w_tab[i][3] );

    		   reminder = inverseAngle2 (T,angle,linkl,w_tab[i][1],
					 w_tab[i][3], s,c,reminder, rob );
   		   if ( reminder == 1 ) 
	             w_tab[i][0] = 1.0 ; 
     		   else
      		     {  
		       /* A solution for angle 2 exists, that means angle 4
         	          can now be calculated.                           */  
      		       w_tab[i][2] = angle[0];
     		       s[2] = Math.sin ( angle[0] );
     		       c[2] = Math.cos ( angle[0] );

     		       reminder = inverseAngle4 ( T,angle,angOld,w_tab[i][1],
				   w_tab[i][2], w_tab[i][3], s,c,reminder,rob);
    		       if ( reminder == 1 ) 
			 w_tab[i][0] = 1.0;
       		       else
  		         { 
			    /* A solution for angle 4 exists, that means 
			       angle 5 can now be calculated.		 */
        		    s[4] = Math.sin ( angle[0] );
     			    c[4] = Math.cos ( angle[0] ); 
     			    w_tab[i][4] = angle[0];
      			    if ( reminder == 5 )
          		      {
           			w_tab[i][0] = 2.0;
         		        w_tab[i][5] = 0.0;
       			        s[5] = 0.0;
       			        c[5] = 1.0; 
        		      }  /* if .. reminder == 5 */
       			    else
       			      { 
			        /* Calculation of angle 5 */
       			        reminder = inverseAngle5(T,angle,w_tab[i][1],
					    w_tab[i][2],w_tab[i][3], 
					    w_tab[i][4],s,c,reminder, rob );
           			if ( reminder == 1 ) 
				  w_tab[i][0] = 1.0; 
           			else 
				  { 
				    w_tab[i][5] = angle[0];
                		    s[5] = Math.sin ( w_tab[i][5] );
                		    c[5] = Math.cos ( w_tab[i][5] );
              			  }
         		      }  /* else .. reminder == 5 */
       			    if ( w_tab[i][0] != 1.0 )
         		      {  
				/* Calculation of angle 6 */
          		        reminder = inverseAngle6(T,rob.angles,angle,
				    w_tab[i][1], w_tab[i][2],w_tab[i][3],
                                    w_tab[i][4],w_tab[i][5],s,c,reminder, rob);
    	                        if ( reminder == 1 )
			          w_tab[i][0] = 1.0 ; 
         		        else
      			          {
      			            w_tab[i][6] = angle[0];
             
         		            /* Copying of the angle values: */
         		            w_tab[i+4][0] = w_tab[i][0];
       		                    w_tab[i+4][1] = w_tab[i][1];
        		            w_tab[i+4][2] = w_tab[i][2];
          		            w_tab[i+4][3] = w_tab[i][3];

              			    if ( Math.abs(w_tab[i][4]) == 0.0 ) 
              			      {
              			        if ( angOld[3].act >= 0.0 ) 
					  w_tab[i+4][4] = w_tab[i][4] +Math.PI;
               				else w_tab[i+4][4] = w_tab[i][4] 
							     - Math.PI;
             			      } 
              			    else 
             		              { 
			 	 	if (w_tab[i][4] > 0.0) 
					  w_tab[i+4][4] = w_tab[i][4] -Math.PI;
                			else w_tab[i+4][4] = w_tab[i][4] 
							     + Math.PI;
             			      } 
            			    w_tab[i+4][5] = -w_tab[i][5];
              
              			    if ( Math.abs(w_tab[i][6]) == 0.0 ) 
             			      {
             			        if ( angOld[5].act >= 0.0 ) 
					  w_tab[i+4][6] = w_tab[i][6] +Math.PI;
              				else w_tab[i+4][6] = w_tab[i][6] 
							     - Math.PI;
             			      } 
             			    else 
           			      { 
				        if (w_tab[i][6] > 0.0) 
					  w_tab[i+4][6] = w_tab[i][6] -Math.PI;
              			        else w_tab[i+4][6] = w_tab[i][6] 
							     + Math.PI;
            			      } 

            		          } /* else .. reminder W6 */  
         		      } /* if ... w_tab[i][0] == 0 */ 

      		          }  /* else .. reminder Angle 4 */
      		      }    /* else .. reminder Angle 2 */
	          } /* endfor  */
  
		/* Determination of the best angle solution */
                      
	        result = 1;
	        zw = 10000;
		for ( i=0 ; i < 8 ; i++ )
 		  {
  		    if ( w_tab[i][0] != 1.0)
   		      {
   		        reminder = 0;
   		        for ( j=0 ; j < 6 ; j++ )
    			  {
   			    w[j].act = w_tab[i][j+1];
    			    if ( w_tab[i][j+1] < rob.angles[j].min ||
    		                 w_tab[i][j+1] > rob.angles[j].max ) 
                              {
                                w_tab[i][0] = 1.0;
                                reminder = 1;
                                break;
                              }
   		 	  } /* endfor */
   		        if ( reminder == 0 )
     			  {
     			    TO6 ( w,linkl,A );
     			    for ( j=0 ; j < 3 ; j++ )
     			      {
      			        if ( Math.abs(T[j][0] - A[j][0]) > 0.01 ||
       			             Math.abs(T[j][1] - A[j][1]) > 0.01 ||
         		             Math.abs(T[j][2] - A[j][2]) > 0.01 ||
          		             Math.abs(T[j][3] - A[j][3]) > 0.1     )
				  { 
                                    w_tab[i][0] = 1.0;  
                                    reminder = 1;
                                    break;
                                  } 
     			      }
      		 	    if ( reminder == 0 )
       			      {
              		        // This combination of angles is allowed
				// and correct. 
          			// Calculation of the target function.
       			        help = 0;
         		        for ( j=0 ; j < 6 ; j++ )
         			  help = help + rob.angleWeights[j] * 
				     Math.abs( w_tab[i][j+1] - angOld[j].act );
         			if ( zw > help ) 
				  {
                          	    zw = help;
                          	    for ( j=0 ; j < 6 ; j++ )
                             	      angNew[j] = w_tab[i][j+1];
                            	  }  
         		        result = 0;
      			      }
     			  }
                      } /* end if */
                  }  /* end for */


                /* If it is the case that f_auto_gerade is 0 ,
      	 	   it will be tested, if there exists a better solution. */

 	        if ( f_a_g == 0 )
  		  {
   		    angMax = Math.abs( angOld[0].act  - angNew[0] );
   		    for ( i=1; i < 6; i++ )
    		      {
   			if (angMax < (Math.abs( angOld[i].act - angNew[i] ) )) 
			  angMax = Math.abs( angOld[i].act - angNew[i] );
  		      }

   		    for ( i=0; i < 8; i++ )
   		      {
    		        if ( w_tab[i][0] != 1 )
       		          {
       			    angHelp = Math.abs( angOld[0].act - w_tab[i][1] );
    		            for ( j=2; j < 7; j++ )
     			      { if ((i > 0) && (i < 7))// meine Einschraenkung
     			        if (angHelp < 
				  (Math.abs(angOld[i-1].act - w_tab[i][j]))) 
			          angHelp = Math.abs(
					       angOld[i-1].act - w_tab[i][j] );
    		              }      
     		            if ( angMax > angHelp )
     			      {
      			        angMax = angHelp;
      			        for ( j=0; j < 6; j++ ) 
			          angNew[j] = w_tab[i][j+1];
   		              }
    		          }
   
 		      }
    
                  }  



 
             } /* if reminder == 0) ... */ 
	   return result;
	} /* end of method: 'inverseCinematic' */
	
}