5 #ifndef HPP_CENTROIDAL_DYNAMICS_UTIL_HH 6 #define HPP_CENTROIDAL_DYNAMICS_UTIL_HH 13 #include <Eigen/Dense> 14 #include <Eigen/src/Core/util/Macros.h> 30 typedef Eigen::Matrix<value_type, 2, 1>
Vector2;
31 typedef Eigen::Matrix<value_type, 1, 2>
RVector2;
32 typedef Eigen::Matrix<value_type, 3, 1>
Vector3;
33 typedef Eigen::Matrix<value_type, 1, 3>
RVector3;
34 typedef Eigen::Matrix<value_type, 6, 1>
Vector6;
35 typedef Eigen::Matrix<value_type, Eigen::Dynamic, 1>
VectorX;
36 typedef Eigen::Matrix<value_type, 1, Eigen::Dynamic>
RVectorX;
37 typedef Eigen::Matrix<value_type, 3, 3, Eigen::RowMajor>
Rotation;
38 typedef Eigen::Matrix<value_type, Eigen::Dynamic, 2, Eigen::RowMajor>
MatrixX2;
39 typedef Eigen::Matrix<value_type, 3, 3, Eigen::RowMajor>
Matrix3;
40 typedef Eigen::Matrix<value_type, Eigen::Dynamic, 3, Eigen::RowMajor>
MatrixX3;
41 typedef Eigen::Matrix<value_type, 3, Eigen::Dynamic, Eigen::RowMajor>
Matrix3X;
42 typedef Eigen::Matrix<value_type, 4, 3, Eigen::RowMajor>
Matrix43;
43 typedef Eigen::Matrix<value_type, 6, Eigen::Dynamic, Eigen::RowMajor>
Matrix6X;
44 typedef Eigen::Matrix<value_type, 6, 2, Eigen::RowMajor>
Matrix62;
45 typedef Eigen::Matrix<value_type, 6, 3, Eigen::RowMajor>
Matrix63;
46 typedef Eigen::Matrix<value_type, Eigen::Dynamic, 6, Eigen::RowMajor>
MatrixX6;
47 typedef Eigen::Matrix<value_type, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
MatrixXX;
70 typedef Eigen::Matrix<value_type, Eigen::Dynamic, 3, Eigen::ColMajor>
MatrixX3ColMajor;
71 typedef Eigen::Matrix<value_type, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>
MatrixXXColMajor;
78 template <
class Matrix>
80 std::ofstream out(filename.c_str(), std::ios::out | std::ios::binary | std::ios::trunc);
81 if (!out.is_open())
return false;
82 typename Matrix::Index rows = matrix.rows(), cols = matrix.cols();
83 out.write((
char*)(&rows),
sizeof(
typename Matrix::Index));
84 out.write((
char*)(&cols),
sizeof(
typename Matrix::Index));
85 out.write((
char*)matrix.data(), rows * cols *
sizeof(
typename Matrix::Scalar));
93 template <
class Matrix>
95 std::ifstream in(filename.c_str(), std::ios::in | std::ios::binary);
96 if (!in.is_open())
return false;
97 typename Matrix::Index rows = 0, cols = 0;
98 in.read((
char*)(&rows),
sizeof(
typename Matrix::Index));
99 in.read((
char*)(&cols),
sizeof(
typename Matrix::Index));
100 matrix.resize(rows, cols);
101 in.read((
char*)matrix.data(), rows * cols *
sizeof(
typename Matrix::Scalar));
125 void uniform3(Cref_vector3 lower_bounds, Cref_vector3 upper_bounds, Ref_vector3 out);
126 void uniform(Cref_matrixXX lower_bounds, Cref_matrixXX upper_bounds, Ref_matrixXX out);
128 void euler_matrix(
double roll,
double pitch,
double yaw, Ref_rotation R);
139 value_type
nchoosek(
const int n,
const int k);
141 template <
typename DerivedV,
typename DerivedU>
142 void doCombs(Eigen::Matrix<typename DerivedU::Scalar, 1, Eigen::Dynamic>& running,
int& running_i,
int& running_j,
143 Eigen::PlainObjectBase<DerivedU>& U,
const Eigen::MatrixBase<DerivedV>& V,
int offset,
int k) {
144 int N = (int)(V.size());
146 U.row(running_i) = running;
150 for (
int i = offset; i <= N - k; ++i) {
151 running(running_j) = V(i);
153 doCombs(running, running_i, running_j, U, V, i + 1, k - 1);
167 template <
typename DerivedV,
typename DerivedU>
168 void nchoosek(
const Eigen::MatrixBase<DerivedV>& V,
const int k, Eigen::PlainObjectBase<DerivedU>& U) {
169 using namespace Eigen;
174 assert((V.cols() == 1 || V.rows() == 1) &&
"V must be a vector");
175 U.resize(
nchoosek((
int)(V.size()), k), k);
178 Matrix<typename DerivedU::Scalar, 1, Dynamic> running(1, k);
179 doCombs(running, running_i, running_j, U, V, 0, k);
183 #endif // HPP_CENTROIDAL_DYNAMICS_UTIL_HH Eigen::Ref< Vector3 > Ref_vector3
Definition: util.hh:50
Eigen::Matrix< value_type, 3, Eigen::Dynamic, Eigen::RowMajor > Matrix3X
Definition: util.hh:41
bool readMatrixFromFile(const std::string &filename, Matrix &matrix)
Definition: util.hh:94
Eigen::Matrix< value_type, Eigen::Dynamic, 1 > VectorX
Definition: util.hh:35
const Eigen::Ref< const Vector2 > & Cref_vector2
Definition: util.hh:58
Definition: centroidal_dynamics.hh:14
bool writeMatrixToFile(const std::string &filename, const Matrix &matrix)
Definition: util.hh:79
Eigen::Matrix< value_type, 6, 1 > Vector6
Definition: util.hh:34
const Eigen::Ref< const Vector3 > & Cref_vector3
Definition: util.hh:59
Eigen::Matrix< value_type, 6, 3, Eigen::RowMajor > Matrix63
Definition: util.hh:45
Eigen::Matrix< value_type, 3, 3, Eigen::RowMajor > Matrix3
Definition: util.hh:39
void euler_matrix(double roll, double pitch, double yaw, Ref_rotation R)
Definition: util.cpp:76
const Eigen::Ref< const Matrix43 > & Cref_matrix43
Definition: util.hh:64
Eigen::Ref< Matrix6X > Ref_matrix6X
Definition: util.hh:55
Eigen::Matrix< value_type, Eigen::Dynamic, 3, Eigen::RowMajor > MatrixX3
Definition: util.hh:40
const Eigen::Ref< const MatrixXX > & Cref_matrixXX
Definition: util.hh:67
value_type nchoosek(const int n, const int k)
Definition: util.cpp:177
Eigen::Matrix< value_type, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor > MatrixXXColMajor
Definition: util.hh:71
Eigen::Ref< Vector2 > Ref_vector2
Definition: util.hh:49
Eigen::Matrix< value_type, 1, 3 > RVector3
Definition: util.hh:33
const Eigen::Ref< const VectorX > & Cref_vectorX
Definition: util.hh:61
Eigen::Matrix< value_type, Eigen::Dynamic, 6, Eigen::RowMajor > MatrixX6
Definition: util.hh:46
Eigen::Ref< MatrixXX > Ref_matrixXX
Definition: util.hh:56
const Eigen::Ref< const Matrix63 > & Cref_matrix63
Definition: util.hh:66
double value_type
Definition: util.hh:27
void doCombs(Eigen::Matrix< typename DerivedU::Scalar, 1, Eigen::Dynamic > &running, int &running_i, int &running_j, Eigen::PlainObjectBase< DerivedU > &U, const Eigen::MatrixBase< DerivedV > &V, int offset, int k)
Definition: util.hh:142
Eigen::Ref< Matrix43 > Ref_matrix43
Definition: util.hh:54
void release_cdd_library()
Definition: util.cpp:52
const Eigen::Ref< const MatrixX3ColMajor > & Cref_matrixX3ColMajor
Definition: util.hh:72
Eigen::Matrix< value_type, 6, 2, Eigen::RowMajor > Matrix62
Definition: util.hh:44
dd_MatrixPtr cone_span_eigen_to_cdd(Cref_matrixXX input, const bool canonicalize=false)
Definition: util.cpp:12
bool generate_rectangle_contacts(double lx, double ly, Cref_vector3 pos, Cref_vector3 rpy, Ref_matrix43 p, Ref_matrix43 N)
Definition: util.cpp:105
void init_cdd_library()
Definition: util.cpp:47
Eigen::Matrix< value_type, 2, 1 > Vector2
Definition: util.hh:30
Eigen::Ref< MatrixX3 > Ref_matrixX3
Definition: util.hh:53
Eigen::Matrix< value_type, 1, 2 > RVector2
Definition: util.hh:31
Eigen::Matrix< value_type, 6, Eigen::Dynamic, Eigen::RowMajor > Matrix6X
Definition: util.hh:43
const Eigen::Ref< const MatrixX3 > & Cref_matrixX3
Definition: util.hh:63
const Eigen::Ref< const Vector6 > & Cref_vector6
Definition: util.hh:60
Eigen::Matrix< value_type, 4, 3, Eigen::RowMajor > Matrix43
Definition: util.hh:42
Eigen::Matrix< value_type, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > MatrixXX
Definition: util.hh:47
Eigen::Matrix< value_type, 1, Eigen::Dynamic > RVectorX
Definition: util.hh:36
const Eigen::Ref< const Matrix6X > & Cref_matrix6X
Definition: util.hh:65
Eigen::Ref< Rotation > Ref_rotation
Definition: util.hh:52
void uniform3(Cref_vector3 lower_bounds, Cref_vector3 upper_bounds, Ref_vector3 out)
Definition: util.cpp:56
void uniform(Cref_matrixXX lower_bounds, Cref_matrixXX upper_bounds, Ref_matrixXX out)
Definition: util.cpp:66
Eigen::Matrix< value_type, Eigen::Dynamic, 3, Eigen::ColMajor > MatrixX3ColMajor
Definition: util.hh:70
Eigen::Matrix< value_type, Eigen::Dynamic, 2, Eigen::RowMajor > MatrixX2
Definition: util.hh:38
Eigen::Ref< VectorX > Ref_vectorX
Definition: util.hh:51
Eigen::Ref< MatrixXXColMajor > & ref_matrixXXColMajor
Definition: util.hh:73
Eigen::Matrix< value_type, 3, 1 > Vector3
Definition: util.hh:32
Rotation crossMatrix(Cref_vector3 x)
Definition: util.cpp:126
Eigen::Matrix< value_type, 3, 3, Eigen::RowMajor > Rotation
Definition: util.hh:37
std::string getDateAndTimeAsString()
Definition: util.cpp:137
const Eigen::Ref< const Rotation > & Cref_rotation
Definition: util.hh:62