GCC Code Coverage Report

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2017 CNRS
3			//
4			// This file is part of tsid
5			// tsid is free software: you can redistribute it
6			// and/or modify it under the terms of the GNU Lesser General Public
7			// License as published by the Free Software Foundation, either version
8			// 3 of the License, or (at your option) any later version.
9			// tsid is distributed in the hope that it will be
10			// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
11			// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12			// General Lesser Public License for more details. You should have
13			// received a copy of the GNU Lesser General Public License along with
14			// tsid If not, see
15			// <http://www.gnu.org/licenses/>.
16			//
17
18			#ifndef __invdyn_math_utils_hpp__
19			#define __invdyn_math_utils_hpp__
20
21			#include "tsid/math/fwd.hpp"
22
23			#include <pinocchio/spatial/se3.hpp>
24			#include <pinocchio/spatial/explog.hpp>
25
26			#include <iostream>
27			#include <fstream>
28			#include <vector>
29
30			#define PRINT_VECTOR(a) \
31			std::cout << #a << "(" << a.rows() << "x" << a.cols() \
32			<< "): " << a.transpose().format(math::CleanFmt) << std::endl
33			#define PRINT_MATRIX(a) \
34			std::cout << #a << "(" << a.rows() << "x" << a.cols() << "):\n" \
35			<< a.format(math::CleanFmt) << std::endl
36
37			namespace tsid {
38			template <typename T>
39		860	std::string toString(const T& v) {
40	1/2 ✓ Branch 1 taken 430 times. ✗ Branch 2 not taken.	860	std::stringstream ss;
41	1/2 ✓ Branch 1 taken 430 times. ✗ Branch 2 not taken.	860	ss << v;
42	1/2 ✓ Branch 1 taken 430 times. ✗ Branch 2 not taken.	1720	return ss.str();
43		860	}
44
45			template <typename T>
46			std::string toString(const std::vector<T>& v,
47			const std::string separator = ", ") {
48			std::stringstream ss;
49			for (int i = 0; i < v.size() - 1; i++) ss << v[i] << separator;
50			ss << v[v.size() - 1];
51			return ss.str();
52			}
53
54			template <typename T, int n>
55			std::string toString(const Eigen::MatrixBase<T>& v,
56			const std::string separator = ", ") {
57			if (v.rows() > v.cols()) return toString(v.transpose(), separator);
58			std::stringstream ss;
59			ss << v;
60			return ss.str();
61			}
62			} // namespace tsid
63
64			namespace tsid {
65			namespace math {
66			static const Eigen::IOFormat CleanFmt(1, 0, ", ", "\n", "[", "]");
67
68			/** List of available parameters of IOFormat constructor:
69			precision number of digits for floating point values, or one of the
70			special constants StreamPrecision and FullPrecision. flags either
71			0, or DontAlignCols, which allows to disable the alignment of columns,
72			resulting in faster code. coeffSeparator string printed between two
73			coefficients of the same row rowSeparator string printed between two rows
74			rowPrefix string printed at the beginning of each row
75			rowSuffix string printed at the end of each row
76			matPrefix string printed at the beginning of the matrix
77			matSuffix string printed at the end of the matrix */
78			static const Eigen::IOFormat matlabPrintFormat(Eigen::FullPrecision,
79			Eigen::DontAlignCols, " ", ";\n",
80			"", "", "[", "];");
81
82			/**
83			* Convert the input SE3 object to a 7D vector of floats [X,Y,Z,Q1,Q2,Q3,Q4].
84			*/
85			void SE3ToXYZQUAT(const pinocchio::SE3& M, RefVector xyzQuat);
86
87			/**
88			* Convert the input SE3 object to a 12D vector of floats
89			* [X,Y,Z,R11,R12,R13,R14,...].
90			*/
91			void SE3ToVector(const pinocchio::SE3& M, RefVector vec);
92
93			void vectorToSE3(RefVector vec, pinocchio::SE3& M);
94
95			void errorInSE3(const pinocchio::SE3& M, const pinocchio::SE3& Mdes,
96			pinocchio::Motion& error);
97
98			void solveWithDampingFromSvd(Eigen::JacobiSVD<Eigen::MatrixXd>& svd,
99			ConstRefVector b, RefVector sol,
100			double damping = 0.0);
101
102			void svdSolveWithDamping(ConstRefMatrix A, ConstRefVector b, RefVector sol,
103			double damping = 0.0);
104
105			void pseudoInverse(ConstRefMatrix A, RefMatrix Apinv, double tolerance,
106			unsigned int computationOptions = Eigen::ComputeThinU \|
107			Eigen::ComputeThinV);
108
109			void pseudoInverse(ConstRefMatrix A,
110			Eigen::JacobiSVD<Eigen::MatrixXd>& svdDecomposition,
111			RefMatrix Apinv, double tolerance,
112			unsigned int computationOptions);
113
114			void pseudoInverse(ConstRefMatrix A,
115			Eigen::JacobiSVD<Eigen::MatrixXd>& svdDecomposition,
116			RefMatrix Apinv, double tolerance, double* nullSpaceBasisOfA,
117			int& nullSpaceRows, int& nullSpaceCols,
118			unsigned int computationOptions);
119
120			void dampedPseudoInverse(ConstRefMatrix A,
121			Eigen::JacobiSVD<Eigen::MatrixXd>& svdDecomposition,
122			RefMatrix Apinv, double tolerance,
123			double dampingFactor,
124			unsigned int computationOptions = Eigen::ComputeThinU \|
125			Eigen::ComputeThinV,
126			double* nullSpaceBasisOfA = 0, int* nullSpaceRows = 0,
127			int* nullSpaceCols = 0);
128
129			void nullSpaceBasisFromDecomposition(
130			const Eigen::JacobiSVD<Eigen::MatrixXd>& svdDecomposition, double tolerance,
131			double* nullSpaceBasisMatrix, int& rows, int& cols);
132
133			void nullSpaceBasisFromDecomposition(
134			const Eigen::JacobiSVD<Eigen::MatrixXd>& svdDecomposition, int rank,
135			double* nullSpaceBasisMatrix, int& rows, int& cols);
136
137			template <typename Derived>
138		3780	inline bool isFinite(const Eigen::MatrixBase<Derived>& x) {
139		3780	return ((x - x).array() == (x - x).array()).all();
140			}
141
142			template <typename Derived>
143			inline bool is_nan(const Eigen::MatrixBase<Derived>& x) {
144			return ((x.array() == x.array())).all();
145			}
146
147			/**
148			* Write the specified matrix to a binary file with the specified name.
149			*/
150			template <class Matrix>
151			bool writeMatrixToFile(const std::string& filename,
152			const Eigen::MatrixBase<Matrix>& matrix) {
153			typedef typename Matrix::Index Index;
154			typedef typename Matrix::Scalar Scalar;
155
156			std::ofstream out(filename.c_str(),
157			std::ios::out \| std::ios::binary \| std::ios::trunc);
158			if (!out.is_open()) return false;
159			Index rows = matrix.rows(), cols = matrix.cols();
160			out.write((char*)(&rows), sizeof(Index));
161			out.write((char*)(&cols), sizeof(Index));
162			out.write((char)matrix.data(), rows cols * sizeof(Scalar));
163			out.close();
164			return true;
165			}
166
167			/**
168			* Read a matrix from the specified input binary file.
169			*/
170			template <class Matrix>
171			bool readMatrixFromFile(const std::string& filename,
172			const Eigen::MatrixBase<Matrix>& matrix) {
173			typedef typename Matrix::Index Index;
174			typedef typename Matrix::Scalar Scalar;
175
176			std::ifstream in(filename.c_str(), std::ios::in \| std::ios::binary);
177			if (!in.is_open()) return false;
178			Index rows = 0, cols = 0;
179			in.read((char*)(&rows), sizeof(Index));
180			in.read((char*)(&cols), sizeof(Index));
181
182			Eigen::MatrixBase<Matrix>& matrix_ =
183			const_cast<Eigen::MatrixBase<Matrix>&>(matrix);
184
185			matrix_.resize(rows, cols);
186			in.read((char)matrix_.data(), rows cols * sizeof(Scalar));
187			in.close();
188			return true;
189			}
190
191			} // namespace math
192			} // namespace tsid
193
194			#endif // ifndef __invdyn_math_utils_hpp__
195