GCC Code Coverage Report

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	2/4 ✓ Branch 1 taken 17 times. ✗ Branch 2 not taken. ✗ Branch 3 not taken. ✓ Branch 4 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