ROBOOP, A Robotics Object Oriented Package in C++
control_select.cpp
Go to the documentation of this file.
1 /*
2 Copyright (C) 2002-2004 Etienne Lachance
3 
4 This library is free software; you can redistribute it and/or modify
5 it under the terms of the GNU Lesser General Public License as
6 published by the Free Software Foundation; either version 2.1 of the
7 License, or (at your option) any later version.
8 
9 This library is distributed in the hope that it will be useful,
10 but WITHOUT ANY WARRANTY; without even the implied warranty of
11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 GNU Lesser General Public License for more details.
13 
14 You should have received a copy of the GNU Lesser General Public
15 License along with this library; if not, write to the Free Software
16 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
17 
18 
19 Report problems and direct all questions to:
20 
21 email: etienne.lachance@polymtl.ca or richard.gourdeau@polymtl.ca
22 
23 -------------------------------------------------------------------------------
24 Revision_history:
25 
26 2004/07/13: Ethan Tira-Thompson
27  -Added support for newmat's use_namespace #define, using ROBOOP namespace
28  -Using config::select_real instead of select_double
29 
30 2004/08/14: Ethan Tira-Thompson
31  -Replace select_real and select_double by select.
32 
33 2004/09/18: Etienne Lachance
34  -Controller type NONE
35 
36 2005/11/06: Etienne Lachance
37  - No need to provide a copy constructor and the assignment operator
38  (operator=) for Control_Select class. Instead we use the one provide by the
39  compiler.
40 -------------------------------------------------------------------------------
41 */
42 
48 #include "config.h"
49 #include "control_select.h"
50 #include "trajectory.h"
51 
52 #ifdef use_namespace
53 namespace ROBOOP {
54  using namespace NEWMAT;
55 #endif
56 
59 {
60  type = NONE;
61  space_type = NONE;
62  dof = 0;
63 }
64 
65 Control_Select::Control_Select(const string & filename)
70 {
71  set_control(filename);
72 }
73 
76 {
77  return dof;
78 }
79 
80 void Control_Select::set_control(const string & filename)
82 {
83  Config conf(true);
84  ifstream inconffile(filename.c_str(), std::ios::in);
85  if (conf.read_conf(inconffile))
86  {
87  cerr << "Control_Select::set_control: unable to read input config file." << endl;
88  }
89 
90  conf.select(CONTROLLER, "type", ControllerName);
91 
92  if (ControllerName == PROPORTIONAL_DERIVATIVE)
93  {
94  type = PD;
95  space_type = JOINT_SPACE;
96  }
97  else if(ControllerName == COMPUTED_TORQUE_METHOD)
98  {
99  type = CTM;
100  space_type = JOINT_SPACE;
101  }
102  else if(ControllerName == RESOLVED_RATE_ACCELERATION)
103  {
104  type = RRA;
105  space_type = CARTESIAN_SPACE;
106  }
107  else if(ControllerName == IMPEDANCE)
108  {
109  type = IMP;
110  space_type = CARTESIAN_SPACE;
111  }
112  else
113  {
114  ControllerName = "";
115  type = NONE;
116  space_type = 0;
117  }
118 
119  conf.select(CONTROLLER, "dof", dof);
120 
121  switch (type) {
122  case PD:
123  {
124  pd = Proportional_Derivative(dof);
125  DiagonalMatrix Kp(dof), Kd(dof);
126  for(int i = 1; i <= dof; i++)
127  {
128  ostringstream Kp_ostr, Kd_ostr;
129  Kp_ostr << "Kp_" << i;
130  Kd_ostr << "Kd_" << i;
131  conf.select("GAINS", Kp_ostr.str(), Kp(i));
132  conf.select("GAINS", Kd_ostr.str(), Kd(i));
133  }
134  pd.set_Kp(Kp);
135  pd.set_Kd(Kd);
136  }
137  break;
138 
139  case CTM:
140  {
141  ctm = Computed_torque_method(dof);
142  DiagonalMatrix Kp(dof), Kd(dof);
143  for(int i = 1; i <= dof; i++)
144  {
145  ostringstream Kp_ostr, Kd_ostr;
146  Kp_ostr << "Kp_" << i;
147  Kd_ostr << "Kd_" << i;
148  conf.select("GAINS", Kp_ostr.str(), Kp(i));
149  conf.select("GAINS", Kd_ostr.str(), Kd(i));
150  }
151  ctm.set_Kp(Kp);
152  ctm.set_Kd(Kd);
153  }
154  break;
155 
156  case RRA:
157  {
158  rra = Resolved_acc(dof);
159  Real Kvp, Kpp, Kvo, Kpo;
160  conf.select("GAINS", "Kvp", Kvp);
161  conf.select("GAINS", "Kpp", Kpp);
162  conf.select("GAINS", "Kvo", Kvo);
163  conf.select("GAINS", "Kpo", Kpo);
164 
165  rra.set_Kvp( Kvp );
166  rra.set_Kpp( Kpp );
167  rra.set_Kvo( Kvo );
168  rra.set_Kpo( Kpo );
169  }
170  break;
171 
172  case IMP:
173  {
174  }
175  break;
176 
177  default:
178  break;
179  }
180 }
181 
182 #ifdef use_namespace
183 }
184 #endif
185