UndercarriageCtrlGeom.cpp

Go to the documentation of this file.

/*
 * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0

 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
 

#include <cob_undercarriage_ctrl/UndercarriageCtrlGeom.h>

// Constructor
UndercarriageCtrlGeom::UndercarriageCtrlGeom(std::string sIniDirectory)
{
        m_sIniDirectory = sIniDirectory;

        // init EMStop flag
        m_bEMStopActive = false;

        IniFile iniFile;
        iniFile.SetFileName(m_sIniDirectory + "Platform.ini", "UnderCarriageCtrlGeom.cpp");
        iniFile.GetKeyInt("Config", "NumberOfWheels", &m_iNumberOfDrives, true);

        // init vectors
        m_vdVelGearDriveRadS.assign(4,0);
        m_vdVelGearSteerRadS.assign(4,0);
        m_vdDltAngGearDriveRad.assign(4,0);
        m_vdAngGearSteerRad.assign(4,0);

        //m_vdVelGearDriveIntpRadS.assign(4,0);
        //m_vdVelGearSteerIntpRadS.assign(4,0);
        //m_vdAngGearSteerIntpRad.assign(4,0);

        m_vdVelGearDriveCmdRadS.assign(4,0);
        m_vdVelGearSteerCmdRadS.assign(4,0);
        m_vdAngGearSteerCmdRad.assign(4,0);

        m_vdWheelXPosMM.assign(4,0);
        m_vdWheelYPosMM.assign(4,0);
        m_vdWheelDistMM.assign(4,0);
        m_vdWheelAngRad.assign(4,0);

        m_vdExWheelXPosMM.assign(4,0);
        m_vdExWheelYPosMM.assign(4,0);
        m_vdExWheelDistMM.assign(4,0);
        m_vdExWheelAngRad.assign(4,0);

        m_vdAngGearSteerTarget1Rad.assign(4,0);
        m_vdVelGearDriveTarget1RadS.assign(4,0);
        m_vdAngGearSteerTarget2Rad.assign(4,0);
        m_vdVelGearDriveTarget2RadS.assign(4,0);
        m_vdAngGearSteerTargetRad.assign(4,0);
        m_vdVelGearDriveTargetRadS.assign(4,0);

        m_dCmdVelLongMMS = 0;
        m_dCmdVelLatMMS = 0;
        m_dCmdRotRobRadS = 0;
        m_dCmdRotVelRadS = 0;

        m_UnderCarriagePrms.WheelNeutralPos.assign(4,0);
        m_UnderCarriagePrms.vdSteerDriveCoupling.assign(4,0);
        m_UnderCarriagePrms.vdFactorVel.assign(4,0);

        m_vdCtrlVal.assign( 4, std::vector<double> (2,0.0) );

        //m_vdDeltaAngIntpRad.assign(4,0);
        //m_vdDeltaDriveIntpRadS.assign(4,0);

        // init Prms of Impedance-Ctrlr
        m_dSpring = 10.0;
        m_dDamp = 2.5;
        m_dVirtM = 0.1;
        m_dDPhiMax = 12.0;
        m_dDDPhiMax = 100.0;

        /*// Logging for debugging
        // Init timestamp for startup of the robot
        m_StartTime.SetNow();
        // open Files for PltfVel
        m_pfileDesVel = fopen("LogCtrl/DesPltfVel.txt","w");
        m_pfileMeasVel = fopen("LogCtrl/MeasPltfVel.txt","w");
        // open Files for corresponding Joint-Configuration
        m_pfileSteerAngTarget1 = fopen("LogCtrl/SteerAngTarget1.txt","w");
        m_pfileSteerAngTarget2 = fopen("LogCtrl/SteerAngTarget2.txt","w");
        m_pfileSteerAngTarget = fopen("LogCtrl/SteerAngTarget.txt","w");
        m_pfileDriveVelTarget = fopen("LogCtrl/DriveVelTarget.txt","w");
        // open Files for resulting Joint-Commands
        m_pfileSteerAngCmd = fopen("LogCtrl/SteerAngCmd.txt","w");
        m_pfileSteerVelCmd = fopen("LogCtrl/SteerVelCmd.txt","w");
        m_pfileDriveVelCmd = fopen("LogCtrl/DriveVelCmd.txt","w");*/

}

// Destructor
UndercarriageCtrlGeom::~UndercarriageCtrlGeom(void)
{
                /*fclose(m_pfileDesVel);
                fclose(m_pfileMeasVel);

                fclose(m_pfileSteerAngTarget1);
                fclose(m_pfileSteerAngTarget2);

                fclose(m_pfileSteerAngTarget);
                fclose(m_pfileDriveVelTarget);

                fclose(m_pfileSteerAngCmd);
                fclose(m_pfileSteerVelCmd);
                fclose(m_pfileDriveVelCmd);*/
}

