ORCA: Optimization-based framework for Robotic Control Applications
Utils.h
Go to the documentation of this file.
1 //| This file is a part of the ORCA framework.
2 //|
3 //| Copyright 2018, Fuzzy Logic Robotics
4 //| Copyright 2017, ISIR / Universite Pierre et Marie Curie (UPMC)
5 //|
6 //| Main contributor(s): Antoine Hoarau, Ryan Lober, and
7 //| Fuzzy Logic Robotics <info@fuzzylogicrobotics.com>
8 //|
9 //| ORCA is a whole-body reactive controller framework for robotics.
10 //|
11 //| This software is governed by the CeCILL-C license under French law and
12 //| abiding by the rules of distribution of free software. You can use,
13 //| modify and/ or redistribute the software under the terms of the CeCILL-C
14 //| license as circulated by CEA, CNRS and INRIA at the following URL
15 //| "http://www.cecill.info".
16 //|
17 //| As a counterpart to the access to the source code and rights to copy,
18 //| modify and redistribute granted by the license, users are provided only
19 //| with a limited warranty and the software's author, the holder of the
20 //| economic rights, and the successive licensors have only limited
21 //| liability.
22 //|
23 //| In this respect, the user's attention is drawn to the risks associated
24 //| with loading, using, modifying and/or developing or reproducing the
25 //| software by the user in light of its specific status of free software,
26 //| that may mean that it is complicated to manipulate, and that also
27 //| therefore means that it is reserved for developers and experienced
28 //| professionals having in-depth computer knowledge. Users are therefore
29 //| encouraged to load and test the software's suitability as regards their
30 //| requirements in conditions enabling the security of their systems and/or
31 //| data to be ensured and, more generally, to use and operate it in the
32 //| same conditions as regards security.
33 //|
34 //| The fact that you are presently reading this means that you have had
35 //| knowledge of the CeCILL-C license and that you accept its terms.
36 
37 #pragma once
38 #include <iostream>
39 #include <Eigen/Dense>
40 #include <memory>
41 #include <cstdlib>
42 #include <ctime>
43 #include <limits>
44 #include <atomic>
45 #include <chrono>
46 #include <list>
47 #include <functional>
48 #include <thread>
49 #include <map>
50 #include "orca/utils/Logger.h"
51 
52 namespace orca
53 {
54 namespace utils
55 {
56 
57 // Waiting for c++17 to have this
58 template<typename T, typename ...Args>
59 std::unique_ptr<T> make_unique( Args&& ...args )
60 {
61  return std::unique_ptr<T>( new T( std::forward<Args>(args)... ) );
62 }
63 
65 {
66  typedef std::chrono::high_resolution_clock high_resolution_clock;
67  typedef std::chrono::nanoseconds nanoseconds;
68 
69 public:
70  const double nsToMs = 1./1e6;
71  const double nsToS = 1./1e9;
72 
73  explicit PosixTimer(bool start_now = false)
74  {
75  if (start_now)
76  start();
77  }
78  void start()
79  {
80  _start = high_resolution_clock::now();
81  }
82  double elapsedNs() const
83  {
84  return static_cast<double>(std::chrono::duration_cast<nanoseconds>(high_resolution_clock::now() - _start).count());
85  }
86  double elapsedMs() const
87  {
88  return nsToMs * elapsedNs();
89  }
90  double elapsed() const
91  {
92  return nsToS * elapsedNs();
93  }
94  template <typename T, typename Traits>
95  friend std::basic_ostream<T, Traits>& operator<<(std::basic_ostream<T, Traits>& out, const PosixTimer& timer)
96  {
97  return out << timer.elapsed();
98  }
99 private:
100  high_resolution_clock::time_point _start;
101 };
102 
104 {
105 public:
108  template <typename Type>
109  Formatter & operator << (const Type & value)
110  {
111  stream_ << value;
112  return *this;
113  }
114 
115  std::string str() const { return stream_.str(); }
116  operator std::string () const { return stream_.str(); }
117 
119  {
120  to_str
121  };
122  std::string operator >> (ConvertToString) { return stream_.str(); }
123 
124 private:
125  std::stringstream stream_;
126 
127  Formatter(const Formatter &);
128  Formatter & operator = (Formatter &);
129 };
130 // Usage :
131 // throw std::runtime_error(Formatter() << foo << 13 << ", bar" << myData); // implicitly cast to std::string
132 // throw std::runtime_error(Formatter() << foo << 13 << ", bar" << myData >> Formatter::to_str); // explicitly cast to std::string
133 
134 // Throwing exceptions with the line number
135 // From https://stackoverflow.com/a/348862
136 class orca_exception : public std::runtime_error {
137  std::string msg;
138 public:
139  orca_exception(const std::string &arg, const char *file, int line) :
140  std::runtime_error(arg) {
141  std::ostringstream o;
142  o << "\033[31m" << file << ":" << line << ": " << arg << "\033[0m";
143  msg = o.str();
144  }
145  ~orca_exception() throw() {}
146  const char *what() const throw() {
147  return msg.c_str();
148  }
149 };
150 
151 inline void orca_throw(const std::string& arg)
152 {
153  throw orca_exception(arg, __FILE__, __LINE__);
154 }
155 
156 // template<class T>
157 // struct SharedPointer
158 // {
159 // using Ptr = std::shared_ptr<T>;
160 // // using ConstPtr = const std::shared_ptr<T>;
161 // // std::shared_ptr<T> createPtr()
162 // // {
163 // // return std::make_shared<T>();
164 // // }
165 // };
166 // class PeriodicPosixThread
167 // {
168 // public:
169 // PeriodicPosixThread(std::function<void(void)> f , const unsigned long period_ms, bool start_now = false)
170 // : f_(f)
171 // , running_(false)
172 // , period_ms_(period_ms)
173 // {
174 // if(start_now)
175 // start();
176 // }
177 //
178 // void start()
179 // {
180 // if(!running_)
181 // {
182 // running_ = true;
183 // th_ = std::thread( std::bind(&PeriodicPosixThread::run,this) );
184 // }
185 // }
186 //
187 // void run()
188 // {
189 // const auto timeWindow = std::chrono::milliseconds(period_ms_);
190 //
191 // while(running_)
192 // {
193 // auto start = std::chrono::steady_clock::now();
194 // f_();
195 // auto end = std::chrono::steady_clock::now();
196 // auto elapsed = end - start;
197 //
198 // auto timeToWait = timeWindow - elapsed;
199 // if(timeToWait > std::chrono::milliseconds::zero())
200 // {
201 // std::this_thread::sleep_for(timeToWait);
202 // }
203 // }
204 // }
205 //
206 // void stop()
207 // {
208 // if(running_)
209 // {
210 // running_ = false;
211 // th_.join();
212 // }
213 // }
214 //
215 // ~PeriodicPosixThread()
216 // {
217 // stop();
218 // }
219 //
220 // private:
221 // std::function<void(void)> f_;
222 // std::thread th_;
223 // std::atomic<bool> running_;
224 // const unsigned long period_ms_;
225 // };
226 
227 template <typename Derived>
228 void assertSize(const Eigen::EigenBase<Derived>& a, const Eigen::EigenBase<Derived>& b)
229 {
230  if(a.cols() == b.cols() && a.rows() == b.rows())
231  return;
232  throw std::length_error(Formatter() << "Size mismatched, provided size (" << a.rows() << " , " << a.cols() << "), but have size (" << b.rows() << " , " << b.cols() << ")");
233 }
234 
235 inline void assertSize(const Eigen::VectorXd& a, int s)
236 {
237  if(a.size() == s)
238  return;
239  throw std::length_error(Formatter() << "Vector size is " << a.size() << ", but should be (" << s << ")");
240 }
241 
242 template<class T> bool exists(const T& t,std::list< T > l){
243  return std::find(l.begin(),l.end(),t) != l.end();
244 }
245 
246 template <typename Key,typename Vals>
247 bool key_exists(const std::map<Key,Vals>& container, const Key& key)
248 {
249  auto it = container.begin();
250  while(it != container.end())
251  {
252  if(it->first == key)
253  return true;
254  ++it;
255  }
256  return false;
257 }
258 
259 // class Param
260 // {
261 // virtual loadFromString() = 0;
262 // set(std::string s);
263 // Param * get() { return this; }
264 // T* as()
265 // {
266 // return static_cast<T>(this);
267 // }
268 // loadFromString(const std::string s)
269 // {
270 // data_ = s.as<T>();
271 // }
272 // T& get(){ return data_ }
273 // T data_;
274 // }
275 // class EigenVectorParam : public Param
276 // {
277 // typedef Eigen::VectorXd type;
278 //
279 // loadFromString(const std::string s)
280 // {
281 // data_ = s.as<type>();
282 // }
283 // Eigen::VectorXd& get(){ return data_ }
284 // Eigen::VectorXd data_;
285 // }
286 //
287 //
288 // std::map<std::string, Param*> parameters_;
289 //
290 // MyTask()
291 // {
292 // private:
293 // EigenVectorParam Kp_;
294 // EigenVectorParam Kd_;
295 // StringParam control_frame_;
296 // MyTask()
297 // {
298 // addParam("Kp",&Kp_);
299 // addParam("Kd",&Kd_);
300 // addParam("control_frame",&control_frame_);
301 // }
302 // }
303 //
304 // MyTask task;
305 // task.proportionalGain() = Eigen::VectorXd::Zero(6);
306 //
307 // myTask:
308 // - kp : [0,0,0,0,0,0,0,0]
309 // - kd : [0,0,0,0,0,0,0,0]
310 // - control_frame : link_7
311 //
312 // loadFromString(string params_str)
313 // {
314 // YAML::Node lineup = YAML::Load("{1B: Prince Fielder, 2B: Rickie Weeks, LF: Ryan Braun}");
315 // for(YAML::const_iterator it=lineup.begin();it!=lineup.end();++it)
316 // {
317 // auto param_name = it->first.as<std::string>(); // "kp"
318 // Param * param = parameters_[param_name];
319 //
320 //
321 // auto param_value_str = << "\n";
322 //
323 // parameters_[param_name]->loadFromNode(it->second)
324 // }
325 //
326 // }
327 //
328 // auto p = MyParam.as<MyParam::type>()
329 //
330 // std::vector< Param > params_;
331 
332 
333 
334 
335 } // namespace utils
336 } // namespace orca
337 
338 
339 // Header Copyright
340 
341 // This file is a part of the orca framework.
342 // Copyright 2017, ISIR / Universite Pierre et Marie Curie (UPMC)
343 // Main contributor(s): Antoine Hoarau, hoarau@isir.upmc.fr
344 //
345 // This software is a computer program whose purpose is to [describe
346 // functionalities and technical features of your software].
347 //
348 // This software is governed by the CeCILL-C license under French law and
349 // abiding by the rules of distribution of free software. You can use,
350 // modify and/ or redistribute the software under the terms of the CeCILL-C
351 // license as circulated by CEA, CNRS and INRIA at the following URL
352 // "http://www.cecill.info".
353 //
354 // As a counterpart to the access to the source code and rights to copy,
355 // modify and redistribute granted by the license, users are provided only
356 // with a limited warranty and the software's author, the holder of the
357 // economic rights, and the successive licensors have only limited
358 // liability.
359 //
360 // In this respect, the user's attention is drawn to the risks associated
361 // with loading, using, modifying and/or developing or reproducing the
362 // software by the user in light of its specific status of free software,
363 // that may mean that it is complicated to manipulate, and that also
364 // therefore means that it is reserved for developers and experienced
365 // professionals having in-depth computer knowledge. Users are therefore
366 // encouraged to load and test the software's suitability as regards their
367 // requirements in conditions enabling the security of their systems and/or
368 // data to be ensured and, more generally, to use and operate it in the
369 // same conditions as regards security.
370 //
371 // The fact that you are presently reading this means that you have had
372 // knowledge of the CeCILL-C license and that you accept its terms.
double elapsed() const
Definition: Utils.h:90
bool exists(const T &t, std::list< T > l)
Definition: Utils.h:242
ConvertToString
Definition: Utils.h:118
void assertSize(const Eigen::EigenBase< Derived > &a, const Eigen::EigenBase< Derived > &b)
Definition: Utils.h:228
double elapsedMs() const
Definition: Utils.h:86
Definition: Utils.h:136
void orca_throw(const std::string &arg)
Definition: Utils.h:151
std::unique_ptr< T > make_unique(Args &&...args)
Definition: Utils.h:59
~orca_exception()
Definition: Utils.h:145
const char * what() const
Definition: Utils.h:146
Definition: Utils.h:64
friend std::basic_ostream< T, Traits > & operator<<(std::basic_ostream< T, Traits > &out, const PosixTimer &timer)
Definition: Utils.h:95
Formatter()
Definition: Utils.h:106
Definition: Utils.h:103
std::string str() const
Definition: Utils.h:115
~Formatter()
Definition: Utils.h:107
PosixTimer(bool start_now=false)
Definition: Utils.h:73
bool key_exists(const std::map< Key, Vals > &container, const Key &key)
Definition: Utils.h:247
double elapsedNs() const
Definition: Utils.h:82
const double nsToS
Definition: Utils.h:71
Definition: CartesianAccelerationPID.h:44
const double nsToMs
Definition: Utils.h:70
void start()
Definition: Utils.h:78
orca_exception(const std::string &arg, const char *file, int line)
Definition: Utils.h:139