utils.hh

Go to the documentation of this file.

/*
 * Copyright 2018, LAAS-CNRS
 * Author: Steve Tonneau
 */
 
#ifndef BEZIER_COM_TRAJ_LIB_UTILS_H
#define BEZIER_COM_TRAJ_LIB_UTILS_H
 
#include <Eigen/Dense>
#include <hpp/bezier-com-traj/definitions.hh>
#include <hpp/bezier-com-traj/flags.hh>
#include <hpp/bezier-com-traj/local_config.hh>
#include <vector>
 
namespace bezier_com_traj {
 
template <typename T>
T initwp();
waypoint_t initwp(const size_t rows, const size_t cols);
waypoint_t operator+(const waypoint_t& w1, const waypoint_t& w2);
waypoint_t operator-(const waypoint_t& w1, const waypoint_t& w2);
waypoint_t operator*(const double k, const waypoint_t& w);
waypoint_t operator*(const waypoint_t& w, const double k);
 
struct waypoint_t {
  MatrixXX first;
  VectorX second;
 
  waypoint_t() : first(MatrixXX()), second(VectorX()) {}
 
  waypoint_t(MatrixXX A, VectorX b) : first(A), second(b) {}
 
  static waypoint_t Zero(size_t dim) { return initwp(dim, dim); }
 
  size_t size() const { return second.size(); }
 
  bool isApprox(const waypoint_t& other,
                const value_type prec =
                    Eigen::NumTraits<value_type>::dummy_precision()) const {
    return first.isApprox(other.first, prec) &&
           second.isApprox(other.second, prec);
  }
 
  bool operator==(const waypoint_t& other) const { return isApprox(other); }
 
  bool operator!=(const waypoint_t& other) const { return !(*this == other); }
};
 
BEZIER_COM_TRAJ_DLLAPI std::vector<ndcurves::Bern<double> >
ComputeBersteinPolynoms(const unsigned int degree);
 
template <typename Bezier, typename Point>
BEZIER_COM_TRAJ_DLLAPI Bezier computeBezierCurve(const ConstraintFlag& flag,
                                                 const double T,
                                                 const std::vector<Point>& pi,
                                                 const Point& x);
 
T_time computeDiscretizedTimeFixed(const VectorX& phaseTimings,
                                   const unsigned int pointsPerPhase);
 
T_time computeDiscretizedTime(const VectorX& phaseTimings,
                              const double timeStep);
 
void printQHullFile(const std::pair<MatrixXX, VectorX>& Ab, VectorX intPoint,
                    const std::string& fileName, bool clipZ = false);
 
BEZIER_COM_TRAJ_DLLAPI Matrix3 skew(point_t_tC x);
 
int Normalize(Ref_matrixXX A, Ref_vectorX b);
 
}  // end namespace bezier_com_traj
 
template <typename Bezier, typename Point>
Bezier bezier_com_traj::computeBezierCurve(const ConstraintFlag& flag,
                                           const double T,
                                           const std::vector<Point>& pi,
                                           const Point& x) {
  std::vector<Point> wps;
  size_t i = 0;
  if (flag & INIT_POS) {
    wps.push_back(pi[i]);
    i++;
    if (flag & INIT_VEL) {
      wps.push_back(pi[i]);
      i++;
      if (flag & INIT_ACC) {
        wps.push_back(pi[i]);
        i++;
      }
    }
  }
  wps.push_back(x);
  i++;
  if (flag & (END_VEL) && !(flag & (END_POS))) {
    wps.push_back(x);
    i++;
  } else {
    if (flag & END_ACC) {
      assert(flag & END_VEL &&
             "You cannot constrain final acceleration if final velocity is not "
             "constrained.");
      wps.push_back(pi[i]);
      i++;
    }
    if (flag & END_VEL) {
      assert(flag & END_POS &&
             "You cannot constrain final velocity if final position is not "
             "constrained.");
      wps.push_back(pi[i]);
      i++;
    }
    if (flag & END_POS) {
      wps.push_back(pi[i]);
      i++;
    }
  }
  return Bezier(wps.begin(), wps.end(), 0., T);
}
 
#endif