// Initialize Parameters for Controller and Kinematics
void UndercarriageCtrlGeom::InitUndercarriageCtrl(void)
{
        //LOG_OUT("Initializing Undercarriage-Controller (Geom)");

        IniFile iniFile;

        iniFile.SetFileName(m_sIniDirectory + "Platform.ini", "UnderCarriageCtrlGeom.cpp");
        iniFile.GetKeyInt("Geom", "DistWheels", &m_UnderCarriagePrms.iDistWheels, true);
        iniFile.GetKeyInt("Geom", "RadiusWheel", &m_UnderCarriagePrms.iRadiusWheelMM, true);
        iniFile.GetKeyInt("Geom", "DistSteerAxisToDriveWheelCenter", &m_UnderCarriagePrms.iDistSteerAxisToDriveWheelMM, true);

        iniFile.GetKeyDouble("Geom", "Wheel1XPos", &m_vdWheelXPosMM[0], true);
        iniFile.GetKeyDouble("Geom", "Wheel1YPos", &m_vdWheelYPosMM[0], true);
        iniFile.GetKeyDouble("Geom", "Wheel2XPos", &m_vdWheelXPosMM[1], true);
        iniFile.GetKeyDouble("Geom", "Wheel2YPos", &m_vdWheelYPosMM[1], true);
        iniFile.GetKeyDouble("Geom", "Wheel3XPos", &m_vdWheelXPosMM[2], true);
        iniFile.GetKeyDouble("Geom", "Wheel3YPos", &m_vdWheelYPosMM[2], true);
        iniFile.GetKeyDouble("Geom", "Wheel4XPos", &m_vdWheelXPosMM[3], true);
        iniFile.GetKeyDouble("Geom", "Wheel4YPos", &m_vdWheelYPosMM[3], true);

        iniFile.GetKeyDouble("DrivePrms", "MaxDriveRate", &m_UnderCarriagePrms.dMaxDriveRateRadpS, true);
        iniFile.GetKeyDouble("DrivePrms", "MaxSteerRate", &m_UnderCarriagePrms.dMaxSteerRateRadpS, true);

        iniFile.GetKeyDouble("DrivePrms", "Wheel1SteerDriveCoupling", &m_UnderCarriagePrms.vdSteerDriveCoupling[0], true);
        iniFile.GetKeyDouble("DrivePrms", "Wheel2SteerDriveCoupling", &m_UnderCarriagePrms.vdSteerDriveCoupling[1], true);
        iniFile.GetKeyDouble("DrivePrms", "Wheel3SteerDriveCoupling", &m_UnderCarriagePrms.vdSteerDriveCoupling[2], true);
        iniFile.GetKeyDouble("DrivePrms", "Wheel4SteerDriveCoupling", &m_UnderCarriagePrms.vdSteerDriveCoupling[3], true);

        iniFile.GetKeyDouble("DrivePrms", "Wheel1NeutralPosition", &m_UnderCarriagePrms.WheelNeutralPos[0], true);
        iniFile.GetKeyDouble("DrivePrms", "Wheel2NeutralPosition", &m_UnderCarriagePrms.WheelNeutralPos[1], true);
        iniFile.GetKeyDouble("DrivePrms", "Wheel3NeutralPosition", &m_UnderCarriagePrms.WheelNeutralPos[2], true);
        iniFile.GetKeyDouble("DrivePrms", "Wheel4NeutralPosition", &m_UnderCarriagePrms.WheelNeutralPos[3], true);

        for(int i = 0; i<4; i++)
        {
                m_UnderCarriagePrms.WheelNeutralPos[i] = MathSup::convDegToRad(m_UnderCarriagePrms.WheelNeutralPos[i]);
                // provisorial --> skip interpolation
                m_vdAngGearSteerCmdRad[i] = m_UnderCarriagePrms.WheelNeutralPos[i];
                //m_vdAngGearSteerIntpRad[i] = m_UnderCarriagePrms.WheelNeutralPos[i];

                // also Init choosen Target angle
                m_vdAngGearSteerTargetRad[i] = m_UnderCarriagePrms.WheelNeutralPos[i];
        }

        iniFile.GetKeyDouble("Thread", "ThrUCarrCycleTimeS", &m_UnderCarriagePrms.dCmdRateS, true);

        // Read Values for Steering Position Controller from IniFile
        iniFile.SetFileName(m_sIniDirectory + "MotionCtrl.ini", "PltfHardwareCoB3.h");
        // Prms of Impedance-Ctrlr
        iniFile.GetKeyDouble("SteerCtrl", "Spring", &m_dSpring, true);
        iniFile.GetKeyDouble("SteerCtrl", "Damp", &m_dDamp, true);
        iniFile.GetKeyDouble("SteerCtrl", "VirtMass", &m_dVirtM, true);
        iniFile.GetKeyDouble("SteerCtrl", "DPhiMax", &m_dDPhiMax, true);
        iniFile.GetKeyDouble("SteerCtrl", "DDPhiMax", &m_dDDPhiMax, true);

        // calculate polar coords of Wheel Axis in robot coordinate frame
        for(int i=0; i<4; i++)
        {
                m_vdWheelDistMM[i] = sqrt( (m_vdWheelXPosMM[i] * m_vdWheelXPosMM[i]) + (m_vdWheelYPosMM[i] * m_vdWheelYPosMM[i]) );
                m_vdWheelAngRad[i] = MathSup::atan4quad(m_vdWheelXPosMM[i], m_vdWheelYPosMM[i]);
        }

        // Calculate exact position of wheels in cart. and polar coords in robot coordinate frame
        CalcExWheelPos();

        // calculate compensation factor for velocity
        for(int i = 0; i<4; i++)
        {
                m_UnderCarriagePrms.vdFactorVel[i] = - m_UnderCarriagePrms.vdSteerDriveCoupling[i]
                                     +(double(m_UnderCarriagePrms.iDistSteerAxisToDriveWheelMM) / double(m_UnderCarriagePrms.iRadiusWheelMM));
        }

}

