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 <Eigen/Dense>
39 #include <limits>
40 
41 namespace orca
42 {
43 namespace math
44 {
45  static const double Infinity = std::numeric_limits<double>::infinity();
46 
47  inline void setToHighest(Eigen::VectorXd& v)
48  {
49  v.setConstant( math::Infinity );
50  }
51 
52  template<typename T> bool equal(const T& a, const T& b)
53  {
54  return std::abs<T>(a - b) < std::numeric_limits<T>::epsilon();
55  }
56 
57  inline void setToLowest(Eigen::VectorXd& v)
58  {
59  v.setConstant( - math::Infinity );
60  }
61 
62  typedef Eigen::Matrix<double,6,1> Vector6d;
63 
64  inline Eigen::Vector3d diffRot(const Eigen::Matrix3d& R_a_b1, const Eigen::Matrix3d& R_a_b2)
65  {
66  Eigen::Matrix3d R_b1_b2 = R_a_b1.transpose() * R_a_b2;
67  Eigen::AngleAxisd aa(Eigen::Map<const Eigen::Matrix3d>(R_b1_b2.data()));
68  return R_a_b1 * aa.angle() * aa.axis();
69  }
70 
71  inline Eigen::Matrix<double,6,1> diffTransform(const Eigen::Matrix4d& t_a_b1, const Eigen::Matrix4d& t_a_b2)
72  {
73  Eigen::Matrix<double,6,1> d_t1_t2;
74 
75  d_t1_t2.head<3>() = t_a_b2.block<3,1>(0,3) - t_a_b1.block<3,1>(0,3);
76  d_t1_t2.tail<3>() = diffRot(t_a_b1.topLeftCorner<3,3>(),t_a_b2.topLeftCorner<3,3>());
77  return d_t1_t2;
78  }
79 
80  inline Eigen::Quaterniond quatFromRPY(double roll,double pitch,double yaw )
81  {
82  return Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX())
83  * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())
84  * Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ());
85  }
86 
87  inline Eigen::Quaterniond quatFromKukaConvention(double A,double B,double C )
88  {
89  return Eigen::AngleAxisd(A, Eigen::Vector3d::UnitZ())
90  * Eigen::AngleAxisd(B, Eigen::Vector3d::UnitY())
91  * Eigen::AngleAxisd(C, Eigen::Vector3d::UnitX());
92  }
93 
94  struct Size
95  {
96  template <typename Derived> Size(const Eigen::MatrixBase<Derived>& mat)
97  : Size(mat.rows(),mat.cols())
98  {}
99 
100  Size(int rows = 0, int cols = 0)
101  : rows_(rows)
102  , cols_(cols)
103  {}
104  Size(const Size& s)
105  : Size(s.rows(),s.cols())
106  {}
107  bool operator==(const Size& s) const
108  {
109  return (s.rows() == rows_) && (s.cols() == cols_);
110  }
111  bool operator!=(const Size& s) const
112  {
113  return !(*this == s);
114  }
115  int rows() const { return rows_;}
116  int cols() const { return cols_;}
117  private:
118  int rows_ = 0;
119  int cols_ = 0;
120  };
121 } // namespace math
122 } // namespace orca
Size(int rows=0, int cols=0)
Definition: Utils.h:100
int cols() const
Definition: Utils.h:116
Eigen::Vector3d diffRot(const Eigen::Matrix3d &R_a_b1, const Eigen::Matrix3d &R_a_b2)
Definition: Utils.h:64
void setToLowest(Eigen::VectorXd &v)
Definition: Utils.h:57
int rows() const
Definition: Utils.h:115
Eigen::Matrix< double, 6, 1 > diffTransform(const Eigen::Matrix4d &t_a_b1, const Eigen::Matrix4d &t_a_b2)
Definition: Utils.h:71
bool operator!=(const Size &s) const
Definition: Utils.h:111
bool equal(const T &a, const T &b)
Definition: Utils.h:52
bool operator==(const Size &s) const
Definition: Utils.h:107
Eigen::Matrix< double, 6, 1 > Vector6d
Definition: Utils.h:62
Eigen::Quaterniond quatFromKukaConvention(double A, double B, double C)
Definition: Utils.h:87
Definition: CartesianAccelerationPID.h:44
Definition: Utils.h:94
static const double Infinity
Definition: Utils.h:45
Size(const Size &s)
Definition: Utils.h:104
Eigen::Quaterniond quatFromRPY(double roll, double pitch, double yaw)
Definition: Utils.h:80
void setToHighest(Eigen::VectorXd &v)
Definition: Utils.h:47
Size(const Eigen::MatrixBase< Derived > &mat)
Definition: Utils.h:96