utils.hpp

Go to the documentation of this file.

//
// Copyright (c) 2017 CNRS
//

#ifndef __invdyn_math_utils_hpp__
#define __invdyn_math_utils_hpp__

#include "tsid/math/fwd.hpp"

#include <pinocchio/spatial/se3.hpp>
#include <pinocchio/spatial/explog.hpp>

#include <iostream>
#include <fstream>
#include <vector>

#define PRINT_VECTOR(a) \
 std::cout << #a << "(" << a.rows() << "x" << a.cols() \
 << "): " << a.transpose().format(math::CleanFmt) << std::endl
#define PRINT_MATRIX(a) \
 std::cout << #a << "(" << a.rows() << "x" << a.cols() << "):\n" \
 << a.format(math::CleanFmt) << std::endl

namespace tsid {
template <typename T>
std::string toString(const T& v) {
 std::stringstream ss;
 ss << v;
 return ss.str();
}

template <typename T>
std::string toString(const std::vector<T>& v,
 const std::string separator = ", ") {
 std::stringstream ss;
 for (int i = 0; i < v.size() - 1; i++) ss << v[i] << separator;
 ss << v[v.size() - 1];
 return ss.str();
}

template <typename T, int n>
std::string toString(const Eigen::MatrixBase<T>& v,
 const std::string separator = ", ") {
 if (v.rows() > v.cols()) return toString(v.transpose(), separator);
 std::stringstream ss;
 ss << v;
 return ss.str();
}
} // namespace tsid

namespace tsid {
namespace math {
static const Eigen::IOFormat CleanFmt(1, 0, ", ", "\n", "[", "]");

static const Eigen::IOFormat matlabPrintFormat(Eigen::FullPrecision,
 Eigen::DontAlignCols, " ", ";\n",
 "", "", "[", "];");

void SE3ToXYZQUAT(const pinocchio::SE3& M, RefVector xyzQuat);

void SE3ToVector(const pinocchio::SE3& M, RefVector vec);

void vectorToSE3(RefVector vec, pinocchio::SE3& M);

void errorInSE3(const pinocchio::SE3& M, const pinocchio::SE3& Mdes,
 pinocchio::Motion& error);

void solveWithDampingFromSvd(Eigen::JacobiSVD<Eigen::MatrixXd>& svd,
 ConstRefVector b, RefVector sol,
 double damping = 0.0);

void svdSolveWithDamping(ConstRefMatrix A, ConstRefVector b, RefVector sol,
 double damping = 0.0);

void pseudoInverse(ConstRefMatrix A, RefMatrix Apinv, double tolerance,
 unsigned int computationOptions = Eigen::ComputeThinU |
 Eigen::ComputeThinV);

void pseudoInverse(ConstRefMatrix A,
 Eigen::JacobiSVD<Eigen::MatrixXd>& svdDecomposition,
 RefMatrix Apinv, double tolerance,
 unsigned int computationOptions);

void pseudoInverse(ConstRefMatrix A,
 Eigen::JacobiSVD<Eigen::MatrixXd>& svdDecomposition,
 RefMatrix Apinv, double tolerance, double* nullSpaceBasisOfA,
 int& nullSpaceRows, int& nullSpaceCols,
 unsigned int computationOptions);

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);

void nullSpaceBasisFromDecomposition(
 const Eigen::JacobiSVD<Eigen::MatrixXd>& svdDecomposition, double tolerance,
 double* nullSpaceBasisMatrix, int& rows, int& cols);

void nullSpaceBasisFromDecomposition(
 const Eigen::JacobiSVD<Eigen::MatrixXd>& svdDecomposition, int rank,
 double* nullSpaceBasisMatrix, int& rows, int& cols);

template <typename Derived>
inline bool isFinite(const Eigen::MatrixBase<Derived>& x) {
 return ((x - x).array() == (x - x).array()).all();
}

template <typename Derived>
inline bool is_nan(const Eigen::MatrixBase<Derived>& x) {
 return ((x.array() == x.array())).all();
}

template <class Matrix>
bool writeMatrixToFile(const std::string& filename,
 const Eigen::MatrixBase<Matrix>& matrix) {
 typedef typename Matrix::Index Index;
 typedef typename Matrix::Scalar Scalar;

 std::ofstream out(filename.c_str(),
 std::ios::out | std::ios::binary | std::ios::trunc);
 if (!out.is_open()) return false;
 Index rows = matrix.rows(), cols = matrix.cols();
 out.write((char*)(&rows), sizeof(Index));
 out.write((char*)(&cols), sizeof(Index));
 out.write((char*)matrix.data(), rows * cols * sizeof(Scalar));
 out.close();
 return true;
}

template <class Matrix>
bool readMatrixFromFile(const std::string& filename,
 const Eigen::MatrixBase<Matrix>& matrix) {
 typedef typename Matrix::Index Index;
 typedef typename Matrix::Scalar Scalar;

 std::ifstream in(filename.c_str(), std::ios::in | std::ios::binary);
 if (!in.is_open()) return false;
 Index rows = 0, cols = 0;
 in.read((char*)(&rows), sizeof(Index));
 in.read((char*)(&cols), sizeof(Index));

 Eigen::MatrixBase<Matrix>& matrix_ =
 const_cast<Eigen::MatrixBase<Matrix>&>(matrix);

 matrix_.resize(rows, cols);
 in.read((char*)matrix_.data(), rows * cols * sizeof(Scalar));
 in.close();
 return true;
}

} // namespace math
} // namespace tsid

#endif // ifndef __invdyn_math_utils_hpp__