cob_undercarriage_ctrl: cob_undercarriage_ctrl: UndercarriageCtrlGeom.h Source 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 #ifndef UndercarriageCtrlGeom_INCLUDEDEF_H
00055 #define UndercarriageCtrlGeom_INCLUDEDEF_H
00056 
00057 //#include <time.h>
00058 
00059 #include <cob_utilities/IniFile.h>
00060 #include <cob_utilities/MathSup.h>
00061 #include <cob_utilities/TimeStamp.h>
00062 
00063 class UndercarriageCtrlGeom
00064 {
00065 private:
00066         
00067         bool m_bEMStopActive;
00068         
00069         int m_iNumberOfDrives;
00070         
00071         std::string m_sIniDirectory;
00072         
00073         /*// Logging for debugging
00074         // create Filepointers
00075         FILE * m_pfileDesVel, * m_pfileMeasVel;
00076         FILE * m_pfileSteerAngTarget1, * m_pfileSteerAngTarget2;
00077         FILE * m_pfileSteerAngTarget, * m_pfileDriveVelTarget;
00078         FILE * m_pfileSteerAngCmd, * m_pfileSteerVelCmd, * m_pfileDriveVelCmd;
00079         // create TimeStamp
00080         TimeStamp m_RawTime, m_StartTime;
00081         double m_dNowTime;*/
00082 
00083         
00084         // Actual Values for PltfMovement (calculated from Actual Wheelspeeds)
00085         double m_dVelLongMMS;
00086         double m_dVelLatMMS;
00087         double m_dRotRobRadS;
00088         double m_dRotVelRadS;
00089 
00090         // Actual Wheelspeed (read from Motor-Ctrls)
00091         std::vector<double> m_vdVelGearDriveRadS;
00092         std::vector<double> m_vdVelGearSteerRadS;
00093         std::vector<double> m_vdDltAngGearDriveRad;
00094         std::vector<double> m_vdAngGearSteerRad;
00095 
00096         // Desired Pltf-Movement (set from PltfHwItf)
00097         double m_dCmdVelLongMMS;
00098         double m_dCmdVelLatMMS;
00099         double m_dCmdRotRobRadS;
00100         double m_dCmdRotVelRadS;
00101 
00102         /*// Interpolated Wheelspeeds (calculated from desired and current Pltf-Config)
00103         std::vector<double> m_vdVelGearDriveIntpRadS;
00104         std::vector<double> m_vdVelGearSteerIntpRadS;
00105         std::vector<double> m_vdAngGearSteerIntpRad;
00106 
00107         // Delta values for interpolating between two commands
00108         std::vector<double> m_vdDeltaAngIntpRad;
00109         std::vector<double> m_vdDeltaDriveIntpRadS;*/
00110 
00111         // Desired Wheelspeeds set to ELMO-Ctrl's (calculated from desired Pltf-Movement)
00112         std::vector<double> m_vdVelGearDriveCmdRadS;
00113         std::vector<double> m_vdVelGearSteerCmdRadS;
00114         std::vector<double> m_vdAngGearSteerCmdRad;
00115 
00116         // Target Wheelspeed and -angle (calculated from desired Pltf-Movement with Inverse without controle!)
00117         // This Values might not be valid (to high step response in steering rate, ...) for commanding the drives
00118         std::vector<double> m_vdAngGearSteerTarget1Rad; // alternativ 1 for steering angle
00119         std::vector<double> m_vdVelGearDriveTarget1RadS;
00120         std::vector<double> m_vdAngGearSteerTarget2Rad; // alternativ 2 for steering angle (+/- PI)
00121         std::vector<double> m_vdVelGearDriveTarget2RadS;
00122         std::vector<double> m_vdAngGearSteerTargetRad; // choosen alternativ for steering angle
00123         std::vector<double> m_vdVelGearDriveTargetRadS;
00124 
00129         std::vector<double> m_vdWheelXPosMM;
00130         std::vector<double> m_vdWheelYPosMM;
00131         std::vector<double> m_vdWheelDistMM;
00132         std::vector<double> m_vdWheelAngRad;
00133 
00138         std::vector<double> m_vdExWheelXPosMM;
00139         std::vector<double> m_vdExWheelYPosMM;
00140         std::vector<double> m_vdExWheelDistMM;
00141         std::vector<double> m_vdExWheelAngRad;
00142 
00143         struct ParamType
00144         {
00145                 int iDistWheels;
00146                 int iRadiusWheelMM;
00147 
00148                 int iDistSteerAxisToDriveWheelMM;
00149 
00150                 double dMaxDriveRateRadpS;
00151                 double dMaxSteerRateRadpS;
00152                 double dCmdRateS;
00153                 std::vector<double> WheelNeutralPos;
00154                 std::vector<double> vdSteerDriveCoupling;
00159                 std::vector<double> vdFactorVel;
00160         };
00161 
00162         ParamType m_UnderCarriagePrms;
00163 
00174         double m_dSpring, m_dDamp, m_dVirtM, m_dDPhiMax, m_dDDPhiMax;
00183         std::vector< std::vector<double> > m_vdCtrlVal;
00184 
00185         // Factor for thread cycle time of ThreadMotionPltfCtrl and ThreadUnderCarriageCtrl     
00186         //double m_dThreadCycleMultiplier;
00187         
00188         // calculate inverse kinematics
00189         void CalcInverse(void);
00190 
00191         // calculate direct kinematics
00192         void CalcDirect(void);
00193 
00194         // calculate Exact Wheel Position in robot coordinates
00195         void CalcExWheelPos(void);
00196 
00197         // perform one discrete Controle Step
00198         void CalcControlStep(void);
00199 
00200 public:
00201 
00202         // Constructor
00203         UndercarriageCtrlGeom(std::string sIniDirectory);
00204 
00205         // Destructor
00206         ~UndercarriageCtrlGeom(void);
00207 
00208         // Initialize Parameters for Controller and Kinematics
00209         void InitUndercarriageCtrl(void);
00210 
00211         // Set desired value for Plattform Velocity to UndercarriageCtrl (Sollwertvorgabe)
00212         void SetDesiredPltfVelocity(double dCmdVelLongMMS, double dCmdVelLatMMS, double dCmdRotRobRadS, double dCmdRotVelRadS);
00213 
00214         // Set actual values of wheels (steer/drive velocity/position) (Istwerte)
00215         void SetActualWheelValues(std::vector<double> vdVelGearDriveRadS, std::vector<double> vdVelGearSteerRadS, std::vector<double> vdDltAngGearDriveRad, std::vector<double> vdAngGearSteerRad);
00216 
00217         // Get result of inverse kinematics (without controller)
00218         void GetSteerDriveSetValues(std::vector<double> & vdVelGearDriveRadS, std::vector<double> & vdAngGearSteerRad);
00219 
00220         // Get set point values for the Wheels (including controller) from UndercarriangeCtrl
00221         void GetNewCtrlStateSteerDriveSetValues(std::vector<double> & vdVelGearDriveRadS, std::vector<double> & vdVelGearSteerRadS, std::vector<double> & vdAngGearSteerRad,
00222                                                 double & dVelLongMMS, double & dVelLatMMS, double & dRotRobRadS, double & dRotVelRadS);
00223 
00224         // Get result of direct kinematics
00225         void GetActualPltfVelocity(double & dDeltaLongMM, double & dDeltaLatMM, double & dDeltaRotRobRad, double & dDeltaRotVelRad,
00226                                         double & dVelLongMMS, double & dVelLatMMS, double & dRotRobRadS, double & dRotVelRadS);
00227 
00228         // Set EM flag and stop Ctrlr
00229         void setEMStopActive(bool bEMStopActive);
00230         
00231         // operator overloading
00232         void operator=(const UndercarriageCtrlGeom & GeomCtrl);
00233 };
00234 #endif
00235