TypeIIRMLSetupModifiedSelectionVector.cpp

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.
//  ----------------------------------------------------------


#include <TypeIIRMLPosition.h>
#include <RMLPositionInputParameters.h>
#include <RMLVector.h>


//****************************************************************************
// SetupModifiedSelectionVector()

void TypeIIRMLPosition::SetupModifiedSelectionVector(void)
{
    unsigned int                i                               =   0;

    *(this->ModifiedSelectionVector)    =   *(this->CurrentInputParameters->SelectionVector);

    for (i = 0; i < this->NumberOfDOFs; i++)
    {
        if ((this->CurrentInputParameters->SelectionVector->VecData)[i])
        {
            if  (   ((this->CurrentInputParameters->TargetVelocityVector->VecData)[i] == 0.0)
                &&  ((this->MinimumExecutionTimes->VecData)[i] <= this->CycleTime))
            {
                (this->ModifiedSelectionVector->VecData)[i] =   false;

                // This degree of freedom has already reached its target state of motion, which
                // is steady, that is, it consists of a zero velocity and zero acceleration value
                // right in the target position. To save CPU time (irrelevant for real-time capability)
                // and to enable a simple and correct calculation of phase-synchronous trajectories
                // the selection vector is modified, and the current state of motion is set to the
                // target state of motion.

                (this->CurrentInputParameters->CurrentPositionVector->VecData)          [i]
                    =   (this->CurrentInputParameters->TargetPositionVector->VecData)   [i];
                (this->CurrentInputParameters->CurrentVelocityVector->VecData)          [i] =   0.0;
                (this->CurrentInputParameters->CurrentAccelerationVector->VecData)      [i] =   0.0;
            }
        }
    }
}