Homepage Demos Overview Downloads Tutorials Reference
Credits

control_select.cpp

Go to the documentation of this file.
00001 /*
00002 Copyright (C) 2002-2004  Etienne Lachance
00003 
00004 This library is free software; you can redistribute it and/or modify
00005 it under the terms of the GNU Lesser General Public License as
00006 published by the Free Software Foundation; either version 2.1 of the
00007 License, or (at your option) any later version.
00008 
00009 This library is distributed in the hope that it will be useful,
00010 but WITHOUT ANY WARRANTY; without even the implied warranty of
00011 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012 GNU Lesser General Public License for more details.
00013 
00014 You should have received a copy of the GNU Lesser General Public
00015 License along with this library; if not, write to the Free Software
00016 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00017 
00018 
00019 Report problems and direct all questions to:
00020 
00021 email: etienne.lachance@polymtl.ca or richard.gourdeau@polymtl.ca
00022 
00023 -------------------------------------------------------------------------------
00024 Revision_history:
00025 
00026 2004/07/13: Ethan Tira-Thompson
00027     -Added support for newmat's use_namespace #define, using ROBOOP namespace
00028     -Using config::select_real instead of select_double
00029 -------------------------------------------------------------------------------
00030 */
00031 
00032 /*!
00033   @file control_select.cpp
00034   @brief Controller selection class.
00035 */
00036 
00037 #include "control_select.h"
00038 
00039 #ifdef use_namespace
00040 namespace ROBOOP {
00041   using namespace NEWMAT;
00042 #endif
00043   //! @brief RCS/CVS version.
00044   static const char rcsid[] __UNUSED__ = "$Id: control_select.cpp,v 1.4 2005/07/26 03:22:09 ejt Exp $";
00045 
00046 Control_Select::Control_Select()
00047 //! @brief Constructor.
00048 {
00049   type = NONE;
00050   space_type = NONE;
00051   dof = 0;
00052 }
00053 
00054 Control_Select::Control_Select(const string & filename)
00055 /*!
00056   @brief Constructor.
00057   @param filename: configuration file (path+name).
00058 */
00059 {
00060     set_control(filename);
00061 }
00062 
00063 Control_Select::Control_Select(const Control_Select & x)
00064 //! @brief Copy constructor.
00065 {
00066     type = x.type;
00067     space_type = x.space_type;
00068     dof = x.dof;
00069     pd = x.pd;
00070     ctm = x.ctm;
00071     rra = x.rra;
00072     impedance = x.impedance;
00073 }
00074 
00075 Control_Select & Control_Select::operator=(const Control_Select & x)
00076 //! @brief Overload = operator.
00077 {
00078     type = x.type;
00079     space_type = x.space_type;
00080     dof = x.dof;
00081     pd = x.pd;
00082     ctm = x.ctm;
00083     rra = x.rra;
00084     impedance = x.impedance;
00085 
00086     return *this;
00087 }
00088 
00089 int Control_Select::get_dof()
00090 //! @brief Return the degree of freedom.
00091 { 
00092     return dof; 
00093 }
00094 
00095 void Control_Select::set_control(const string & filename)
00096 //! @brief Select the proper controller from filename. 
00097 {
00098     Config conf(filename);
00099     conf.read_conf();
00100 
00101     conf.select_string(CONTROLLER, "type", ControllerName);
00102 
00103     if (ControllerName == PROPORTIONAL_DERIVATIVE)
00104     {
00105   type = PD;
00106   space_type = JOINT_SPACE;
00107     }
00108     else if(ControllerName == COMPUTED_TORQUE_METHOD)
00109     {
00110   type = CTM;
00111   space_type = JOINT_SPACE;
00112     }
00113     else if(ControllerName == RESOLVED_RATE_ACCELERATION)
00114     {
00115   type = RRA;
00116   space_type = CARTESIAN_SPACE;
00117     }
00118     else if(ControllerName == IMPEDANCE)
00119     {
00120   type = IMP;
00121   space_type = CARTESIAN_SPACE;
00122     }
00123     else 
00124     {
00125   ControllerName = "";
00126   type = 0;
00127   space_type = 0;
00128     }
00129     
00130     conf.select_int(CONTROLLER, "dof", dof);
00131 
00132     switch (type) {
00133   case PD:
00134   {     
00135       pd = Proportional_Derivative(dof);
00136       DiagonalMatrix Kp(dof), Kd(dof);
00137       for(int i = 1; i <= dof; i++)
00138       {
00139 #ifdef __WATCOMC__
00140     ostrstream Kp_ostr, Kd_ostr;
00141     Kp_ostr << "Kp_" << i;
00142     string temp = Kp_ostr.str();
00143     temp[Kp_ostr.pcount()] = 0;
00144     conf.select_real("GAINS", temp.c_str(), Kp(i));
00145     Kd_ostr << "Kd_" << i;
00146     temp = Kd_ostr.str();
00147     temp[Kd_ostr.pcount()] = 0;
00148     conf.select_real("GAINS", temp.c_str(), Kd(i));
00149 #else
00150     ostringstream Kp_ostr, Kd_ostr;
00151     Kp_ostr << "Kp_" << i;
00152     conf.select_real("GAINS", Kp_ostr.str(), Kp(i));
00153     Kd_ostr << "Kd_" << i;
00154     conf.select_real("GAINS", Kd_ostr.str(), Kd(i));
00155 #endif
00156       }
00157       pd.set_Kp(Kp);
00158       pd.set_Kd(Kd);      
00159   }
00160   break;
00161   
00162   case CTM:
00163   {     
00164       ctm = Computed_torque_method(dof);
00165       DiagonalMatrix Kp(dof), Kd(dof);
00166       for(int i = 1; i <= dof; i++)
00167       {
00168 #ifdef __WATCOMC__
00169     ostrstream Kp_ostr, Kd_ostr;
00170     Kp_ostr << "Kp_" << i;
00171     string temp = Kp_ostr.str();
00172     temp[Kp_ostr.pcount()] = 0;
00173     conf.select_real("GAINS", temp.c_str(), Kp(i));
00174     Kd_ostr << "Kd_" << i;
00175     temp = Kd_ostr.str();
00176     temp[Kd_ostr.pcount()] = 0;
00177     conf.select_real("GAINS", temp.c_str(), Kd(i));
00178 #else
00179     ostringstream Kp_ostr, Kd_ostr;
00180     Kp_ostr << "Kp_" << i;
00181     conf.select_real("GAINS", Kp_ostr.str(), Kp(i));
00182     Kd_ostr << "Kd_" << i;
00183     conf.select_real("GAINS", Kd_ostr.str(), Kd(i));
00184 #endif
00185       }
00186       ctm.set_Kp(Kp);
00187       ctm.set_Kd(Kd);
00188   }
00189   break;
00190       
00191   case RRA:
00192   {
00193       rra = Resolved_acc(dof);
00194       Real Kvp, Kpp, Kvo, Kpo;
00195       conf.select_real("GAINS", "Kvp", Kvp);
00196       conf.select_real("GAINS", "Kpp", Kpp);
00197       conf.select_real("GAINS", "Kvo", Kvo);
00198       conf.select_real("GAINS", "Kpo", Kpo);
00199       rra.set_Kvp( Kvp );
00200       rra.set_Kpp( Kpp );
00201       rra.set_Kvo( Kvo );
00202       rra.set_Kpo( Kpo );
00203   }
00204   break;
00205 
00206   case IMP:
00207   {
00208   }
00209   break;
00210   
00211   default:
00212       break;
00213     }
00214 }
00215 
00216 #ifdef use_namespace
00217 }
00218 #endif
00219 

ROBOOP v1.21a
Generated Tue Aug 16 16:32:14 2005 by Doxygen 1.4.4