GCC Code Coverage Report

Directory: ./
File: include/hpp/centroidal-dynamics/util.hh
Date: 2025-05-17 04:04:54
Exec Total Coverage
Lines: 11 11 100.0%
Functions: 3 3 100.0%
Branches: 8 16 50.0%
Line Branch Exec Source
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
21 namespace centroidal_dynamics {
22
23 // #define USE_FLOAT 1;
24 #ifdef USE_FLOAT
25 typedef 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;
47 typedef 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
71 /**Column major definitions for compatibility with classical eigen use**/
72 typedef Eigen::Matrix<value_type, Eigen::Dynamic, 3, Eigen::ColMajor>
73 MatrixX3ColMajor;
74 typedef 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
80 /**
81 * Write the specified matrix to a binary file with the specified name.
82 */
83 template <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
97 /**
98 * Read a matrix from the specified input binary file.
99 */
100 template <class Matrix>
101 20 bool readMatrixFromFile(const std::string& filename, Matrix& matrix) {
102
1/2
✓ Branch 3 taken 17 times.
✗ Branch 4 not taken.
 20 std::ifstream in(filename.c_str(), std::ios::in | std::ios::binary);
103
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 17 times.
 20 if (!in.is_open()) return false;
104 20 typename Matrix::Index rows = 0, cols = 0;
105
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 20 in.read((char*)(&rows), sizeof(typename Matrix::Index));
106
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 20 in.read((char*)(&cols), sizeof(typename Matrix::Index));
107
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 20 matrix.resize(rows, cols);
108
2/4
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
 20 in.read((char*)matrix.data(), rows * cols * sizeof(typename Matrix::Scalar));
109
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 20 in.close();
110 20 return true;
111 20 }
112
113 /**
114 * Convert the specified list of rays from Eigen to cdd format.
115 * @param input The mXn input Eigen matrix contains m rays of dimension n.
116 * @param bool whether to remove redundant inequalities
117 * @return The mX(n+1) output cdd matrix, which contains an additional column,
118 * the first one, with all zeros.
119 */
120 dd_MatrixPtr cone_span_eigen_to_cdd(Cref_matrixXX input,
121 const bool canonicalize = false);
122
123 /**
124 * Compute the cross-product skew-symmetric matrix associated to the specified
125 * vector.
126 */
127 Rotation crossMatrix(Cref_vector3 x);
128
129 void init_cdd_library();
130
131 void release_cdd_library();
132
133 // in some distribution the conversion Ref_matrixXX to Ref_vector3 does not
134 // compile
135 void uniform3(Cref_vector3 lower_bounds, Cref_vector3 upper_bounds,
136 Ref_vector3 out);
137 void uniform(Cref_matrixXX lower_bounds, Cref_matrixXX upper_bounds,
138 Ref_matrixXX out);
139
140 void euler_matrix(double roll, double pitch, double yaw, Ref_rotation R);
141
142 bool generate_rectangle_contacts(double lx, double ly, Cref_vector3 pos,
143 Cref_vector3 rpy, Ref_matrix43 p,
144 Ref_matrix43 N);
145
146 std::string getDateAndTimeAsString();
147
148 /**
149 * Computes a binomal coefficient
150 * @return n!/((n–k)! k!).
151 */
152 value_type nchoosek(const int n, const int k);
153
154 template <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
173 /**
174 * Computes a matrix C containing all possible combinations of the elements of
175 * vector v taken k at a time. Matrix C has k columns and n!/((n–k)! k!) rows,
176 * where n is length(v).
177 * @param V n-long vector of elements
178 * @param k size of sub-set to consider
179 * @param U result matrix
180 * @return nchoosek by k long matrix where each row is a unique k-size
181 * the first one, with all zeros.
182 */
183 template <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
201