// Set desired value for Plattfrom Velocity to UndercarriageCtrl (Sollwertvorgabe)
void UndercarriageCtrlGeom::SetDesiredPltfVelocity(double dCmdVelLongMMS, double dCmdVelLatMMS, double dCmdRotRobRadS, double dCmdRotVelRadS)
{
        // declare auxiliary variables
        double dCurrentPosWheelRAD;
        double dtempDeltaPhi1RAD, dtempDeltaPhi2RAD;    // difference between possible steering angels and current steering angle
        double dtempDeltaPhiCmd1RAD, dtempDeltaPhiCmd2RAD;      // difference between possible steering angels and last target steering angle
        double dtempWeightedDelta1RAD, dtempWeightedDelta2RAD; // weighted Summ of the two distance values

        // copy function parameters to member variables
        m_dCmdVelLongMMS = dCmdVelLongMMS;
        m_dCmdVelLatMMS = dCmdVelLatMMS;
        m_dCmdRotRobRadS = dCmdRotRobRadS;
        m_dCmdRotVelRadS = dCmdRotVelRadS;

        CalcInverse();

        // determine optimal Pltf-Configuration
        for (int i = 0; i<4; i++)
        {
                // Normalize Actual Wheel Position before calculation
                dCurrentPosWheelRAD = m_vdAngGearSteerRad[i];
                MathSup::normalizePi(dCurrentPosWheelRAD);

                // Calculate differences between current config to possible set-points
                dtempDeltaPhi1RAD = m_vdAngGearSteerTarget1Rad[i] - dCurrentPosWheelRAD;
                dtempDeltaPhi2RAD = m_vdAngGearSteerTarget2Rad[i] - dCurrentPosWheelRAD;
                MathSup::normalizePi(dtempDeltaPhi1RAD);
                MathSup::normalizePi(dtempDeltaPhi2RAD);
                // Calculate differences between last steering target to possible set-points
                dtempDeltaPhiCmd1RAD = m_vdAngGearSteerTarget1Rad[i] - m_vdAngGearSteerTargetRad[i];
                dtempDeltaPhiCmd2RAD = m_vdAngGearSteerTarget2Rad[i] - m_vdAngGearSteerTargetRad[i];
                MathSup::normalizePi(dtempDeltaPhiCmd1RAD);
                MathSup::normalizePi(dtempDeltaPhiCmd2RAD);

                // determine optimal setpoint value
                // 1st which set point is closest to current cinfog
                //     but: avoid permanent switching (if next target is about PI/2 from current config)
                // 2nd which set point is closest to last set point
                // "fitness criteria" to choose optimal set point:
                // calculate accumulted (+ weighted) difference between targets, current config. and last command
                dtempWeightedDelta1RAD = 0.6*fabs(dtempDeltaPhi1RAD) + 0.4*fabs(dtempDeltaPhiCmd1RAD);
                dtempWeightedDelta2RAD = 0.6*fabs(dtempDeltaPhi2RAD) + 0.4*fabs(dtempDeltaPhiCmd2RAD);

                // check which set point "minimizes fitness criteria"
                if (dtempWeightedDelta1RAD <= dtempWeightedDelta2RAD)
                {
                        // Target1 is "optimal"
                        m_vdVelGearDriveTargetRadS[i] = m_vdVelGearDriveTarget1RadS[i];
                        m_vdAngGearSteerTargetRad[i] = m_vdAngGearSteerTarget1Rad[i];
                }
                else
                {
                        // Target2 is "optimal"
                        m_vdVelGearDriveTargetRadS[i] = m_vdVelGearDriveTarget2RadS[i];
                        m_vdAngGearSteerTargetRad[i] = m_vdAngGearSteerTarget2Rad[i];
                }

                // provisorial --> skip interpolation and always take Target1
                //m_vdVelGearDriveCmdRadS[i] = m_vdVelGearDriveTarget1RadS[i];
                //m_vdAngGearSteerCmdRad[i] = m_vdAngGearSteerTarget1Rad[i];

                /*// interpolate between last setpoint and theone of the new setpoint, which is closest to the current configuration
                if (fabs(dtempDeltaPhi1RAD) <= fabs(dtempDeltaPhi2RAD))
                {
                        // difference between new target orientation and last (interpolated) target orientation
                        dtempDeltaPhi1RAD = m_vdAngGearSteerTarget1Rad[i] - m_vdAngGearSteerIntpRad[i];
                        MathSup::normalizePi(dtempDeltaPhi1RAD);

                        // calculate interpolation step sizes, to reach target at end of the cycle
                        m_vdDeltaAngIntpRad[i] = dtempDeltaPhi1RAD;
                        m_vdDeltaDriveIntpRadS[i] = (m_vdVelGearDriveTarget1RadS[i] - m_vdVelGearDriveIntpRadS[i]);

                        // additionally calculate meen change in angular config for feedforward cmd
                        //m_vdVelGearSteerIntpRadS[i] = dtempDeltaPhi1RAD/m_UnderCarriagePrms.dCmdRateS;
                }
                else
                {
                        // difference between new target orientation and last (interpolated) target orientation
                        dtempDeltaPhi2RAD = m_vdAngGearSteerTarget2Rad[i] - m_vdAngGearSteerIntpRad[i];
                        MathSup::normalizePi(dtempDeltaPhi2RAD);

                        // calculate interpolation step sizes, to reach target at end of the cycle
                        m_vdDeltaAngIntpRad[i] = dtempDeltaPhi2RAD;
                        m_vdDeltaDriveIntpRadS[i] = (m_vdVelGearDriveTarget2RadS[i] - m_vdVelGearDriveIntpRadS[i]);

                        // additionally calculate meen change in angular config for feedforward cmd
                        //m_vdVelGearSteerIntpRadS[i] = dtempDeltaPhi2RAD/m_UnderCarriagePrms.dCmdRateS;
                }*/
        }

        /*// Logging for debugging
        // get current time
        m_RawTime.SetNow();
        m_dNowTime = m_RawTime - m_StartTime;
        // Log out Pltf-Velocities
        fprintf(m_pfileDesVel, "%f %f %f %f \n", m_dNowTime, dCmdVelLongMMS, dCmdVelLatMMS, dCmdRotRobRadS);
        fprintf(m_pfileMeasVel, "%f %f %f %f \n", m_dNowTime, m_dVelLongMMS, m_dVelLatMMS, m_dRotRobRadS);
        // Log out corresponding Joint-Configuration
        fprintf(m_pfileSteerAngTarget1, "%f %f %f %f %f \n", m_dNowTime, m_vdAngGearSteerTarget1Rad[0], m_vdAngGearSteerTarget1Rad[1], m_vdAngGearSteerTarget1Rad[2], m_vdAngGearSteerTarget1Rad[3]);
        fprintf(m_pfileSteerAngTarget2, "%f %f %f %f %f \n", m_dNowTime, m_vdAngGearSteerTarget2Rad[0], m_vdAngGearSteerTarget2Rad[1], m_vdAngGearSteerTarget2Rad[2], m_vdAngGearSteerTarget2Rad[3]);
        fprintf(m_pfileSteerAngTarget, "%f %f %f %f %f \n", m_dNowTime, m_vdAngGearSteerTargetRad[0], m_vdAngGearSteerTargetRad[1], m_vdAngGearSteerTargetRad[2], m_vdAngGearSteerTargetRad[3]);
        fprintf(m_pfileDriveVelTarget, "%f %f %f %f %f \n", m_dNowTime, m_vdVelGearDriveTargetRadS[0], m_vdVelGearDriveTargetRadS[1], m_vdVelGearDriveTargetRadS[2], m_vdVelGearDriveTargetRadS[3]);*/
}

