 |
Reflexxes Motion Libraries
Manual and Documentation (Type II, Version 1.2.6)
|
|
Reflexxes Motion Libraries
Manual and Documentation (Type II, Version 1.2.6)
|
00001 // ---------------------- Doxygen info ---------------------- 00040 // ---------------------------------------------------------- 00041 // For a convenient reading of this file's source code, 00042 // please use a tab width of four characters. 00043 // ---------------------------------------------------------- 00044 00045 00046 #ifndef __TypeIIRMLPosition__ 00047 #define __TypeIIRMLPosition__ 00048 00049 00050 #include <RMLVelocityInputParameters.h> 00051 #include <RMLVelocityOutputParameters.h> 00052 #include <RMLPositionInputParameters.h> 00053 #include <RMLPositionOutputParameters.h> 00054 #include <RMLPositionFlags.h> 00055 #include <RMLVelocityFlags.h> 00056 #include <RMLVector.h> 00057 #include <TypeIIRMLPolynomial.h> 00058 #include <TypeIIRMLStep1Profiles.h> 00059 #include <TypeIIRMLVelocity.h> 00060 00061 00062 using namespace TypeIIRMLMath; 00063 00064 00065 // ---------------------- Doxygen info ---------------------- 00144 // ---------------------------------------------------------- 00145 class TypeIIRMLPosition 00146 { 00147 public: 00148 00149 00150 // ---------------------- Doxygen info ---------------------- 00173 // ---------------------------------------------------------- 00174 TypeIIRMLPosition( const unsigned int &DegreesOfFreedom 00175 , const double &CycleTimeInSeconds); 00176 00177 00178 // ---------------------- Doxygen info ---------------------- 00189 // ---------------------------------------------------------- 00190 ~TypeIIRMLPosition(void); 00191 00192 00193 // ---------------------- Doxygen info ---------------------- 00269 // ---------------------------------------------------------- 00270 int GetNextStateOfMotion( const RMLPositionInputParameters &InputValues 00271 , RMLPositionOutputParameters *OutputValues 00272 , const RMLPositionFlags &Flags); 00273 00274 00275 // ---------------------- Doxygen info ---------------------- 00331 // ---------------------------------------------------------- 00332 int GetNextStateOfMotionAtTime( const double &TimeValueInSeconds 00333 , RMLPositionOutputParameters *OutputValues ) const; 00334 00335 protected: 00336 00337 // ---------------------- Doxygen info ---------------------- 00342 // ---------------------------------------------------------- 00343 enum FunctionResults 00344 { 00346 FUNC_SUCCESS = false, 00348 FUNC_ERROR_OCCURRED = true 00349 }; 00350 00351 00352 // ---------------------- Doxygen info ---------------------- 00371 // ---------------------------------------------------------- 00372 enum DominatValueForPhaseSync 00373 { 00375 UNDEFINED = 0, 00380 POSITION = 1, 00384 CURRENT_VELOCITY = 3, 00388 TARGET_VELOCITY = 4 00389 }; 00390 00391 00392 // ---------------------- Doxygen info ---------------------- 00418 // ---------------------------------------------------------- 00419 void CompareInitialAndTargetStateofMotion(void); 00420 00421 00422 // ---------------------- Doxygen info ---------------------- 00461 // ---------------------------------------------------------- 00462 void Step1(void); 00463 00464 00465 // ---------------------- Doxygen info ---------------------- 00508 // ---------------------------------------------------------- 00509 void Step2(void); 00510 00511 00512 // ---------------------- Doxygen info ---------------------- 00548 // ---------------------------------------------------------- 00549 int Step3( const double &TimeValueInSeconds 00550 , RMLPositionOutputParameters *OP ) const; 00551 00552 00553 // ---------------------- Doxygen info ---------------------- 00623 // ---------------------------------------------------------- 00624 void FallBackStrategy( const RMLPositionInputParameters &InputValues 00625 , RMLPositionOutputParameters *OutputValues 00626 , const RMLPositionFlags &InputsFlags); 00627 00628 00629 // ---------------------- Doxygen info ---------------------- 00665 // ---------------------------------------------------------- 00666 bool IsWithinAnInoperativeTimeInterval( const double &SynchronizationTimeCandidate 00667 , const RMLDoubleVector &MaximalExecutionTime 00668 , const RMLDoubleVector &AlternativeExecutionTime) const; 00669 00670 00671 // ---------------------- Doxygen info ---------------------- 00734 // ---------------------------------------------------------- 00735 bool IsPhaseSynchronizationPossible(RMLDoubleVector *ReferenceVector); 00736 00737 00738 // ---------------------- Doxygen info ---------------------- 00763 // ---------------------------------------------------------- 00764 void Step2PhaseSynchronization(void); 00765 00766 00767 // ---------------------- Doxygen info ---------------------- 00815 // ---------------------------------------------------------- 00816 void CalculatePositionalExtrems( const double &TimeValueInSeconds 00817 , RMLPositionOutputParameters *OP ) const; 00818 00819 00820 // ---------------------- Doxygen info ---------------------- 00858 // ---------------------------------------------------------- 00859 void SetPositionalExtremsToZero(RMLPositionOutputParameters *OP) const; 00860 00861 00862 // ---------------------- Doxygen info ---------------------- 00899 // ---------------------------------------------------------- 00900 void SetupModifiedSelectionVector(void); 00901 00902 00903 // ---------------------- Doxygen info ---------------------- 00917 // ---------------------------------------------------------- 00918 unsigned int GetNumberOfSelectedDOFs(const RMLBoolVector &BoolVector) const; 00919 00920 00921 // ---------------------- Doxygen info ---------------------- 00931 // ---------------------------------------------------------- 00932 bool CurrentTrajectoryIsPhaseSynchronized; 00933 00934 00935 // ---------------------- Doxygen info ---------------------- 00944 // ---------------------------------------------------------- 00945 bool CurrentTrajectoryIsNotSynchronized; 00946 00947 00948 // ---------------------- Doxygen info ---------------------- 00955 // ---------------------------------------------------------- 00956 bool CalculatePositionalExtremsFlag; 00957 00958 00959 // ---------------------- Doxygen info ---------------------- 00967 // ---------------------------------------------------------- 00968 int ReturnValue; 00969 00970 00971 // ---------------------- Doxygen info ---------------------- 00978 // ---------------------------------------------------------- 00979 unsigned int NumberOfDOFs; 00980 00981 00982 // ---------------------- Doxygen info ---------------------- 00991 // ---------------------------------------------------------- 00992 unsigned int GreatestDOFForPhaseSynchronization; 00993 00994 00995 // ---------------------- Doxygen info ---------------------- 01003 // ---------------------------------------------------------- 01004 unsigned int MotionProfileForPhaseSynchronization; 01005 01006 01007 // ---------------------- Doxygen info ---------------------- 01014 // ---------------------------------------------------------- 01015 double CycleTime; 01016 01017 01018 // ---------------------- Doxygen info ---------------------- 01028 // ---------------------------------------------------------- 01029 double SynchronizationTime; 01030 01031 01032 // ---------------------- Doxygen info ---------------------- 01041 // ---------------------------------------------------------- 01042 double InternalClockInSeconds; 01043 01044 01045 // ---------------------- Doxygen info ---------------------- 01059 // ---------------------------------------------------------- 01060 RMLPositionFlags OldFlags; 01061 01062 01063 // ---------------------- Doxygen info ---------------------- 01074 // ---------------------------------------------------------- 01075 RMLBoolVector *ModifiedSelectionVector; 01076 01077 01078 // ---------------------- Doxygen info ---------------------- 01087 // ---------------------------------------------------------- 01088 RMLVector<Step1_Profile> *UsedStep1AProfiles; 01089 01090 01091 // ---------------------- Doxygen info ---------------------- 01107 // ---------------------------------------------------------- 01108 RMLDoubleVector *StoredTargetPosition; 01109 01110 01111 // ---------------------- Doxygen info ---------------------- 01119 // ---------------------------------------------------------- 01120 RMLDoubleVector *MinimumExecutionTimes; 01121 01122 01123 // ---------------------- Doxygen info ---------------------- 01132 // ---------------------------------------------------------- 01133 RMLDoubleVector *BeginningsOfInoperativeTimeIntervals; 01134 01135 01136 // ---------------------- Doxygen info ---------------------- 01145 // ---------------------------------------------------------- 01146 RMLDoubleVector *EndingsOfInoperativeTimeIntervals; 01147 01148 01149 // ---------------------- Doxygen info ---------------------- 01158 // ---------------------------------------------------------- 01159 RMLDoubleVector *PhaseSynchronizationReferenceVector; 01160 01161 01162 // ---------------------- Doxygen info ---------------------- 01171 // ---------------------------------------------------------- 01172 RMLDoubleVector *PhaseSynchronizationCurrentPositionVector; 01173 01174 01175 // ---------------------- Doxygen info ---------------------- 01184 // ---------------------------------------------------------- 01185 RMLDoubleVector *PhaseSynchronizationTargetPositionVector; 01186 01187 01188 // ---------------------- Doxygen info ---------------------- 01198 // ---------------------------------------------------------- 01199 RMLDoubleVector *PhaseSynchronizationPositionDifferenceVector; 01200 01201 01202 // ---------------------- Doxygen info ---------------------- 01211 // ---------------------------------------------------------- 01212 RMLDoubleVector *PhaseSynchronizationCurrentVelocityVector; 01213 01214 01215 // ---------------------- Doxygen info ---------------------- 01224 // ---------------------------------------------------------- 01225 RMLDoubleVector *PhaseSynchronizationTargetVelocityVector; 01226 01227 01228 // ---------------------- Doxygen info ---------------------- 01238 // ---------------------------------------------------------- 01239 RMLDoubleVector *PhaseSynchronizationMaxVelocityVector; 01240 01241 01242 // ---------------------- Doxygen info ---------------------- 01252 // ---------------------------------------------------------- 01253 RMLDoubleVector *PhaseSynchronizationMaxAccelerationVector; 01254 01255 01256 // ---------------------- Doxygen info ---------------------- 01265 // ---------------------------------------------------------- 01266 RMLDoubleVector *PhaseSynchronizationTimeVector; 01267 01268 01269 // ---------------------- Doxygen info ---------------------- 01278 // ---------------------------------------------------------- 01279 RMLDoubleVector *PhaseSynchronizationCheckVector; 01280 01281 01282 // ---------------------- Doxygen info ---------------------- 01292 // ---------------------------------------------------------- 01293 RMLDoubleVector *ArrayOfSortedTimes; 01294 01295 01296 // ---------------------- Doxygen info ---------------------- 01301 // ---------------------------------------------------------- 01302 RMLDoubleVector *ZeroVector; 01303 01304 01305 // ---------------------- Doxygen info ---------------------- 01320 // ---------------------------------------------------------- 01321 RMLPositionInputParameters *OldInputParameters; 01322 01323 01324 // ---------------------- Doxygen info ---------------------- 01333 // ---------------------------------------------------------- 01334 RMLPositionInputParameters *CurrentInputParameters; 01335 01336 01337 // ---------------------- Doxygen info ---------------------- 01350 // ---------------------------------------------------------- 01351 RMLPositionOutputParameters *OutputParameters; 01352 01353 01354 // ---------------------- Doxygen info ---------------------- 01368 // ---------------------------------------------------------- 01369 TypeIIRMLVelocity *RMLVelocityObject; 01370 01371 01372 // ---------------------- Doxygen info ---------------------- 01388 // ---------------------------------------------------------- 01389 RMLVelocityInputParameters *VelocityInputParameters; 01390 01391 01392 // ---------------------- Doxygen info ---------------------- 01408 // ---------------------------------------------------------- 01409 RMLVelocityOutputParameters *VelocityOutputParameters; 01410 01411 01412 // ---------------------- Doxygen info ---------------------- 01428 // ---------------------------------------------------------- 01429 RMLVelocityFlags VelocityFlags; 01430 01431 01432 // ---------------------- Doxygen info ---------------------- 01444 // ---------------------------------------------------------- 01445 MotionPolynomials *Polynomials; 01446 01447 01448 // ---------------------- Doxygen info ---------------------- 01460 // ---------------------------------------------------------- 01461 DominatValueForPhaseSync PhaseSynchronizationMagnitude; 01462 01463 01464 }; // class TypeIIRMLPosition 01465 01466 01467 #endif 01468 01469