Go to the documentation of this file.
18 #ifndef __invdyn_math_utils_hpp__
19 #define __invdyn_math_utils_hpp__
23 #include <pinocchio/spatial/se3.hpp>
24 #include <pinocchio/spatial/explog.hpp>
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
44 std::string
toString(
const std::vector<T>& v,
const std::string separator=
", ")
47 for(
int i=0; i<v.size()-1; i++)
53 template<
typename T,
int n>
54 std::string
toString(
const Eigen::MatrixBase<T>& v,
const std::string separator=
", ")
57 return toString(v.transpose(), separator);
68 static const Eigen::IOFormat CleanFmt(1, 0,
", ",
"\n",
"[",
"]");
79 static const Eigen::IOFormat matlabPrintFormat(Eigen::FullPrecision, Eigen::DontAlignCols,
" ",
";\n",
"",
"",
"[",
"];");
95 pinocchio::Motion & error);
107 unsigned int computationOptions = Eigen::ComputeThinU | Eigen::ComputeThinV);
110 Eigen::JacobiSVD<Eigen::MatrixXd>& svdDecomposition,
113 unsigned int computationOptions);
116 Eigen::JacobiSVD<Eigen::MatrixXd>& svdDecomposition,
119 double * nullSpaceBasisOfA,
122 unsigned int computationOptions);
125 Eigen::JacobiSVD<Eigen::MatrixXd>& svdDecomposition,
128 double dampingFactor,
129 unsigned int computationOptions = Eigen::ComputeThinU | Eigen::ComputeThinV,
130 double * nullSpaceBasisOfA=0,
131 int *nullSpaceRows=0,
int *nullSpaceCols=0);
135 double * nullSpaceBasisMatrix,
136 int &rows,
int &cols);
140 double * nullSpaceBasisMatrix,
141 int &rows,
int &cols);
143 template<
typename Derived>
144 inline bool isFinite(
const Eigen::MatrixBase<Derived>& x)
146 return ( (x - x).array() == (x - x).array()).all();
149 template<
typename Derived>
150 inline bool is_nan(
const Eigen::MatrixBase<Derived>& x)
152 return ((x.array() == x.array())).all();
158 template<
class Matrix>
160 const Eigen::MatrixBase<Matrix> & matrix)
165 std::ofstream out(filename.c_str(), std::ios::out | std::ios::binary | std::ios::trunc);
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) );
179 template<
class Matrix>
181 const Eigen::MatrixBase<Matrix> & matrix)
186 std::ifstream in(filename.c_str(), std::ios::in | std::ios::binary);
189 Index rows=0, cols=0;
190 in.read((
char*) (&rows),
sizeof(
Index));
191 in.read((
char*) (&cols),
sizeof(
Index));
193 Eigen::MatrixBase<Matrix> & matrix_ =
const_cast< Eigen::MatrixBase<Matrix>&
>(matrix);
195 matrix_.resize(rows, cols);
196 in.read( (
char *) matrix_.data() , rows*cols*
sizeof(
Scalar) );
204 #endif // ifndef __invdyn_math_utils_hpp__
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
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
Eigen::Ref< Matrix > RefMatrix
Definition: fwd.hpp:52
void pseudoInverse(ConstRefMatrix A, RefMatrix Apinv, double tolerance, unsigned int computationOptions=Eigen::ComputeThinU|Eigen::ComputeThinV)
Definition: utils.cpp:90
pinocchio::SE3 SE3
Definition: trajectory-base.hpp:34
std::size_t Index
Definition: fwd.hpp:55
Eigen::Ref< Vector > RefVector
Definition: fwd.hpp:49
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
const typedef Eigen::Ref< const Vector > ConstRefVector
Definition: fwd.hpp:50
void SE3ToVector(const pinocchio::SE3 &M, RefVector vec)
Definition: utils.cpp:32
void svdSolveWithDamping(ConstRefMatrix A, ConstRefVector b, RefVector sol, double damping=0.0)
Definition: utils.cpp:80
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
Definition: constraint-bound.hpp:26
void SE3ToXYZQUAT(const pinocchio::SE3 &M, RefVector xyzQuat)
Definition: utils.cpp:25
const typedef Eigen::Ref< const Matrix > ConstRefMatrix
Definition: fwd.hpp:53
bool writeMatrixToFile(const std::string &filename, const Eigen::MatrixBase< Matrix > &matrix)
Definition: utils.hpp:159
std::string toString(const T &v)
Definition: utils.hpp:36
void nullSpaceBasisFromDecomposition(const Eigen::JacobiSVD< Eigen::MatrixXd > &svdDecomposition, double tolerance, double *nullSpaceBasisMatrix, int &rows, int &cols)
Definition: utils.cpp:185