Robot

Dependencies:   mbed QEI

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers MotionCommand.cpp Source File

MotionCommand.cpp

00001 #include "MotionCommand.h"
00002 template <typename T>
00003 inline T  sign (T  a) {
00004   return (a > 0 ? 1:-1);
00005 }
00006 
00007 MotionCommand::MotionCommand(Robot* Robot1):PrevPosition(Robot1->GetDOF()){
00008     _Robot = Robot1;
00009     IsFinalized = false;
00010     index = 0;
00011 }
00012 MotionCommand::~MotionCommand(){
00013     delete[] _Trajectory;
00014 }
00015 void MotionCommand::MoveJ(End_Effector qf,float v0,float v1,float a,float samplingtime,float time,int zone,Trajectory_Types _trajectory){
00016     SamplingTime = samplingtime;
00017     IsFinalized = false;
00018     PrevPosition = 0;
00019     float q0[3];
00020     Vector<float> PositionF(3),PositionInit(3);
00021     PositionF(0) = qf.x;
00022     PositionF(1) = qf.y;
00023     PositionF(2) = qf.z;
00024     
00025     _Trajectory = new Trajectory[3];
00026     
00027     
00028     for (int i=0; i<_Robot->GetDOF();i++){
00029         if(i<=3){
00030             _Trajectory[i] = Trajectory(_Robot->GetDOF(),samplingtime);
00031         }
00032         q0[i] = _Robot->ReadSensor(i);
00033     }
00034     _Robot->ForwardKinematics(q0);
00035     PositionInit = _Robot->GetEndPosition();
00036     
00037     for (int i=0; i<3;i++){
00038         switch(_trajectory){
00039        case TRAPEZOIDAL:
00040            Trapezoid(PositionInit(i),PositionF(i), v0, a,samplingtime,_Robot->ConstraintsVelocityMotor(i),_Robot->ConstraintsAccelerationMotor(i));
00041            break;
00042        case SCURVE:
00043            SCurve(PositionInit(i),PositionF(i), v0, v1, a,samplingtime,_Robot->ConstraintsVelocityMotor(i),_Robot->ConstraintsAccelerationMotor(i), _Robot->ConstraintsJerkMotor(i));
00044            break;
00045        case SPLINE5:
00046            Spline_5th(PositionInit(i),PositionF(i), v0, v1, a,samplingtime,time,_Robot->ConstraintsVelocityMotor(i),_Robot->ConstraintsAccelerationMotor(i));
00047            break;
00048        case SPLINE7:
00049            Spline_7th(PositionInit(i),PositionF(i), v0, v1, a,samplingtime,time,_Robot->ConstraintsVelocityMotor(i),_Robot->ConstraintsAccelerationMotor(i));
00050            break;
00051         }     
00052     }
00053     this->StartMoveL1();
00054     while(!IsFinalized) {
00055         asm("nop");
00056     }
00057     this->StopMoveL1();
00058     
00059 
00060 }
00061 void MotionCommand::MoveL1(End_Effector qf, float v, float a,float samplingtime){
00062     SamplingTime = samplingtime;
00063     IsFinalized = false;
00064     PrevPosition = 0;
00065     float q0[_Robot->GetDOF()];
00066     Vector<float> PositionF(3),PositionInit(3);
00067     PositionF(0) = qf.x;
00068     PositionF(1) = qf.y;
00069     PositionF(2) = qf.z;
00070     _Trajectory = new Trajectory[3];
00071     
00072     
00073     for (int i=0; i<_Robot->GetDOF();i++){
00074         if(i<=3){
00075             _Trajectory[i] = Trajectory(_Robot->GetDOF(),samplingtime);
00076         }
00077         q0[i] = _Robot->ReadSensor(i);
00078     }
00079     _Robot->ForwardKinematics(q0);
00080     PositionInit = _Robot->GetEndPosition();
00081     
00082     for (int i=0; i<3;i++){
00083         Trapezoid(PositionInit(i),PositionF(i), v, a,samplingtime,_Robot->ConstraintsVelocityMotor(i),_Robot->ConstraintsAccelerationMotor(i));     
00084     }
00085     this->StartMoveL1();
00086     while(!IsFinalized) {
00087         asm("nop");
00088     }
00089     this->StopMoveL1();
00090 }
00091 
00092 void MotionCommand::MoveL2(End_Effector qf, float v, float a,float samplingtime){
00093     SamplingTime = samplingtime;
00094     IsFinalized = false;
00095     PrevPosition = 0;
00096     float q0[3];
00097     Vector<float> PositionF(3),PositionInit(3);
00098     PositionF(0) = qf.x;
00099     PositionF(1) = qf.y;
00100     PositionF(2) = qf.z;
00101     _Trajectory = new Trajectory[3];
00102     
00103     
00104     for (int i=0; i<_Robot->GetDOF();i++){
00105         if(i<=3){
00106             _Trajectory[i] = Trajectory(_Robot->GetDOF(),samplingtime);
00107         }
00108         q0[i] = _Robot->ReadSensor(i);
00109     }
00110     _Robot->ForwardKinematics(q0);
00111     PositionInit = _Robot->GetEndPosition();
00112     
00113     for (int i=0; i<3;i++){
00114         Trapezoid(PositionInit(i),PositionF(i), v, a,samplingtime,_Robot->ConstraintsVelocityMotor(i),_Robot->ConstraintsAccelerationMotor(i));     
00115     }
00116     this->StartMoveL2();
00117     while(!IsFinalized) {
00118         asm("nop");
00119     }
00120     this->StopMoveL2();
00121 }
00122 
00123 
00124 void MotionCommand::Trapezoid(float _q0,float q1, float qd, float qdd,float stime,float Constraints_v,float Constraints_a){
00125     float s, signo, t1, t2, tfin, q,qprim,acel;
00126     s = q1 - _q0;
00127     signo = sign(s);
00128     CurrentCycle = 0;
00129         
00130         if (fabsf(s) > (powf(Constraints_v,2.0F) / Constraints_a)){
00131             t1 = Constraints_v / Constraints_a;
00132             t2 = fabsf(s) / Constraints_v;
00133             tfin = fabsf(s) / Constraints_v + Constraints_v / Constraints_a;
00134         }
00135         else{
00136             t1 = sqrtf(fabsf(s) / Constraints_a);
00137             tfin = 2.0F*t1;
00138             t2 = 0.0F;
00139         }
00140         q = _q0;
00141         qprim = 0;
00142         float t=0;
00143         TotalCycle = (int)tfin/stime+1;
00144         for(int tim=0; tim<((int)(tfin/stime)+1);tim++){
00145                 t = tim*stime;
00146                 //Calculation of the velocity and position for phase 1 (acceleration)
00147                 if (t < t1){
00148                     qdd = signo*Constraints_a;
00149                     qprim=qprim+acel*stime;
00150                     q=q+qprim*stime+0.5F*acel*stime*stime;
00151                 }
00152                 //Calculation of the velocity and position for phase 2 (constant velocity)
00153                 else if ((t >= t1) && (t < t2)){
00154                     qdd = 0.0F;
00155                     qprim=signo*qd;
00156                     q=q+qprim*stime;
00157                 }
00158                 //Calculation of the velocity and position for phase 3 (deceleration)
00159                 else if ((t >= t2) && (t < tfin)){
00160                     acel = -signo*qdd;
00161                     qprim=qprim + acel*stime;
00162                     q=q + qprim*stime+0.5F*acel*powf(stime,2.0F);
00163                 }else{
00164                     qdd = 0.0F;
00165                     qd = 0.0F;
00166                     q = q1;
00167                 }
00168                 _Trajectory[index].Insert(q,qd,qdd);
00169         }
00170         index++;
00171 }
00172     
00173 
00174 void MotionCommand::StartMoveL1(){
00175     TimInterrupt.attach(this,&MotionCommand::TimFunctionMoveL1,SamplingTime);
00176 }   
00177  void MotionCommand::StopMoveL1(){
00178     TimInterrupt.detach();
00179 }   
00180 void MotionCommand::TimFunctionMoveL1(){
00181     if(CurrentCycle <=  TotalCycle ){
00182         Vector<float> x(_Robot->GetDOF()),v(_Robot->GetDOF());
00183         Vector<float> Kp(_Robot->GetDOF()),Kv(_Robot->GetDOF());
00184         int isCorrect;
00185         float q[_Robot->GetDOF()],xref[_Robot->GetDOF()], vref[_Robot->GetDOF()];
00186         
00187         
00188         for (int i=0; i<_Robot->GetDOF();i++){
00189             q[i] = _Robot->ReadSensor(i);
00190             xref[i] =  _Trajectory[i].GetXref(CurrentCycle);
00191             vref[i] =  _Trajectory[i].GetVref(CurrentCycle);
00192         }
00193         
00194         _Robot->ForwardKinematics(q);
00195         x = _Robot->GetEndPosition();
00196         
00197         if(CurrentCycle == 0){
00198             v(0) = 0;
00199             v(1) = 0;
00200             v(2) = 0;
00201         }else{
00202             v(0) = (x(0) - PrevPosition(0)) / SamplingTime;
00203             v(1) = (x(1) - PrevPosition(1)) / SamplingTime;
00204             v(2) = (x(2) - PrevPosition(2)) / SamplingTime;
00205         }
00206         
00207         Kp = _Robot->GetKP();
00208         Kv = _Robot->GetKV();
00209         
00210         Vector<float> CartesianForce (6);
00211         Vector<float> G (_Robot->GetDOF());
00212         Matrix<float> Jacobian(6,_Robot->GetDOF());
00213         Matrix<float> JacobianTranspose(_Robot->GetDOF(),6);
00214         
00215         Vector<float> Control(_Robot->GetDOF());
00216     
00217         for (int i=0; i<6;i++){
00218             if(i>=3){
00219                 CartesianForce(i) = 0;
00220                 continue;
00221             }
00222             CartesianForce(i) = Kp(i) * (xref[i] -x(i)) + Kv(i) * (vref[i] -v(i));
00223         }
00224     
00225         _Robot->GravityTorque(q);
00226         G = _Robot->GetTorque();
00227         
00228         _Robot->Jacobian(q);
00229         Jacobian = _Robot->GetJacobian();
00230         JacobianTranspose = Jacobian.Transpose();
00231         
00232         Control = (JacobianTranspose * CartesianForce) + G;
00233         
00234         Vector<float> SaturationControl(_Robot->GetDOF());
00235         
00236         SaturationControl = _Robot->ReturnSaturationControl(Control);
00237         
00238         isCorrect = _Robot->SendData(SaturationControl);
00239         
00240         PrevPosition = x;    
00241         CurrentCycle++;
00242     }else{
00243         IsFinalized = true;   
00244     }
00245     
00246 }    
00247     
00248     
00249     
00250 void MotionCommand::StartMoveL2(){
00251     TimInterrupt.attach(this,&MotionCommand::TimFunctionMoveL2,SamplingTime);
00252 }   
00253  void MotionCommand::StopMoveL2(){
00254     TimInterrupt.detach();
00255 }   
00256 void MotionCommand::TimFunctionMoveL2(){
00257     if(CurrentCycle <=  TotalCycle ){
00258         float _q[_Robot->GetDOF()],qsec[_Robot->GetDOF()], q_prim[_Robot->GetDOF()];
00259         Vector<float> Kp(_Robot->GetDOF()),Kv(_Robot->GetDOF()),q(_Robot->GetDOF()),qref(_Robot->GetDOF()), q_prim_ref(_Robot->GetDOF()), q_sec_ref(_Robot->GetDOF()),
00260         xref(6), x_prim_ref(6), x_sec_ref(6),Tau(_Robot->GetDOF());
00261         int isCorrect;
00262         End_Effector Eref;
00263         
00264         for(int i=0; i<6;i++){
00265             xref(i) =  _Trajectory[i].GetXref(CurrentCycle);
00266             x_prim_ref(i) =  _Trajectory[i].GetVref(CurrentCycle);
00267             x_sec_ref(i) =  _Trajectory[i].GetAref(CurrentCycle);
00268         }
00269         for (int i=0; i<_Robot->GetDOF();i++){
00270             q(i) = _q[i] = _Robot->ReadSensor(i);
00271             
00272             if(CurrentCycle == 0){
00273                 q_prim[i] = 0;
00274             }else{
00275                 q_prim[i] = (q(i) - PrevPosition(0)) / SamplingTime;
00276             }
00277             
00278         }
00279         
00280         Kp = _Robot->GetKP();
00281         Kv = _Robot->GetKV();
00282         
00283         Eref.x = xref(0);
00284         Eref.y = xref(1);
00285         Eref.z = xref(2);
00286         Eref.roll = xref(3);
00287         Eref.pitch = xref(4);
00288         Eref.yaw = xref(5);
00289         
00290         qref = _Robot->InverseKinematics(&Eref,0);
00291         Matrix<float> PseudoInverse(_Robot->GetDOF(),6);
00292         Matrix<float> Jacobian(6,_Robot->GetDOF());
00293         _Robot->Jacobian(_q);
00294         Jacobian = _Robot->GetJacobian();
00295         
00296         if(_Robot->GetDOF() == 6){
00297             q_prim_ref = Jacobian.Inverse() * x_prim_ref;
00298         }else{       
00299             q_prim_ref = Jacobian.PseudoInverse() * x_prim_ref;
00300         }
00301         
00302         
00303         for (int i=0; i<6;i++){
00304             qsec[i] = Kp(i) * (qref(i) -q(i)) + Kv(i) * (q_prim_ref(i) -q_prim[i]);
00305         }
00306         
00307         
00308         _Robot->EulerNewton(_q,q_prim,qsec);
00309         Tau = _Robot->GetTorque();
00310         Vector<float> SaturationControl(_Robot->GetDOF());
00311         
00312         SaturationControl = _Robot->ReturnSaturationControl(Tau);
00313         
00314         isCorrect = _Robot->SendData(SaturationControl);
00315          
00316         PrevPosition = _q; 
00317         CurrentCycle++;
00318     }else{
00319         IsFinalized = true;   
00320     }
00321     
00322 }        
00323     
00324 int MotionCommand::SCurve(float q0,float q1, float qd0,float qdf, float qdd,float stime,float Constraints_v,float Constraints_a, float Constraints_j){
00325         float _q,_qd,_qdd,_q0,_q1,v0,v1,dq,dv,time_set_velocity,time_reach_acc,Tj,sigma,sigm1,sigm2, vmax,vmin,amax,amin,jmax,jmin,Tj1,Tj2,Ta,Td,Tv,T,amax_squared,
00326                 sqrt_delta,alima,alimd,vlim,q,qprim,acel;
00327         CurrentCycle = 0;
00328         bool isFeasable;
00329          if (Constraints_v<abs(qd0)){
00330                 v0 = sign(qd0)*Constraints_v;
00331          }
00332             if (Constraints_v<abs(qdf)){
00333                 v1 = sign(qdf)*Constraints_v;
00334          }
00335          
00336          //Check if its feasable
00337          dq=abs(q1 - q0);
00338          dv=abs(v1-v0);
00339          
00340          time_set_velocity=sqrtf((dv)/Constraints_j);
00341          time_reach_acc=Constraints_a/Constraints_j;
00342             Tj = min(time_set_velocity,time_reach_acc);
00343             if (Tj < time_reach_acc){
00344                 if (dq<Tj*(v0+v1)){
00345                     isFeasable = false;
00346                 }
00347                 else{
00348                      isFeasable = true;
00349                 }
00350             }
00351             else if (Tj == time_reach_acc){
00352                  if (dq < (0.5F *(v0+v1)*(Tj+dv/Constraints_a))){
00353                     isFeasable = false;
00354                  }
00355                  else{
00356                      isFeasable=true;
00357                  }
00358             }
00359             else{
00360              isFeasable=false;
00361             }         
00362     
00363     if (isFeasable == false){
00364         printf("Is not feasable this configuration \n");
00365         return 0;
00366     }
00367     //Sign change
00368         sigma = sign(q1-q0);
00369         _q0 = sigma*q0;
00370         _q1 = sigma*q1;
00371         v0 = sigma*v0;
00372         v1 = sigma*v1;
00373         sigm1 = (sigma+1.0F)/2.0F;
00374         sigm2 = (sigma-1.0F)/2.0F;
00375         vmax = Constraints_v*sigm1 - Constraints_v*sigm2;
00376         amax = Constraints_a*sigm1 - Constraints_a*sigm2;
00377         jmax = Constraints_j*sigm1 - Constraints_j*sigm2;
00378         
00379         vmin = -vmax;
00380         amin = -amax;
00381         jmin = -jmax;
00382         
00383       if ((vmax- v0)* jmax < amax* amax){
00384              Tj1 = sqrtf((vmax - v0)/ jmax);
00385              Ta = 2.0F * Tj1; 
00386       }
00387       else{ 
00388              Tj1 = amax/jmax;
00389              Ta =  Tj1 + (vmax - v0)/amax;
00390       }
00391       if ((vmax - v1)* jmax < amax * amax){
00392             Tj2 = sqrtf((vmax - v1)/ jmax);
00393             Td = 2.0F* Tj2;       
00394       }
00395       else{ 
00396              Tj2 =  amax / jmax;
00397              Td = Tj2 + (vmax - v1)/ amax;
00398       }
00399          Tv = (_q1 - _q0)/vmax -  Ta*(1.0F + v0 / vmax)/2.0F -  Td *(1.0F + v1 / vmax)/2.0F;
00400     
00401      if (Tv<0){
00402          float it = 0;
00403          float l = 0.99F;
00404          float max_iter = 200;
00405          while ((it < max_iter) && (amax >= 0.01F)){
00406                 //Vmax not reach
00407                 Tj1 = amax / jmax;
00408                 Tj2 = Tj1;
00409                 Tj = Tj2;
00410                 amax_squared = amax * amax;
00411                 sqrt_delta = sqrtf( ((amax_squared*amax_squared)/(jmax * jmax)) + 2.0F*(v0 * v0+  v1 * v1)
00412                     + amax * (4.0F*(_q1 - _q0) - 2.0F* amax *(v0 + v1)/jmax));
00413                 Ta = (amax_squared/jmax - 2.0F * v0 + sqrt_delta)/(2.0F * amax);
00414                 Td = (amax_squared/jmax - 2.0F * v1 + sqrt_delta)/(2.0F * amax);
00415                 Tv = 0;
00416                 if (Ta < 0.0F){
00417                     Td = 2.0F*((_q1 - _q0) / (v1 + v0));
00418                     Tj2 = (jmax *(_q1 - _q0)-sqrtf(jmax *(jmax*(powf((_q1-_q0),2.0F))+(powf((v1+v0),2.0F))*
00419                     (v1-v0))))/(jmax*(v1+v0));
00420                     Ta = 0;
00421                     Tj1 = 0;
00422                 }
00423                 if(Td < 0.0F){
00424                     Ta = 2.0F*((_q1-_q0)/(v1+v0));
00425                     Tj1 = (jmax*(_q1-_q0 )-sqrtf(jmax *(jmax *(powf((_q1 -_q0 ),2.0F))-(powf((v1 +v0 ),2.0F))*
00426                     (v1 -v0 ))))/(jmax *(v1 +v0 ));
00427                     Td  = 0;
00428                     Tj2  = 0;
00429                 }
00430             
00431                 if ((Ta  < 2.0F*Tj1 ) || (Td  < 2.0F*Tj2 )){
00432                     amax  =amax * l;
00433                 }
00434                 else{
00435                     break;
00436                 }
00437         }
00438      }
00439      alima  = jmax *Tj1 ;
00440      alimd  = -jmax  * Tj2 ;
00441      vmax  = v0  + (Ta  - Tj1 )*alima ;
00442      vlim  = vmax ;
00443      T  = Ta +Tv +Td ;
00444         
00445      qprim = 0;
00446      float t=0;
00447      TotalCycle = (int)T/stime+1;
00448      for(int tim=0; tim<((int)(T/stime)+1);tim++){
00449         t = tim*stime;
00450         if (t<Tj1 ){
00451             _q  = sigma *(_q0 +v0 *t+jmax *powf(t,0.5F));
00452             _qd = sigma*(v0+jmax*powf(t,2.0F)/2);
00453             _qdd = sigma*(jmax*t); 
00454         }
00455         else if (t<(Ta -Tj1 )){
00456             _q  = sigma *(_q0 +v0 *t+(3.0F*powf(t,2.0F)-3.0F*Tj1 *t+powf(Tj1 ,2.0F))*(alima /6.0F));
00457             _qd = sigma*(v0+alima*(t-(Tj1/2.0F)));
00458             _qdd = sigma*alima;
00459         }
00460         else if (t<Ta ){
00461             _q  = sigma *(_q0 +(vlim +v0 )*(Ta /2.0F)-vlim *(Ta -t)-jmin *powf((Ta -t),0.5F));
00462             _qd = sigma*(vlim+jmin*(powf((Ta-t),2.0F)/2.0F));
00463             _qdd = sigma*(-jmin*(Ta-t));
00464         }
00465         else if (t<(Ta +Tv )){
00466             _q  = sigma *(_q0 +(vlim +v0 )*(Ta /2.0F)+vlim *(t-Ta ));
00467             _qd = sigma*vlim;
00468             _qdd = 0;
00469         }
00470         else if (t<(T -Td +Tj2 )){
00471             _q  = sigma *(_q1 -(vlim +v1 )*(Td /2.0F)+vlim *(t-T +Td )-jmax *(powf((t-T +Td ),0.5F)));
00472             _qd = sigma*(vlim-jmax*(powf((t-T+Td),2.0F)/2.0F));
00473             _qdd = sigma*(-jmax*(t-T+Td));
00474         }
00475         else if (t<(T -Tj2 )){
00476             _q  = sigma *(_q1 -(vlim +v1 )*(Td /2.0F)+vlim *(t-T +Td )+(alimd /6.0F)*(3.0F*powf((t-T +Td ),2.0F)-3.0F*Tj2 *(t-T +Td )+powf(Tj2 ,2.0F)));
00477             _qd = sigma*(vlim+alimd*(t-T+Td-(Tj2/2.0F)));
00478             _qdd = sigma*alimd;
00479         }
00480         else if (t<=T ){
00481             _q  = sigma *(_q1 -v1 *(T -t)-jmax *(powf((T -t),3.0F))/6.0F);
00482             _qd = sigma*(v1+jmax*(powf((T-t),2.0F))/2.0F);
00483             _qdd = sigma*(-jmax*(T-t));
00484         }
00485         else{
00486             _q  = sigma *_q0 ;
00487             _qd = sigma*v1;
00488             _qdd = 0;
00489         }
00490         _Trajectory[index].Insert(_q,_qd,_qdd);
00491     }
00492     index++;
00493     return 1;
00494 }
00495 
00496 void MotionCommand::Spline_5th (float q0,float q1, float qd0, float qd1, float qdd,float stime,float tfin,float Constraints_v ,float Constraints_a){
00497     float a, b, c, d, e, f,_q,_qd,_qdd;
00498 
00499     if(qd0>Constraints_v){
00500         qd0 = Constraints_v;
00501     }
00502     if(qdd>Constraints_a){
00503         qdd = Constraints_a;
00504     }
00505     //Calc the parameters
00506     f  = (6.0F*q1)/powf(tfin,5.0F) - (6.0F*q0)/powf(tfin,5.0F) - (3.0F*qd0)/powf(tfin,4.0F) 
00507     - (3.0F*qd1)/powf(tfin,4.0F) - qdd/(2*powf(tfin,3.0F));
00508     e  = (15.0F*q0)/powf(tfin,4.0F) - (15.0F*q1)/powf(tfin,4.0F)+ (8.0F*qd0)/powf(tfin,3.0F) 
00509     + (7.0F*qd1)/powf(tfin,3.0F) + (3.0F*qdd)/(2.0F*powf(tfin,2.0F));
00510     d  = (10.F*q1)/powf(tfin,3.0F) - (10.0F*q0)/powf(tfin,3.0F) - (6.0F*qd0)/powf(tfin,2.0F) 
00511     - (4.0F*qd1)/powf(tfin,2.0F) - (3.0F*qdd)/(2.0F*tfin);
00512     c  = qdd/2.0F; 
00513     b  = qd0;
00514     a  = q0 ;
00515     
00516     TotalCycle = (int) tfin/stime;
00517 
00518     //Sampling in units of st (sampling time)
00519     for(int t=0;t<(int)((tfin/stime)+1);t++){
00520         _q  = a  + b *((float)t)*stime + c *powf(((float)t)*stime,2.0F) +
00521                         d *powf(((float)t)*stime,3.0F) + e *powf(((float)t)*stime,4.0F) + f *powf(((float)t)*stime,5.0F);
00522         _qd  = b  + 2.0F*c *((float)t)*stime + 3.0F*d *powf(((float)t)*stime,2.0F) +
00523                         4.0F*e *powf(((float)t)*stime,3.0F) + 5.0F*f *powf(((float)t)*stime,4.0F);
00524         _qdd  = 2.0F*c  + 6.0F*d *((float)t)*stime + 12.0F*e *powf(((float)t)*stime,2.0F) +
00525                         20*f *powf(((float)t)*stime,3.0F);
00526         _Trajectory[index].Insert(_q,_qd,_qdd);
00527     }
00528     index++;
00529 }
00530 
00531 
00532 void MotionCommand::Spline_7th(float q0,float q1, float qd0, float qd1, float qdd,float stime,float tfin,float Constraints_v ,float Constraints_a){
00533    float a, b, c, d, e, f, g, h,_q,_qd,_qdd;
00534    if(qd0>Constraints_v){
00535         qd0 = Constraints_v;
00536     }
00537     if(qdd>Constraints_a){
00538         qdd = Constraints_a;
00539     }
00540 
00541     //Calc the parameters  
00542     h = (35.0F*q1)/powf(tfin,4.0F)- (35.0F*q0)/powf(tfin,4.0F) - (20.0F*qd0)/powf(tfin,3.0F) - (15.0F*qd1)/powf(tfin,3.0F) - (5.0F*qdd)/powf(tfin,2.0F);
00543     g = (84.0F*q0)/powf(tfin,5.0F) - (84.0F*q1)/powf(tfin,5.0F) + (45.0F*qd0)/powf(tfin,4.0F) + (39.0F*qd1)/powf(tfin,4.0F) + (10.0F*qdd)/powf(tfin,3.0F);
00544     f = (70.0F*q1)/powf(tfin,6.0F) - (70.0F*q0)/powf(tfin,6.0F) - (36.0F*qd0)/powf(tfin,5.0F) - (34.0F*qd1)/powf(tfin,5.0F) - (15.0F*qdd)/(2.0F*powf(tfin,4.0F));
00545     e = (35.0F*q1)/powf(tfin,4.0F) - (35.0F*q0)/powf(tfin,4.0F) - (20.0F*qd0)/powf(tfin,3.0F) - (15.0F*qd1)/powf(tfin,3.0F) - (5.0F*qdd)/powf(tfin,2.0F);
00546     d = 0.0F;
00547     c = Constraints_a/2.0F;
00548     b = Constraints_v;
00549     a = q0;
00550     TotalCycle = (int) tfin/stime;
00551     //Sampling in units of st (sampling time)
00552     for(int t=0;t<(int) tfin/stime;t++){
00553             _q = a + b*((float)t)*stime + c*powf(((float)t)*stime,2.0F) + d*powf(((float)t)*stime,3.0F) +
00554                                                 e*powf(((float)t)*stime,4.0F) + f*powf(((float)t)*stime,5.0F) + g*powf(((float)t)*stime,6.0F)
00555                                                 + h*powf(((float)t)*stime,7.0F);
00556             _qd = b + 2.0F*c*((float)t)*stime + 3.0F*d*powf(((float)t)*stime,2.0F) +
00557                                                    4.0F*e*powf(((float)t)*stime,3.0F) + 5.0F*f*powf(((float)t)*stime,4.0F) +
00558                                                    6.0F*g*powf(((float)t)*stime,5.0F) + 7.0F*h*powf(((float)t)*stime,6.0F);
00559             _qdd = 2.0F*c + 6.0F*d*((float)t)*stime + 12.0F*e*powf(((float)t)*stime,2.0F) +
00560                                                     20.0F*f*powf(((float)t)*stime,3.0F) + 30.0F*g*powf(((float)t)*stime,4.0F) +
00561                                                     42.0F*h*powf(((float)t)*stime,5.0F);
00562             _Trajectory[index].Insert(_q,_qd,_qdd);
00563     }
00564     index++;
00565 
00566 }
00567