 |
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 ---------------------- 00043 // ---------------------------------------------------------- 00044 // For a convenient reading of this file's source code, 00045 // please use a tab width of four characters. 00046 // ---------------------------------------------------------- 00047 00048 00049 #ifndef __RMLPositionInputParameters__ 00050 #define __RMLPositionInputParameters__ 00051 00052 00053 #include <RMLInputParameters.h> 00054 #include <RMLVector.h> 00055 #include <string.h> 00056 #include <math.h> 00057 00058 00059 // ---------------------- Doxygen info ---------------------- 00078 // ---------------------------------------------------------- 00079 class RMLPositionInputParameters : public RMLInputParameters 00080 { 00081 public: 00082 00083 00084 // ---------------------- Doxygen info ---------------------- 00096 // ---------------------------------------------------------- 00097 RMLPositionInputParameters(const unsigned int DegreesOfFreedom) : RMLInputParameters(DegreesOfFreedom) 00098 { 00099 this->MaxVelocityVector = new RMLDoubleVector (DegreesOfFreedom) ; 00100 this->TargetPositionVector = new RMLDoubleVector (DegreesOfFreedom) ; 00101 this->AlternativeTargetVelocityVector = new RMLDoubleVector (DegreesOfFreedom) ; 00102 00103 memset(this->MaxVelocityVector->VecData , 0x0 , DegreesOfFreedom * sizeof(double)); 00104 memset(this->TargetPositionVector->VecData , 0x0 , DegreesOfFreedom * sizeof(double)); 00105 memset(this->AlternativeTargetVelocityVector->VecData , 0x0 , DegreesOfFreedom * sizeof(double)); 00106 } 00107 00108 00109 // ---------------------- Doxygen info ---------------------- 00121 // ---------------------------------------------------------- 00122 RMLPositionInputParameters(const RMLPositionInputParameters &IP) : RMLInputParameters(IP) 00123 { 00124 this->MaxVelocityVector = new RMLDoubleVector ((IP.CurrentPositionVector)->GetVecDim()) ; 00125 this->TargetPositionVector = new RMLDoubleVector ((IP.CurrentPositionVector)->GetVecDim()) ; 00126 this->AlternativeTargetVelocityVector = new RMLDoubleVector ((IP.CurrentPositionVector)->GetVecDim()) ; 00127 00128 *(this->MaxVelocityVector) = *(IP.MaxVelocityVector) ; 00129 *(this->TargetPositionVector) = *(IP.TargetPositionVector) ; 00130 *(this->AlternativeTargetVelocityVector) = *(IP.AlternativeTargetVelocityVector) ; 00131 } 00132 00133 00134 // ---------------------- Doxygen info ---------------------- 00139 // ---------------------------------------------------------- 00140 ~RMLPositionInputParameters(void) 00141 { 00142 delete this->MaxVelocityVector ; 00143 delete this->TargetPositionVector ; 00144 delete this->AlternativeTargetVelocityVector ; 00145 00146 this->MaxVelocityVector = NULL ; 00147 this->TargetPositionVector = NULL ; 00148 this->AlternativeTargetVelocityVector = NULL ; 00149 } 00150 00151 00152 // ---------------------- Doxygen info ---------------------- 00160 // ---------------------------------------------------------- 00161 RMLPositionInputParameters &operator = (const RMLPositionInputParameters &IP) 00162 { 00163 RMLInputParameters::operator=(IP); 00164 00165 *(this->MaxVelocityVector ) = *(IP.MaxVelocityVector) ; 00166 *(this->TargetPositionVector ) = *(IP.TargetPositionVector) ; 00167 *(this->AlternativeTargetVelocityVector ) = *(IP.AlternativeTargetVelocityVector) ; 00168 00169 return(*this); 00170 } 00171 00172 00173 // ############################################################################# 00174 00175 00176 // ---------------------- Doxygen info ---------------------- 00190 // ---------------------------------------------------------- 00191 inline void SetMaxVelocityVector(const RMLDoubleVector &InputVector) 00192 { 00193 *(this->MaxVelocityVector) = InputVector; 00194 } 00195 00196 00197 // ---------------------- Doxygen info ---------------------- 00211 // ---------------------------------------------------------- 00212 inline void SetMaxVelocityVector(const double *InputVector) 00213 { 00214 memcpy( (void*)this->MaxVelocityVector->VecData 00215 , (void*)InputVector 00216 , (this->MaxVelocityVector->GetVecDim() * sizeof(double)) ); 00217 } 00218 00219 00220 // ---------------------- Doxygen info ---------------------- 00236 // ---------------------------------------------------------- 00237 inline void SetMaxVelocityVectorElement( const double &InputValue 00238 , const unsigned int &Index) 00239 { 00240 (*this->MaxVelocityVector)[Index] = InputValue; 00241 } 00242 00243 00244 // ---------------------- Doxygen info ---------------------- 00259 // ---------------------------------------------------------- 00260 inline void GetMaxVelocityVector(RMLDoubleVector *InputVector) const 00261 { 00262 *InputVector = *(this->MaxVelocityVector); 00263 } 00264 00265 00266 // ---------------------- Doxygen info ---------------------- 00289 // ---------------------------------------------------------- 00290 inline void GetMaxVelocityVector( double *InputVector 00291 , const unsigned int &SizeInBytes) const 00292 { 00293 memcpy( (void*)InputVector 00294 , (void*)this->MaxVelocityVector->VecData 00295 , SizeInBytes ); 00296 } 00297 00298 00299 // ---------------------- Doxygen info ---------------------- 00320 // ---------------------------------------------------------- 00321 inline void GetMaxVelocityVectorElement( double *InputValue 00322 , const unsigned int &Index) const 00323 { 00324 if ( ( Index + 1 ) > ((unsigned int) this->MaxVelocityVector->GetVecDim() ) ) 00325 { 00326 *InputValue = 0.0; 00327 } 00328 else 00329 { 00330 *InputValue = (*this->MaxVelocityVector)[Index]; 00331 } 00332 } 00333 00334 00335 // ---------------------- Doxygen info ---------------------- 00350 // ---------------------------------------------------------- 00351 inline double GetMaxVelocityVectorElement(const unsigned int &Index) const 00352 { 00353 if ( ( Index + 1 ) > ((unsigned int) this->MaxVelocityVector->GetVecDim() ) ) 00354 { 00355 return(0.0); 00356 } 00357 else 00358 { 00359 return( (*this->MaxVelocityVector)[Index] ); 00360 } 00361 } 00362 00363 00364 // ############################################################################# 00365 00366 00367 // ---------------------- Doxygen info ---------------------- 00381 // ---------------------------------------------------------- 00382 inline void SetTargetPositionVector(const RMLDoubleVector &InputVector) 00383 { 00384 *(this->TargetPositionVector) = InputVector; 00385 } 00386 00387 00388 // ---------------------- Doxygen info ---------------------- 00402 // ---------------------------------------------------------- 00403 inline void SetTargetPositionVector(const double *InputVector) 00404 { 00405 memcpy( (void*)this->TargetPositionVector->VecData 00406 , (void*)InputVector 00407 , (this->TargetPositionVector->GetVecDim() * sizeof(double)) ); 00408 } 00409 00410 00411 // ---------------------- Doxygen info ---------------------- 00427 // ---------------------------------------------------------- 00428 inline void SetTargetPositionVectorElement( const double &InputValue 00429 , const unsigned int &Index) 00430 { 00431 (*this->TargetPositionVector)[Index] = InputValue; 00432 } 00433 00434 00435 // ---------------------- Doxygen info ---------------------- 00450 // ---------------------------------------------------------- 00451 inline void GetTargetPositionVector(RMLDoubleVector *InputVector) const 00452 { 00453 *InputVector = *(this->TargetPositionVector); 00454 } 00455 00456 00457 // ---------------------- Doxygen info ---------------------- 00480 // ---------------------------------------------------------- 00481 inline void GetTargetPositionVector( double *InputVector 00482 , const unsigned int &SizeInBytes) const 00483 { 00484 memcpy( (void*)InputVector 00485 , (void*)this->TargetPositionVector->VecData 00486 , SizeInBytes ); 00487 } 00488 00489 00490 // ---------------------- Doxygen info ---------------------- 00511 // ---------------------------------------------------------- 00512 inline void GetTargetPositionVectorElement( double *InputValue 00513 , const unsigned int &Index) const 00514 { 00515 if ( ( Index + 1 ) > ((unsigned int) this->TargetPositionVector->GetVecDim() ) ) 00516 { 00517 *InputValue = 0.0; 00518 } 00519 else 00520 { 00521 *InputValue = (*this->TargetPositionVector)[Index]; 00522 } 00523 } 00524 00525 00526 // ---------------------- Doxygen info ---------------------- 00541 // ---------------------------------------------------------- 00542 inline double GetTargetPositionVectorElement(const unsigned int &Index) const 00543 { 00544 if ( ( Index + 1 ) > ((unsigned int) this->TargetPositionVector->GetVecDim() ) ) 00545 { 00546 return(0.0); 00547 } 00548 else 00549 { 00550 return( (*this->TargetPositionVector)[Index] ); 00551 } 00552 } 00553 00554 00555 // ############################################################################# 00556 00557 00558 // ---------------------- Doxygen info ---------------------- 00580 // ---------------------------------------------------------- 00581 inline void SetAlternativeTargetVelocityVector(const RMLDoubleVector &InputVector) 00582 { 00583 *(this->AlternativeTargetVelocityVector) = InputVector; 00584 } 00585 00586 00587 // ---------------------- Doxygen info ---------------------- 00609 // ---------------------------------------------------------- 00610 inline void SetAlternativeTargetVelocityVector(const double *InputVector) 00611 { 00612 memcpy( (void*)this->AlternativeTargetVelocityVector->VecData 00613 , (void*)InputVector 00614 , (this->AlternativeTargetVelocityVector->GetVecDim() * sizeof(double)) ); 00615 } 00616 00617 00618 // ---------------------- Doxygen info ---------------------- 00643 // ---------------------------------------------------------- 00644 inline void SetAlternativeTargetVelocityVectorElement( const double &InputValue 00645 , const unsigned int &Index) 00646 { 00647 (*this->AlternativeTargetVelocityVector)[Index] = InputValue; 00648 } 00649 00650 00651 // ---------------------- Doxygen info ---------------------- 00667 // ---------------------------------------------------------- 00668 inline void GetAlternativeTargetVelocityVector(RMLDoubleVector *InputVector) const 00669 { 00670 *InputVector = *(this->AlternativeTargetVelocityVector); 00671 } 00672 00673 00674 // ---------------------- Doxygen info ---------------------- 00698 // ---------------------------------------------------------- 00699 inline void GetAlternativeTargetVelocityVector( double *InputVector 00700 , const unsigned int &SizeInBytes) const 00701 { 00702 memcpy( (void*)InputVector 00703 , (void*)this->AlternativeTargetVelocityVector->VecData 00704 , SizeInBytes ); 00705 } 00706 00707 00708 // ---------------------- Doxygen info ---------------------- 00730 // ---------------------------------------------------------- 00731 inline void GetAlternativeTargetVelocityVectorElement( double *InputValue 00732 , const unsigned int &Index) const 00733 { 00734 if ( ( Index + 1 ) > ((unsigned int) this->AlternativeTargetVelocityVector->GetVecDim() ) ) 00735 { 00736 *InputValue = 0.0; 00737 } 00738 else 00739 { 00740 *InputValue = (*this->AlternativeTargetVelocityVector)[Index]; 00741 } 00742 } 00743 00744 00745 // ---------------------- Doxygen info ---------------------- 00760 // ---------------------------------------------------------- 00761 inline double GetAlternativeTargetVelocityVectorElement(const unsigned int &Index) const 00762 { 00763 if ( ( Index + 1 ) > ((unsigned int) this->AlternativeTargetVelocityVector->GetVecDim() ) ) 00764 { 00765 return(0.0); 00766 } 00767 else 00768 { 00769 return( (*this->AlternativeTargetVelocityVector)[Index] ); 00770 } 00771 } 00772 00773 00774 // ############################################################################# 00775 00776 00777 // ---------------------- Doxygen info ---------------------- 00822 // ---------------------------------------------------------- 00823 bool CheckForValidity(void) const 00824 { 00825 unsigned int i = 0; 00826 00827 double MinimumOrderOfMagnitude = 0.0 00828 , MaximumOrderOfMagnitude = 0.0; 00829 00830 for (i = 0; i < this->NumberOfDOFs; i++) 00831 { 00832 if ((this->SelectionVector->VecData)[i]) 00833 { 00834 if ( (this->MaxVelocityVector->VecData [i] <= 0.0 ) 00835 || (this->MaxAccelerationVector->VecData [i] <= 0.0 ) 00836 || ( fabs(this->TargetVelocityVector->VecData [i] ) 00837 > (this->MaxVelocityVector->VecData) [i] ) ) 00838 { 00839 return(false); 00840 } 00841 00842 if ( ((this->MaxVelocityVector->VecData)[i] >= (this->MaxAccelerationVector->VecData) [i] ) 00843 && ((this->MaxVelocityVector->VecData)[i] >= fabs((this->CurrentPositionVector->VecData) [i])) 00844 && ((this->MaxVelocityVector->VecData)[i] >= fabs((this->TargetPositionVector->VecData) [i])) 00845 && ((this->MaxVelocityVector->VecData)[i] >= fabs((this->CurrentVelocityVector->VecData) [i])) 00846 && ((this->MaxVelocityVector->VecData)[i] >= fabs((this->CurrentAccelerationVector->VecData) [i]))) 00847 { 00848 MaximumOrderOfMagnitude = (this->MaxVelocityVector->VecData)[i]; 00849 } 00850 else 00851 { 00852 if ( ((this->MaxAccelerationVector->VecData)[i] >= fabs((this->CurrentPositionVector->VecData) [i])) 00853 && ((this->MaxAccelerationVector->VecData)[i] >= fabs((this->TargetPositionVector->VecData) [i])) 00854 && ((this->MaxAccelerationVector->VecData)[i] >= fabs((this->CurrentVelocityVector->VecData) [i])) 00855 && ((this->MaxAccelerationVector->VecData)[i] >= fabs((this->CurrentAccelerationVector->VecData) [i]))) 00856 { 00857 MaximumOrderOfMagnitude = (this->MaxAccelerationVector->VecData)[i]; 00858 } 00859 else 00860 { 00861 if ( (fabs((this->CurrentPositionVector->VecData)[i]) >= fabs((this->TargetPositionVector->VecData) [i])) 00862 && (fabs((this->CurrentPositionVector->VecData)[i]) >= fabs((this->CurrentVelocityVector->VecData) [i])) 00863 && (fabs((this->CurrentPositionVector->VecData)[i]) >= fabs((this->CurrentAccelerationVector->VecData) [i]))) 00864 { 00865 MaximumOrderOfMagnitude = fabs((this->CurrentPositionVector->VecData)[i]); 00866 } 00867 else 00868 { 00869 if ( (fabs((this->TargetPositionVector->VecData)[i]) >= fabs((this->CurrentVelocityVector->VecData) [i])) 00870 && (fabs((this->TargetPositionVector->VecData)[i]) >= fabs((this->CurrentAccelerationVector->VecData) [i]))) 00871 { 00872 MaximumOrderOfMagnitude = fabs((this->TargetPositionVector->VecData)[i]); 00873 } 00874 else 00875 { 00876 MaximumOrderOfMagnitude = fabs((this->CurrentAccelerationVector->VecData)[i]); 00877 } 00878 } 00879 } 00880 } 00881 00882 if ((this->MaxVelocityVector->VecData)[i] <= (this->MaxAccelerationVector->VecData) [i] ) 00883 { 00884 MinimumOrderOfMagnitude = (this->MaxVelocityVector->VecData)[i]; 00885 } 00886 else 00887 { 00888 MinimumOrderOfMagnitude = (this->MaxAccelerationVector->VecData)[i]; 00889 } 00890 00891 // The target velocity value does not have to be checked as we 00892 // already know that is lesser than the maximum velocity value. 00893 // The alternative target velocity vector does not have to be 00894 // checked. 00895 00896 // The value of MinimumOrderOfMagnitude is greater than 00897 // zero: 00898 if ( (MaximumOrderOfMagnitude / MinimumOrderOfMagnitude) 00899 > (double)pow((float)10, (int)(RMLPositionInputParameters::MAXIMUM_MAGNITUDE_RANGE))) 00900 { 00901 return(false); 00902 } 00903 } 00904 } 00905 00906 if (this->MinimumSynchronizationTime > 1e10) 00907 { 00908 return(false); 00909 } 00910 00911 return(true); 00912 } 00913 00914 00915 // ---------------------- Doxygen info ---------------------- 00923 // ---------------------------------------------------------- 00924 void Echo(FILE* FileHandler = stdout) const 00925 { 00926 unsigned int i = 0; 00927 00928 if (FileHandler == NULL) 00929 { 00930 return; 00931 } 00932 00933 RMLInputParameters::Echo(FileHandler); 00934 00935 fprintf(FileHandler, "Max. velocity vector : "); 00936 for (i = 0; i < this->NumberOfDOFs; i++) 00937 { 00938 fprintf(FileHandler, " %.20le ", this->MaxVelocityVector->VecData[i]); 00939 } 00940 fprintf(FileHandler, "\nTarget position vector : "); 00941 for (i = 0; i < this->NumberOfDOFs; i++) 00942 { 00943 fprintf(FileHandler, " %.20le ", this->TargetPositionVector->VecData[i]); 00944 } 00945 fprintf(FileHandler, "\nAlternative trgt vel. vector: "); 00946 for (i = 0; i < this->NumberOfDOFs; i++) 00947 { 00948 fprintf(FileHandler, " %.20le ", this->AlternativeTargetVelocityVector->VecData[i]); 00949 } 00950 fprintf(FileHandler, "\n"); 00951 00952 00953 return; 00954 } 00955 00956 00957 protected: 00958 00959 00960 enum 00961 { 00962 // ---------------------- Doxygen info ---------------------- 00966 // ---------------------------------------------------------- 00967 MAXIMUM_MAGNITUDE_RANGE = 8 00968 }; 00969 00970 00971 public: 00972 00973 00974 // ---------------------- Doxygen info ---------------------- 00989 // ---------------------------------------------------------- 00990 RMLDoubleVector *MaxVelocityVector; 00991 00992 00993 // ---------------------- Doxygen info ---------------------- 01008 // ---------------------------------------------------------- 01009 RMLDoubleVector *TargetPositionVector; 01010 01011 01012 // ---------------------- Doxygen info ---------------------- 01051 // ---------------------------------------------------------- 01052 RMLDoubleVector *AlternativeTargetVelocityVector; 01053 01054 01055 };// class RMLPositionInputParameters 01056 01057 01058 01059 #endif 01060 01061