hpp-centroidal-dynamics: include/hpp/centroidal-dynamics/util.hh Source File

hpp-centroidal-dynamics 9.0.2 Utility classes for testing (robust) equilibrium of a system in contact with the environment, and other centroidal dynamics methods.

Loading...
Searching...
No Matches
util.hh
Go to the documentation of this file.
1/*
2 * Copyright 2015, LAAS-CNRS
3 * Author: Andrea Del Prete
4 */
5#ifndef HPP_CENTROIDAL_DYNAMICS_UTIL_HH
6#define HPP_CENTROIDAL_DYNAMICS_UTIL_HH
7
8#include <Eigen/Dense>
9// Macros.h needs to be included after Dense
10#include <Eigen/src/Core/util/Macros.h>
11
12#include <cassert>
13#include <cmath>
14#include <fstream>
15#include <iostream>
16
17#include "setoper.h"
18// cdd.h needs to be included after setoper.h
19#include "cdd.h"
20
21namespace centroidal_dynamics {
22
23// #define USE_FLOAT 1;
24#ifdef USE_FLOAT
25typedef float value_type;
26#else
27 typedef double value_type;
28#endif
29
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;
47typedef Eigen::Matrix<value_type, Eigen::Dynamic, Eigen::Dynamic,
48 Eigen::RowMajor>
49 MatrixXX;
50
51 typedef Eigen::Ref<Vector2> Ref_vector2;
52 typedef Eigen::Ref<Vector3> Ref_vector3;
53 typedef Eigen::Ref<VectorX> Ref_vectorX;
54 typedef Eigen::Ref<Rotation> Ref_rotation;
55 typedef Eigen::Ref<MatrixX3> Ref_matrixX3;
56 typedef Eigen::Ref<Matrix43> Ref_matrix43;
57 typedef Eigen::Ref<Matrix6X> Ref_matrix6X;
58 typedef Eigen::Ref<MatrixXX> Ref_matrixXX;
59
60 typedef const Eigen::Ref<const Vector2>& Cref_vector2;
61 typedef const Eigen::Ref<const Vector3>& Cref_vector3;
62 typedef const Eigen::Ref<const Vector6>& Cref_vector6;
63 typedef const Eigen::Ref<const VectorX>& Cref_vectorX;
64 typedef const Eigen::Ref<const Rotation>& Cref_rotation;
65 typedef const Eigen::Ref<const MatrixX3>& Cref_matrixX3;
66 typedef const Eigen::Ref<const Matrix43>& Cref_matrix43;
67 typedef const Eigen::Ref<const Matrix6X>& Cref_matrix6X;
68 typedef const Eigen::Ref<const Matrix63>& Cref_matrix63;
69 typedef const Eigen::Ref<const MatrixXX>& Cref_matrixXX;
70
72typedef Eigen::Matrix<value_type, Eigen::Dynamic, 3, Eigen::ColMajor>
73 MatrixX3ColMajor;
74typedef Eigen::Matrix<value_type, Eigen::Dynamic, Eigen::Dynamic,
75 Eigen::ColMajor>
76 MatrixXXColMajor;
77 typedef const Eigen::Ref<const MatrixX3ColMajor>& Cref_matrixX3ColMajor;
78 typedef Eigen::Ref<MatrixXXColMajor>& ref_matrixXXColMajor;
79
83template <class Matrix>
84 bool writeMatrixToFile(const std::string& filename, const Matrix& matrix) {
85 std::ofstream out(filename.c_str(),
86 std::ios::out | std::ios::binary | std::ios::trunc);
87 if (!out.is_open()) return false;
88 typename Matrix::Index rows = matrix.rows(), cols = matrix.cols();
89 out.write((char*)(&rows), sizeof(typename Matrix::Index));
90 out.write((char*)(&cols), sizeof(typename Matrix::Index));
91 out.write((char*)matrix.data(),
92 rows * cols * sizeof(typename Matrix::Scalar));
93 out.close();
94 return true;
95}
96
100template <class Matrix>
101 bool readMatrixFromFile(const std::string& filename, Matrix& matrix) {
102 std::ifstream in(filename.c_str(), std::ios::in | std::ios::binary);
103 if (!in.is_open()) return false;
104 typename Matrix::Index rows = 0, cols = 0;
105 in.read((char*)(&rows), sizeof(typename Matrix::Index));
106 in.read((char*)(&cols), sizeof(typename Matrix::Index));
107 matrix.resize(rows, cols);
108 in.read((char*)matrix.data(), rows * cols * sizeof(typename Matrix::Scalar));
109 in.close();
110 return true;
111}
112
120dd_MatrixPtr cone_span_eigen_to_cdd(Cref_matrixXX input,
121 const bool canonicalize = false);
122
127 Rotation crossMatrix(Cref_vector3 x);
128
129void init_cdd_library();
130
131void release_cdd_library();
132
133// in some distribution the conversion Ref_matrixXX to Ref_vector3 does not
134// compile
135void uniform3(Cref_vector3 lower_bounds, Cref_vector3 upper_bounds,
136 Ref_vector3 out);
137void uniform(Cref_matrixXX lower_bounds, Cref_matrixXX upper_bounds,
138 Ref_matrixXX out);
139
140void euler_matrix(double roll, double pitch, double yaw, Ref_rotation R);
141
142bool generate_rectangle_contacts(double lx, double ly, Cref_vector3 pos,
143 Cref_vector3 rpy, Ref_matrix43 p,
144 Ref_matrix43 N);
145
146std::string getDateAndTimeAsString();
147
152 value_type nchoosek(const int n, const int k);
153
154template <typename DerivedV, typename DerivedU>
155 void doCombs(
156 Eigen::Matrix<typename DerivedU::Scalar, 1, Eigen::Dynamic>& running,
157 int& running_i, int& running_j, Eigen::PlainObjectBase<DerivedU>& U,
158 const Eigen::MatrixBase<DerivedV>& V, int offset, int k) {
159 int N = (int)(V.size());
160 if (k == 0) {
161 U.row(running_i) = running;
162 running_i++;
163 return;
164 }
165 for (int i = offset; i <= N - k; ++i) {
166 running(running_j) = V(i);
167 running_j++;
168 doCombs(running, running_i, running_j, U, V, i + 1, k - 1);
169 running_j--;
170 }
171}
172
183template <typename DerivedV, typename DerivedU>
184 void nchoosek(const Eigen::MatrixBase<DerivedV>& V, const int k,
185 Eigen::PlainObjectBase<DerivedU>& U) {
186 using namespace Eigen;
187 if (V.size() == 0) {
188 U.resize(0, k);
189 return;
190 }
191 assert((V.cols() == 1 || V.rows() == 1) && "V must be a vector");
192 U.resize(nchoosek((int)(V.size()), k), k);
193 int running_i = 0;
194 int running_j = 0;
195 Matrix<typename DerivedU::Scalar, 1, Dynamic> running(1, k);
196 doCombs(running, running_i, running_j, U, V, 0, k);
197}
198} // namespace centroidal_dynamics
199
200#endif // HPP_CENTROIDAL_DYNAMICS_UTIL_HH
centroidal_dynamics
Definition centroidal_dynamics.hh:14
centroidal_dynamics::Cref_matrix63
const Eigen::Ref< const Matrix63 > & Cref_matrix63
Definition util.hh:68
centroidal_dynamics::Vector3
Eigen::Matrix< value_type, 3, 1 > Vector3
Definition util.hh:32
centroidal_dynamics::Ref_matrixX3
Eigen::Ref< MatrixX3 > Ref_matrixX3
Definition util.hh:55
centroidal_dynamics::Vector6
Eigen::Matrix< value_type, 6, 1 > Vector6
Definition util.hh:34
centroidal_dynamics::Ref_rotation
Eigen::Ref< Rotation > Ref_rotation
Definition util.hh:54
centroidal_dynamics::init_cdd_library
void init_cdd_library()
Definition util.cpp:47
centroidal_dynamics::Cref_matrix43
const Eigen::Ref< const Matrix43 > & Cref_matrix43
Definition util.hh:66
centroidal_dynamics::Matrix62
Eigen::Matrix< value_type, 6, 2, Eigen::RowMajor > Matrix62
Definition util.hh:44
centroidal_dynamics::readMatrixFromFile
bool readMatrixFromFile(const std::string &filename, Matrix &matrix)
Definition util.hh:101
centroidal_dynamics::nchoosek
value_type nchoosek(const int n, const int k)
Definition util.cpp:184
centroidal_dynamics::getDateAndTimeAsString
std::string getDateAndTimeAsString()
Definition util.cpp:144
centroidal_dynamics::MatrixX6
Eigen::Matrix< value_type, Eigen::Dynamic, 6, Eigen::RowMajor > MatrixX6
Definition util.hh:46
centroidal_dynamics::Ref_vector2
Eigen::Ref< Vector2 > Ref_vector2
Definition util.hh:51
centroidal_dynamics::uniform
void uniform(Cref_matrixXX lower_bounds, Cref_matrixXX upper_bounds, Ref_matrixXX out)
Definition util.cpp:69
centroidal_dynamics::Cref_vector6
const Eigen::Ref< const Vector6 > & Cref_vector6
Definition util.hh:62
centroidal_dynamics::release_cdd_library
void release_cdd_library()
Definition util.cpp:52
centroidal_dynamics::RVectorX
Eigen::Matrix< value_type, 1, Eigen::Dynamic > RVectorX
Definition util.hh:36
centroidal_dynamics::MatrixX3ColMajor
Eigen::Matrix< value_type, Eigen::Dynamic, 3, Eigen::ColMajor > MatrixX3ColMajor
Definition util.hh:73
centroidal_dynamics::Ref_matrix6X
Eigen::Ref< Matrix6X > Ref_matrix6X
Definition util.hh:57
centroidal_dynamics::Cref_matrixX3
const Eigen::Ref< const MatrixX3 > & Cref_matrixX3
Definition util.hh:65
centroidal_dynamics::euler_matrix
void euler_matrix(double roll, double pitch, double yaw, Ref_rotation R)
Definition util.cpp:82
centroidal_dynamics::cone_span_eigen_to_cdd
dd_MatrixPtr cone_span_eigen_to_cdd(Cref_matrixXX input, const bool canonicalize=false)
Definition util.cpp:11
centroidal_dynamics::MatrixXXColMajor
Eigen::Matrix< value_type, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor > MatrixXXColMajor
Definition util.hh:76
centroidal_dynamics::Cref_rotation
const Eigen::Ref< const Rotation > & Cref_rotation
Definition util.hh:64
centroidal_dynamics::Vector2
Eigen::Matrix< value_type, 2, 1 > Vector2
Definition util.hh:30
centroidal_dynamics::Matrix3X
Eigen::Matrix< value_type, 3, Eigen::Dynamic, Eigen::RowMajor > Matrix3X
Definition util.hh:41
centroidal_dynamics::VectorX
Eigen::Matrix< value_type, Eigen::Dynamic, 1 > VectorX
Definition util.hh:35
centroidal_dynamics::Cref_vector3
const Eigen::Ref< const Vector3 > & Cref_vector3
Definition util.hh:61
centroidal_dynamics::Rotation
Eigen::Matrix< value_type, 3, 3, Eigen::RowMajor > Rotation
Definition util.hh:37
centroidal_dynamics::generate_rectangle_contacts
bool generate_rectangle_contacts(double lx, double ly, Cref_vector3 pos, Cref_vector3 rpy, Ref_matrix43 p, Ref_matrix43 N)
Definition util.cpp:111
centroidal_dynamics::MatrixX2
Eigen::Matrix< value_type, Eigen::Dynamic, 2, Eigen::RowMajor > MatrixX2
Definition util.hh:38
centroidal_dynamics::uniform3
void uniform3(Cref_vector3 lower_bounds, Cref_vector3 upper_bounds, Ref_vector3 out)
Definition util.cpp:56
centroidal_dynamics::RVector2
Eigen::Matrix< value_type, 1, 2 > RVector2
Definition util.hh:31
centroidal_dynamics::Cref_matrix6X
const Eigen::Ref< const Matrix6X > & Cref_matrix6X
Definition util.hh:67
centroidal_dynamics::Ref_matrixXX
Eigen::Ref< MatrixXX > Ref_matrixXX
Definition util.hh:58
centroidal_dynamics::value_type
double value_type
Definition util.hh:27
centroidal_dynamics::writeMatrixToFile
bool writeMatrixToFile(const std::string &filename, const Matrix &matrix)
Definition util.hh:84
centroidal_dynamics::RVector3
Eigen::Matrix< value_type, 1, 3 > RVector3
Definition util.hh:33
centroidal_dynamics::Matrix63
Eigen::Matrix< value_type, 6, 3, Eigen::RowMajor > Matrix63
Definition util.hh:45
centroidal_dynamics::Matrix3
Eigen::Matrix< value_type, 3, 3, Eigen::RowMajor > Matrix3
Definition util.hh:39
centroidal_dynamics::Ref_vectorX
Eigen::Ref< VectorX > Ref_vectorX
Definition util.hh:53
centroidal_dynamics::Cref_vectorX
const Eigen::Ref< const VectorX > & Cref_vectorX
Definition util.hh:63
centroidal_dynamics::crossMatrix
Rotation crossMatrix(Cref_vector3 x)
Definition util.cpp:133
centroidal_dynamics::Cref_matrixXX
const Eigen::Ref< const MatrixXX > & Cref_matrixXX
Definition util.hh:69
centroidal_dynamics::Cref_matrixX3ColMajor
const Eigen::Ref< const MatrixX3ColMajor > & Cref_matrixX3ColMajor
Definition util.hh:77
centroidal_dynamics::Ref_matrix43
Eigen::Ref< Matrix43 > Ref_matrix43
Definition util.hh:56
centroidal_dynamics::doCombs
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:155
centroidal_dynamics::Cref_vector2
const Eigen::Ref< const Vector2 > & Cref_vector2
Definition util.hh:60
centroidal_dynamics::Matrix6X
Eigen::Matrix< value_type, 6, Eigen::Dynamic, Eigen::RowMajor > Matrix6X
Definition util.hh:43
centroidal_dynamics::ref_matrixXXColMajor
Eigen::Ref< MatrixXXColMajor > & ref_matrixXXColMajor
Definition util.hh:78
centroidal_dynamics::MatrixXX
Eigen::Matrix< value_type, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > MatrixXX
Definition util.hh:49
centroidal_dynamics::Matrix43
Eigen::Matrix< value_type, 4, 3, Eigen::RowMajor > Matrix43
Definition util.hh:42
centroidal_dynamics::MatrixX3
Eigen::Matrix< value_type, Eigen::Dynamic, 3, Eigen::RowMajor > MatrixX3
Definition util.hh:40
centroidal_dynamics::Ref_vector3
Eigen::Ref< Vector3 > Ref_vector3
Definition util.hh:52