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  }
157  const T& get() const
158  {
159  try {
160  return ParameterData<T>::get();
161  } catch (std::exception& e) {
162  utils::orca_throw(utils::Formatter() << "Parameter '" << getName() << "' : " << e.what());
163  }
164  }
165 
166  template<class T2>
168  {
169  this->set(val);
170  return *this;
171  }
172 // TODO : figure out how to remove the need to param->get()
173 // These dont work yet
174 // template<class T2>
175 // T2& operator=(Parameter<T>& val)
176 // {
177 // return this->get();
178 // }
179 // template<class T2>
180 // T2& operator=(const Parameter<T>& val)
181 // {
182 // return this->get();
183 // }
184 };
185 
186 template<class T>
187 class Parameter<std::list<T > > : public ParameterBase, public ParameterData< std::list<T > >
188 {
189 public:
191  virtual ~Parameter() {}
192  Parameter(const std::list<T >& t)
193  {
195  }
196  bool onLoadFromString(const std::string& s)
197  {
198  YAML::Node node = YAML::Load(s);
199  std::list<T > l;
200  for(auto n : node)
201  {
202  auto task_base_name = n.first.as<std::string>();
203  Parameter<T> p;
204  p.setName(task_base_name);
205  p.setRequired(true);
206  YAML::Emitter out;
207  out << n.second;
208  if(p.loadFromString(out.c_str()))
209  l.push_back( p.get() );
210  }
212  return true;
213  }
214 
215  void print() const
216  {
217  std::cout << "Parameter '" << getName() << "' : list of " << typeid(T).name() << '\n';
218  for(auto p : this->get())
219  {
220  p->print();
221  }
222  }
223 
224  bool isSet() const
225  {
226  return ParameterData<std::list<T > >::isSet();
227  }
228 
229  std::list<T >& get()
230  {
231  try {
232  return ParameterData< std::list<T > >::get();
233  } catch (std::exception& e) {
234  utils::orca_throw(utils::Formatter() << "Parameter '" << getName() << "' : " << e.what());
235  }
236  }
237  const std::list<T >& get() const
238  {
239  try {
240  return ParameterData< std::list<T > >::get();
241  } catch (std::exception& e) {
242  utils::orca_throw(utils::Formatter() << "Parameter '" << getName() << "' : " << e.what());
243  }
244  }
245 
246  template<class T2>
247  Parameter<std::list<T > >& operator=(std::list<std::shared_ptr<T2> > val)
248  {
249  this->set(val);
250  return *this;
251  }
252 };
253 
254 } // namespace common
255 } // namespace orca
256 
257 template<class T>
258 ::std::ostream& operator<<(::std::ostream& os, const orca::common::Parameter<T>& p)
259 {
260  return os << p.get();
261 }
std::list< T > & get()
Definition: Parameter.h:229
Parameter(const std::list< T > &t)
Definition: Parameter.h:192
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
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:153
QPSolverImplType
Definition: QPSolverImplType.h:44
bool onLoadFromString(const std::string &s)
Definition: Parameter.h:196
void setName(const std::string &name)
Definition: ParameterBase.h:51
void print() const
Definition: Parameter.h:215
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:167
Parameter(const T2 &t)
Definition: Parameter.h:119
QPSolverImplType QPSolverImplTypeFromString(const std::string &rs)
Definition: QPSolverImplType.h:58
Parameter()
Definition: Parameter.h:190
Definition: Utils.h:105
static Node encode(const orca::optim::ResolutionStrategy &s)
Definition: Parameter.h:87
Parameter()
Definition: Parameter.h:116
ResolutionStrategy
Definition: ResolutionStrategy.h:44
Definition: CartesianAccelerationPID.h:44
void print() const
Definition: Parameter.h:139
Parameter< std::list< T > > & operator=(std::list< std::shared_ptr< T2 > > val)
Definition: Parameter.h:247
Definition: Parameter.h:8
bool isSet() const
Definition: Parameter.h:224
ParameterBase is the public interface to any parameter.
Definition: ParameterBase.h:21
virtual ~Parameter()
Definition: Parameter.h:191
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
bool isSet() const
Definition: Parameter.h:144