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 
33 namespace 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 
64 namespace 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__
std::size_t Index
Definition: fwd.hpp:55
Eigen::Ref< Vector > RefVector
Definition: fwd.hpp:49
const Eigen::Ref< const Matrix > ConstRefMatrix
Definition: fwd.hpp:53
void SE3ToVector(const pinocchio::SE3 &M, RefVector vec)
Definition: utils.cpp:32
std::string toString(const T &v)
Definition: utils.hpp:36
bool readMatrixFromFile(const std::string &filename, const Eigen::MatrixBase< Matrix > &matrix)
Definition: utils.hpp:180
bool is_nan(const Eigen::MatrixBase< Derived > &x)
Definition: utils.hpp:150
void solveWithDampingFromSvd(Eigen::JacobiSVD< Eigen::MatrixXd > &svd, ConstRefVector b, RefVector sol, double damping=0.0)
Definition: utils.cpp:59
double Scalar
Definition: fwd.hpp:36
pinocchio::SE3 SE3
Definition: trajectory-base.hpp:34
void svdSolveWithDamping(ConstRefMatrix A, ConstRefVector b, RefVector sol, double damping=0.0)
Definition: utils.cpp:80
bool writeMatrixToFile(const std::string &filename, const Eigen::MatrixBase< Matrix > &matrix)
Definition: utils.hpp:159
void SE3ToXYZQUAT(const pinocchio::SE3 &M, RefVector xyzQuat)
Definition: utils.cpp:25
void nullSpaceBasisFromDecomposition(const Eigen::JacobiSVD< Eigen::MatrixXd > &svdDecomposition, double tolerance, double *nullSpaceBasisMatrix, int &rows, int &cols)
Definition: utils.cpp:185
bool isFinite(const Eigen::MatrixBase< Derived > &x)
Definition: utils.hpp:144
void errorInSE3(const pinocchio::SE3 &M, const pinocchio::SE3 &Mdes, pinocchio::Motion &error)
Definition: utils.cpp:48
const Eigen::Ref< const Vector > ConstRefVector
Definition: fwd.hpp:50
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
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
Eigen::Ref< Matrix > RefMatrix
Definition: fwd.hpp:52
Definition: constraint-bound.hpp:26