Loading...
Searching...
No Matches
utils.hpp
Go to the documentation of this file.
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) std::cout<<#a<<"("<<a.rows()<<"x"<<a.cols()<<"): "<<a.transpose().format(math::CleanFmt)<<std::endl
31#define PRINT_MATRIX(a) std::cout<<#a<<"("<<a.rows()<<"x"<<a.cols()<<"):\n"<<a.format(math::CleanFmt)<<std::endl
32
33namespace tsid
34{
35 template<typename T>
36 std::string toString(const T& v)
37 {
38 std::stringstream ss;
39 ss<<v;
40 return ss.str();
41 }
42
43 template<typename T>
44 std::string toString(const std::vector<T>& v, const std::string separator=", ")
45 {
46 std::stringstream ss;
47 for(int i=0; i<v.size()-1; i++)
48 ss<<v[i]<<separator;
49 ss<<v[v.size()-1];
50 return ss.str();
51 }
52
53 template<typename T, int n>
54 std::string toString(const Eigen::MatrixBase<T>& v, const std::string separator=", ")
55 {
56 if(v.rows()>v.cols())
57 return toString(v.transpose(), separator);
58 std::stringstream ss;
59 ss<<v;
60 return ss.str();
61 }
62}
63
64namespace tsid
65{
66 namespace math
67 {
68 static const Eigen::IOFormat CleanFmt(1, 0, ", ", "\n", "[", "]");
69
79 static const Eigen::IOFormat matlabPrintFormat(Eigen::FullPrecision, Eigen::DontAlignCols, " ", ";\n", "", "", "[", "];");
80
84 void SE3ToXYZQUAT(const pinocchio::SE3 & M, RefVector xyzQuat);
85
89 void SE3ToVector(const pinocchio::SE3 & M, RefVector vec);
90
91 void vectorToSE3(RefVector vec, pinocchio::SE3 & M);
92
93 void errorInSE3 (const pinocchio::SE3 & M,
94 const pinocchio::SE3 & Mdes,
95 pinocchio::Motion & error);
96
97 void solveWithDampingFromSvd(Eigen::JacobiSVD<Eigen::MatrixXd> & svd,
99 RefVector sol, double damping=0.0);
100
102 RefVector sol, double damping=0.0);
103
105 RefMatrix Apinv,
106 double tolerance,
107 unsigned int computationOptions = Eigen::ComputeThinU | Eigen::ComputeThinV);
108
110 Eigen::JacobiSVD<Eigen::MatrixXd>& svdDecomposition,
111 RefMatrix Apinv,
112 double tolerance,
113 unsigned int computationOptions);
114
116 Eigen::JacobiSVD<Eigen::MatrixXd>& svdDecomposition,
117 RefMatrix Apinv,
118 double tolerance,
119 double * nullSpaceBasisOfA,
120 int &nullSpaceRows,
121 int &nullSpaceCols,
122 unsigned int computationOptions);
123
125 Eigen::JacobiSVD<Eigen::MatrixXd>& svdDecomposition,
126 RefMatrix Apinv,
127 double tolerance,
128 double dampingFactor,
129 unsigned int computationOptions = Eigen::ComputeThinU | Eigen::ComputeThinV,
130 double * nullSpaceBasisOfA=0,
131 int *nullSpaceRows=0, int *nullSpaceCols=0);
132
133 void nullSpaceBasisFromDecomposition(const Eigen::JacobiSVD<Eigen::MatrixXd> & svdDecomposition,
134 double tolerance,
135 double * nullSpaceBasisMatrix,
136 int &rows, int &cols);
137
138 void nullSpaceBasisFromDecomposition(const Eigen::JacobiSVD<Eigen::MatrixXd> & svdDecomposition,
139 int rank,
140 double * nullSpaceBasisMatrix,
141 int &rows, int &cols);
142
143 template<typename Derived>
144 inline bool isFinite(const Eigen::MatrixBase<Derived>& x)
145 {
146 return ( (x - x).array() == (x - x).array()).all();
147 }
148
149 template<typename Derived>
150 inline bool is_nan(const Eigen::MatrixBase<Derived>& x)
151 {
152 return ((x.array() == x.array())).all();
153 }
154
158 template<class Matrix>
159 bool writeMatrixToFile(const std::string & filename,
160 const Eigen::MatrixBase<Matrix> & matrix)
161 {
162 typedef typename Matrix::Index Index;
163 typedef typename Matrix::Scalar Scalar;
164
165 std::ofstream out(filename.c_str(), std::ios::out | std::ios::binary | std::ios::trunc);
166 if(!out.is_open())
167 return false;
168 Index rows=matrix.rows(), cols=matrix.cols();
169 out.write((char*) (&rows), sizeof(Index));
170 out.write((char*) (&cols), sizeof(Index));
171 out.write((char*) matrix.data(), rows*cols*sizeof(Scalar) );
172 out.close();
173 return true;
174 }
175
179 template<class Matrix>
180 bool readMatrixFromFile(const std::string & filename,
181 const Eigen::MatrixBase<Matrix> & matrix)
182 {
183 typedef typename Matrix::Index Index;
184 typedef typename Matrix::Scalar Scalar;
185
186 std::ifstream in(filename.c_str(), std::ios::in | std::ios::binary);
187 if(!in.is_open())
188 return false;
189 Index rows=0, cols=0;
190 in.read((char*) (&rows),sizeof(Index));
191 in.read((char*) (&cols),sizeof(Index));
192
193 Eigen::MatrixBase<Matrix> & matrix_ = const_cast< Eigen::MatrixBase<Matrix>& >(matrix);
194
195 matrix_.resize(rows, cols);
196 in.read( (char *) matrix_.data() , rows*cols*sizeof(Scalar) );
197 in.close();
198 return true;
199 }
200
201 }
202}
203
204#endif // ifndef __invdyn_math_utils_hpp__
Eigen::Ref< Matrix > RefMatrix
Definition: fwd.hpp:52
void nullSpaceBasisFromDecomposition(const Eigen::JacobiSVD< Eigen::MatrixXd > &svdDecomposition, double tolerance, double *nullSpaceBasisMatrix, int &rows, int &cols)
Definition: utils.cpp:185
void svdSolveWithDamping(ConstRefMatrix A, ConstRefVector b, RefVector sol, double damping=0.0)
Definition: utils.cpp:80
std::size_t Index
Definition: fwd.hpp:55
bool is_nan(const Eigen::MatrixBase< Derived > &x)
Definition: utils.hpp:150
void vectorToSE3(RefVector vec, pinocchio::SE3 &M)
Definition: utils.cpp:40
void pseudoInverse(ConstRefMatrix A, RefMatrix Apinv, double tolerance, unsigned int computationOptions=Eigen::ComputeThinU|Eigen::ComputeThinV)
Definition: utils.cpp:90
bool isFinite(const Eigen::MatrixBase< Derived > &x)
Definition: utils.hpp:144
void solveWithDampingFromSvd(Eigen::JacobiSVD< Eigen::MatrixXd > &svd, ConstRefVector b, RefVector sol, double damping=0.0)
Definition: utils.cpp:59
void SE3ToXYZQUAT(const pinocchio::SE3 &M, RefVector xyzQuat)
Definition: utils.cpp:25
const Eigen::Ref< const Matrix > ConstRefMatrix
Definition: fwd.hpp:53
const Eigen::Ref< const Vector > ConstRefVector
Definition: fwd.hpp:50
bool readMatrixFromFile(const std::string &filename, const Eigen::MatrixBase< Matrix > &matrix)
Definition: utils.hpp:180
bool writeMatrixToFile(const std::string &filename, const Eigen::MatrixBase< Matrix > &matrix)
Definition: utils.hpp:159
void dampedPseudoInverse(ConstRefMatrix A, Eigen::JacobiSVD< Eigen::MatrixXd > &svdDecomposition, RefMatrix Apinv, double tolerance, double dampingFactor, unsigned int computationOptions=Eigen::ComputeThinU|Eigen::ComputeThinV, double *nullSpaceBasisOfA=0, int *nullSpaceRows=0, int *nullSpaceCols=0)
Definition: utils.cpp:146
double Scalar
Definition: fwd.hpp:36
void errorInSE3(const pinocchio::SE3 &M, const pinocchio::SE3 &Mdes, pinocchio::Motion &error)
Definition: utils.cpp:48
Eigen::Ref< Vector > RefVector
Definition: fwd.hpp:49
void SE3ToVector(const pinocchio::SE3 &M, RefVector vec)
Definition: utils.cpp:32
Definition: constraint-bound.hpp:27
std::string toString(const T &v)
Definition: utils.hpp:36