Robot
Embed:
(wiki syntax)
Show/hide line numbers
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
Generated on Thu Jul 14 2022 08:46:33 by
1.7.2