TypeIIRMLStep2PhaseSynchronization.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 <TypeIIRMLMath.h>
#include <TypeIIRMLDecisionTree2.h>

using namespace TypeIIRMLMath;


//****************************************************************************
// Step2PhaseSynchronization()

void TypeIIRMLPosition::Step2PhaseSynchronization(void)
{
    unsigned int        i                                   =   0
                    ,   j                                   =   0   ;

    double              P_a0                                =   0.0
                    ,   P_a1                                =   0.0
                    ,   P_a2                                =   0.0
                    ,   V_a0                                =   0.0
                    ,   V_a1                                =   0.0
                    ,   V_a2                                =   0.0
                    ,   A_a0                                =   0.0
                    ,   A_a1                                =   0.0
                    ,   A_a2                                =   0.0
                    ,   DeltaT                              =   0.0
                    ,   ScalingValueFromReferenceVector     =   0.0
                    ,   V_ErrorAtBeginning                  =   0.0
                    ,   P_ErrorAtEnd                        =   0.0
                    ,   V_ErrorAtEnd                        =   0.0 ;

    // Calculate the trajectory of the reference DOF

    TypeIIRMLDecisionTree2(     (this->CurrentInputParameters->CurrentPositionVector->VecData   )[this->GreatestDOFForPhaseSynchronization]
                            ,   (this->CurrentInputParameters->CurrentVelocityVector->VecData   )[this->GreatestDOFForPhaseSynchronization]
                            ,   (this->CurrentInputParameters->TargetPositionVector->VecData    )[this->GreatestDOFForPhaseSynchronization]
                            ,   (this->CurrentInputParameters->TargetVelocityVector->VecData    )[this->GreatestDOFForPhaseSynchronization]
                            ,   (this->CurrentInputParameters->MaxVelocityVector->VecData       )[this->GreatestDOFForPhaseSynchronization]
                            ,   (this->CurrentInputParameters->MaxAccelerationVector->VecData   )[this->GreatestDOFForPhaseSynchronization]
                            ,   this->SynchronizationTime
                            ,   &((this->Polynomials)[this->GreatestDOFForPhaseSynchronization])    );



    // Adapt the synchronization time value to the time value, at which the reference DOF reaches
    // its final state of motion. Although, this should not differ from the originally calculated
    // synchronization time value, it differs due to numerical inaccuracies. In oder to compensate
    // these inaccuracies, we use the Step 2 result value of the reference DOF.
    this->SynchronizationTime   =   (((this->Polynomials)[this->GreatestDOFForPhaseSynchronization]).PolynomialTimes)[((this->Polynomials)[this->GreatestDOFForPhaseSynchronization]).ValidPolynomials - 2];

    // Calculate the trajectory of all other selected DOFs

    for (i = 0; i < this->NumberOfDOFs; i++)
    {
        if ( (this->ModifiedSelectionVector->VecData)[i] && (i != this->GreatestDOFForPhaseSynchronization) )
        {
            ScalingValueFromReferenceVector =   (this->PhaseSynchronizationReferenceVector->VecData)[i]
                                                    / (this->PhaseSynchronizationReferenceVector->VecData)[this->GreatestDOFForPhaseSynchronization];

            for (j = 0; j < ((this->Polynomials)[this->GreatestDOFForPhaseSynchronization]).ValidPolynomials; j++)
            {
                ((this->Polynomials)[this->GreatestDOFForPhaseSynchronization]).PositionPolynomial[j].GetCoefficients       (&P_a2, &P_a1,  &P_a0,  &DeltaT);
                ((this->Polynomials)[this->GreatestDOFForPhaseSynchronization]).VelocityPolynomial[j].GetCoefficients       (&V_a2, &V_a1,  &V_a0,  &DeltaT);
                ((this->Polynomials)[this->GreatestDOFForPhaseSynchronization]).AccelerationPolynomial[j].GetCoefficients   (&A_a2, &A_a1,  &A_a0,  &DeltaT);

                P_a2    *=  ScalingValueFromReferenceVector;
                P_a1    *=  ScalingValueFromReferenceVector;
                P_a0    =   ((this->CurrentInputParameters->CurrentPositionVector->VecData)[i]
                            + (P_a0
                            - (this->CurrentInputParameters->CurrentPositionVector->VecData)[this->GreatestDOFForPhaseSynchronization])
                            * ScalingValueFromReferenceVector);

                V_a2    *=  ScalingValueFromReferenceVector;
                V_a1    *=  ScalingValueFromReferenceVector;
                V_a0    *=  ScalingValueFromReferenceVector;

                A_a2    *=  ScalingValueFromReferenceVector;
                A_a1    *=  ScalingValueFromReferenceVector;
                A_a0    *=  ScalingValueFromReferenceVector;

                ((this->Polynomials)[i]).PositionPolynomial[j].SetCoefficients      (P_a2, P_a1, P_a0, DeltaT);
                ((this->Polynomials)[i]).VelocityPolynomial[j].SetCoefficients      (V_a2, V_a1, V_a0, DeltaT);
                ((this->Polynomials)[i]).AccelerationPolynomial[j].SetCoefficients  (A_a2, A_a1, A_a0, DeltaT);

                ((this->Polynomials)[i]).PolynomialTimes[j] = ((this->Polynomials)[this->GreatestDOFForPhaseSynchronization]).PolynomialTimes[j];
            }

            ((this->Polynomials)[i]).ValidPolynomials   =   ((this->Polynomials)[this->GreatestDOFForPhaseSynchronization]).ValidPolynomials;

            // ----------------------------------------------------------
            // Correcting numerical errors by adding a polynomial of degree one to the existing polynomials
            // ----------------------------------------------------------

            if (this->SynchronizationTime > this->CycleTime)
            {
                V_ErrorAtBeginning  =   (this->CurrentInputParameters->CurrentVelocityVector->VecData)[i]
                                        -   ((this->Polynomials)[i]).VelocityPolynomial[0].CalculateValue(0.0);

                V_ErrorAtEnd        =   (this->CurrentInputParameters->TargetVelocityVector->VecData)[i]
                                        -   ((this->Polynomials)[i]).VelocityPolynomial[((this->Polynomials)[i]).ValidPolynomials - 1].CalculateValue(this->SynchronizationTime);

                for (j = 0; j < ((this->Polynomials)[i]).ValidPolynomials; j++)
                {
                    ((this->Polynomials)[i]).PositionPolynomial[j].GetCoefficients      (&P_a2, &P_a1,  &P_a0,  &DeltaT);
                    ((this->Polynomials)[i]).VelocityPolynomial[j].GetCoefficients      (&V_a2, &V_a1,  &V_a0,  &DeltaT);

                    V_a1    +=  (V_ErrorAtEnd - V_ErrorAtBeginning) / this->SynchronizationTime;
                    V_a0    +=  V_ErrorAtBeginning - DeltaT * (V_ErrorAtEnd - V_ErrorAtBeginning) / this->SynchronizationTime;

                    P_a1    =   V_a0;

                    ((this->Polynomials)[i]).PositionPolynomial[j].SetCoefficients      (P_a2, P_a1, P_a0, DeltaT);
                    ((this->Polynomials)[i]).VelocityPolynomial[j].SetCoefficients      (V_a2, V_a1, V_a0, DeltaT);
                }

                P_ErrorAtEnd        =   (this->CurrentInputParameters->TargetPositionVector->VecData)[i]
                                        -   ((this->Polynomials)[i]).PositionPolynomial[((this->Polynomials)[i]).ValidPolynomials - 1].CalculateValue(this->SynchronizationTime);

                for (j = 0; j < ((this->Polynomials)[i]).ValidPolynomials; j++)
                {
                    ((this->Polynomials)[i]).PositionPolynomial[j].GetCoefficients      (&P_a2, &P_a1,  &P_a0,  &DeltaT);

                    P_a1    +=  P_ErrorAtEnd / this->SynchronizationTime;
                    P_a0    -=  DeltaT * P_ErrorAtEnd / this->SynchronizationTime;

                    ((this->Polynomials)[i]).PositionPolynomial[j].SetCoefficients      (P_a2, P_a1, P_a0, DeltaT);
                }
            }
            // ----------------------------------------------------------
        }
    }

}