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 2004/08/14: Ethan Tira-Thompson
00031     -Replace select_real and select_double by select.
00032 
00033 2004/09/18: Etienne Lachance
00034     -Controller type NONE
00035 
00036 2005/11/06: Etienne Lachance
00037     - No need to provide a copy constructor and the assignment operator 
00038       (operator=) for Control_Select class. Instead we use the one provide by the
00039       compiler.
00040 -------------------------------------------------------------------------------
00041 */
00042 
00048 
00049 static const char rcsid[] = "$Id: control_select.cpp,v 1.7 2006/05/16 19:24:26 gourdeau Exp $";
00050 
00051 #include "config.h"
00052 #include "control_select.h"
00053 #include "trajectory.h"
00054 
00055 #ifdef use_namespace
00056 namespace ROBOOP {
00057   using namespace NEWMAT;
00058 #endif
00059 
00060 Control_Select::Control_Select()
00062 {
00063   type = NONE;
00064   space_type = NONE;
00065   dof = 0;
00066 }
00067 
00068 Control_Select::Control_Select(const string & filename)
00073 {
00074     set_control(filename);
00075 }
00076 
00077 int Control_Select::get_dof()
00079 { 
00080     return dof; 
00081 }
00082 
00083 void Control_Select::set_control(const string & filename)
00085 {
00086    Config conf(true);
00087    ifstream inconffile(filename.c_str(), std::ios::in);
00088    if (conf.read_conf(inconffile))
00089    {
00090        cerr << "Control_Select::set_control: unable to read input config file." << endl;
00091    }
00092 
00093     conf.select(CONTROLLER, "type", ControllerName);
00094 
00095     if (ControllerName == PROPORTIONAL_DERIVATIVE)
00096     {
00097         type = PD;
00098         space_type = JOINT_SPACE;
00099     }
00100     else if(ControllerName == COMPUTED_TORQUE_METHOD)
00101     {
00102         type = CTM;
00103         space_type = JOINT_SPACE;
00104     }
00105     else if(ControllerName == RESOLVED_RATE_ACCELERATION)
00106     {
00107         type = RRA;
00108         space_type = CARTESIAN_SPACE;
00109     }
00110     else if(ControllerName == IMPEDANCE)
00111     {
00112         type = IMP;
00113         space_type = CARTESIAN_SPACE;
00114     }
00115     else 
00116     {
00117         ControllerName = "";
00118         type = NONE;
00119         space_type = 0;
00120     }
00121     
00122     conf.select(CONTROLLER, "dof", dof);
00123 
00124     switch (type) {
00125         case PD:
00126         {           
00127             pd = Proportional_Derivative(dof);
00128             DiagonalMatrix Kp(dof), Kd(dof);
00129             for(int i = 1; i <= dof; i++)
00130             {
00131                 ostringstream Kp_ostr, Kd_ostr;
00132                 Kp_ostr << "Kp_" << i;
00133                 Kd_ostr << "Kd_" << i;
00134                 conf.select("GAINS", Kp_ostr.str(), Kp(i));     
00135                 conf.select("GAINS", Kd_ostr.str(), Kd(i));
00136             }
00137             pd.set_Kp(Kp);
00138             pd.set_Kd(Kd);          
00139         }
00140         break;
00141         
00142         case CTM:
00143         {           
00144             ctm = Computed_torque_method(dof);
00145             DiagonalMatrix Kp(dof), Kd(dof);
00146             for(int i = 1; i <= dof; i++)
00147             {
00148                 ostringstream Kp_ostr, Kd_ostr;
00149                 Kp_ostr << "Kp_" << i;
00150                 Kd_ostr << "Kd_" << i;
00151                 conf.select("GAINS", Kp_ostr.str(), Kp(i));
00152                 conf.select("GAINS", Kd_ostr.str(), Kd(i));
00153             }
00154             ctm.set_Kp(Kp);
00155             ctm.set_Kd(Kd);
00156         }
00157         break;
00158             
00159         case RRA:
00160         {
00161             rra = Resolved_acc(dof);
00162             Real Kvp, Kpp, Kvo, Kpo;
00163             conf.select("GAINS", "Kvp", Kvp);
00164             conf.select("GAINS", "Kpp", Kpp);
00165             conf.select("GAINS", "Kvo", Kvo);
00166             conf.select("GAINS", "Kpo", Kpo);
00167 
00168             rra.set_Kvp( Kvp );
00169             rra.set_Kpp( Kpp );
00170             rra.set_Kvo( Kvo );
00171             rra.set_Kpo( Kpo );
00172         }
00173         break;
00174 
00175         case IMP:
00176         {
00177         }
00178         break;
00179         
00180         default:
00181             break;
00182     }
00183 }
00184 
00185 #ifdef use_namespace
00186 }
00187 #endif
00188 


kni
Author(s): Martin Günther
autogenerated on Mon Aug 14 2017 02:44:12