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 //! @brief RCS/CVS version.
00038 static const char rcsid[] = "$Id: control_select.cpp,v 1.3 2004/07/14 02:46:43 ejt Exp $";
00039 
00040 
00041 #include "control_select.h"
00042 
00043 #ifdef use_namespace
00044 namespace ROBOOP {
00045   using namespace NEWMAT;
00046 #endif
00047 
00048 Control_Select::Control_Select()
00049 //! @brief Constructor.
00050 {
00051   type = NONE;
00052   space_type = NONE;
00053   dof = 0;
00054 }
00055 
00056 Control_Select::Control_Select(const string & filename)
00057 /*!
00058   @brief Constructor.
00059   @param filename: configuration file (path+name).
00060 */
00061 {
00062     set_control(filename);
00063 }
00064 
00065 Control_Select::Control_Select(const Control_Select & x)
00066 //! @brief Copy constructor.
00067 {
00068     type = x.type;
00069     space_type = x.space_type;
00070     dof = x.dof;
00071     pd = x.pd;
00072     ctm = x.ctm;
00073     rra = x.rra;
00074     impedance = x.impedance;
00075 }
00076 
00077 Control_Select & Control_Select::operator=(const Control_Select & x)
00078 //! @brief Overload = operator.
00079 {
00080     type = x.type;
00081     space_type = x.space_type;
00082     dof = x.dof;
00083     pd = x.pd;
00084     ctm = x.ctm;
00085     rra = x.rra;
00086     impedance = x.impedance;
00087 
00088     return *this;
00089 }
00090 
00091 int Control_Select::get_dof()
00092 //! @brief Return the degree of freedom.
00093 { 
00094     return dof; 
00095 }
00096 
00097 void Control_Select::set_control(const string & filename)
00098 //! @brief Select the proper controller from filename. 
00099 {
00100     Config conf(filename);
00101     conf.read_conf();
00102 
00103     conf.select_string(CONTROLLER, "type", ControllerName);
00104 
00105     if (ControllerName == PROPORTIONAL_DERIVATIVE)
00106     {
00107   type = PD;
00108   space_type = JOINT_SPACE;
00109     }
00110     else if(ControllerName == COMPUTED_TORQUE_METHOD)
00111     {
00112   type = CTM;
00113   space_type = JOINT_SPACE;
00114     }
00115     else if(ControllerName == RESOLVED_RATE_ACCELERATION)
00116     {
00117   type = RRA;
00118   space_type = CARTESIAN_SPACE;
00119     }
00120     else if(ControllerName == IMPEDANCE)
00121     {
00122   type = IMP;
00123   space_type = CARTESIAN_SPACE;
00124     }
00125     else 
00126     {
00127   ControllerName = "";
00128   type = 0;
00129   space_type = 0;
00130     }
00131     
00132     conf.select_int(CONTROLLER, "dof", dof);
00133 
00134     switch (type) {
00135   case PD:
00136   {     
00137       pd = Proportional_Derivative(dof);
00138       DiagonalMatrix Kp(dof), Kd(dof);
00139       for(int i = 1; i <= dof; i++)
00140       {
00141 #ifdef __WATCOMC__
00142     ostrstream Kp_ostr, Kd_ostr;
00143     Kp_ostr << "Kp_" << i;
00144     string temp = Kp_ostr.str();
00145     temp[Kp_ostr.pcount()] = 0;
00146     conf.select_real("GAINS", temp.c_str(), Kp(i));
00147     Kd_ostr << "Kd_" << i;
00148     temp = Kd_ostr.str();
00149     temp[Kd_ostr.pcount()] = 0;
00150     conf.select_real("GAINS", temp.c_str(), Kd(i));
00151 #else
00152     ostringstream Kp_ostr, Kd_ostr;
00153     Kp_ostr << "Kp_" << i;
00154     conf.select_real("GAINS", Kp_ostr.str(), Kp(i));
00155     Kd_ostr << "Kd_" << i;
00156     conf.select_real("GAINS", Kd_ostr.str(), Kd(i));
00157 #endif
00158       }
00159       pd.set_Kp(Kp);
00160       pd.set_Kd(Kd);      
00161   }
00162   break;
00163   
00164   case CTM:
00165   {     
00166       ctm = Computed_torque_method(dof);
00167       DiagonalMatrix Kp(dof), Kd(dof);
00168       for(int i = 1; i <= dof; i++)
00169       {
00170 #ifdef __WATCOMC__
00171     ostrstream Kp_ostr, Kd_ostr;
00172     Kp_ostr << "Kp_" << i;
00173     string temp = Kp_ostr.str();
00174     temp[Kp_ostr.pcount()] = 0;
00175     conf.select_real("GAINS", temp.c_str(), Kp(i));
00176     Kd_ostr << "Kd_" << i;
00177     temp = Kd_ostr.str();
00178     temp[Kd_ostr.pcount()] = 0;
00179     conf.select_real("GAINS", temp.c_str(), Kd(i));
00180 #else
00181     ostringstream Kp_ostr, Kd_ostr;
00182     Kp_ostr << "Kp_" << i;
00183     conf.select_real("GAINS", Kp_ostr.str(), Kp(i));
00184     Kd_ostr << "Kd_" << i;
00185     conf.select_real("GAINS", Kd_ostr.str(), Kd(i));
00186 #endif
00187       }
00188       ctm.set_Kp(Kp);
00189       ctm.set_Kd(Kd);
00190   }
00191   break;
00192       
00193   case RRA:
00194   {
00195       rra = Resolved_acc(dof);
00196       Real Kvp, Kpp, Kvo, Kpo;
00197       conf.select_real("GAINS", "Kvp", Kvp);
00198       conf.select_real("GAINS", "Kpp", Kpp);
00199       conf.select_real("GAINS", "Kvo", Kvo);
00200       conf.select_real("GAINS", "Kpo", Kpo);
00201       rra.set_Kvp( Kvp );
00202       rra.set_Kpp( Kpp );
00203       rra.set_Kvo( Kvo );
00204       rra.set_Kpo( Kpo );
00205   }
00206   break;
00207 
00208   case IMP:
00209   {
00210   }
00211   break;
00212   
00213   default:
00214       break;
00215     }
00216 }
00217 
00218 #ifdef use_namespace
00219 }
00220 #endif
00221 

ROBOOP v1.21a
Generated Tue Jan 4 15:42:24 2005 by Doxygen 1.4.0