ORCA: Optimization-based framework for Robotic Control Applications
Parameter.h
Go to the documentation of this file.
1 #pragma once
4 // TODO : Try to remove this from headers
5 #include <yaml-cpp/yaml.h>
6 
7 // Support for Eigen Vectors and Matrices
8 namespace YAML {
9 
10  template < typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
11  struct convert< Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> >
12  {
13  static Node encode(const Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& matrix)
14  {
15  Node node(NodeType::Sequence);
16 
17  // Save data given as a vector
18  if (_Rows == 1 || _Cols == 1) {
19  for (auto row=0; row<matrix.rows(); row++)
20  for (auto col=0; col<matrix.cols(); col++)
21  node.push_back(matrix(row,col));
22  return node;
23  }
24 
25  return node;
26  }
27 
28  static bool decode(const Node& node, Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& matrix)
29  {
30  // Read data given as a vector
31  if (_Rows == 1 || _Cols == 1) {
32  (_Rows == 1 ? matrix.resize(_Rows, node.size()) : matrix.resize(node.size(), _Cols));
33  for (auto id=0; id<node.size(); id++)
34  (node[0].size() == 0 ? matrix(id) = node[id].as<_Scalar>() : matrix(id) = node[id][0].as<_Scalar>());
35  return true;
36  }
37 
38  return true;
39  }
40  };
41 
42 } // namespace YAML
43 
44 // Special case for shared_ptr
45 namespace YAML {
46  template < typename T>
47  struct convert< std::shared_ptr<T> >
48  {
49  static Node encode(const std::shared_ptr<T>& s)
50  {
51  (void)s;
52  return Node();
53  }
54 
55  static bool decode(const Node& node, std::shared_ptr<T>& e)
56  {
57  return true;
58  }
59  };
60 }
61 
63 namespace YAML {
64  template<>
65  struct convert< orca::optim::QPSolverImplType >
66  {
67  static Node encode(const orca::optim::QPSolverImplType& s)
68  {
69  auto node = YAML::Node();
70  node.push_back(orca::optim::QPSolverImplTypeToString(s));
71  return node;
72  }
73 
74  static bool decode(const Node& node, orca::optim::QPSolverImplType& e)
75  {
76  e = orca::optim::QPSolverImplTypeFromString(node[0].as<std::string>());
77  return true;
78  }
79  };
80 }
81 
83 namespace YAML {
84  template<>
85  struct convert< orca::optim::ResolutionStrategy >
86  {
88  {
89  auto node = YAML::Node();
90  node.push_back(orca::optim::ResolutionStrategyToString(s));
91  return node;
92  }
93 
94  static bool decode(const Node& node, orca::optim::ResolutionStrategy& e)
95  {
96  e = orca::optim::ResolutionStrategyFromString(node[0].as<std::string>());
97  return true;
98  }
99  };
100 }
101 
102 
103 namespace orca
104 {
105 namespace common
106 {
107 
112 template<class T>
113 class Parameter : public ParameterBase, public ParameterData<T>
114 {
115 public:
117  {}
118  template<class T2>
119  Parameter(const T2& t)
120  {
122  }
123  bool onLoadFromString(const std::string& s)
124  {
125  YAML::Node node = YAML::Load(s);
126  try
127  {
128  ParameterData<T>::set( node.as<T>() );
129  }
130  catch(std::exception& e)
131  {
132  utils::orca_throw(utils::Formatter() << "parameter '" << getName() << "': "
133  << "Could not convert \"" << s
134  << "\" to the type asked\n" << e.what());
135  }
136  return true;
137  }
138 
139  void print() const
140  {
141  std::cout << "Parameter '" << getName() << "': " << ParameterData<T>::get() << '\n';
142  }
143 
144  bool isSet() const
145  {
146  return ParameterData<T>::isSet();
147  }
148 
149  T& get()
150  {
151  try {
152  return ParameterData<T>::get();
153  } catch (std::exception& e) {
154  utils::orca_throw(utils::Formatter() << "Parameter '" << getName() << "' : " << e.what());
155  }
156  return __fix_warnings__;
157  }
158  const T& get() const
159  {
160  try {
161  return ParameterData<T>::get();
162  } catch (std::exception& e) {
163  utils::orca_throw(utils::Formatter() << "Parameter '" << getName() << "' : " << e.what());
164  }
165  return __fix_warnings__;
166  }
167 
168  template<class T2>
170  {
171  this->set(val);
172  return *this;
173  }
174 private:
175  T __fix_warnings__;
176 // TODO : figure out how to remove the need to param->get()
177 // These dont work yet
178 // template<class T2>
179 // T2& operator=(Parameter<T>& val)
180 // {
181 // return this->get();
182 // }
183 // template<class T2>
184 // T2& operator=(const Parameter<T>& val)
185 // {
186 // return this->get();
187 // }
188 };
189 
190 template<class T>
191 class Parameter<std::list<T > > : public ParameterBase, public ParameterData< std::list<T > >
192 {
193 public:
195  virtual ~Parameter() {}
196  Parameter(const std::list<T >& t)
197  {
199  }
200  bool onLoadFromString(const std::string& s)
201  {
202  YAML::Node node = YAML::Load(s);
203  std::list<T > l;
204  for(auto n : node)
205  {
206  auto task_base_name = n.first.as<std::string>();
207  Parameter<T> p;
208  p.setName(task_base_name);
209  p.setRequired(true);
210  YAML::Emitter out;
211  out << n.second;
212  if(p.loadFromString(out.c_str()))
213  l.push_back( p.get() );
214  }
216  return true;
217  }
218 
219  void print() const
220  {
221  std::cout << "Parameter '" << getName() << "' : list of " << typeid(T).name() << '\n';
222  for(auto p : this->get())
223  {
224  p->print();
225  }
226  }
227 
228  bool isSet() const
229  {
230  return ParameterData<std::list<T > >::isSet();
231  }
232 
233  std::list<T >& get()
234  {
235  try {
236  return ParameterData< std::list<T > >::get();
237  } catch (std::exception& e) {
238  utils::orca_throw(utils::Formatter() << "Parameter '" << getName() << "' : " << e.what());
239  }
240  return __fix_warnings__;
241  }
242  const std::list<T >& get() const
243  {
244  try {
245  return ParameterData< std::list<T > >::get();
246  } catch (std::exception& e) {
247  utils::orca_throw(utils::Formatter() << "Parameter '" << getName() << "' : " << e.what());
248  }
249  return __fix_warnings__;
250  }
251 
252  template<class T2>
253  Parameter<std::list<T > >& operator=(std::list<std::shared_ptr<T2> > val)
254  {
255  this->set(val);
256  return *this;
257  }
258 private:
259  std::list<T> __fix_warnings__;
260 };
261 
262 } // namespace common
263 } // namespace orca
264 
265 template<class T>
266 ::std::ostream& operator<<(::std::ostream& os, const orca::common::Parameter<T>& p)
267 {
268  return os << p.get();
269 }
std::list< T > & get()
Definition: Parameter.h:233
Parameter(const std::list< T > &t)
Definition: Parameter.h:196
static bool decode(const Node &node, orca::optim::QPSolverImplType &e)
Definition: Parameter.h:74
static bool decode(const Node &node, std::shared_ptr< T > &e)
Definition: Parameter.h:55
static Node encode(const Eigen::Matrix< _Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols > &matrix)
Definition: Parameter.h:13
This class holds the data for a parameter of any type.
Definition: ParameterData.h:14
bool onLoadFromString(const std::string &s)
Definition: Parameter.h:123
ResolutionStrategy ResolutionStrategyFromString(const std::string &rs)
Definition: ResolutionStrategy.h:58
static bool decode(const Node &node, orca::optim::ResolutionStrategy &e)
Definition: Parameter.h:94
void print() const
Definition: Parameter.h:219
std::string QPSolverImplTypeToString(QPSolverImplType rs)
Definition: QPSolverImplType.h:51
static Node encode(const orca::optim::QPSolverImplType &s)
Definition: Parameter.h:67
void orca_throw(const std::string &arg)
Definition: Utils.h:151
QPSolverImplType
Definition: QPSolverImplType.h:44
bool onLoadFromString(const std::string &s)
Definition: Parameter.h:200
void setName(const std::string &name)
Definition: ParameterBase.h:51
bool isSet() const
Definition: Parameter.h:228
std::string ResolutionStrategyToString(ResolutionStrategy rs)
Definition: ResolutionStrategy.h:51
static Node encode(const std::shared_ptr< T > &s)
Definition: Parameter.h:49
Parameter< T > & operator=(T2 val)
Definition: Parameter.h:169
Parameter(const T2 &t)
Definition: Parameter.h:119
QPSolverImplType QPSolverImplTypeFromString(const std::string &rs)
Definition: QPSolverImplType.h:58
Parameter()
Definition: Parameter.h:194
Definition: Utils.h:103
static Node encode(const orca::optim::ResolutionStrategy &s)
Definition: Parameter.h:87
Parameter()
Definition: Parameter.h:116
void print() const
Definition: Parameter.h:139
bool isSet() const
Definition: Parameter.h:144
ResolutionStrategy
Definition: ResolutionStrategy.h:44
Definition: CartesianAccelerationPID.h:44
Parameter< std::list< T > > & operator=(std::list< std::shared_ptr< T2 > > val)
Definition: Parameter.h:253
Definition: Parameter.h:8
ParameterBase is the public interface to any parameter.
Definition: ParameterBase.h:21
virtual ~Parameter()
Definition: Parameter.h:195
This class holds the conversion from a string (YAML string) to the data type.
Definition: Parameter.h:113
static bool decode(const Node &node, Eigen::Matrix< _Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols > &matrix)
Definition: Parameter.h:28