// Set actual values of wheels (steer/drive velocity/position) (Istwerte)
void UndercarriageCtrlGeom::SetActualWheelValues(std::vector<double> vdVelGearDriveRadS, std::vector<double> vdVelGearSteerRadS, std::vector<double> vdDltAngGearDriveRad, std::vector<double> vdAngGearSteerRad)
{
        //LOG_OUT("Set Wheel Position to Controller");

        m_vdVelGearDriveRadS = vdVelGearDriveRadS;
        m_vdVelGearSteerRadS = vdVelGearSteerRadS;
        m_vdDltAngGearDriveRad = vdDltAngGearDriveRad;
        m_vdAngGearSteerRad = vdAngGearSteerRad;

        // calc exact Wheel Positions (taking into account lever arm)
        CalcExWheelPos();

        // Peform calculation of direct kinematics (approx.) based on corrected Wheel Positions
        CalcDirect();
}

// Get result of inverse kinematics (without controller)
void UndercarriageCtrlGeom::GetSteerDriveSetValues(std::vector<double> & vdVelGearDriveRadS, std::vector<double> & vdAngGearSteerRad)
{
        //LOG_OUT("Calculate Inverse for given Velocity Command");

        CalcInverse();

        vdVelGearDriveRadS = m_vdVelGearDriveTarget1RadS;
        vdAngGearSteerRad = m_vdAngGearSteerTarget1Rad;
}

// Get set point values for the Wheels (including controller) from UndercarriangeCtrl
void UndercarriageCtrlGeom::GetNewCtrlStateSteerDriveSetValues(std::vector<double> & vdVelGearDriveRadS, std::vector<double> & vdVelGearSteerRadS, std::vector<double> & vdAngGearSteerRad,
                                                                 double & dVelLongMMS, double & dVelLatMMS, double & dRotRobRadS, double & dRotVelRadS)
{

        if(m_bEMStopActive == false)
        {
                //Calculate next step
                CalcControlStep();
        }

        vdVelGearDriveRadS = m_vdVelGearDriveCmdRadS;
        vdVelGearSteerRadS = m_vdVelGearSteerCmdRadS;
        vdAngGearSteerRad = m_vdAngGearSteerCmdRad;

        dVelLongMMS = m_dCmdVelLongMMS;
        dVelLatMMS = m_dCmdVelLatMMS;
        dRotRobRadS = m_dCmdRotRobRadS;
        dRotVelRadS = m_dCmdRotVelRadS;

        /*// Logging for debugging
        // get current time
        m_RawTime.SetNow();
        m_dNowTime = m_RawTime - m_StartTime;
        // Log out resulting joint-commands
        fprintf(m_pfileSteerAngCmd, "%f %f %f %f %f \n", m_dNowTime, m_vdAngGearSteerCmdRad[0], m_vdAngGearSteerCmdRad[1], m_vdAngGearSteerCmdRad[2], m_vdAngGearSteerCmdRad[3]);
        fprintf(m_pfileSteerVelCmd, "%f %f %f %f %f \n", m_dNowTime, m_vdVelGearSteerCmdRadS[0], m_vdVelGearSteerCmdRadS[1], m_vdVelGearSteerCmdRadS[2], m_vdVelGearSteerCmdRadS[3]);
        fprintf(m_pfileDriveVelCmd, "%f %f %f %f %f \n", m_dNowTime, m_vdVelGearDriveCmdRadS[0], m_vdVelGearDriveCmdRadS[1], m_vdVelGearDriveCmdRadS[2], m_vdVelGearDriveCmdRadS[3]);*/
}

