Cheetah Software  1.0
ControlParameters.cpp
Go to the documentation of this file.
1 
8 #include "INIReader.h"
9 #include "ParamHandler.hpp"
10 #include "Utilities/utilities.h"
11 
12 #include <utility>
13 
14 #define YAML_COLLECTION_NAME_KEY "__collection-name__"
15 
17  ControlParameterValueKind valueKind) {
18  switch (valueKind) {
20  return "s64";
22  return "double";
24  return "float";
26  return "vec3d";
28  return "vec3f";
29  default:
30  return "unknown-ControlParameterValueKind";
31  }
32 }
33 
36  std::string result;
37  switch (kind) {
39  result += numberToString(value.d);
40  break;
42  result += numberToString(value.f);
43  break;
45  result += std::to_string(value.i);
46  break;
48  result += "[";
49  result += numberToString(value.vec3f[0]) + ", ";
50  result += numberToString(value.vec3f[1]) + ", ";
51  result += numberToString(value.vec3f[2]) + "]";
52  break;
54  result += "[";
55  result += numberToString(value.vec3d[0]) + ", ";
56  result += numberToString(value.vec3d[1]) + ", ";
57  result += numberToString(value.vec3d[2]) + "]";
58  break;
59  default:
60  result += "<unknown type " + std::to_string((u32)(kind)) +
61  "> (add it yourself in ControlParameterInterface.h!)";
62  break;
63  }
64  return result;
65 }
66 
68  for (auto& kv : _map) {
69  if (!kv.second->_set) {
70  return false;
71  }
72  }
73  return true;
74 }
75 
77  for (auto& kv : _map) {
78  kv.second->_set = false;
79  }
80 }
81 
83  for(auto& kv : _map) {
84  delete kv.second;
85  }
86  _map.clear();
87 }
88 
90  std::string result;
91  for (auto& kv : collection._map) {
92  if (!kv.second->_set) {
93  result += kv.second->_name + " :\n";
94  }
95  }
96 
97  return result;
98 }
99 
101  std::string result = ";; Generated on " + getCurrentTimeAndDate() + "\n";
102 
103  // ini section
104  result += "[";
105  result += _name + "]\n\n";
106 
107  std::vector<std::string> lines;
108 
109  // names (and max name length)
110  int maxLength_name = 0;
111  for (auto& kv : _map) {
112  maxLength_name = std::max(maxLength_name, (int)kv.first.length());
113  lines.push_back(kv.first);
114  }
115 
116  // pad, equals sign, and number
117  size_t i = 0;
118  int maxLength_number = 0;
119  for (auto& kv : _map) {
120  int charsToAdd = maxLength_name - (int)lines[i].length();
121  assert(charsToAdd >= 0);
122  for (int j = 0; j < charsToAdd; j++) {
123  lines[i].push_back(' ');
124  }
125  lines[i] += " = ";
126  lines[i] += kv.second->toString();
127  maxLength_number = std::max(maxLength_number, (int)lines[i].length());
128  i++;
129  }
130 
131  // pad, ;;, and units
132  i = 0;
133  for (auto& kv : _map) {
134  int charsToAdd = maxLength_number - (int)lines[i].length();
135  assert(charsToAdd >= 0);
136  for (int j = 0; j < charsToAdd; j++) {
137  lines[i].push_back(' ');
138  }
139  lines[i] += " ;; ";
140  if (kv.second->_units.empty()) {
141  lines[i] += "No units specified. Add them!";
142  } else {
143  lines[i] += kv.second->_units;
144  }
145 
146  i++;
147  }
148 
149  // combine lines
150  for (auto& line : lines) {
151  result += line + "\n";
152  }
153 
154  return result;
155 }
156 
158  std::string result = "# Generated on " + getCurrentTimeAndDate() + "\n";
159 
160  result += YAML_COLLECTION_NAME_KEY;
161  result += ": ";
162  result += _name + "\n\n";
163 
164  std::vector<std::string> lines;
165 
166  // names
167  int maxLength_name = 0;
168  for (auto& kv : _map) {
169  maxLength_name = std::max(maxLength_name, (int)kv.first.length());
170  lines.push_back(kv.first);
171  }
172 
173  // name pad, :, and number
174  size_t i = 0;
175  int maxLength_number = 0;
176  for (auto& kv : _map) {
177  int charsToAdd = maxLength_name - (int)lines[i].length();
178  assert(charsToAdd >= 0);
179  for (int j = 0; j < charsToAdd; j++) {
180  lines[i].push_back(' ');
181  }
182  lines[i] += ": ";
183  lines[i] += kv.second->toString();
184  maxLength_number = std::max(maxLength_number, (int)lines[i].length());
185  i++;
186  }
187 
188  // combine lines
189  for (auto& line : lines) {
190  result += line + "\n";
191  }
192 
193  return result;
194 }
195 
196 void ControlParameters::writeToIniFile(const std::string& path) {
197  writeStringToFile(path, collection.printToIniString());
198 }
199 
200 void ControlParameters::writeToYamlFile(const std::string& path) {
201  writeStringToFile(path, collection.printToYamlString());
202 }
203 
204 void ControlParameters::initializeFromIniFile(const std::string& path) {
205  INIReader iniReader(path);
206  if (iniReader.ParseError() < 0) {
207  printf(
208  "[ERROR] Could not open ini file %s : not initializing control "
209  "parameters!\n",
210  path.c_str());
211  throw std::runtime_error("ini file bad");
212  }
213 
214  std::set<std::string> sections = iniReader.GetSections();
215 
216  if (sections.size() != 1) {
217  printf(
218  "[ERROR] INI file %s had %ld sections (expected 1) : not initializing "
219  "control parameters\n",
220  path.c_str(), sections.size());
221  throw std::runtime_error("ini file bad");
222  }
223 
224  std::string sectionName = *(sections.begin());
225 
226  if (sectionName != _name) {
227  printf(
228  "[ERROR] INI file %s has section name %s, which cannot be used to "
229  "initialize %s\n",
230  path.c_str(), sectionName.c_str(), _name.c_str());
231  throw std::runtime_error("ini file bad");
232  }
233 
234  std::set<std::string> parameterNames = iniReader.GetFields(sectionName);
235 
236  for (auto& name : parameterNames) {
237  ControlParameter& cp = collection.lookup(name);
238  switch (cp._kind) {
240  cp.initializeDouble(iniReader.GetReal(sectionName, name, 0.));
241  break;
243  cp.initializeFloat((float)iniReader.GetReal(sectionName, name, 0.));
244  break;
246  cp.initializeInteger(iniReader.GetInteger(sectionName, name, 0));
247  break;
248  default:
249  throw std::runtime_error("can't read type " +
250  std::to_string((u32)cp._kind) +
251  " from ini file");
252  break;
253  }
254  }
255 }
256 
258  if(str.find('[') != std::string::npos) {
259  // vec type
260  if(str.find('f') != std::string::npos) {
262  } else {
264  }
265  } else {
266  // scalar type
267  if(str.find('.') != std::string::npos) {
268  if(str.find('f') != std::string::npos) {
270  } else {
272  }
273  } else {
274  // integer
276  }
277  }
278 }
279 
281  ParamHandler paramHandler(path);
282 
283  if (!paramHandler.fileOpenedSuccessfully()) {
284  printf(
285  "[ERROR] Could not open yaml file %s : not initializing control "
286  "parameters!\n",
287  path.c_str());
288  throw std::runtime_error("yaml file bad");
289  }
290 
291  std::string name;
292  if (!paramHandler.getString(YAML_COLLECTION_NAME_KEY, name)) {
293  printf("[ERROR] YAML doesn't have a a collection name field named %s\n",
295  throw std::runtime_error("yaml file bad");
296  }
297 
298  if (name != _name) {
299  printf(
300  "[ERROR] YAML file %s has collection name %s which cannot be used to "
301  "initialize %s\n",
302  path.c_str(), name.c_str(), _name.c_str());
303  throw std::runtime_error("yaml file bad");
304  }
305 
306  std::vector<std::string> keys = paramHandler.getKeys();
307 
308  for (auto& key : keys) {
309  if (key == YAML_COLLECTION_NAME_KEY) continue;
310  std::string valueString;
311  paramHandler.getString(key, valueString);
313  if(valueString.empty()) {
315  } else {
317  }
318 
319  ControlParameter* cp = new ControlParameter(key, kind);
320  collection.addParameter(cp, key);
321  switch (cp->_kind) {
323  double d;
324  assert(paramHandler.getValue(key, d));
325  cp->initializeDouble(d);
326  } break;
327 
329  float f;
330  assert(paramHandler.getValue(key, f));
331  cp->initializeFloat(f);
332  } break;
333 
335  s64 f;
336  assert(paramHandler.getValue(key, f));
337  cp->initializeInteger(f);
338  } break;
339 
341  std::vector<double> vv;
342  assert(paramHandler.getVector(key, vv));
343  assert(vv.size() == 3);
344  Vec3<double> v(vv[0], vv[1], vv[2]);
345  cp->initializeVec3d(v);
346  } break;
347 
349  std::vector<float> vv;
350  assert(paramHandler.getVector(key, vv));
351  assert(vv.size() == 3);
352  Vec3<float> v(vv[0], vv[1], vv[2]);
353  cp->initializeVec3f(v);
354  } break;
355 
356  default:
357  throw std::runtime_error("can't read type " +
358  std::to_string((u32)cp->_kind) +
359  " from yaml file");
360  break;
361  }
362  }
363 }
364 
365 void ControlParameters::initializeFromYamlFile(const std::string& path) {
366  ParamHandler paramHandler(path);
367 
368  if (!paramHandler.fileOpenedSuccessfully()) {
369  printf(
370  "[ERROR] Could not open yaml file %s : not initializing control "
371  "parameters!\n",
372  path.c_str());
373  throw std::runtime_error("yaml file bad");
374  }
375 
376  std::string name;
377  if (!paramHandler.getString(YAML_COLLECTION_NAME_KEY, name)) {
378  printf("[ERROR] YAML doesn't have a a collection name field named %s\n",
380  throw std::runtime_error("yaml file bad");
381  }
382 
383  if (name != _name) {
384  printf(
385  "[ERROR] YAML file %s has collection name %s which cannot be used to "
386  "initialize %s\n",
387  path.c_str(), name.c_str(), _name.c_str());
388  throw std::runtime_error("yaml file bad");
389  }
390 
391  std::vector<std::string> keys = paramHandler.getKeys();
392 
393  for (auto& key : keys) {
394  if (key == YAML_COLLECTION_NAME_KEY) continue;
395  ControlParameter& cp = collection.lookup(key);
396  switch (cp._kind) {
398  double d;
399  assert(paramHandler.getValue(key, d));
400  cp.initializeDouble(d);
401  } break;
402 
404  float f;
405  assert(paramHandler.getValue(key, f));
406  cp.initializeFloat(f);
407  } break;
408 
410  s64 f;
411  assert(paramHandler.getValue(key, f));
412  cp.initializeInteger(f);
413  } break;
414 
416  std::vector<double> vv;
417  assert(paramHandler.getVector(key, vv));
418  assert(vv.size() == 3);
419  Vec3<double> v(vv[0], vv[1], vv[2]);
420  cp.initializeVec3d(v);
421  } break;
422 
424  std::vector<float> vv;
425  assert(paramHandler.getVector(key, vv));
426  assert(vv.size() == 3);
427  Vec3<float> v(vv[0], vv[1], vv[2]);
428  cp.initializeVec3f(v);
429  } break;
430 
431  default:
432  throw std::runtime_error("can't read type " +
433  std::to_string((u32)cp._kind) +
434  " from yaml file");
435  break;
436  }
437  }
438 }
ControlParameterValueKind getControlParameterValueKindFromString(const std::string &str)
std::string getCurrentTimeAndDate()
Definition: utilities.cpp:18
ControlParameterValueKind _kind
ControlParameterValueKind
std::map< std::string, ControlParameter * > _map
std::string printToIniString()
print all control parameters in the INI file format
std::string numberToString(T number)
Definition: utilities.h:130
typename Eigen::Matrix< T, 3, 1 > Vec3
Definition: cppTypes.h:26
int64_t s64
Definition: cTypes.h:24
void initializeVec3d(const Vec3< double > &v)
void initializeInteger(s64 i)
std::string controlParameterValueToString(ControlParameterValue value, ControlParameterValueKind kind)
void initializeDouble(double d)
void writeStringToFile(const std::string &fileName, const std::string &fileData)
Definition: utilities.cpp:7
void writeToYamlFile(const std::string &path)
uint32_t u32
Definition: cTypes.h:18
#define YAML_COLLECTION_NAME_KEY
bool checkIfAllSet()
are all the control parameters initialized?
void writeToIniFile(const std::string &path)
void initializeVec3f(const Vec3< float > &v)
void defineAndInitializeFromYamlFile(const std::string &path)
void initializeFloat(float f)
Interface to set gains/control parameters for simulator and robot These are designed to be updated in...
void initializeFromIniFile(const std::string &path)
std::string controlParameterValueKindToString(ControlParameterValueKind valueKind)
void initializeFromYamlFile(const std::string &path)
std::string generateUnitializedList()
MX f(const MX &x, const MX &u)