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