// Get result of direct kinematics
void UndercarriageCtrlGeom::GetActualPltfVelocity(double & dDeltaLongMM, double & dDeltaLatMM, double & dDeltaRotRobRad, double & dDeltaRotVelRad,
                                                                                                  double & dVelLongMMS, double & dVelLatMMS, double & dRotRobRadS, double & dRotVelRadS)
{
        dVelLongMMS = m_dVelLongMMS;
        dVelLatMMS = m_dVelLatMMS;
        dRotRobRadS = m_dRotRobRadS;
        dRotVelRadS = m_dRotVelRadS;

        // calculate travelled distance and angle (from velocity) for output
        // ToDo: make sure this corresponds to cycle-freqnecy of calling node
        //       --> specify via config file
        dDeltaLongMM = dVelLongMMS * m_UnderCarriagePrms.dCmdRateS;
        dDeltaLatMM = dVelLatMMS * m_UnderCarriagePrms.dCmdRateS;
        dDeltaRotRobRad = dRotRobRadS * m_UnderCarriagePrms.dCmdRateS;
        dDeltaRotVelRad = dRotVelRadS * m_UnderCarriagePrms.dCmdRateS;
}

// calculate inverse kinematics
void UndercarriageCtrlGeom::CalcInverse(void)
{
        // help variable to store velocities of the steering axis in mm/s
        double dtempAxVelXRobMMS, dtempAxVelYRobMMS;

        // check if zero movement commanded -> keep orientation of wheels, set wheel velocity to zero
        if((m_dCmdVelLongMMS == 0) && (m_dCmdVelLatMMS == 0) && (m_dCmdRotRobRadS == 0) && (m_dCmdRotVelRadS == 0))
        {
                for(int i = 0; i<4; i++)
                {
                        m_vdAngGearSteerTarget1Rad[i] = m_vdAngGearSteerRad[i];
                        m_vdVelGearDriveTarget1RadS[i] = 0.0;
                        m_vdAngGearSteerTarget2Rad[i] = m_vdAngGearSteerRad[i];
                        m_vdVelGearDriveTarget2RadS[i] = 0.0;
                }
                return;
        }

        // calculate sets of possible Steering Angle // Drive-Velocity combinations
        for (int i = 0; i<4; i++)
        {
                // calculate velocity and direction of single wheel motion
                // Translational Portion
                dtempAxVelXRobMMS = m_dCmdVelLongMMS;
                dtempAxVelYRobMMS = m_dCmdVelLatMMS;
                // Rotational Portion
                dtempAxVelXRobMMS += m_dCmdRotRobRadS * m_vdExWheelDistMM[i] * -sin(m_vdExWheelAngRad[i]);
                dtempAxVelYRobMMS += m_dCmdRotRobRadS * m_vdExWheelDistMM[i] * cos(m_vdExWheelAngRad[i]);

                // calculate resulting steering angle
                // Wheel has to move in direction of resulting velocity vector of steering axis
                m_vdAngGearSteerTarget1Rad[i] = MathSup::atan4quad(dtempAxVelYRobMMS, dtempAxVelXRobMMS);
                // calculate corresponding angle in opposite direction (+180 degree)
                m_vdAngGearSteerTarget2Rad[i] = m_vdAngGearSteerTarget1Rad[i] + MathSup::PI;
                MathSup::normalizePi(m_vdAngGearSteerTarget2Rad[i]);

                // calculate absolute value of rotational rate of driving wheels in rad/s
                m_vdVelGearDriveTarget1RadS[i] = sqrt( (dtempAxVelXRobMMS * dtempAxVelXRobMMS) +
                                                   (dtempAxVelYRobMMS * dtempAxVelYRobMMS) ) / (double)m_UnderCarriagePrms.iRadiusWheelMM;
                // now adapt to direction (forward/backward) of wheel
                m_vdVelGearDriveTarget2RadS[i] = - m_vdVelGearDriveTarget1RadS[i];
        }
}

