RMLPositionInputParameters.h

Go to the documentation of this file.

//  ---------------------- Doxygen info ----------------------
//  ----------------------------------------------------------
//   For a convenient reading of this file's source code,
//   please use a tab width of four characters.
//  ----------------------------------------------------------


#ifndef __RMLPositionInputParameters__
#define __RMLPositionInputParameters__


#include "RMLInputParameters.h"
#include "RMLVector.h"
#include <string.h>
#include <math.h>


//  ---------------------- Doxygen info ----------------------
//  ----------------------------------------------------------
class RMLPositionInputParameters : public RMLInputParameters
{
public:


//  ---------------------- Doxygen info ----------------------
//  ----------------------------------------------------------
    RMLPositionInputParameters(const unsigned int DegreesOfFreedom) : RMLInputParameters(DegreesOfFreedom)
    {
        this->MaxVelocityVector                 =   new RMLDoubleVector (DegreesOfFreedom)  ;
        this->TargetPositionVector              =   new RMLDoubleVector (DegreesOfFreedom)  ;
        this->AlternativeTargetVelocityVector   =   new RMLDoubleVector (DegreesOfFreedom)  ;

        memset(this->MaxVelocityVector->VecData                 ,   0x0 ,       DegreesOfFreedom * sizeof(double));
        memset(this->TargetPositionVector->VecData              ,   0x0 ,       DegreesOfFreedom * sizeof(double));
        memset(this->AlternativeTargetVelocityVector->VecData   ,   0x0 ,       DegreesOfFreedom * sizeof(double));
    }


//  ---------------------- Doxygen info ----------------------
//  ----------------------------------------------------------
    RMLPositionInputParameters(const RMLPositionInputParameters &IP) : RMLInputParameters(IP)
    {
        this->MaxVelocityVector                     =   new RMLDoubleVector ((IP.CurrentPositionVector)->GetVecDim())   ;
        this->TargetPositionVector                  =   new RMLDoubleVector ((IP.CurrentPositionVector)->GetVecDim())   ;
        this->AlternativeTargetVelocityVector       =   new RMLDoubleVector ((IP.CurrentPositionVector)->GetVecDim())   ;

        *(this->MaxVelocityVector)                  =   *(IP.MaxVelocityVector)                                         ;
        *(this->TargetPositionVector)               =   *(IP.TargetPositionVector)                                      ;
        *(this->AlternativeTargetVelocityVector)    =   *(IP.AlternativeTargetVelocityVector)                           ;
    }


//  ---------------------- Doxygen info ----------------------
//  ----------------------------------------------------------
    ~RMLPositionInputParameters(void)
    {
        delete  this->MaxVelocityVector                     ;
        delete  this->TargetPositionVector                  ;
        delete  this->AlternativeTargetVelocityVector       ;

        this->MaxVelocityVector                 =   NULL    ;
        this->TargetPositionVector              =   NULL    ;
        this->AlternativeTargetVelocityVector   =   NULL    ;
    }


//  ---------------------- Doxygen info ----------------------
//  ----------------------------------------------------------
    RMLPositionInputParameters &operator = (const RMLPositionInputParameters &IP)
    {
        RMLInputParameters::operator=(IP);

        *(this->MaxVelocityVector               )   =   *(IP.MaxVelocityVector)                 ;
        *(this->TargetPositionVector            )   =   *(IP.TargetPositionVector)              ;
        *(this->AlternativeTargetVelocityVector )   =   *(IP.AlternativeTargetVelocityVector)   ;

        return(*this);
    }


// #############################################################################


//  ---------------------- Doxygen info ----------------------
//  ----------------------------------------------------------
    inline void SetMaxVelocityVector(const RMLDoubleVector &InputVector)
    {
        *(this->MaxVelocityVector) = InputVector;
    }


//  ---------------------- Doxygen info ----------------------
//  ----------------------------------------------------------
    inline void SetMaxVelocityVector(const double *InputVector)
    {
        memcpy(     (void*)this->MaxVelocityVector->VecData
                ,   (void*)InputVector
                ,   (this->MaxVelocityVector->GetVecDim() * sizeof(double)) );
    }


//  ---------------------- Doxygen info ----------------------
//  ----------------------------------------------------------
    inline void SetMaxVelocityVectorElement(    const double        &InputValue
                                            ,   const unsigned int  &Index)
    {
        (*this->MaxVelocityVector)[Index]   =   InputValue;
    }


//  ---------------------- Doxygen info ----------------------
//  ----------------------------------------------------------
    inline void GetMaxVelocityVector(RMLDoubleVector *InputVector) const
    {
        *InputVector    =   *(this->MaxVelocityVector);
    }


//  ---------------------- Doxygen info ----------------------
//  ----------------------------------------------------------
    inline void GetMaxVelocityVector(       double              *InputVector
                                        ,   const unsigned int  &SizeInBytes) const
    {
        memcpy(     (void*)InputVector
                ,   (void*)this->MaxVelocityVector->VecData
                ,   SizeInBytes );
    }


//  ---------------------- Doxygen info ----------------------
//  ----------------------------------------------------------
    inline void GetMaxVelocityVectorElement(    double              *InputValue
                                            ,   const unsigned int  &Index) const
    {
        if ( ( Index + 1 ) > ((unsigned int) this->MaxVelocityVector->GetVecDim() ) )
        {
            *InputValue =   0.0;
        }
        else
        {
            *InputValue =   (*this->MaxVelocityVector)[Index];
        }
    }


//  ---------------------- Doxygen info ----------------------
//  ----------------------------------------------------------
    inline double GetMaxVelocityVectorElement(const unsigned int &Index) const
    {
        if ( ( Index + 1 ) > ((unsigned int) this->MaxVelocityVector->GetVecDim() ) )
        {
            return(0.0);
        }
        else
        {
            return( (*this->MaxVelocityVector)[Index] );
        }
    }


// #############################################################################


//  ---------------------- Doxygen info ----------------------
//  ----------------------------------------------------------
    inline void SetTargetPositionVector(const RMLDoubleVector &InputVector)
    {
        *(this->TargetPositionVector) = InputVector;
    }


//  ---------------------- Doxygen info ----------------------
//  ----------------------------------------------------------
    inline void SetTargetPositionVector(const double *InputVector)
    {
        memcpy(     (void*)this->TargetPositionVector->VecData
                ,   (void*)InputVector
                ,   (this->TargetPositionVector->GetVecDim() * sizeof(double))  );
    }


//  ---------------------- Doxygen info ----------------------
//  ----------------------------------------------------------
    inline void SetTargetPositionVectorElement(     const double        &InputValue
                                                ,   const unsigned int  &Index)
    {
        (*this->TargetPositionVector)[Index]    =   InputValue;
    }


//  ---------------------- Doxygen info ----------------------
//  ----------------------------------------------------------
    inline void GetTargetPositionVector(RMLDoubleVector *InputVector) const
    {
        *InputVector    =   *(this->TargetPositionVector);
    }


//  ---------------------- Doxygen info ----------------------
//  ----------------------------------------------------------
    inline void GetTargetPositionVector(    double              *InputVector
                                        ,   const unsigned int  &SizeInBytes) const
    {
        memcpy(     (void*)InputVector
                ,   (void*)this->TargetPositionVector->VecData
                ,   SizeInBytes );
    }


//  ---------------------- Doxygen info ----------------------
//  ----------------------------------------------------------
    inline void GetTargetPositionVectorElement(     double              *InputValue
                                                ,   const unsigned int  &Index) const
    {
        if ( ( Index + 1 ) > ((unsigned int) this->TargetPositionVector->GetVecDim() ) )
        {
            *InputValue =   0.0;
        }
        else
        {
            *InputValue =   (*this->TargetPositionVector)[Index];
        }
    }


//  ---------------------- Doxygen info ----------------------
//  ----------------------------------------------------------
    inline double GetTargetPositionVectorElement(const unsigned int &Index) const
    {
        if ( ( Index + 1 ) > ((unsigned int) this->TargetPositionVector->GetVecDim() ) )
        {
            return(0.0);
        }
        else
        {
            return( (*this->TargetPositionVector)[Index] );
        }
    }


// #############################################################################


//  ---------------------- Doxygen info ----------------------
//  ----------------------------------------------------------
    inline void SetAlternativeTargetVelocityVector(const RMLDoubleVector &InputVector)
    {
        *(this->AlternativeTargetVelocityVector) = InputVector;
    }


//  ---------------------- Doxygen info ----------------------
//  ----------------------------------------------------------
    inline void SetAlternativeTargetVelocityVector(const double *InputVector)
    {
        memcpy(     (void*)this->AlternativeTargetVelocityVector->VecData
                ,   (void*)InputVector
                ,   (this->AlternativeTargetVelocityVector->GetVecDim() * sizeof(double))   );
    }


//  ---------------------- Doxygen info ----------------------
//  ----------------------------------------------------------
    inline void SetAlternativeTargetVelocityVectorElement(      const double        &InputValue
                                                            ,   const unsigned int  &Index)
    {
        (*this->AlternativeTargetVelocityVector)[Index] =   InputValue;
    }


//  ---------------------- Doxygen info ----------------------
//  ----------------------------------------------------------
    inline void GetAlternativeTargetVelocityVector(RMLDoubleVector *InputVector) const
    {
        *InputVector    =   *(this->AlternativeTargetVelocityVector);
    }


//  ---------------------- Doxygen info ----------------------
//  ----------------------------------------------------------
    inline void GetAlternativeTargetVelocityVector(     double              *InputVector
                                                    ,   const unsigned int  &SizeInBytes) const
    {
        memcpy(     (void*)InputVector
                ,   (void*)this->AlternativeTargetVelocityVector->VecData
                ,   SizeInBytes );
    }


//  ---------------------- Doxygen info ----------------------
//  ----------------------------------------------------------
    inline void GetAlternativeTargetVelocityVectorElement(      double              *InputValue
                                                            ,   const unsigned int  &Index) const
    {
        if ( ( Index + 1 ) > ((unsigned int) this->AlternativeTargetVelocityVector->GetVecDim() ) )
        {
            *InputValue =   0.0;
        }
        else
        {
            *InputValue =   (*this->AlternativeTargetVelocityVector)[Index];
        }
    }


//  ---------------------- Doxygen info ----------------------
//  ----------------------------------------------------------
    inline double GetAlternativeTargetVelocityVectorElement(const unsigned int &Index) const
    {
        if ( ( Index + 1 ) > ((unsigned int) this->AlternativeTargetVelocityVector->GetVecDim() ) )
        {
            return(0.0);
        }
        else
        {
            return( (*this->AlternativeTargetVelocityVector)[Index] );
        }
    }


// #############################################################################


//  ---------------------- Doxygen info ----------------------
//  ----------------------------------------------------------
    bool CheckForValidity(void) const
    {
        unsigned int        i                           =   0;

        double              MinimumOrderOfMagnitude     =   0.0
                        ,   MaximumOrderOfMagnitude     =   0.0;

        for (i = 0; i < this->NumberOfDOFs; i++)
        {
            if ((this->SelectionVector->VecData)[i])
            {
                if (    (this->MaxVelocityVector->VecData               [i] <=  0.0 )
                    ||  (this->MaxAccelerationVector->VecData           [i] <=  0.0 )
                    ||  (   fabs(this->TargetVelocityVector->VecData    [i]         )
                        >   (this->MaxVelocityVector->VecData)          [i]         )   )
                {
                    return(false);
                }

                if (    ((this->MaxVelocityVector->VecData)[i]  >=  (this->MaxAccelerationVector->VecData)          [i] )
                    &&  ((this->MaxVelocityVector->VecData)[i]  >=  fabs((this->CurrentPositionVector->VecData)     [i]))
                    &&  ((this->MaxVelocityVector->VecData)[i]  >=  fabs((this->TargetPositionVector->VecData)      [i]))
                    &&  ((this->MaxVelocityVector->VecData)[i]  >=  fabs((this->CurrentVelocityVector->VecData)     [i]))
                    &&  ((this->MaxVelocityVector->VecData)[i]  >=  fabs((this->CurrentAccelerationVector->VecData) [i])))
                {
                    MaximumOrderOfMagnitude =   (this->MaxVelocityVector->VecData)[i];
                }
                else
                {
                    if (    ((this->MaxAccelerationVector->VecData)[i]  >=  fabs((this->CurrentPositionVector->VecData)     [i]))
                        &&  ((this->MaxAccelerationVector->VecData)[i]  >=  fabs((this->TargetPositionVector->VecData)      [i]))
                        &&  ((this->MaxAccelerationVector->VecData)[i]  >=  fabs((this->CurrentVelocityVector->VecData)     [i]))
                        &&  ((this->MaxAccelerationVector->VecData)[i]  >=  fabs((this->CurrentAccelerationVector->VecData) [i])))
                    {
                        MaximumOrderOfMagnitude =   (this->MaxAccelerationVector->VecData)[i];
                    }
                    else
                    {
                        if (    (fabs((this->CurrentPositionVector->VecData)[i])    >=  fabs((this->TargetPositionVector->VecData)      [i]))
                            &&  (fabs((this->CurrentPositionVector->VecData)[i])    >=  fabs((this->CurrentVelocityVector->VecData)     [i]))
                            &&  (fabs((this->CurrentPositionVector->VecData)[i])    >=  fabs((this->CurrentAccelerationVector->VecData) [i])))
                        {
                            MaximumOrderOfMagnitude =   fabs((this->CurrentPositionVector->VecData)[i]);
                        }
                        else
                        {
                            if (    (fabs((this->TargetPositionVector->VecData)[i]) >=  fabs((this->CurrentVelocityVector->VecData)     [i]))
                                &&  (fabs((this->TargetPositionVector->VecData)[i]) >=  fabs((this->CurrentAccelerationVector->VecData) [i])))
                            {
                                MaximumOrderOfMagnitude =   fabs((this->TargetPositionVector->VecData)[i]);
                            }
                            else
                            {
                                MaximumOrderOfMagnitude =   fabs((this->CurrentAccelerationVector->VecData)[i]);
                            }
                        }
                    }
                }

                if ((this->MaxVelocityVector->VecData)[i]   <=  (this->MaxAccelerationVector->VecData)  [i] )
                {
                    MinimumOrderOfMagnitude =   (this->MaxVelocityVector->VecData)[i];
                }
                else
                {
                    MinimumOrderOfMagnitude =   (this->MaxAccelerationVector->VecData)[i];
                }

                // The target velocity value does not have to be checked as we
                // already know that is lesser than the maximum velocity value.
                // The alternative target velocity vector does not have to be
                // checked.

                // The value of MinimumOrderOfMagnitude is greater than
                // zero:
                if (    (MaximumOrderOfMagnitude / MinimumOrderOfMagnitude)
                    >   (double)pow((float)10, (int)(RMLPositionInputParameters::MAXIMUM_MAGNITUDE_RANGE)))
                {
                    return(false);
                }
            }
        }

        if (this->MinimumSynchronizationTime > 1e10)
        {
            return(false);
        }

        return(true);
    }


//  ---------------------- Doxygen info ----------------------
//  ----------------------------------------------------------
    void Echo(FILE* FileHandler = stdout) const
    {
        unsigned int        i   =   0;

        if (FileHandler == NULL)
        {
            return;
        }

        RMLInputParameters::Echo(FileHandler);

        fprintf(FileHandler, "Max. velocity vector       : ");
        for (i = 0; i < this->NumberOfDOFs; i++)
        {
            fprintf(FileHandler, " %.20le ", this->MaxVelocityVector->VecData[i]);
        }
        fprintf(FileHandler,   "\nTarget position vector     : ");
        for (i = 0; i < this->NumberOfDOFs; i++)
        {
            fprintf(FileHandler, " %.20le ", this->TargetPositionVector->VecData[i]);
        }
        fprintf(FileHandler,   "\nAlternative trgt vel. vector: ");
        for (i = 0; i < this->NumberOfDOFs; i++)
        {
            fprintf(FileHandler, " %.20le ", this->AlternativeTargetVelocityVector->VecData[i]);
        }
        fprintf(FileHandler, "\n");


        return;
    }


protected:


    enum
    {
//  ---------------------- Doxygen info ----------------------
//  ----------------------------------------------------------
        MAXIMUM_MAGNITUDE_RANGE     =   8
    };


public:


//  ---------------------- Doxygen info ----------------------
//  ----------------------------------------------------------
    RMLDoubleVector         *MaxVelocityVector;


//  ---------------------- Doxygen info ----------------------
//  ----------------------------------------------------------
    RMLDoubleVector         *TargetPositionVector;


//  ---------------------- Doxygen info ----------------------
//  ----------------------------------------------------------
    RMLDoubleVector         *AlternativeTargetVelocityVector;


};// class RMLPositionInputParameters



#endif