|
Fast Research Interface Library
Manual and Documentation
|
00001 // ---------------------- Doxygen info ---------------------- 00046 // ---------------------------------------------------------- 00047 // For a convenient reading of this file's source code, 00048 // please use a tab width of four characters. 00049 // ---------------------------------------------------------- 00050 00051 #include <TypeIRML.h> 00052 #include <TypeIRMLPolynomial.h> 00053 #include <TypeIRMLMath.h> 00054 #include <TypeIRMLVector.h> 00055 #include <TypeIRMLInputParameters.h> 00056 #include <TypeIRMLOutputParameters.h> 00057 #include <TypeIRMLDecision.h> 00058 #include <TypeIRMLProfiles.h> 00059 #include <string.h> 00060 #include <math.h> 00061 00062 using namespace TypeIRMLMath; 00063 00064 //************************************************************************************ 00065 // Constructor 00066 00067 00068 TypeIRML::TypeIRML(const unsigned int &NoOfDOFs, const double &CycleTimeInSeconds) 00069 { 00070 this->CycleTime = CycleTimeInSeconds; 00071 this->NumberOfDOFs = NoOfDOFs; 00072 this->TrajectoryExecutionTimeForTheUser = -1.0; 00073 this->InternalClockInSeconds = 0.0; 00074 00075 this->CurrentInputParameters = new TypeIRMLInputParameters (NumberOfDOFs); 00076 this->OutputParameters = new TypeIRMLOutputParameters(NumberOfDOFs); 00077 00078 this->Polynomials = new TypeIMotionPolynomials [NumberOfDOFs]; 00079 } 00080 00081 00082 //************************************************************************************ 00083 // Destructor 00084 00085 TypeIRML::~TypeIRML() 00086 { 00087 delete(this->CurrentInputParameters ); 00088 delete(this->OutputParameters ); 00089 delete[](this->Polynomials ); 00090 } 00091 00092 00093 //************************************************************************************ 00094 // GetNextMotionState_Position 00095 00096 int TypeIRML::GetNextMotionState_Position( const double* CurrentPosition 00097 , const double* CurrentVelocity 00098 , const double* MaxVelocity 00099 , const double* MaxAcceleration 00100 , const double* TargetPosition 00101 , const bool* SelectionVector 00102 , double* NewPosition 00103 , double* NewVelocity ) 00104 { 00105 int i = 0 00106 , ReturnValue = TypeIRML::RML_ERROR; 00107 00108 double MinimumSynchronizationTime = 0.0; 00109 00110 for (i = 0; i < this->NumberOfDOFs; i++) 00111 { 00112 (*this->CurrentInputParameters->SelectionVector) [i] = SelectionVector [i]; 00113 if (SelectionVector[i]) 00114 { 00115 if (MaxVelocity[i] < RML_MIN_VALUE_FOR_MAXVELOCITY) 00116 { 00117 return (TypeIRML::RML_MAX_VELOCITY_ERROR); 00118 } 00119 if (MaxAcceleration[i] < RML_MIN_VALUE_FOR_MAXACCELERATION) 00120 { 00121 return (TypeIRML::RML_MAX_ACCELERATION_ERROR); 00122 } 00123 00124 (*this->CurrentInputParameters->CurrentPosition ) [i] = CurrentPosition [i]; 00125 (*this->CurrentInputParameters->CurrentVelocity ) [i] = CurrentVelocity [i]; 00126 (*this->CurrentInputParameters->MaxVelocity ) [i] = MaxVelocity [i]; 00127 (*this->CurrentInputParameters->MaxAcceleration ) [i] = MaxAcceleration [i]; 00128 (*this->CurrentInputParameters->TargetPosition ) [i] = TargetPosition [i]; 00129 } 00130 } 00131 00132 // ******************************************************************************* 00133 // * Step 1: Calculate the minimum possile synchronization time 00134 // ******************************************************************************* 00135 00136 MinimumSynchronizationTime = TypeIRML::CalculateMinimumSynchronizationTime(*(this->CurrentInputParameters)); 00137 00138 // ******************************************************************************* 00139 // * Step 2: Synchronize all selected degrees of freedom 00140 // ******************************************************************************* 00141 00142 TypeIRML::SynchronizeTrajectory( *(this->CurrentInputParameters) 00143 , MinimumSynchronizationTime 00144 , this->Polynomials); 00145 00146 // ******************************************************************************* 00147 // * Step 3: Calculate output values 00148 // ******************************************************************************* 00149 00150 ReturnValue = TypeIRML::CalculateOutputValues( this->Polynomials 00151 , *(this->CurrentInputParameters->SelectionVector) 00152 , OutputParameters); 00153 00154 TrajectoryExecutionTimeForTheUser = MinimumSynchronizationTime; 00155 00156 if (ReturnValue != TypeIRML::RML_ERROR) 00157 { 00158 for (i = 0; i < this->NumberOfDOFs; i++) 00159 { 00160 if (SelectionVector[i]) 00161 { 00162 NewPosition[i] = (*OutputParameters->NewPosition)[i]; 00163 NewVelocity[i] = (*OutputParameters->NewVelocity)[i]; 00164 } 00165 } 00166 } 00167 00168 return (ReturnValue); 00169 } 00170 00171 00172 //************************************************************************************ 00173 // GetNextMotionState_Position 00174 00175 int TypeIRML::GetNextMotionState_Position( const TypeIRMLInputParameters &IP 00176 , TypeIRMLOutputParameters *OP ) 00177 { 00178 return(GetNextMotionState_Position( IP.CurrentPosition->VecData 00179 , IP.CurrentVelocity->VecData 00180 , IP.MaxVelocity->VecData 00181 , IP.MaxAcceleration->VecData 00182 , IP.TargetPosition->VecData 00183 , IP.SelectionVector->VecData 00184 , OP->NewPosition->VecData 00185 , OP->NewVelocity->VecData )); 00186 } 00187 00188 00189 //************************************************************************************ 00190 // CalculateMinimumSynchronizationTime() 00191 00192 double TypeIRML::CalculateMinimumSynchronizationTime(const TypeIRMLInputParameters &IP) const 00193 { 00194 int i = 0; 00195 00196 double CurrentPosition = 0.0 00197 , CurrentVelocity = 0.0 00198 , MaxVelocity = 0.0 00199 , MaxAcceleration = 0.0 00200 , TargetPosition = 0.0 00201 , ExecutionTimeForCurrentDOF = 0.0 00202 , MinimumExecutionTime = 0.0; 00203 00204 for (i = 0; i < this->NumberOfDOFs; i++) 00205 { 00206 if ((*IP.SelectionVector)[i]) 00207 { 00208 CurrentPosition = (*IP.CurrentPosition )[i]; 00209 CurrentVelocity = (*IP.CurrentVelocity )[i]; 00210 MaxVelocity = (*IP.MaxVelocity )[i]; 00211 MaxAcceleration = (*IP.MaxAcceleration )[i]; 00212 TargetPosition = (*IP.TargetPosition )[i]; 00213 ExecutionTimeForCurrentDOF = 0.0; 00214 00215 if (!TypeIRMLMath::Decision_1001(CurrentVelocity)) 00216 { 00217 CurrentPosition = -CurrentPosition ; 00218 CurrentVelocity = - CurrentVelocity ; 00219 TargetPosition = -TargetPosition ; 00220 } 00221 00222 if (!TypeIRMLMath::Decision_1002( CurrentVelocity 00223 , MaxVelocity)) 00224 { 00225 // v --> vmax 00226 ExecutionTimeForCurrentDOF += (CurrentVelocity - MaxVelocity) / MaxAcceleration; 00227 CurrentPosition += 0.5 * (pow2(CurrentVelocity) - pow2(MaxVelocity)) 00228 / MaxAcceleration; 00229 CurrentVelocity = MaxVelocity; 00230 } 00231 00232 if (!TypeIRMLMath::Decision_1003( CurrentPosition 00233 , CurrentVelocity 00234 , MaxAcceleration 00235 , TargetPosition)) 00236 { 00237 // v --> 0 00238 ExecutionTimeForCurrentDOF += CurrentVelocity / MaxAcceleration; 00239 CurrentPosition += 0.5 * pow2(CurrentVelocity) / MaxAcceleration; 00240 CurrentVelocity = 0.0; 00241 00242 // switch signs 00243 CurrentPosition = -CurrentPosition ; 00244 CurrentVelocity = -CurrentVelocity ; 00245 TargetPosition = -TargetPosition ; 00246 00247 } 00248 00249 if (TypeIRMLMath::Decision_1004( CurrentPosition 00250 , CurrentVelocity 00251 , MaxVelocity 00252 , MaxAcceleration 00253 , TargetPosition)) 00254 { 00255 ExecutionTimeForCurrentDOF += TypeIRMLMath::ProfileStep1PosTrap( CurrentPosition 00256 , CurrentVelocity 00257 , MaxVelocity 00258 , MaxAcceleration 00259 , TargetPosition); 00260 } 00261 else 00262 { 00263 ExecutionTimeForCurrentDOF += TypeIRMLMath::ProfileStep1PosTri( CurrentPosition 00264 , CurrentVelocity 00265 , MaxAcceleration 00266 , TargetPosition); 00267 } 00268 } 00269 00270 if (ExecutionTimeForCurrentDOF > MinimumExecutionTime) 00271 { 00272 MinimumExecutionTime = ExecutionTimeForCurrentDOF; 00273 } 00274 } 00275 00276 return (MinimumExecutionTime); 00277 } 00278 00279 00280 //************************************************************************************ 00281 // SynchronizeTrajectory() 00282 00283 void TypeIRML::SynchronizeTrajectory( const TypeIRMLInputParameters &IP 00284 , const double &SynchronizationTime 00285 , TypeIMotionPolynomials *PolynomialArray) 00286 { 00287 bool SignsWereSwitched = false; 00288 00289 int i = 0; 00290 00291 double CurrentPosition = 0.0 00292 , CurrentVelocity = 0.0 00293 , MaxVelocity = 0.0 00294 , MaxAcceleration = 0.0 00295 , TargetPosition = 0.0 00296 , ElapsedTime = 0.0; 00297 00298 for (i = 0; i < this->NumberOfDOFs; i++) 00299 { 00300 if ((*IP.SelectionVector)[i]) 00301 { 00302 CurrentPosition = (*IP.CurrentPosition )[i]; 00303 CurrentVelocity = (*IP.CurrentVelocity )[i]; 00304 MaxVelocity = (*IP.MaxVelocity )[i]; 00305 MaxAcceleration = (*IP.MaxAcceleration )[i]; 00306 TargetPosition = (*IP.TargetPosition )[i]; 00307 00308 SignsWereSwitched = false; 00309 ElapsedTime = 0.0; 00310 00311 Polynomials[i].ValidPolynomials = 0; 00312 00313 if (!TypeIRMLMath::Decision_2001(CurrentVelocity)) 00314 { 00315 SignsWereSwitched = !SignsWereSwitched ; 00316 CurrentPosition = -CurrentPosition ; 00317 CurrentVelocity = -CurrentVelocity ; 00318 TargetPosition = -TargetPosition ; 00319 } 00320 00321 if (!TypeIRMLMath::Decision_2002( CurrentVelocity 00322 , MaxVelocity)) 00323 { 00324 TypeIRMLMath::ProfileStep2V_To_Vmax( &CurrentPosition 00325 , &CurrentVelocity 00326 , MaxVelocity 00327 , MaxAcceleration 00328 , &ElapsedTime 00329 , SignsWereSwitched 00330 , &(PolynomialArray[i])); 00331 } 00332 00333 if (TypeIRMLMath::Decision_2003( CurrentPosition 00334 , CurrentVelocity 00335 , MaxAcceleration 00336 , TargetPosition)) 00337 { 00338 00339 if (TypeIRMLMath::Decision_2004( CurrentPosition 00340 , CurrentVelocity 00341 , MaxAcceleration 00342 , TargetPosition 00343 , SynchronizationTime)) 00344 { 00345 TypeIRMLMath::ProfileStep2PosTrap( &CurrentPosition 00346 , &CurrentVelocity 00347 , MaxAcceleration 00348 , TargetPosition 00349 , SynchronizationTime 00350 , &ElapsedTime 00351 , SignsWereSwitched 00352 , &(PolynomialArray[i])); 00353 00354 } 00355 else 00356 { 00357 TypeIRMLMath::ProfileStep2NegHldNegLin( &CurrentPosition 00358 , &CurrentVelocity 00359 , MaxAcceleration 00360 , TargetPosition 00361 , SynchronizationTime 00362 , &ElapsedTime 00363 , SignsWereSwitched 00364 , &(PolynomialArray[i])); 00365 } 00366 } 00367 else 00368 { 00369 TypeIRMLMath::ProfileStep2V_To_Zero( &CurrentPosition 00370 , &CurrentVelocity 00371 , MaxAcceleration 00372 , &ElapsedTime 00373 , SignsWereSwitched 00374 , &(PolynomialArray[i])); 00375 00376 // Switch signs 00377 SignsWereSwitched = !SignsWereSwitched ; 00378 CurrentPosition = -CurrentPosition ; 00379 CurrentVelocity = -CurrentVelocity ; 00380 TargetPosition = -TargetPosition ; 00381 00382 TypeIRMLMath::ProfileStep2PosTrap( &CurrentPosition 00383 , &CurrentVelocity 00384 , MaxAcceleration 00385 , TargetPosition 00386 , SynchronizationTime 00387 , &ElapsedTime 00388 , SignsWereSwitched 00389 , &(PolynomialArray[i])); 00390 } 00391 } 00392 } 00393 00394 return; 00395 } 00396 00397 00398 //************************************************************************************ 00399 // CalculateOutputValues() 00400 00401 int TypeIRML::CalculateOutputValues( const TypeIMotionPolynomials *PolynomialArray 00402 , const TypeIRMLBoolVector &SelectionVector 00403 , TypeIRMLOutputParameters *OP) 00404 { 00405 int i = 0 00406 , SegmentCounter = 0 00407 , ReturnValue = TypeIRML::RML_FINAL_STATE_REACHED; 00408 00409 00410 for (i = 0; i < this->NumberOfDOFs; i++) 00411 { 00412 SegmentCounter = 0; 00413 00414 if (SelectionVector[i]) 00415 { 00416 while (this->CycleTime >= PolynomialArray[i].PolynomialTimes[SegmentCounter]) 00417 { 00418 SegmentCounter++; 00419 if (SegmentCounter > PolynomialArray[i].ValidPolynomials) 00420 { 00421 return(TypeIRML::RML_ERROR); 00422 } 00423 } 00424 00425 (*OP->NewPosition)[i] = PolynomialArray[i].PositionPolynomial[SegmentCounter].CalculateValue(this->CycleTime); 00426 (*OP->NewVelocity)[i] = PolynomialArray[i].VelocityPolynomial[SegmentCounter].CalculateValue(this->CycleTime); 00427 00428 00429 if (SegmentCounter + 1 < PolynomialArray[i].ValidPolynomials) 00430 { 00431 ReturnValue = TypeIRML::RML_WORKING; 00432 } 00433 } 00434 } 00435 00436 return (ReturnValue); 00437 } 00438 00439 00440 //************************************************************************************ 00441 // GetNextMotionState_Velocity 00442 00443 int TypeIRML::GetNextMotionState_Velocity( const double* CurrentPosition 00444 , const double* CurrentVelocity 00445 , const double* MaxAcceleration 00446 , const double* TargetVelocity 00447 , const bool* SelectionVector 00448 , double* NewPosition 00449 , double* NewVelocity ) 00450 { 00451 int i = 0 00452 , ReturnValue = TypeIRML::RML_FINAL_STATE_REACHED; 00453 00454 double MinimumExecutionTime = 0.0; 00455 00456 this->TrajectoryExecutionTimeForTheUser = 0.0; 00457 00458 for (i = 0; i < this->NumberOfDOFs; i++) 00459 { 00460 if (SelectionVector[i]) 00461 { 00462 if (MaxAcceleration[i] < RML_MIN_VALUE_FOR_MAXACCELERATION) 00463 { 00464 return (TypeIRML::RML_MAX_ACCELERATION_ERROR); 00465 } 00466 00467 MinimumExecutionTime = fabs(CurrentVelocity[i] - TargetVelocity[i]) / MaxAcceleration[i]; 00468 00469 if (MinimumExecutionTime > this->TrajectoryExecutionTimeForTheUser) 00470 { 00471 this->TrajectoryExecutionTimeForTheUser = MinimumExecutionTime; 00472 } 00473 00474 if (MinimumExecutionTime > this->CycleTime) 00475 { 00476 if (CurrentVelocity[i] > TargetVelocity[i]) 00477 { 00478 NewVelocity[i] = CurrentVelocity[i] - MaxAcceleration[i] * this->CycleTime; 00479 } 00480 else 00481 { 00482 NewVelocity[i] = CurrentVelocity[i] + MaxAcceleration[i] * this->CycleTime; 00483 } 00484 NewPosition[i] = CurrentPosition[i] + 0.5 * (NewVelocity[i] + CurrentVelocity[i]) * this->CycleTime; 00485 00486 ReturnValue = TypeIRML::RML_WORKING; 00487 } 00488 else 00489 { 00490 NewVelocity[i] = TargetVelocity[i]; 00491 NewPosition[i] = CurrentPosition[i] + 0.5 * MinimumExecutionTime 00492 * (CurrentVelocity[i] - TargetVelocity[i]) 00493 + TargetVelocity[i] * this->CycleTime; 00494 } 00495 } 00496 } 00497 00498 return(ReturnValue); 00499 } 00500 00501 00502 //************************************************************************************ 00503 // GetNextMotionState_Velocity 00504 00505 int TypeIRML::GetNextMotionState_Velocity( const TypeIRMLInputParameters &IP 00506 , TypeIRMLOutputParameters *OP ) 00507 { 00508 return(GetNextMotionState_Velocity( IP.CurrentPosition->VecData 00509 , IP.CurrentVelocity->VecData 00510 , IP.MaxAcceleration->VecData 00511 , IP.TargetVelocity->VecData 00512 , IP.SelectionVector->VecData 00513 , OP->NewPosition->VecData 00514 , OP->NewVelocity->VecData )); 00515 } 00516 00517 00518 //******************************************************************************************* 00519 // GetExecutionTime() 00520 00521 double TypeIRML::GetExecutionTime(void) const 00522 { 00523 return(TrajectoryExecutionTimeForTheUser); 00524 } 00525