// calculate direct kinematics
void UndercarriageCtrlGeom::CalcDirect(void)
{
        // declare auxilliary variables
        double dtempVelXRobMMS;         // Robot-Velocity in x-Direction (longitudinal) in mm/s (in Robot-Coordinateframe)
        double dtempVelYRobMMS;         // Robot-Velocity in y-Direction (lateral) in mm/s (in Robot-Coordinateframe)
        double dtempRotRobRADPS;        // Robot-Rotation-Rate in rad/s (in Robot-Coordinateframe)
        double dtempDiffXMM;            // Difference in X-Coordinate of two wheels in mm
        double dtempDiffYMM;            // Difference in Y-Coordinate of two wheels in mm
        double dtempRelPhiWheelsRAD;    // Angle between axis of two wheels w.r.t the X-Axis of the Robot-Coordinate-System in rad
        double dtempRelDistWheelsMM;    // distance of two wheels in mm
        double dtempRelPhiWheel1RAD;    // Steering Angle of (im math. pos. direction) first Wheel w.r.t. the linking axis of the two wheels
        double dtempRelPhiWheel2RAD;    // Steering Angle of (im math. pos. direction) first Wheel w.r.t. the linking axis of the two wheels
        std::vector<double> vdtempVelWheelMMS(4);       // Wheel-Velocities (all Wheels) in mm/s

        // initial values
        dtempVelXRobMMS = 0;                    // Robot-Velocity in x-Direction (longitudinal) in mm/s (in Robot-Coordinateframe)
        dtempVelYRobMMS = 0;                    // Robot-Velocity in y-Direction (lateral) in mm/s (in Robot-Coordinateframe)
        dtempRotRobRADPS = 0;



        // calculate corrected wheel velocities
        for(int i = 0; i<m_iNumberOfDrives; i++)
        {
                // calc effective Driving-Velocity
                vdtempVelWheelMMS[i] = m_UnderCarriagePrms.iRadiusWheelMM * (m_vdVelGearDriveRadS[i] - m_UnderCarriagePrms.vdFactorVel[i]* m_vdVelGearSteerRadS[i]);
        }

        // calculate rotational rate of robot and current "virtual" axis between all wheels
        for(int i = 0; i< (m_iNumberOfDrives-1) ; i++)
        {
                // calc Parameters (Dist,Phi) of virtual linking axis of the two considered wheels
                dtempDiffXMM = m_vdExWheelXPosMM[i+1] - m_vdExWheelXPosMM[i];
                dtempDiffYMM = m_vdExWheelYPosMM[i+1] - m_vdExWheelYPosMM[i];
                dtempRelDistWheelsMM = sqrt( dtempDiffXMM*dtempDiffXMM + dtempDiffYMM*dtempDiffYMM );
                dtempRelPhiWheelsRAD = MathSup::atan4quad( dtempDiffYMM, dtempDiffXMM );

                // transform velocity of wheels into relative coordinate frame of linking axes -> subtract angles
                dtempRelPhiWheel1RAD = m_vdAngGearSteerRad[i] - dtempRelPhiWheelsRAD;
                dtempRelPhiWheel2RAD = m_vdAngGearSteerRad[i+1] - dtempRelPhiWheelsRAD;

                dtempRotRobRADPS += (vdtempVelWheelMMS[i+1] * sin(dtempRelPhiWheel2RAD) - vdtempVelWheelMMS[i] * sin(dtempRelPhiWheel1RAD))/dtempRelDistWheelsMM;
        }

        // calculate last missing axis (between last wheel and 1.)
        // calc. Parameters (Dist,Phi) of virtual linking axis of the two considered wheels
        dtempDiffXMM = m_vdExWheelXPosMM[0] - m_vdExWheelXPosMM[m_iNumberOfDrives-1];
        dtempDiffYMM = m_vdExWheelYPosMM[0] - m_vdExWheelYPosMM[m_iNumberOfDrives-1];
        dtempRelDistWheelsMM = sqrt( dtempDiffXMM*dtempDiffXMM + dtempDiffYMM*dtempDiffYMM );
        dtempRelPhiWheelsRAD = MathSup::atan4quad( dtempDiffYMM, dtempDiffXMM );

        // transform velocity of wheels into relative coordinate frame of linking axes -> subtract angles
        dtempRelPhiWheel1RAD = m_vdAngGearSteerRad[m_iNumberOfDrives-1] - dtempRelPhiWheelsRAD;
        dtempRelPhiWheel2RAD = m_vdAngGearSteerRad[0] - dtempRelPhiWheelsRAD;

        // close calculation of robots rotational velocity
        dtempRotRobRADPS += (vdtempVelWheelMMS[0]*sin(dtempRelPhiWheel2RAD) - vdtempVelWheelMMS[m_iNumberOfDrives-1]*sin(dtempRelPhiWheel1RAD))/dtempRelDistWheelsMM;

        // calculate linear velocity of robot
        for(int i = 0; i<m_iNumberOfDrives; i++)
        {
                dtempVelXRobMMS += vdtempVelWheelMMS[i]*cos(m_vdAngGearSteerRad[i]);
                dtempVelYRobMMS += vdtempVelWheelMMS[i]*sin(m_vdAngGearSteerRad[i]);
        }

        // assign rotational velocities for output
        m_dRotRobRadS = dtempRotRobRADPS/m_iNumberOfDrives;
        m_dRotVelRadS = 0; // currently not used to represent 3rd degree of freedom -> set to zero

        // assign linear velocity of robot for output
        m_dVelLongMMS = dtempVelXRobMMS/m_iNumberOfDrives;
        m_dVelLatMMS = dtempVelYRobMMS/m_iNumberOfDrives;



}

