UndercarriageCtrlGeom.cpp
Go to the documentation of this file.
00001 /****************************************************************
00002  *
00003  * Copyright (c) 2010
00004  *
00005  * Fraunhofer Institute for Manufacturing Engineering   
00006  * and Automation (IPA)
00007  *
00008  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00009  *
00010  * Project name: care-o-bot
00011  * ROS stack name: cob_driver
00012  * ROS package name: cob_undercarriage_ctrl
00013  * Description:
00014  *                                                              
00015  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00016  *                      
00017  * Author: Christian Connette, email:christian.connette@ipa.fhg.de
00018  * Supervised by: Christian Connette, email:christian.connette@ipa.fhg.de
00019  *
00020  * Date of creation: April 2010:
00021  * ToDo:
00022  *
00023  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00024  *
00025  * Redistribution and use in source and binary forms, with or without
00026  * modification, are permitted provided that the following conditions are met:
00027  *
00028  *     * Redistributions of source code must retain the above copyright
00029  *       notice, this list of conditions and the following disclaimer.
00030  *     * Redistributions in binary form must reproduce the above copyright
00031  *       notice, this list of conditions and the following disclaimer in the
00032  *       documentation and/or other materials provided with the distribution.
00033  *     * Neither the name of the Fraunhofer Institute for Manufacturing 
00034  *       Engineering and Automation (IPA) nor the names of its
00035  *       contributors may be used to endorse or promote products derived from
00036  *       this software without specific prior written permission.
00037  *
00038  * This program is free software: you can redistribute it and/or modify
00039  * it under the terms of the GNU Lesser General Public License LGPL as 
00040  * published by the Free Software Foundation, either version 3 of the 
00041  * License, or (at your option) any later version.
00042  * 
00043  * This program is distributed in the hope that it will be useful,
00044  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00045  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00046  * GNU Lesser General Public License LGPL for more details.
00047  * 
00048  * You should have received a copy of the GNU Lesser General Public 
00049  * License LGPL along with this program. 
00050  * If not, see <http://www.gnu.org/licenses/>.
00051  *
00052  ****************************************************************/
00053 
00054 #include <cob_undercarriage_ctrl/UndercarriageCtrlGeom.h>
00055 
00056 // Constructor
00057 UndercarriageCtrlGeom::UndercarriageCtrlGeom(std::string sIniDirectory)
00058 {
00059         m_sIniDirectory = sIniDirectory;
00060         
00061         // init EMStop flag
00062         m_bEMStopActive = false;
00063         
00064         IniFile iniFile;
00065         iniFile.SetFileName(m_sIniDirectory + "Platform.ini", "UnderCarriageCtrlGeom.cpp");
00066         iniFile.GetKeyInt("Config", "NumberOfWheels", &m_iNumberOfDrives, true);
00067 
00068         // init vectors
00069         m_vdVelGearDriveRadS.assign(4,0);
00070         m_vdVelGearSteerRadS.assign(4,0);
00071         m_vdDltAngGearDriveRad.assign(4,0);
00072         m_vdAngGearSteerRad.assign(4,0);
00073 
00074         //m_vdVelGearDriveIntpRadS.assign(4,0);
00075         //m_vdVelGearSteerIntpRadS.assign(4,0);
00076         //m_vdAngGearSteerIntpRad.assign(4,0);
00077 
00078         m_vdVelGearDriveCmdRadS.assign(4,0);
00079         m_vdVelGearSteerCmdRadS.assign(4,0);
00080         m_vdAngGearSteerCmdRad.assign(4,0);
00081 
00082         m_vdWheelXPosMM.assign(4,0);
00083         m_vdWheelYPosMM.assign(4,0);
00084         m_vdWheelDistMM.assign(4,0);
00085         m_vdWheelAngRad.assign(4,0);
00086 
00087         m_vdExWheelXPosMM.assign(4,0);
00088         m_vdExWheelYPosMM.assign(4,0);
00089         m_vdExWheelDistMM.assign(4,0);
00090         m_vdExWheelAngRad.assign(4,0);
00091 
00092         m_vdAngGearSteerTarget1Rad.assign(4,0);
00093         m_vdVelGearDriveTarget1RadS.assign(4,0);
00094         m_vdAngGearSteerTarget2Rad.assign(4,0);
00095         m_vdVelGearDriveTarget2RadS.assign(4,0);
00096         m_vdAngGearSteerTargetRad.assign(4,0);
00097         m_vdVelGearDriveTargetRadS.assign(4,0);
00098 
00099         m_dCmdVelLongMMS = 0;
00100         m_dCmdVelLatMMS = 0;
00101         m_dCmdRotRobRadS = 0;
00102         m_dCmdRotVelRadS = 0;
00103         
00104         m_UnderCarriagePrms.WheelNeutralPos.assign(4,0);
00105         m_UnderCarriagePrms.vdSteerDriveCoupling.assign(4,0);
00106         m_UnderCarriagePrms.vdFactorVel.assign(4,0);
00107         
00108         m_vdCtrlVal.assign( 4, std::vector<double> (2,0.0) );
00109         
00110         //m_vdDeltaAngIntpRad.assign(4,0);
00111         //m_vdDeltaDriveIntpRadS.assign(4,0);
00112 
00113         // init Prms of Impedance-Ctrlr
00114         m_dSpring = 10.0;
00115         m_dDamp = 2.5;
00116         m_dVirtM = 0.1;
00117         m_dDPhiMax = 12.0;
00118         m_dDDPhiMax = 100.0;
00119 
00120         /*// Logging for debugging
00121         // Init timestamp for startup of the robot
00122         m_StartTime.SetNow();
00123         // open Files for PltfVel
00124         m_pfileDesVel = fopen("LogCtrl/DesPltfVel.txt","w");
00125         m_pfileMeasVel = fopen("LogCtrl/MeasPltfVel.txt","w");
00126         // open Files for corresponding Joint-Configuration
00127         m_pfileSteerAngTarget1 = fopen("LogCtrl/SteerAngTarget1.txt","w");
00128         m_pfileSteerAngTarget2 = fopen("LogCtrl/SteerAngTarget2.txt","w");
00129         m_pfileSteerAngTarget = fopen("LogCtrl/SteerAngTarget.txt","w");
00130         m_pfileDriveVelTarget = fopen("LogCtrl/DriveVelTarget.txt","w");
00131         // open Files for resulting Joint-Commands
00132         m_pfileSteerAngCmd = fopen("LogCtrl/SteerAngCmd.txt","w");
00133         m_pfileSteerVelCmd = fopen("LogCtrl/SteerVelCmd.txt","w");
00134         m_pfileDriveVelCmd = fopen("LogCtrl/DriveVelCmd.txt","w");*/
00135 
00136 }
00137 
00138 // Destructor
00139 UndercarriageCtrlGeom::~UndercarriageCtrlGeom(void)
00140 {
00141                 /*fclose(m_pfileDesVel);
00142                 fclose(m_pfileMeasVel);
00143 
00144                 fclose(m_pfileSteerAngTarget1);
00145                 fclose(m_pfileSteerAngTarget2);
00146 
00147                 fclose(m_pfileSteerAngTarget);
00148                 fclose(m_pfileDriveVelTarget);
00149 
00150                 fclose(m_pfileSteerAngCmd);
00151                 fclose(m_pfileSteerVelCmd);
00152                 fclose(m_pfileDriveVelCmd);*/
00153 }
00154 
00155 // Initialize Parameters for Controller and Kinematics
00156 void UndercarriageCtrlGeom::InitUndercarriageCtrl(void)
00157 {
00158         //LOG_OUT("Initializing Undercarriage-Controller (Geom)");
00159 
00160         IniFile iniFile;
00161 
00162         iniFile.SetFileName(m_sIniDirectory + "Platform.ini", "UnderCarriageCtrlGeom.cpp");
00163         iniFile.GetKeyInt("Geom", "DistWheels", &m_UnderCarriagePrms.iDistWheels, true);
00164         iniFile.GetKeyInt("Geom", "RadiusWheel", &m_UnderCarriagePrms.iRadiusWheelMM, true);
00165         iniFile.GetKeyInt("Geom", "DistSteerAxisToDriveWheelCenter", &m_UnderCarriagePrms.iDistSteerAxisToDriveWheelMM, true);
00166 
00167         iniFile.GetKeyDouble("Geom", "Wheel1XPos", &m_vdWheelXPosMM[0], true);
00168         iniFile.GetKeyDouble("Geom", "Wheel1YPos", &m_vdWheelYPosMM[0], true);
00169         iniFile.GetKeyDouble("Geom", "Wheel2XPos", &m_vdWheelXPosMM[1], true);
00170         iniFile.GetKeyDouble("Geom", "Wheel2YPos", &m_vdWheelYPosMM[1], true);
00171         iniFile.GetKeyDouble("Geom", "Wheel3XPos", &m_vdWheelXPosMM[2], true);
00172         iniFile.GetKeyDouble("Geom", "Wheel3YPos", &m_vdWheelYPosMM[2], true);
00173         iniFile.GetKeyDouble("Geom", "Wheel4XPos", &m_vdWheelXPosMM[3], true);
00174         iniFile.GetKeyDouble("Geom", "Wheel4YPos", &m_vdWheelYPosMM[3], true);
00175 
00176         iniFile.GetKeyDouble("DrivePrms", "MaxDriveRate", &m_UnderCarriagePrms.dMaxDriveRateRadpS, true);
00177         iniFile.GetKeyDouble("DrivePrms", "MaxSteerRate", &m_UnderCarriagePrms.dMaxSteerRateRadpS, true);
00178 
00179         iniFile.GetKeyDouble("DrivePrms", "Wheel1SteerDriveCoupling", &m_UnderCarriagePrms.vdSteerDriveCoupling[0], true);
00180         iniFile.GetKeyDouble("DrivePrms", "Wheel2SteerDriveCoupling", &m_UnderCarriagePrms.vdSteerDriveCoupling[1], true);
00181         iniFile.GetKeyDouble("DrivePrms", "Wheel3SteerDriveCoupling", &m_UnderCarriagePrms.vdSteerDriveCoupling[2], true);
00182         iniFile.GetKeyDouble("DrivePrms", "Wheel4SteerDriveCoupling", &m_UnderCarriagePrms.vdSteerDriveCoupling[3], true);
00183 
00184         iniFile.GetKeyDouble("DrivePrms", "Wheel1NeutralPosition", &m_UnderCarriagePrms.WheelNeutralPos[0], true);
00185         iniFile.GetKeyDouble("DrivePrms", "Wheel2NeutralPosition", &m_UnderCarriagePrms.WheelNeutralPos[1], true);
00186         iniFile.GetKeyDouble("DrivePrms", "Wheel3NeutralPosition", &m_UnderCarriagePrms.WheelNeutralPos[2], true);
00187         iniFile.GetKeyDouble("DrivePrms", "Wheel4NeutralPosition", &m_UnderCarriagePrms.WheelNeutralPos[3], true);
00188         
00189         for(int i = 0; i<4; i++)
00190         {       
00191                 m_UnderCarriagePrms.WheelNeutralPos[i] = MathSup::convDegToRad(m_UnderCarriagePrms.WheelNeutralPos[i]);
00192                 // provisorial --> skip interpolation
00193                 m_vdAngGearSteerCmdRad[i] = m_UnderCarriagePrms.WheelNeutralPos[i];
00194                 //m_vdAngGearSteerIntpRad[i] = m_UnderCarriagePrms.WheelNeutralPos[i];
00195                 
00196                 // also Init choosen Target angle
00197                 m_vdAngGearSteerTargetRad[i] = m_UnderCarriagePrms.WheelNeutralPos[i];
00198         }
00199         
00200         iniFile.GetKeyDouble("Thread", "ThrUCarrCycleTimeS", &m_UnderCarriagePrms.dCmdRateS, true);
00201 
00202         // Read Values for Steering Position Controller from IniFile
00203         iniFile.SetFileName(m_sIniDirectory + "MotionCtrl.ini", "PltfHardwareCoB3.h");
00204         // Prms of Impedance-Ctrlr
00205         iniFile.GetKeyDouble("SteerCtrl", "Spring", &m_dSpring, true);
00206         iniFile.GetKeyDouble("SteerCtrl", "Damp", &m_dDamp, true);
00207         iniFile.GetKeyDouble("SteerCtrl", "VirtMass", &m_dVirtM, true);
00208         iniFile.GetKeyDouble("SteerCtrl", "DPhiMax", &m_dDPhiMax, true);
00209         iniFile.GetKeyDouble("SteerCtrl", "DDPhiMax", &m_dDDPhiMax, true);
00210 
00211         // calculate polar coords of Wheel Axis in robot coordinate frame
00212         for(int i=0; i<4; i++)
00213         {
00214                 m_vdWheelDistMM[i] = sqrt( (m_vdWheelXPosMM[i] * m_vdWheelXPosMM[i]) + (m_vdWheelYPosMM[i] * m_vdWheelYPosMM[i]) );
00215                 m_vdWheelAngRad[i] = MathSup::atan4quad(m_vdWheelXPosMM[i], m_vdWheelYPosMM[i]);
00216         }
00217 
00218         // Calculate exact position of wheels in cart. and polar coords in robot coordinate frame
00219         CalcExWheelPos();
00220 
00221         // calculate compensation factor for velocity
00222         for(int i = 0; i<4; i++)
00223         {
00224                 m_UnderCarriagePrms.vdFactorVel[i] = - m_UnderCarriagePrms.vdSteerDriveCoupling[i]
00225                                      +(double(m_UnderCarriagePrms.iDistSteerAxisToDriveWheelMM) / double(m_UnderCarriagePrms.iRadiusWheelMM));
00226         }
00227 
00228 }
00229 
00230 // Set desired value for Plattfrom Velocity to UndercarriageCtrl (Sollwertvorgabe)
00231 void UndercarriageCtrlGeom::SetDesiredPltfVelocity(double dCmdVelLongMMS, double dCmdVelLatMMS, double dCmdRotRobRadS, double dCmdRotVelRadS)
00232 {       
00233         // declare auxiliary variables
00234         double dCurrentPosWheelRAD;
00235         double dtempDeltaPhi1RAD, dtempDeltaPhi2RAD;    // difference between possible steering angels and current steering angle
00236         double dtempDeltaPhiCmd1RAD, dtempDeltaPhiCmd2RAD;      // difference between possible steering angels and last target steering angle
00237         double dtempWeightedDelta1RAD, dtempWeightedDelta2RAD; // weighted Summ of the two distance values
00238 
00239         // copy function parameters to member variables
00240         m_dCmdVelLongMMS = dCmdVelLongMMS;
00241         m_dCmdVelLatMMS = dCmdVelLatMMS;
00242         m_dCmdRotRobRadS = dCmdRotRobRadS;
00243         m_dCmdRotVelRadS = dCmdRotVelRadS;
00244 
00245         CalcInverse();
00246 
00247         // determine optimal Pltf-Configuration
00248         for (int i = 0; i<4; i++)
00249         {
00250                 // Normalize Actual Wheel Position before calculation
00251                 dCurrentPosWheelRAD = m_vdAngGearSteerRad[i];
00252                 MathSup::normalizePi(dCurrentPosWheelRAD);
00253                 
00254                 // Calculate differences between current config to possible set-points
00255                 dtempDeltaPhi1RAD = m_vdAngGearSteerTarget1Rad[i] - dCurrentPosWheelRAD;
00256                 dtempDeltaPhi2RAD = m_vdAngGearSteerTarget2Rad[i] - dCurrentPosWheelRAD;
00257                 MathSup::normalizePi(dtempDeltaPhi1RAD);
00258                 MathSup::normalizePi(dtempDeltaPhi2RAD);
00259                 // Calculate differences between last steering target to possible set-points
00260                 dtempDeltaPhiCmd1RAD = m_vdAngGearSteerTarget1Rad[i] - m_vdAngGearSteerTargetRad[i];
00261                 dtempDeltaPhiCmd2RAD = m_vdAngGearSteerTarget2Rad[i] - m_vdAngGearSteerTargetRad[i];
00262                 MathSup::normalizePi(dtempDeltaPhiCmd1RAD);
00263                 MathSup::normalizePi(dtempDeltaPhiCmd2RAD);
00264                 
00265                 // determine optimal setpoint value
00266                 // 1st which set point is closest to current cinfog
00267                 //     but: avoid permanent switching (if next target is about PI/2 from current config)
00268                 // 2nd which set point is closest to last set point
00269                 // "fitness criteria" to choose optimal set point:
00270                 // calculate accumulted (+ weighted) difference between targets, current config. and last command
00271                 dtempWeightedDelta1RAD = 0.6*fabs(dtempDeltaPhi1RAD) + 0.4*fabs(dtempDeltaPhiCmd1RAD);
00272                 dtempWeightedDelta2RAD = 0.6*fabs(dtempDeltaPhi2RAD) + 0.4*fabs(dtempDeltaPhiCmd2RAD);
00273 
00274                 // check which set point "minimizes fitness criteria"
00275                 if (dtempWeightedDelta1RAD <= dtempWeightedDelta2RAD)
00276                 {
00277                         // Target1 is "optimal"
00278                         m_vdVelGearDriveTargetRadS[i] = m_vdVelGearDriveTarget1RadS[i];
00279                         m_vdAngGearSteerTargetRad[i] = m_vdAngGearSteerTarget1Rad[i];
00280                 }
00281                 else
00282                 {
00283                         // Target2 is "optimal"
00284                         m_vdVelGearDriveTargetRadS[i] = m_vdVelGearDriveTarget2RadS[i];
00285                         m_vdAngGearSteerTargetRad[i] = m_vdAngGearSteerTarget2Rad[i];
00286                 }
00287                 
00288                 // provisorial --> skip interpolation and always take Target1
00289                 //m_vdVelGearDriveCmdRadS[i] = m_vdVelGearDriveTarget1RadS[i];
00290                 //m_vdAngGearSteerCmdRad[i] = m_vdAngGearSteerTarget1Rad[i];
00291 
00292                 /*// interpolate between last setpoint and theone of the new setpoint, which is closest to the current configuration
00293                 if (fabs(dtempDeltaPhi1RAD) <= fabs(dtempDeltaPhi2RAD))
00294                 {
00295                         // difference between new target orientation and last (interpolated) target orientation
00296                         dtempDeltaPhi1RAD = m_vdAngGearSteerTarget1Rad[i] - m_vdAngGearSteerIntpRad[i];
00297                         MathSup::normalizePi(dtempDeltaPhi1RAD);
00298 
00299                         // calculate interpolation step sizes, to reach target at end of the cycle
00300                         m_vdDeltaAngIntpRad[i] = dtempDeltaPhi1RAD;
00301                         m_vdDeltaDriveIntpRadS[i] = (m_vdVelGearDriveTarget1RadS[i] - m_vdVelGearDriveIntpRadS[i]);
00302 
00303                         // additionally calculate meen change in angular config for feedforward cmd
00304                         //m_vdVelGearSteerIntpRadS[i] = dtempDeltaPhi1RAD/m_UnderCarriagePrms.dCmdRateS;
00305                 }
00306                 else
00307                 {
00308                         // difference between new target orientation and last (interpolated) target orientation
00309                         dtempDeltaPhi2RAD = m_vdAngGearSteerTarget2Rad[i] - m_vdAngGearSteerIntpRad[i];
00310                         MathSup::normalizePi(dtempDeltaPhi2RAD);
00311 
00312                         // calculate interpolation step sizes, to reach target at end of the cycle
00313                         m_vdDeltaAngIntpRad[i] = dtempDeltaPhi2RAD;
00314                         m_vdDeltaDriveIntpRadS[i] = (m_vdVelGearDriveTarget2RadS[i] - m_vdVelGearDriveIntpRadS[i]);
00315 
00316                         // additionally calculate meen change in angular config for feedforward cmd
00317                         //m_vdVelGearSteerIntpRadS[i] = dtempDeltaPhi2RAD/m_UnderCarriagePrms.dCmdRateS;
00318                 }*/
00319         }
00320 
00321         /*// Logging for debugging      
00322         // get current time
00323         m_RawTime.SetNow();
00324         m_dNowTime = m_RawTime - m_StartTime;
00325         // Log out Pltf-Velocities
00326         fprintf(m_pfileDesVel, "%f %f %f %f \n", m_dNowTime, dCmdVelLongMMS, dCmdVelLatMMS, dCmdRotRobRadS);
00327         fprintf(m_pfileMeasVel, "%f %f %f %f \n", m_dNowTime, m_dVelLongMMS, m_dVelLatMMS, m_dRotRobRadS);
00328         // Log out corresponding Joint-Configuration
00329         fprintf(m_pfileSteerAngTarget1, "%f %f %f %f %f \n", m_dNowTime, m_vdAngGearSteerTarget1Rad[0], m_vdAngGearSteerTarget1Rad[1], m_vdAngGearSteerTarget1Rad[2], m_vdAngGearSteerTarget1Rad[3]);
00330         fprintf(m_pfileSteerAngTarget2, "%f %f %f %f %f \n", m_dNowTime, m_vdAngGearSteerTarget2Rad[0], m_vdAngGearSteerTarget2Rad[1], m_vdAngGearSteerTarget2Rad[2], m_vdAngGearSteerTarget2Rad[3]);
00331         fprintf(m_pfileSteerAngTarget, "%f %f %f %f %f \n", m_dNowTime, m_vdAngGearSteerTargetRad[0], m_vdAngGearSteerTargetRad[1], m_vdAngGearSteerTargetRad[2], m_vdAngGearSteerTargetRad[3]);
00332         fprintf(m_pfileDriveVelTarget, "%f %f %f %f %f \n", m_dNowTime, m_vdVelGearDriveTargetRadS[0], m_vdVelGearDriveTargetRadS[1], m_vdVelGearDriveTargetRadS[2], m_vdVelGearDriveTargetRadS[3]);*/
00333 }
00334 
00335 // Set actual values of wheels (steer/drive velocity/position) (Istwerte)
00336 void UndercarriageCtrlGeom::SetActualWheelValues(std::vector<double> vdVelGearDriveRadS, std::vector<double> vdVelGearSteerRadS, std::vector<double> vdDltAngGearDriveRad, std::vector<double> vdAngGearSteerRad)
00337 {
00338         //LOG_OUT("Set Wheel Position to Controller");
00339 
00340         m_vdVelGearDriveRadS = vdVelGearDriveRadS;
00341         m_vdVelGearSteerRadS = vdVelGearSteerRadS;
00342         m_vdDltAngGearDriveRad = vdDltAngGearDriveRad;
00343         m_vdAngGearSteerRad = vdAngGearSteerRad;
00344 
00345         // calc exact Wheel Positions (taking into account lever arm)
00346         CalcExWheelPos();
00347         
00348         // Peform calculation of direct kinematics (approx.) based on corrected Wheel Positions
00349         CalcDirect();
00350 }
00351 
00352 // Get result of inverse kinematics (without controller)
00353 void UndercarriageCtrlGeom::GetSteerDriveSetValues(std::vector<double> & vdVelGearDriveRadS, std::vector<double> & vdAngGearSteerRad)
00354 {
00355         //LOG_OUT("Calculate Inverse for given Velocity Command");
00356 
00357         CalcInverse();
00358 
00359         vdVelGearDriveRadS = m_vdVelGearDriveTarget1RadS;
00360         vdAngGearSteerRad = m_vdAngGearSteerTarget1Rad;
00361 }
00362 
00363 // Get set point values for the Wheels (including controller) from UndercarriangeCtrl
00364 void UndercarriageCtrlGeom::GetNewCtrlStateSteerDriveSetValues(std::vector<double> & vdVelGearDriveRadS, std::vector<double> & vdVelGearSteerRadS, std::vector<double> & vdAngGearSteerRad,
00365                                                                  double & dVelLongMMS, double & dVelLatMMS, double & dRotRobRadS, double & dRotVelRadS)
00366 {
00367 
00368         if(m_bEMStopActive == false)
00369         {
00370                 //Calculate next step
00371                 CalcControlStep();
00372         }
00373 
00374         vdVelGearDriveRadS = m_vdVelGearDriveCmdRadS;
00375         vdVelGearSteerRadS = m_vdVelGearSteerCmdRadS;
00376         vdAngGearSteerRad = m_vdAngGearSteerCmdRad;
00377 
00378         dVelLongMMS = m_dCmdVelLongMMS;
00379         dVelLatMMS = m_dCmdVelLatMMS;
00380         dRotRobRadS = m_dCmdRotRobRadS;
00381         dRotVelRadS = m_dCmdRotVelRadS;
00382 
00383         /*// Logging for debugging              
00384         // get current time
00385         m_RawTime.SetNow();
00386         m_dNowTime = m_RawTime - m_StartTime;
00387         // Log out resulting joint-commands
00388         fprintf(m_pfileSteerAngCmd, "%f %f %f %f %f \n", m_dNowTime, m_vdAngGearSteerCmdRad[0], m_vdAngGearSteerCmdRad[1], m_vdAngGearSteerCmdRad[2], m_vdAngGearSteerCmdRad[3]);
00389         fprintf(m_pfileSteerVelCmd, "%f %f %f %f %f \n", m_dNowTime, m_vdVelGearSteerCmdRadS[0], m_vdVelGearSteerCmdRadS[1], m_vdVelGearSteerCmdRadS[2], m_vdVelGearSteerCmdRadS[3]);
00390         fprintf(m_pfileDriveVelCmd, "%f %f %f %f %f \n", m_dNowTime, m_vdVelGearDriveCmdRadS[0], m_vdVelGearDriveCmdRadS[1], m_vdVelGearDriveCmdRadS[2], m_vdVelGearDriveCmdRadS[3]);*/
00391 }
00392 
00393 // Get result of direct kinematics
00394 void UndercarriageCtrlGeom::GetActualPltfVelocity(double & dDeltaLongMM, double & dDeltaLatMM, double & dDeltaRotRobRad, double & dDeltaRotVelRad,
00395                                                                                                   double & dVelLongMMS, double & dVelLatMMS, double & dRotRobRadS, double & dRotVelRadS)
00396 {
00397         dVelLongMMS = m_dVelLongMMS;
00398         dVelLatMMS = m_dVelLatMMS;
00399         dRotRobRadS = m_dRotRobRadS;
00400         dRotVelRadS = m_dRotVelRadS;
00401 
00402         // calculate travelled distance and angle (from velocity) for output
00403         // ToDo: make sure this corresponds to cycle-freqnecy of calling node
00404         //       --> specify via config file
00405         dDeltaLongMM = dVelLongMMS * m_UnderCarriagePrms.dCmdRateS;
00406         dDeltaLatMM = dVelLatMMS * m_UnderCarriagePrms.dCmdRateS;
00407         dDeltaRotRobRad = dRotRobRadS * m_UnderCarriagePrms.dCmdRateS;
00408         dDeltaRotVelRad = dRotVelRadS * m_UnderCarriagePrms.dCmdRateS;
00409 }
00410 
00411 // calculate inverse kinematics
00412 void UndercarriageCtrlGeom::CalcInverse(void)
00413 {       
00414         // help variable to store velocities of the steering axis in mm/s
00415         double dtempAxVelXRobMMS, dtempAxVelYRobMMS;    
00416 
00417         // check if zero movement commanded -> keep orientation of wheels, set wheel velocity to zero
00418         if((m_dCmdVelLongMMS == 0) && (m_dCmdVelLatMMS == 0) && (m_dCmdRotRobRadS == 0) && (m_dCmdRotVelRadS == 0))
00419         {
00420                 for(int i = 0; i<4; i++)
00421                 {
00422                         m_vdAngGearSteerTarget1Rad[i] = m_vdAngGearSteerRad[i];
00423                         m_vdVelGearDriveTarget1RadS[i] = 0.0;
00424                         m_vdAngGearSteerTarget2Rad[i] = m_vdAngGearSteerRad[i];
00425                         m_vdVelGearDriveTarget2RadS[i] = 0.0;
00426                 }
00427                 return;
00428         }
00429 
00430         // calculate sets of possible Steering Angle // Drive-Velocity combinations
00431         for (int i = 0; i<4; i++)
00432         {       
00433                 // calculate velocity and direction of single wheel motion
00434                 // Translational Portion
00435                 dtempAxVelXRobMMS = m_dCmdVelLongMMS;
00436                 dtempAxVelYRobMMS = m_dCmdVelLatMMS;
00437                 // Rotational Portion
00438                 dtempAxVelXRobMMS += m_dCmdRotRobRadS * m_vdExWheelDistMM[i] * -sin(m_vdExWheelAngRad[i]);
00439                 dtempAxVelYRobMMS += m_dCmdRotRobRadS * m_vdExWheelDistMM[i] * cos(m_vdExWheelAngRad[i]);
00440                 
00441                 // calculate resulting steering angle 
00442                 // Wheel has to move in direction of resulting velocity vector of steering axis 
00443                 m_vdAngGearSteerTarget1Rad[i] = MathSup::atan4quad(dtempAxVelYRobMMS, dtempAxVelXRobMMS);
00444                 // calculate corresponding angle in opposite direction (+180 degree)
00445                 m_vdAngGearSteerTarget2Rad[i] = m_vdAngGearSteerTarget1Rad[i] + MathSup::PI;
00446                 MathSup::normalizePi(m_vdAngGearSteerTarget2Rad[i]);
00447                 
00448                 // calculate absolute value of rotational rate of driving wheels in rad/s
00449                 m_vdVelGearDriveTarget1RadS[i] = sqrt( (dtempAxVelXRobMMS * dtempAxVelXRobMMS) + 
00450                                                    (dtempAxVelYRobMMS * dtempAxVelYRobMMS) ) / (double)m_UnderCarriagePrms.iRadiusWheelMM;
00451                 // now adapt to direction (forward/backward) of wheel
00452                 m_vdVelGearDriveTarget2RadS[i] = - m_vdVelGearDriveTarget1RadS[i];
00453         }
00454 }
00455 
00456 // calculate direct kinematics
00457 void UndercarriageCtrlGeom::CalcDirect(void)
00458 {
00459         // declare auxilliary variables
00460         double dtempVelXRobMMS;         // Robot-Velocity in x-Direction (longitudinal) in mm/s (in Robot-Coordinateframe)
00461         double dtempVelYRobMMS;         // Robot-Velocity in y-Direction (lateral) in mm/s (in Robot-Coordinateframe)
00462         double dtempRotRobRADPS;        // Robot-Rotation-Rate in rad/s (in Robot-Coordinateframe)
00463         double dtempDiffXMM;            // Difference in X-Coordinate of two wheels in mm
00464         double dtempDiffYMM;            // Difference in Y-Coordinate of two wheels in mm
00465         double dtempRelPhiWheelsRAD;    // Angle between axis of two wheels w.r.t the X-Axis of the Robot-Coordinate-System in rad
00466         double dtempRelDistWheelsMM;    // distance of two wheels in mm 
00467         double dtempRelPhiWheel1RAD;    // Steering Angle of (im math. pos. direction) first Wheel w.r.t. the linking axis of the two wheels
00468         double dtempRelPhiWheel2RAD;    // Steering Angle of (im math. pos. direction) first Wheel w.r.t. the linking axis of the two wheels
00469         std::vector<double> vdtempVelWheelMMS(4);       // Wheel-Velocities (all Wheels) in mm/s
00470 
00471         // initial values
00472         dtempVelXRobMMS = 0;                    // Robot-Velocity in x-Direction (longitudinal) in mm/s (in Robot-Coordinateframe)
00473         dtempVelYRobMMS = 0;                    // Robot-Velocity in y-Direction (lateral) in mm/s (in Robot-Coordinateframe)
00474         dtempRotRobRADPS = 0;
00475 
00476 
00477 
00478         // calculate corrected wheel velocities
00479         for(int i = 0; i<m_iNumberOfDrives; i++)
00480         {
00481                 // calc effective Driving-Velocity
00482                 vdtempVelWheelMMS[i] = m_UnderCarriagePrms.iRadiusWheelMM * (m_vdVelGearDriveRadS[i] - m_UnderCarriagePrms.vdFactorVel[i]* m_vdVelGearSteerRadS[i]);
00483         }
00484 
00485         // calculate rotational rate of robot and current "virtual" axis between all wheels
00486         for(int i = 0; i< (m_iNumberOfDrives-1) ; i++)
00487         {
00488                 // calc Parameters (Dist,Phi) of virtual linking axis of the two considered wheels
00489                 dtempDiffXMM = m_vdExWheelXPosMM[i+1] - m_vdExWheelXPosMM[i];
00490                 dtempDiffYMM = m_vdExWheelYPosMM[i+1] - m_vdExWheelYPosMM[i];
00491                 dtempRelDistWheelsMM = sqrt( dtempDiffXMM*dtempDiffXMM + dtempDiffYMM*dtempDiffYMM );
00492                 dtempRelPhiWheelsRAD = MathSup::atan4quad( dtempDiffYMM, dtempDiffXMM );
00493 
00494                 // transform velocity of wheels into relative coordinate frame of linking axes -> subtract angles
00495                 dtempRelPhiWheel1RAD = m_vdAngGearSteerRad[i] - dtempRelPhiWheelsRAD;
00496                 dtempRelPhiWheel2RAD = m_vdAngGearSteerRad[i+1] - dtempRelPhiWheelsRAD;
00497 
00498                 dtempRotRobRADPS += (vdtempVelWheelMMS[i+1] * sin(dtempRelPhiWheel2RAD) - vdtempVelWheelMMS[i] * sin(dtempRelPhiWheel1RAD))/dtempRelDistWheelsMM;
00499         }
00500 
00501         // calculate last missing axis (between last wheel and 1.)
00502         // calc. Parameters (Dist,Phi) of virtual linking axis of the two considered wheels
00503         dtempDiffXMM = m_vdExWheelXPosMM[0] - m_vdExWheelXPosMM[m_iNumberOfDrives-1];
00504         dtempDiffYMM = m_vdExWheelYPosMM[0] - m_vdExWheelYPosMM[m_iNumberOfDrives-1];
00505         dtempRelDistWheelsMM = sqrt( dtempDiffXMM*dtempDiffXMM + dtempDiffYMM*dtempDiffYMM );
00506         dtempRelPhiWheelsRAD = MathSup::atan4quad( dtempDiffYMM, dtempDiffXMM );
00507 
00508         // transform velocity of wheels into relative coordinate frame of linking axes -> subtract angles
00509         dtempRelPhiWheel1RAD = m_vdAngGearSteerRad[m_iNumberOfDrives-1] - dtempRelPhiWheelsRAD;
00510         dtempRelPhiWheel2RAD = m_vdAngGearSteerRad[0] - dtempRelPhiWheelsRAD;
00511 
00512         // close calculation of robots rotational velocity
00513         dtempRotRobRADPS += (vdtempVelWheelMMS[0]*sin(dtempRelPhiWheel2RAD) - vdtempVelWheelMMS[m_iNumberOfDrives-1]*sin(dtempRelPhiWheel1RAD))/dtempRelDistWheelsMM;
00514 
00515         // calculate linear velocity of robot
00516         for(int i = 0; i<m_iNumberOfDrives; i++)
00517         {
00518                 dtempVelXRobMMS += vdtempVelWheelMMS[i]*cos(m_vdAngGearSteerRad[i]);
00519                 dtempVelYRobMMS += vdtempVelWheelMMS[i]*sin(m_vdAngGearSteerRad[i]);
00520         }
00521 
00522         // assign rotational velocities for output
00523         m_dRotRobRadS = dtempRotRobRADPS/m_iNumberOfDrives;
00524         m_dRotVelRadS = 0; // currently not used to represent 3rd degree of freedom -> set to zero
00525 
00526         // assign linear velocity of robot for output
00527         m_dVelLongMMS = dtempVelXRobMMS/m_iNumberOfDrives;
00528         m_dVelLatMMS = dtempVelYRobMMS/m_iNumberOfDrives;
00529 
00530         
00531 
00532 }
00533 
00534 // calculate Exact Wheel Position in robot coordinates
00535 void UndercarriageCtrlGeom::CalcExWheelPos(void)
00536 {
00537         // calculate wheel position and velocity
00538         for(int i = 0; i<4; i++)
00539         {
00540                 // calculate current geometry of robot (exact wheel position, taking into account steering offset of wheels)
00541                 m_vdExWheelXPosMM[i] = m_vdWheelXPosMM[i] + m_UnderCarriagePrms.iDistSteerAxisToDriveWheelMM * sin(m_vdAngGearSteerRad[i]);
00542                 m_vdExWheelYPosMM[i] = m_vdWheelYPosMM[i] - m_UnderCarriagePrms.iDistSteerAxisToDriveWheelMM * cos(m_vdAngGearSteerRad[i]);
00543                                 
00544                 // calculate distance from platform center to wheel center
00545                 m_vdExWheelDistMM[i] = sqrt( (m_vdExWheelXPosMM[i] * m_vdExWheelXPosMM[i]) + (m_vdExWheelYPosMM[i] * m_vdExWheelYPosMM[i]) );
00546                 
00547                 // calculate direction of rotational vector
00548                 m_vdExWheelAngRad[i] = MathSup::atan4quad( m_vdExWheelYPosMM[i], m_vdExWheelXPosMM[i]);
00549         }
00550 }
00551 
00552 // perform one discrete Control Step (controls steering angle)
00553 void UndercarriageCtrlGeom::CalcControlStep(void)
00554 {       
00555         // check if zero movement commanded -> keep orientation of wheels, set steer velocity to zero
00556         if ((m_dCmdVelLongMMS == 0) && (m_dCmdVelLatMMS == 0) && (m_dCmdRotRobRadS == 0) && (m_dCmdRotVelRadS == 0))
00557         {
00558                 m_vdVelGearDriveCmdRadS.assign(4,0.0);          // set velocity for drives to zero
00559                 m_vdVelGearSteerCmdRadS.assign(4,0.0);          // set velocity for steers to zero
00560 
00561                 // set internal states of controller to zero
00562                 for(int i=0; i<4; i++)
00563                 {
00564                         m_vdCtrlVal[i][0] = 0.0;
00565                         m_vdCtrlVal[i][1] = 0.0;
00566                 }
00567                 return;
00568         }
00569 
00570         // declare auxilliary variables
00571         double dCurrentPosWheelRAD;
00572         double dDeltaPhi;
00573         double dForceDamp, dForceProp, dAccCmd, dVelCmdInt; // PI- and Impedance-Ctrl
00574         
00575         for (int i=0; i<4; i++)
00576         {
00577                 // provisorial --> skip interpolation and always take Target
00578                 m_vdVelGearDriveCmdRadS[i] = m_vdVelGearDriveTargetRadS[i];
00579                 m_vdAngGearSteerCmdRad[i] = m_vdAngGearSteerTargetRad[i];
00580 
00581                 // provisorial --> skip interpolation and always take Target1
00582                 //m_vdVelGearDriveCmdRadS[i] = m_vdVelGearDriveTarget1RadS[i];
00583                 //m_vdAngGearSteerCmdRad[i] = m_vdAngGearSteerTarget1Rad[i];
00584 
00585                 /*m_vdAngGearSteerIntpRad[i] += m_vdDeltaAngIntpRad[i];
00586                 MathSup::normalizePi(m_vdAngGearSteerIntpRad[i]);
00587                 m_vdVelGearDriveIntpRadS[i] += m_vdDeltaDriveIntpRadS[i];
00588 
00589                 m_vdVelGearDriveCmdRadS[i] = m_vdVelGearDriveIntpRadS[i];
00590                 m_vdAngGearSteerCmdRad[i] = m_vdAngGearSteerIntpRad[i];*/
00591         }
00592 
00593 
00594         for (int i = 0; i<4; i++)
00595         {
00596                 // Normalize Actual Wheel Position before calculation
00597                 dCurrentPosWheelRAD = m_vdAngGearSteerRad[i];
00598                 MathSup::normalizePi(dCurrentPosWheelRAD);
00599                 dDeltaPhi = m_vdAngGearSteerCmdRad[i] - dCurrentPosWheelRAD;
00600                 MathSup::normalizePi(dDeltaPhi);
00601 
00602                 // Impedance-Ctrl
00603                 // Calculate resulting desired forces, velocities
00604                 // double dForceDamp, dForceProp, dAccCmd, dVelCmdInt;
00605                 dForceDamp = - m_dDamp * m_vdCtrlVal[i][1];
00606                 dForceProp = m_dSpring * dDeltaPhi;
00607                 dAccCmd = (dForceDamp + dForceProp) / m_dVirtM;
00608                 if (dAccCmd > m_dDDPhiMax)
00609                 {
00610                         dAccCmd = m_dDDPhiMax;
00611                 }
00612                 else if (dAccCmd < -m_dDDPhiMax)
00613                 {
00614                         dAccCmd = -m_dDDPhiMax;
00615                 }
00616                 dVelCmdInt = m_vdCtrlVal[i][1] + m_UnderCarriagePrms.dCmdRateS * dAccCmd;
00617                 if (dVelCmdInt > m_dDPhiMax)
00618                 {
00619                         dVelCmdInt = m_dDPhiMax;
00620                 }
00621                 else if (dVelCmdInt < -m_dDPhiMax)
00622                 {
00623                         dVelCmdInt = -m_dDPhiMax;
00624                 }
00625                 // Store internal ctrlr-states
00626                 m_vdCtrlVal[i][0] = dDeltaPhi;
00627                 m_vdCtrlVal[i][1] = dVelCmdInt;
00628                 // set outputs
00629                 m_vdVelGearSteerCmdRadS[i] = dVelCmdInt;
00630 
00631                 // Check if Steeringvelocity overgo maximum allowed rates.
00632                 if(fabs(m_vdVelGearSteerCmdRadS[i]) > m_UnderCarriagePrms.dMaxSteerRateRadpS)
00633                 {
00634                         if (m_vdVelGearSteerCmdRadS[i] > 0)
00635                                 m_vdVelGearSteerCmdRadS[i] = m_UnderCarriagePrms.dMaxSteerRateRadpS;
00636                         else
00637                                 m_vdVelGearSteerCmdRadS[i] = -m_UnderCarriagePrms.dMaxSteerRateRadpS;
00638                 }
00639         }
00640         
00641         // Correct Driving-Wheel-Velocity, because of coupling and axis-offset
00642         for (int i = 0; i<4; i++)
00643         {
00644                 m_vdVelGearDriveCmdRadS[i] += m_vdVelGearSteerCmdRadS[i] * m_UnderCarriagePrms.vdFactorVel[i];
00645         }
00646 
00647 }
00648 
00649 // operator overloading
00650 void UndercarriageCtrlGeom::operator=(const UndercarriageCtrlGeom & GeomCtrl)
00651 {
00652         // Actual Values for PltfMovement (calculated from Actual Wheelspeeds)
00653         m_dVelLongMMS = GeomCtrl.m_dVelLongMMS;
00654         m_dVelLatMMS = GeomCtrl.m_dVelLatMMS;
00655         m_dRotRobRadS = GeomCtrl.m_dRotRobRadS;
00656         m_dRotVelRadS = GeomCtrl.m_dRotVelRadS;
00657 
00658         // Actual Wheelspeed (read from Motor-Ctrls)
00659         m_vdVelGearDriveRadS = GeomCtrl.m_vdVelGearDriveRadS;
00660         m_vdVelGearSteerRadS = GeomCtrl.m_vdVelGearSteerRadS;
00661         m_vdDltAngGearDriveRad = GeomCtrl.m_vdDltAngGearDriveRad;
00662         m_vdAngGearSteerRad = GeomCtrl.m_vdAngGearSteerRad;
00663 
00664         // Desired Pltf-Movement (set from PltfHwItf)
00665         m_dCmdVelLongMMS = GeomCtrl.m_dCmdVelLongMMS;
00666         m_dCmdVelLatMMS = GeomCtrl.m_dCmdVelLatMMS;
00667         m_dCmdRotRobRadS = GeomCtrl.m_dCmdRotRobRadS;
00668         m_dCmdRotVelRadS = GeomCtrl.m_dCmdRotVelRadS;
00669 
00670         // Desired Wheelspeeds (calculated from desired ICM-configuration)
00671         m_vdVelGearDriveCmdRadS = GeomCtrl.m_vdVelGearDriveCmdRadS;
00672         m_vdVelGearSteerCmdRadS = GeomCtrl.m_vdVelGearSteerCmdRadS;
00673         m_vdAngGearSteerCmdRad = GeomCtrl.m_vdAngGearSteerCmdRad;
00674 
00675         // Target Wheelspeed and -angle (calculated from desired Pltf-Movement with Inverse without controle!)
00676         // alternativ 1 for steering angle
00677         m_vdAngGearSteerTarget1Rad = GeomCtrl.m_vdAngGearSteerTarget1Rad;
00678         m_vdVelGearDriveTarget1RadS = GeomCtrl.m_vdVelGearDriveTarget1RadS;
00679         // alternativ 2 for steering angle (+/- PI)
00680         m_vdAngGearSteerTarget2Rad = GeomCtrl.m_vdAngGearSteerTarget2Rad;
00681         m_vdVelGearDriveTarget2RadS = GeomCtrl.m_vdVelGearDriveTarget2RadS;
00682 
00683         // Position of the Wheels' Steering Axis'
00684         m_vdWheelXPosMM = GeomCtrl.m_vdWheelXPosMM;
00685         m_vdWheelYPosMM = GeomCtrl.m_vdWheelYPosMM;
00686         m_vdWheelDistMM = GeomCtrl.m_vdWheelDistMM;
00687         m_vdWheelAngRad = GeomCtrl.m_vdWheelAngRad;
00688 
00689         // Exact Position of the Wheels' itself
00690         m_vdExWheelXPosMM = GeomCtrl.m_vdExWheelXPosMM;
00691         m_vdExWheelYPosMM = GeomCtrl.m_vdExWheelYPosMM;
00692         m_vdExWheelDistMM = GeomCtrl.m_vdExWheelDistMM;
00693         m_vdExWheelAngRad = GeomCtrl.m_vdExWheelAngRad;
00694 
00695         // Prms
00696         m_UnderCarriagePrms = GeomCtrl.m_UnderCarriagePrms;
00697 
00698         // Position Controller Steer Wheels
00699         // Impedance-Ctrlr
00700         m_dSpring = GeomCtrl.m_dSpring;
00701         m_dDamp = GeomCtrl.m_dDamp;
00702         m_dVirtM = GeomCtrl.m_dDPhiMax;
00703         m_dDPhiMax = GeomCtrl.m_dDPhiMax;
00704         m_dDDPhiMax = GeomCtrl.m_dDDPhiMax;
00705         // Storage for internal controller states
00706         m_vdCtrlVal = GeomCtrl.m_vdCtrlVal;
00707 }
00708 
00709 // set EM Flag and stop ctrlr if active
00710 void UndercarriageCtrlGeom::setEMStopActive(bool bEMStopActive)
00711 {
00712         m_bEMStopActive = bEMStopActive;
00713 
00714         // if emergency stop reset ctrlr to zero
00715         if(m_bEMStopActive)
00716         {
00717                 // Steermodules
00718                 for(int i=0; i<4; i++)
00719                 {
00720                         for(int j=0; j< 2; j++)
00721                         {
00722                                 m_vdCtrlVal[i][j] = 0.0;
00723                         }
00724                 }
00725                 // Outputs
00726                 for(int i=0; i<4; i++)
00727                 {
00728                         m_vdVelGearDriveCmdRadS[i] = 0.0;
00729                         m_vdVelGearSteerCmdRadS[i] = 0.0;
00730                 }
00731         }
00732 
00733 }


cob_undercarriage_ctrl
Author(s): Christian Connette
autogenerated on Sun Oct 5 2014 23:06:53