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