// calculate Exact Wheel Position in robot coordinates
void UndercarriageCtrlGeom::CalcExWheelPos(void)
{
        // calculate wheel position and velocity
        for(int i = 0; i<4; i++)
        {
                // calculate current geometry of robot (exact wheel position, taking into account steering offset of wheels)
                m_vdExWheelXPosMM[i] = m_vdWheelXPosMM[i] + m_UnderCarriagePrms.iDistSteerAxisToDriveWheelMM * sin(m_vdAngGearSteerRad[i]);
                m_vdExWheelYPosMM[i] = m_vdWheelYPosMM[i] - m_UnderCarriagePrms.iDistSteerAxisToDriveWheelMM * cos(m_vdAngGearSteerRad[i]);

                // calculate distance from platform center to wheel center
                m_vdExWheelDistMM[i] = sqrt( (m_vdExWheelXPosMM[i] * m_vdExWheelXPosMM[i]) + (m_vdExWheelYPosMM[i] * m_vdExWheelYPosMM[i]) );

                // calculate direction of rotational vector
                m_vdExWheelAngRad[i] = MathSup::atan4quad( m_vdExWheelYPosMM[i], m_vdExWheelXPosMM[i]);
        }
}

// perform one discrete Control Step (controls steering angle)
void UndercarriageCtrlGeom::CalcControlStep(void)
{
        // check if zero movement commanded -> keep orientation of wheels, set steer velocity to zero
        if ((m_dCmdVelLongMMS == 0) && (m_dCmdVelLatMMS == 0) && (m_dCmdRotRobRadS == 0) && (m_dCmdRotVelRadS == 0))
        {
                m_vdVelGearDriveCmdRadS.assign(4,0.0);          // set velocity for drives to zero
                m_vdVelGearSteerCmdRadS.assign(4,0.0);          // set velocity for steers to zero

                // set internal states of controller to zero
                for(int i=0; i<4; i++)
                {
                        m_vdCtrlVal[i][0] = 0.0;
                        m_vdCtrlVal[i][1] = 0.0;
                }
                return;
        }

        // declare auxilliary variables
        double dCurrentPosWheelRAD;
        double dDeltaPhi;
        double dForceDamp, dForceProp, dAccCmd, dVelCmdInt; // PI- and Impedance-Ctrl

        for (int i=0; i<4; i++)
        {
                // provisorial --> skip interpolation and always take Target
                m_vdVelGearDriveCmdRadS[i] = m_vdVelGearDriveTargetRadS[i];
                m_vdAngGearSteerCmdRad[i] = m_vdAngGearSteerTargetRad[i];

                // provisorial --> skip interpolation and always take Target1
                //m_vdVelGearDriveCmdRadS[i] = m_vdVelGearDriveTarget1RadS[i];
                //m_vdAngGearSteerCmdRad[i] = m_vdAngGearSteerTarget1Rad[i];

                /*m_vdAngGearSteerIntpRad[i] += m_vdDeltaAngIntpRad[i];
                MathSup::normalizePi(m_vdAngGearSteerIntpRad[i]);
                m_vdVelGearDriveIntpRadS[i] += m_vdDeltaDriveIntpRadS[i];

                m_vdVelGearDriveCmdRadS[i] = m_vdVelGearDriveIntpRadS[i];
                m_vdAngGearSteerCmdRad[i] = m_vdAngGearSteerIntpRad[i];*/
        }


        for (int i = 0; i<4; i++)
        {
                // Normalize Actual Wheel Position before calculation
                dCurrentPosWheelRAD = m_vdAngGearSteerRad[i];
                MathSup::normalizePi(dCurrentPosWheelRAD);
                dDeltaPhi = m_vdAngGearSteerCmdRad[i] - dCurrentPosWheelRAD;
                MathSup::normalizePi(dDeltaPhi);

                // Impedance-Ctrl
                // Calculate resulting desired forces, velocities
                // double dForceDamp, dForceProp, dAccCmd, dVelCmdInt;
                dForceDamp = - m_dDamp * m_vdCtrlVal[i][1];
                dForceProp = m_dSpring * dDeltaPhi;
                dAccCmd = (dForceDamp + dForceProp) / m_dVirtM;
                if (dAccCmd > m_dDDPhiMax)
                {
                        dAccCmd = m_dDDPhiMax;
                }
                else if (dAccCmd < -m_dDDPhiMax)
                {
                        dAccCmd = -m_dDDPhiMax;
                }
                dVelCmdInt = m_vdCtrlVal[i][1] + m_UnderCarriagePrms.dCmdRateS * dAccCmd;
                if (dVelCmdInt > m_dDPhiMax)
                {
                        dVelCmdInt = m_dDPhiMax;
                }
                else if (dVelCmdInt < -m_dDPhiMax)
                {
                        dVelCmdInt = -m_dDPhiMax;
                }
                // Store internal ctrlr-states
                m_vdCtrlVal[i][0] = dDeltaPhi;
                m_vdCtrlVal[i][1] = dVelCmdInt;
                // set outputs
                m_vdVelGearSteerCmdRadS[i] = dVelCmdInt;

                // Check if Steeringvelocity overgo maximum allowed rates.
                if(fabs(m_vdVelGearSteerCmdRadS[i]) > m_UnderCarriagePrms.dMaxSteerRateRadpS)
                {
                        if (m_vdVelGearSteerCmdRadS[i] > 0)
                                m_vdVelGearSteerCmdRadS[i] = m_UnderCarriagePrms.dMaxSteerRateRadpS;
                        else
                                m_vdVelGearSteerCmdRadS[i] = -m_UnderCarriagePrms.dMaxSteerRateRadpS;
                }
        }

        // Correct Driving-Wheel-Velocity, because of coupling and axis-offset
        for (int i = 0; i<4; i++)
        {
                m_vdVelGearDriveCmdRadS[i] += m_vdVelGearSteerCmdRadS[i] * m_UnderCarriagePrms.vdFactorVel[i];
        }

}

