referenceTraj.cpp

00001 /*****************************************************************************
00002 
00003  referenceTraj.c
00004 
00005  created: 00/11/21  by  Preparing SnIFF
00006 Liu Hong
00007 
00008  $Revision: 1.2 $  $Author: liu $    $Date: 2001/03/15 12:44:12 $   
00009 
00010  (C) by Institute of Robotics and Mechatronics, DLR
00011 
00012  Description:
00013 
00014    Please place your comment here !!!
00015         
00016  Contains:         
00017  
00018  Dependencies:     
00019 
00020  Target platforms: unspecified
00021 
00022  *****************************************************************************/
00023 
00024 
00025 //#include <stdio.h>            /* Changed by mhjin. */
00026 #include  "math.h"             /* Changed by mhjin. */
00027 //#include "FingCtrlFrame.h"    /* Changed by mhjin. */
00028 //#include "ctrlCommon.h"
00029 #include "referenceTraj.h"
00030 //#include "matrix.h"           /* Changed by mhjin. */
00031 //#include "dingyi.h"           /* Changed by mhjin. */
00032 
00033 void reference_trajectory(int INIT,                  /* init start */
00034                           double TS,                  /* sampling time */
00035                           double SP,                  /* interval  */
00036                           double *Tacc,               /* acceleration time */
00037                           PREF ref,                  /* static variable */
00038                           float *MAX_STEP,           /* max step limitation */
00039                           float *posRe,              /* real position */
00040                           float *posFl,              /* desire position */
00041                           float *trajRef )           /* ref. Traj. 0..2 -> q_d, 3..5-> v_d, 6..8->a_d */
00042 {
00043    /* desire trajectory definition      */
00044    int i;
00045    float  h[3], md_para[3], N[3], N1[3], K[3];
00046    //   float det_M[3], det_L[3]; 
00047    float det_N[3];
00048    /*   float MAX_STEP[3];*/
00049    /*    float t[3]={0,0,0}; */
00050    /*    static int init_counter=0; */
00051    /*static float L[3]={0,0,0}, M[3]={0,0,0}; */
00052    //   float T1[3], t1[3]; 
00053    
00054     /*
00055      init_counter += 1;
00056      if( init_counter <= 1){
00057           for(i=0; i<=2; i++){
00058                 t[i] = SP - (*(Tacc+i));
00059                 M[i] = (*(posRe + i));
00060             L[i] = M[i];
00061             det_M[i] = 0.0;
00062           }
00063      }else{
00064           init_counter = 1;
00065      }
00066     */
00067    //    for(i=0; i<=2; i++){
00068        //       det_L[i] = ref->det_L[i];   /* reading para */
00069        //       det_M[i] = ref->det_M[i];   /* reading para */
00070        //       t1[i] =    ref->t1[i];          /* reading para */
00071        //       T1[i] =    ref->T1[i];          /* reading para */
00072    //    }
00073     /* following is for trajectory test */
00074     if(INIT == TRUE){
00075        for(i=0; i<=2; i++){
00076           ref->t[i] = SP - (*(Tacc+i));
00077           ref->M[i] = (*(posRe + i));
00078           ref->N[i] = (*(posRe + i)); /* final position */
00079           ref->L[i] = ref->M[i];
00080           ref->det_M[i] = 0.0;
00081     //      ref->det_L[i] = 0.0;            // Add by mhjin
00082           *(trajRef + i) = posRe[i];
00083           *(trajRef + 3 + i) = 0.0; 
00084           *(trajRef + 6 + i) = 0.0;
00085     //      ref->T1[i]=1.0/SP;          // Add by mhjin
00086     //      ref->t[i]= -(*(Tacc+i)) - TS;   // Add by mhjin
00087     //      ref->t1[i] = 1/(*(Tacc+i)); // Add by mhjin
00088        }
00089     }else{
00090        for(i=0; i<=2; i++){ 
00091           ref->t[i] += TS;
00092           if(ref->t[i] < (*(Tacc+i)/*- 0.0001*/)){
00093              h[i]=0.5*(1.0+ref->t[i]*ref->t1[i]);
00094              md_para[i]=ref->det_M[i]*(*(Tacc+i))*ref->T1[i]+ref->det_L[i];
00095              *(trajRef + i) = ((md_para[i])*(2.0-h[i])*h[i]*h[i]-2.0*ref->det_L[i])*h[i]+ref->L[i]+ref->det_L[i];
00096              *(trajRef + 3 + i) = ((md_para[i])*(1.5-h[i])*2.0*h[i]*h[i]-ref->det_L[i])*ref->t1[i];
00097              *(trajRef + 6 + i) = (md_para[i])*(1.0-h[i])*3.0*h[i]*ref->t1[i]*ref->t1[i];
00098           }else if(ref->t[i] < (SP - *(Tacc+i) - TS/* + 0.0001*/)){     //  Bug in here. 0.1ms
00099              h[i]=ref->t[i]*ref->T1[i];
00100              *(trajRef + i) = (ref->det_M[i]*h[i]+ref->L[i]);
00101              *(trajRef + 3 + i) = ref->det_M[i]*ref->T1[i];
00102              *(trajRef + 6 + i) = 0.0;
00103           }else{
00104              /*             MAX_STEP[i] = fg->glb->c1.mxm.MaxStep[i];*/
00105              h[i]=ref->t[i]*ref->T1[i];
00106              *(trajRef + i) = (ref->det_M[i]*h[i]+ref->L[i]);
00107              *(trajRef + 3 + i) = ref->det_M[i]*ref->T1[i];
00108              *(trajRef + 6 + i) = 0.0;
00109              N1[i] = ref->N[i] ; /* last time final position  */
00110              N[i] = *(posFl + i);
00111              det_N[i] = N[i] - N1[i]; /*  absolute max speed */
00112              if(det_N[i] >= MAX_STEP[i]){
00113                 N[i] = N1[i] + MAX_STEP[i];
00114              }else if (det_N[i] <= -MAX_STEP[i]){
00115                 N[i] = N1[i] - MAX_STEP[i];
00116              }
00117              ref->N[i] = N[i]; /* old final position */
00118              
00119              K[i] = *(trajRef + i);
00120              ref->L[i] = ref->M[i]; 
00121              ref->M[i] = N[i];   
00122              ref->det_L[i] = K[i] - ref->L[i]; 
00123              ref->det_M[i] = ref->M[i] - ref->L[i]; 
00124              if(SP <= 2.0*(*(Tacc+i))) SP = 2.0*(*(Tacc+i)); 
00125              ref->T1[i]=1.0/SP;
00126              ref->t[i]= -(*(Tacc+i)) - TS;
00127              ref->t1[i] = 1/(*(Tacc+i));
00128           }
00129        }
00130     }
00131 }
00132 
00133 
00134 void arm_reference_trajectory(int INIT,                  /* init start */
00135                           double TS,                  /* sampling time */
00136                           double SP,                  /* interval  */
00137                           double *Tacc,               /* acceleration time */
00138                           PARMREF ref,                  /* static variable */
00139                           float *MAX_STEP,           /* max step limitation */
00140                           float *posRe,              /* real position */
00141                           float *posFl,              /* desire position */
00142                           float *trajRef )           /* ref. Traj. 0..2 -> q_d, 3..5-> v_d, 6..8->a_d */
00143 {
00144    /* desire trajectory definition      */
00145    int i;
00146    float  h[5], md_para[5], N[5], N1[5], K[5];
00147    float det_N[5];
00148    
00149     /* following is for trajectory test */
00150     if(INIT == TRUE){
00151        for(i=0; i<5; i++){
00152           ref->t[i] = SP - (*(Tacc+i));
00153           ref->M[i] = (*(posRe + i));
00154           ref->N[i] = (*(posRe + i)); /* final position */
00155           ref->L[i] = ref->M[i];
00156           ref->det_M[i] = 0.0;
00157     //      ref->det_L[i] = 0.0;            // Add by mhjin
00158           *(trajRef + i) = posRe[i];
00159           *(trajRef + 5 + i) = 0.0; 
00160           *(trajRef + 10 + i) = 0.0;
00161     //      ref->T1[i]=1.0/SP;          // Add by mhjin
00162     //      ref->t[i]= -(*(Tacc+i)) - TS;   // Add by mhjin
00163     //      ref->t1[i] = 1/(*(Tacc+i)); // Add by mhjin
00164        }
00165     }else{
00166        for(i=0; i<5; i++){ 
00167           ref->t[i] += TS;
00168           if(ref->t[i] < (*(Tacc+i)/*- 0.0001*/)){
00169              h[i]=0.5*(1.0+ref->t[i]*ref->t1[i]);
00170              md_para[i]=ref->det_M[i]*(*(Tacc+i))*ref->T1[i]+ref->det_L[i];
00171              *(trajRef + i) = ((md_para[i])*(2.0-h[i])*h[i]*h[i]-2.0*ref->det_L[i])*h[i]+ref->L[i]+ref->det_L[i];
00172              *(trajRef + 5 + i) = ((md_para[i])*(1.5-h[i])*2.0*h[i]*h[i]-ref->det_L[i])*ref->t1[i];
00173              *(trajRef + 10 + i) = (md_para[i])*(1.0-h[i])*3.0*h[i]*ref->t1[i]*ref->t1[i];
00174           }else if(ref->t[i] < (SP - *(Tacc+i) - TS/* + 0.0001*/)){     //  Bug in here. 0.1ms
00175              h[i]=ref->t[i]*ref->T1[i];
00176              *(trajRef + i) = (ref->det_M[i]*h[i]+ref->L[i]);
00177              *(trajRef + 5 + i) = ref->det_M[i]*ref->T1[i];
00178              *(trajRef + 10 + i) = 0.0;
00179           }else{
00180              /*             MAX_STEP[i] = fg->glb->c1.mxm.MaxStep[i];*/
00181              h[i]=ref->t[i]*ref->T1[i];
00182              *(trajRef + i) = (ref->det_M[i]*h[i]+ref->L[i]);
00183              *(trajRef + 5 + i) = ref->det_M[i]*ref->T1[i];
00184              *(trajRef + 10 + i) = 0.0;
00185              N1[i] = ref->N[i] ; /* last time final position  */
00186              N[i] = *(posFl + i);
00187              det_N[i] = N[i] - N1[i]; /*  absolute max speed */
00188              if(det_N[i] >= MAX_STEP[i]){
00189                 N[i] = N1[i] + MAX_STEP[i];
00190              }else if (det_N[i] <= -MAX_STEP[i]){
00191                 N[i] = N1[i] - MAX_STEP[i];
00192              }
00193              ref->N[i] = N[i]; /* old final position */
00194              
00195              K[i] = *(trajRef + i);
00196              ref->L[i] = ref->M[i]; 
00197              ref->M[i] = N[i];   
00198              ref->det_L[i] = K[i] - ref->L[i]; 
00199              ref->det_M[i] = ref->M[i] - ref->L[i]; 
00200              if(SP <= 2.0*(*(Tacc+i))) SP = 2.0*(*(Tacc+i)); 
00201              ref->T1[i]=1.0/SP;
00202              ref->t[i]= -(*(Tacc+i)) - TS;
00203              ref->t1[i] = 1/(*(Tacc+i));
00204           }
00205        }
00206     }
00207 }
00208