|
Fast Research Interface Library
Manual and Documentation
|
00001 // ---------------------- Doxygen info ---------------------- 00047 // ---------------------------------------------------------- 00048 // For a convenient reading of this file's source code, 00049 // please use a tab width of four characters. 00050 // ---------------------------------------------------------- 00051 00052 00053 00054 #include <TypeIRMLProfiles.h> 00055 #include <TypeIRMLMath.h> 00056 #include <math.h> 00057 00058 00059 00060 00061 //************************************************************************************ 00062 // ProfileStep1PosTri() 00063 00064 double TypeIRMLMath::ProfileStep1PosTri( const double &CurrentPosition 00065 , const double &CurrentVelocity 00066 , const double &MaxAcceleration 00067 , const double &TargetPosition) 00068 { 00069 return ( ( 2.0 00070 * TypeIRMLMath::RMLSqrt(MaxAcceleration 00071 * (TargetPosition - CurrentPosition) + 0.5 00072 * pow2(CurrentVelocity)) - CurrentVelocity) 00073 / MaxAcceleration); 00074 } 00075 00076 00077 //************************************************************************************ 00078 // ProfileStep1PosTrap() 00079 00080 double TypeIRMLMath::ProfileStep1PosTrap( const double &CurrentPosition 00081 , const double &CurrentVelocity 00082 , const double &MaxVelocity 00083 , const double &MaxAcceleration 00084 , const double &TargetPosition) 00085 { 00086 return ( ( TargetPosition - CurrentPosition) 00087 / MaxVelocity + ( MaxVelocity 00088 - CurrentVelocity + 0.5 * pow2(CurrentVelocity) 00089 / MaxVelocity ) / MaxAcceleration); 00090 } 00091 00092 00093 //************************************************************************************ 00094 // ProfileStep2V_To_Vmax() 00095 00096 void TypeIRMLMath::ProfileStep2V_To_Vmax( double *CurrentPosition 00097 , double *CurrentVelocity 00098 , const double &MaxVelocity 00099 , const double &MaxAcceleration 00100 , double *ElapsedTime 00101 , const bool &SignsAreInverted 00102 , TypeIRMLMath::TypeIMotionPolynomials *P ) 00103 { 00104 double TimeForThisSegment = 0.0; 00105 00106 TimeForThisSegment = fabs((*CurrentVelocity - MaxVelocity) / MaxAcceleration ); 00107 00108 P->PolynomialTimes[P->ValidPolynomials] = *ElapsedTime + TimeForThisSegment; 00109 00110 if (SignsAreInverted) 00111 { 00112 P->PositionPolynomial[P->ValidPolynomials].SetCoefficients( (0.5 * MaxAcceleration), -*CurrentVelocity, -*CurrentPosition, *ElapsedTime); 00113 P->VelocityPolynomial[P->ValidPolynomials].SetCoefficients( 0.0, MaxAcceleration, -*CurrentVelocity, *ElapsedTime); 00114 } 00115 else 00116 { 00117 P->PositionPolynomial[P->ValidPolynomials].SetCoefficients( (-0.5 * MaxAcceleration), *CurrentVelocity, *CurrentPosition, *ElapsedTime); 00118 P->VelocityPolynomial[P->ValidPolynomials].SetCoefficients( 0.0, -MaxAcceleration, *CurrentVelocity, *ElapsedTime); 00119 } 00120 00121 P->ValidPolynomials++; 00122 00123 *CurrentPosition += 0.5 * (pow2(*CurrentVelocity) - pow2(MaxVelocity)) / MaxAcceleration; 00124 *CurrentVelocity = MaxVelocity; 00125 *ElapsedTime += TimeForThisSegment; 00126 00127 00128 return; 00129 } 00130 00131 00132 //************************************************************************************ 00133 // ProfileStep2V_To_Zero() 00134 00135 void TypeIRMLMath::ProfileStep2V_To_Zero( double *CurrentPosition 00136 , double *CurrentVelocity 00137 , const double &MaxAcceleration 00138 , double *ElapsedTime 00139 , const bool &SignsAreInverted 00140 , TypeIRMLMath::TypeIMotionPolynomials *P ) 00141 { 00142 double TimeForThisSegment = 0.0; 00143 00144 TimeForThisSegment = fabs( *CurrentVelocity / MaxAcceleration ); 00145 00146 P->PolynomialTimes[P->ValidPolynomials] = *ElapsedTime + TimeForThisSegment; 00147 00148 if (SignsAreInverted) 00149 { 00150 P->PositionPolynomial[P->ValidPolynomials].SetCoefficients( (0.5 * MaxAcceleration), -*CurrentVelocity, -*CurrentPosition, *ElapsedTime); 00151 P->VelocityPolynomial[P->ValidPolynomials].SetCoefficients( 0.0, MaxAcceleration, -*CurrentVelocity, *ElapsedTime); 00152 } 00153 else 00154 { 00155 P->PositionPolynomial[P->ValidPolynomials].SetCoefficients( (-0.5 * MaxAcceleration), *CurrentVelocity, *CurrentPosition, *ElapsedTime); 00156 P->VelocityPolynomial[P->ValidPolynomials].SetCoefficients( 0.0, -MaxAcceleration, *CurrentVelocity, *ElapsedTime); 00157 } 00158 00159 P->ValidPolynomials++; 00160 00161 *CurrentPosition += 0.5 * pow2(*CurrentVelocity) / MaxAcceleration; 00162 *CurrentVelocity = 0.0; 00163 00164 *ElapsedTime += TimeForThisSegment; 00165 00166 00167 return; 00168 } 00169 00170 00171 //************************************************************************************ 00172 // ProfileStep2PosTrap() 00173 00174 void TypeIRMLMath::ProfileStep2PosTrap( double *CurrentPosition 00175 , double *CurrentVelocity 00176 , const double &MaxAcceleration 00177 , const double &TargetPosition 00178 , const double &SynchronizationTime 00179 , double *ElapsedTime 00180 , const bool &SignsAreInverted 00181 , TypeIRMLMath::TypeIMotionPolynomials *P ) 00182 { 00183 double HoldVelocity = 0.0 00184 , Time2 = 0.0 00185 , Time3 = 0.0 00186 , Position2 = 0.0 00187 , Position3 = 0.0; 00188 00189 // Calculate all trajectory parameters 00190 00191 HoldVelocity = 0.5 * (MaxAcceleration * (SynchronizationTime - *ElapsedTime) + *CurrentVelocity) 00192 - 0.5 * TypeIRMLMath::RMLSqrt(4.0 * MaxAcceleration * (*CurrentPosition - TargetPosition) 00193 + MaxAcceleration * (SynchronizationTime - *ElapsedTime) 00194 * (MaxAcceleration * (SynchronizationTime - *ElapsedTime) + 2.0 * *CurrentVelocity) 00195 - pow2(*CurrentVelocity)); 00196 00197 Time2 = *ElapsedTime + (HoldVelocity - *CurrentVelocity) / MaxAcceleration; 00198 Time3 = SynchronizationTime - HoldVelocity / MaxAcceleration; 00199 Position2 = *CurrentPosition + 0.5 * (Time2 - *ElapsedTime) * (HoldVelocity + *CurrentVelocity); 00200 Position3 = TargetPosition - 0.5 * (SynchronizationTime - Time3) * HoldVelocity; 00201 00202 // Set up up polynomials 00203 00204 // 1st segment 00205 00206 P->PolynomialTimes[P->ValidPolynomials] = Time2; 00207 00208 if (SignsAreInverted) 00209 { 00210 P->PositionPolynomial[P->ValidPolynomials].SetCoefficients( (-0.5 * MaxAcceleration), -*CurrentVelocity, -*CurrentPosition, *ElapsedTime); 00211 P->VelocityPolynomial[P->ValidPolynomials].SetCoefficients( 0.0, -MaxAcceleration, -*CurrentVelocity, *ElapsedTime); 00212 } 00213 else 00214 { 00215 P->PositionPolynomial[P->ValidPolynomials].SetCoefficients( (0.5 * MaxAcceleration), *CurrentVelocity, *CurrentPosition, *ElapsedTime); 00216 P->VelocityPolynomial[P->ValidPolynomials].SetCoefficients( 0.0, MaxAcceleration, *CurrentVelocity, *ElapsedTime); 00217 } 00218 00219 P->ValidPolynomials++; 00220 00221 *CurrentPosition = Position2; 00222 *CurrentVelocity = HoldVelocity; 00223 *ElapsedTime = Time2; 00224 00225 00226 // 2nd segment 00227 00228 P->PolynomialTimes[P->ValidPolynomials] = Time3; 00229 00230 if (SignsAreInverted) 00231 { 00232 P->PositionPolynomial[P->ValidPolynomials].SetCoefficients( 0.0, -*CurrentVelocity, -*CurrentPosition, *ElapsedTime); 00233 P->VelocityPolynomial[P->ValidPolynomials].SetCoefficients( 0.0, 0.0, -*CurrentVelocity, *ElapsedTime); 00234 } 00235 else 00236 { 00237 P->PositionPolynomial[P->ValidPolynomials].SetCoefficients( 0.0, *CurrentVelocity, *CurrentPosition, *ElapsedTime); 00238 P->VelocityPolynomial[P->ValidPolynomials].SetCoefficients( 0.0, 0.0, *CurrentVelocity, *ElapsedTime); 00239 } 00240 00241 P->ValidPolynomials++; 00242 00243 *CurrentPosition = Position3; 00244 *CurrentVelocity = HoldVelocity; 00245 *ElapsedTime = Time3; 00246 00247 00248 // 3rd segment 00249 00250 P->PolynomialTimes[P->ValidPolynomials] = SynchronizationTime; 00251 00252 if (SignsAreInverted) 00253 { 00254 P->PositionPolynomial[P->ValidPolynomials].SetCoefficients( (0.5 * MaxAcceleration), -*CurrentVelocity, -*CurrentPosition, *ElapsedTime); 00255 P->VelocityPolynomial[P->ValidPolynomials].SetCoefficients( 0.0, MaxAcceleration, -*CurrentVelocity, *ElapsedTime); 00256 } 00257 else 00258 { 00259 P->PositionPolynomial[P->ValidPolynomials].SetCoefficients( (-0.5 * MaxAcceleration), *CurrentVelocity, *CurrentPosition, *ElapsedTime); 00260 P->VelocityPolynomial[P->ValidPolynomials].SetCoefficients( 0.0, -MaxAcceleration, *CurrentVelocity, *ElapsedTime); 00261 } 00262 00263 P->ValidPolynomials++; 00264 00265 *CurrentPosition = TargetPosition; 00266 *CurrentVelocity = 0.0; 00267 *ElapsedTime = SynchronizationTime; 00268 00269 00270 // Final segment 00271 00272 P->PolynomialTimes[P->ValidPolynomials] = RML_INFINITY; 00273 00274 if (SignsAreInverted) 00275 { 00276 P->PositionPolynomial[P->ValidPolynomials].SetCoefficients( 0.0, 0.0, -*CurrentPosition, *ElapsedTime); 00277 P->VelocityPolynomial[P->ValidPolynomials].SetCoefficients( 0.0, 0.0, 0.0, *ElapsedTime); 00278 } 00279 else 00280 { 00281 P->PositionPolynomial[P->ValidPolynomials].SetCoefficients( 0.0, 0.0, *CurrentPosition, *ElapsedTime); 00282 P->VelocityPolynomial[P->ValidPolynomials].SetCoefficients( 0.0, 0.0, 0.0, *ElapsedTime); 00283 } 00284 00285 P->ValidPolynomials++; 00286 00287 return; 00288 } 00289 00290 00291 //************************************************************************************ 00292 // ProfileStep2NegHldNegLin() 00293 00294 void TypeIRMLMath::ProfileStep2NegHldNegLin( double *CurrentPosition 00295 , double *CurrentVelocity 00296 , const double &MaxAcceleration 00297 , const double &TargetPosition 00298 , const double &SynchronizationTime 00299 , double *ElapsedTime 00300 , const bool &SignsAreInverted 00301 , TypeIRMLMath::TypeIMotionPolynomials *P ) 00302 { 00303 double HoldVelocity = 0.0 00304 , Time2 = 0.0 00305 , Time3 = 0.0 00306 , Position2 = 0.0 00307 , Position3 = 0.0 00308 , Denominator = 0.0; 00309 00310 // Calculate all trajectory parameters 00311 00312 Denominator = (2.0 * MaxAcceleration * (SynchronizationTime - *ElapsedTime) 00313 - 2.0 * *CurrentVelocity); 00314 00315 if (fabs(Denominator) < RML_DENOMINATOR_EPSILON) 00316 { 00317 HoldVelocity = *CurrentVelocity; 00318 Time2 = *ElapsedTime; 00319 Time3 = *ElapsedTime; 00320 Position2 = *CurrentPosition; 00321 Position3 = *CurrentPosition; 00322 } 00323 else 00324 { 00325 HoldVelocity = (2.0 * MaxAcceleration 00326 * (TargetPosition - *CurrentPosition) 00327 - pow2(*CurrentVelocity)) 00328 / Denominator; 00329 00330 Time2 = *ElapsedTime + (*CurrentVelocity - HoldVelocity) / MaxAcceleration; 00331 Time3 = SynchronizationTime - HoldVelocity / MaxAcceleration; 00332 Position2 = *CurrentPosition + 0.5 * (Time2 - *ElapsedTime) * (HoldVelocity + *CurrentVelocity); 00333 Position3 = TargetPosition - 0.5 * (SynchronizationTime - Time3) * HoldVelocity; 00334 } 00335 00336 // Set up up polynomials 00337 00338 // 1st segment 00339 00340 P->PolynomialTimes[P->ValidPolynomials] = Time2; 00341 00342 if (SignsAreInverted) 00343 { 00344 P->PositionPolynomial[P->ValidPolynomials].SetCoefficients( (0.5 * MaxAcceleration), -*CurrentVelocity, -*CurrentPosition, *ElapsedTime); 00345 P->VelocityPolynomial[P->ValidPolynomials].SetCoefficients( 0.0, MaxAcceleration, -*CurrentVelocity, *ElapsedTime); 00346 } 00347 else 00348 { 00349 P->PositionPolynomial[P->ValidPolynomials].SetCoefficients( (-0.5 * MaxAcceleration), *CurrentVelocity, *CurrentPosition, *ElapsedTime); 00350 P->VelocityPolynomial[P->ValidPolynomials].SetCoefficients( 0.0, -MaxAcceleration, *CurrentVelocity, *ElapsedTime); 00351 } 00352 00353 P->ValidPolynomials++; 00354 00355 *CurrentPosition = Position2; 00356 *CurrentVelocity = HoldVelocity; 00357 *ElapsedTime = Time2; 00358 00359 00360 // 2nd segment 00361 00362 P->PolynomialTimes[P->ValidPolynomials] = Time3; 00363 00364 if (SignsAreInverted) 00365 { 00366 P->PositionPolynomial[P->ValidPolynomials].SetCoefficients( 0.0, -*CurrentVelocity, -*CurrentPosition, *ElapsedTime); 00367 P->VelocityPolynomial[P->ValidPolynomials].SetCoefficients( 0.0, 0.0, -*CurrentVelocity, *ElapsedTime); 00368 } 00369 else 00370 { 00371 P->PositionPolynomial[P->ValidPolynomials].SetCoefficients( 0.0, *CurrentVelocity, *CurrentPosition, *ElapsedTime); 00372 P->VelocityPolynomial[P->ValidPolynomials].SetCoefficients( 0.0, 0.0, *CurrentVelocity, *ElapsedTime); 00373 } 00374 00375 P->ValidPolynomials++; 00376 00377 *CurrentPosition = Position3; 00378 *CurrentVelocity = HoldVelocity; 00379 *ElapsedTime = Time3; 00380 00381 00382 // 3rd segment 00383 00384 P->PolynomialTimes[P->ValidPolynomials] = SynchronizationTime; 00385 00386 if (SignsAreInverted) 00387 { 00388 P->PositionPolynomial[P->ValidPolynomials].SetCoefficients( (0.5 * MaxAcceleration), -*CurrentVelocity, -*CurrentPosition, *ElapsedTime); 00389 P->VelocityPolynomial[P->ValidPolynomials].SetCoefficients( 0.0, MaxAcceleration, -*CurrentVelocity, *ElapsedTime); 00390 } 00391 else 00392 { 00393 P->PositionPolynomial[P->ValidPolynomials].SetCoefficients( (-0.5 * MaxAcceleration), *CurrentVelocity, *CurrentPosition, *ElapsedTime); 00394 P->VelocityPolynomial[P->ValidPolynomials].SetCoefficients( 0.0, -MaxAcceleration, *CurrentVelocity, *ElapsedTime); 00395 } 00396 00397 P->ValidPolynomials++; 00398 00399 *CurrentPosition = TargetPosition; 00400 *CurrentVelocity = 0.0; 00401 *ElapsedTime = SynchronizationTime; 00402 00403 00404 // Final segment 00405 00406 P->PolynomialTimes[P->ValidPolynomials] = RML_INFINITY; 00407 00408 if (SignsAreInverted) 00409 { 00410 P->PositionPolynomial[P->ValidPolynomials].SetCoefficients( 0.0, 0.0, -*CurrentPosition, *ElapsedTime); 00411 P->VelocityPolynomial[P->ValidPolynomials].SetCoefficients( 0.0, 0.0, 0.0, *ElapsedTime); 00412 } 00413 else 00414 { 00415 P->PositionPolynomial[P->ValidPolynomials].SetCoefficients( 0.0, 0.0, *CurrentPosition, *ElapsedTime); 00416 P->VelocityPolynomial[P->ValidPolynomials].SetCoefficients( 0.0, 0.0, 0.0, *ElapsedTime); 00417 } 00418 00419 P->ValidPolynomials++; 00420 00421 return; 00422 }