// operator overloading
void UndercarriageCtrlGeom::operator=(const UndercarriageCtrlGeom & GeomCtrl)
{
        // Actual Values for PltfMovement (calculated from Actual Wheelspeeds)
        m_dVelLongMMS = GeomCtrl.m_dVelLongMMS;
        m_dVelLatMMS = GeomCtrl.m_dVelLatMMS;
        m_dRotRobRadS = GeomCtrl.m_dRotRobRadS;
        m_dRotVelRadS = GeomCtrl.m_dRotVelRadS;

        // Actual Wheelspeed (read from Motor-Ctrls)
        m_vdVelGearDriveRadS = GeomCtrl.m_vdVelGearDriveRadS;
        m_vdVelGearSteerRadS = GeomCtrl.m_vdVelGearSteerRadS;
        m_vdDltAngGearDriveRad = GeomCtrl.m_vdDltAngGearDriveRad;
        m_vdAngGearSteerRad = GeomCtrl.m_vdAngGearSteerRad;

        // Desired Pltf-Movement (set from PltfHwItf)
        m_dCmdVelLongMMS = GeomCtrl.m_dCmdVelLongMMS;
        m_dCmdVelLatMMS = GeomCtrl.m_dCmdVelLatMMS;
        m_dCmdRotRobRadS = GeomCtrl.m_dCmdRotRobRadS;
        m_dCmdRotVelRadS = GeomCtrl.m_dCmdRotVelRadS;

        // Desired Wheelspeeds (calculated from desired ICM-configuration)
        m_vdVelGearDriveCmdRadS = GeomCtrl.m_vdVelGearDriveCmdRadS;
        m_vdVelGearSteerCmdRadS = GeomCtrl.m_vdVelGearSteerCmdRadS;
        m_vdAngGearSteerCmdRad = GeomCtrl.m_vdAngGearSteerCmdRad;

        // Target Wheelspeed and -angle (calculated from desired Pltf-Movement with Inverse without controle!)
        // alternativ 1 for steering angle
        m_vdAngGearSteerTarget1Rad = GeomCtrl.m_vdAngGearSteerTarget1Rad;
        m_vdVelGearDriveTarget1RadS = GeomCtrl.m_vdVelGearDriveTarget1RadS;
        // alternativ 2 for steering angle (+/- PI)
        m_vdAngGearSteerTarget2Rad = GeomCtrl.m_vdAngGearSteerTarget2Rad;
        m_vdVelGearDriveTarget2RadS = GeomCtrl.m_vdVelGearDriveTarget2RadS;

        // Position of the Wheels' Steering Axis'
        m_vdWheelXPosMM = GeomCtrl.m_vdWheelXPosMM;
        m_vdWheelYPosMM = GeomCtrl.m_vdWheelYPosMM;
        m_vdWheelDistMM = GeomCtrl.m_vdWheelDistMM;
        m_vdWheelAngRad = GeomCtrl.m_vdWheelAngRad;

        // Exact Position of the Wheels' itself
        m_vdExWheelXPosMM = GeomCtrl.m_vdExWheelXPosMM;
        m_vdExWheelYPosMM = GeomCtrl.m_vdExWheelYPosMM;
        m_vdExWheelDistMM = GeomCtrl.m_vdExWheelDistMM;
        m_vdExWheelAngRad = GeomCtrl.m_vdExWheelAngRad;

        // Prms
        m_UnderCarriagePrms = GeomCtrl.m_UnderCarriagePrms;

        // Position Controller Steer Wheels
        // Impedance-Ctrlr
        m_dSpring = GeomCtrl.m_dSpring;
        m_dDamp = GeomCtrl.m_dDamp;
        m_dVirtM = GeomCtrl.m_dDPhiMax;
        m_dDPhiMax = GeomCtrl.m_dDPhiMax;
        m_dDDPhiMax = GeomCtrl.m_dDDPhiMax;
        // Storage for internal controller states
        m_vdCtrlVal = GeomCtrl.m_vdCtrlVal;
}

// set EM Flag and stop ctrlr if active
void UndercarriageCtrlGeom::setEMStopActive(bool bEMStopActive)
{
        m_bEMStopActive = bEMStopActive;

        // if emergency stop reset ctrlr to zero
        if(m_bEMStopActive)
        {
                // Steermodules
                for(int i=0; i<4; i++)
                {
                        for(int j=0; j< 2; j++)
                        {
                                m_vdCtrlVal[i][j] = 0.0;
                        }
                }
                // Outputs
                for(int i=0; i<4; i++)
                {
                        m_vdVelGearDriveCmdRadS[i] = 0.0;
                        m_vdVelGearSteerCmdRadS[i] = 0.0;
                }
        }

}