spline.hh

Go to the documentation of this file.

// Copyright (c) 2017 CNRS
// Authors: Joseph Mirabel
//
 
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
// DAMAGE.
 
#ifndef HPP_CORE_PATH_SPLINE_HH
#define HPP_CORE_PATH_SPLINE_HH
 
#include <hpp/core/config.hh>
#include <hpp/core/fwd.hh>
#include <hpp/core/path.hh>
#include <hpp/core/steering-method/fwd.hh>
#include <hpp/pinocchio/device.hh>
#include <hpp/pinocchio/liegroup-element.hh>
 
namespace hpp {
namespace core {
namespace path {
 
enum PolynomeBasisType { CanonicalPolynomeBasis, BernsteinBasis };
 
namespace internal {
template <int SplineType, int Degree>
struct spline_basis_function;
template <int SplineType, int Degree>
struct sbf_traits {
  enum { NbCoeffs = Degree + 1 };
  typedef Eigen::Matrix<value_type, NbCoeffs, 1> Coeffs_t;
  typedef Eigen::Matrix<value_type, NbCoeffs, NbCoeffs> IntegralCoeffs_t;
};
}  // namespace internal
 
template <int _PolynomeBasis, int _Order>
class HPP_CORE_DLLAPI Spline : public Path {
 public:
  enum {
    PolynomeBasis = _PolynomeBasis,
    Order = _Order,
    NbCoeffs = _Order + 1,
    NbPowerOfT = 2 * NbCoeffs + 1
  };
 
  typedef internal::sbf_traits<PolynomeBasis, Order> sbf_traits;
  typedef internal::spline_basis_function<PolynomeBasis, Order> BasisFunction_t;
  typedef Eigen::Matrix<value_type, NbPowerOfT, 1> PowersOfT_t;
  typedef typename sbf_traits::Coeffs_t BasisFunctionVector_t;
  typedef typename sbf_traits::IntegralCoeffs_t BasisFunctionIntegralMatrix_t;
 
  typedef Eigen::Matrix<value_type, NbCoeffs, Eigen::Dynamic, Eigen::RowMajor>
      ParameterMatrix_t;
  typedef Eigen::Map<const vector_t, Eigen::Aligned> ConstParameterVector_t;
  typedef Eigen::Map<vector_t, Eigen::Aligned> ParameterVector_t;
 
  typedef shared_ptr<Spline> Ptr_t;
  typedef weak_ptr<Spline> WkPtr_t;
 
  size_type parameterSize() const { return parameterSize_; }
 
  void parameterDerivativeCoefficients(vectorOut_t res,
                                       const value_type& t) const {
    assert(res.size() == NbCoeffs);
    impl_paramDerivative(res, t);
  }
 
  void parameterIntegrate(vectorIn_t dParam) {
    assert(dParam.size() == NbCoeffs * parameterSize_);
    impl_paramIntegrate(dParam);
  }
 
  value_type squaredNormIntegral(const size_type order) const;
 
  void squaredNormIntegralDerivative(const size_type order,
                                     vectorOut_t res) const;
 
  static void timeFreeBasisFunctionDerivative(const size_type order,
                                              const value_type& u,
                                              BasisFunctionVector_t& res);
 
  static void timeFreeBasisFunctionDerivative(const size_type order,
                                              const value_type& u,
                                              vectorOut_t res) {
    assert(res.size() == NbCoeffs);
    BasisFunctionVector_t tmp;
    timeFreeBasisFunctionDerivative(order, u, tmp);
    res = tmp;
  }
 
  void basisFunctionDerivative(const size_type order, const value_type& u,
                               BasisFunctionVector_t& res) const;
 
  void basisFunctionDerivative(const size_type order, const value_type& u,
                               vectorOut_t res) const {
    assert(res.size() == NbCoeffs);
    BasisFunctionVector_t tmp;
    basisFunctionDerivative(order, u, tmp);
    res = tmp;
  }
 
  void maxVelocity(vectorOut_t res) const;
 
  void squaredNormBasisFunctionIntegral(
      const size_type order, BasisFunctionIntegralMatrix_t& res) const;
 
  void squaredNormBasisFunctionIntegral(const size_type order,
                                        matrixOut_t res) const {
    // assert (res.size() == NbCoeffs);
    BasisFunctionIntegralMatrix_t tmp;
    squaredNormBasisFunctionIntegral(order, tmp);
    res = tmp;
  }
 
  virtual Configuration_t initial() const {
    Configuration_t q(outputSize());
    bool res = eval(q, timeRange().first);
    assert(res);
    (void)res;
    return q;
  }
 
  virtual Configuration_t end() const {
    Configuration_t q(outputSize());
    bool res = eval(q, timeRange().second);
    assert(res);
    (void)res;
    return q;
  }
 
  const Configuration_t& base() const { return base_.vector(); }
 
  void base(const Configuration_t& q) { base_.vector() = q; }
 
  const ParameterMatrix_t& parameters() const { return parameters_; }
 
  void parameters(const ParameterMatrix_t& m) { parameters_ = m; }
 
  ConstParameterVector_t rowParameters() const {
    return ConstParameterVector_t(parameters_.data(), parameters_.size());
  }
 
  void rowParameters(vectorIn_t p) {
    ParameterVector_t(parameters_.data(), parameters_.size()) = p;
  }
 
  PathPtr_t copy() const {
    Ptr_t other(new Spline(*this));
    other->init(other);
    return other;
  }
 
  PathPtr_t copy(const ConstraintSetPtr_t& constraints) const {
    Ptr_t other(new Spline(*this, constraints));
    other->init(other);
    return other;
  }
 
  virtual ~Spline() {}
 
  static Ptr_t create(const DevicePtr_t& robot, const interval_t& interval,
                      const ConstraintSetPtr_t& constraints,
                      bool se3Output = false) {
    Ptr_t shPtr(new Spline(robot, interval, constraints, se3Output));
    shPtr->init(shPtr);
    return shPtr;
  }
 
  static void value(pinocchio::LiegroupElementConstRef base,
                    Eigen::Ref<const ParameterMatrix_t> params,
                    const value_type& u, ConfigurationOut_t config,
                    vectorOut_t velocity);
 
 protected:
  Spline(const DevicePtr_t& robot, const interval_t& interval,
         const ConstraintSetPtr_t& constraints, bool se3Output = false)
      : Path(interval, robot->configSize(), robot->numberDof(), constraints),
        parameterSize_(robot->numberDof()),
        robot_(robot),
        base_(robot->RnxSOnConfigSpace()->vectorSpacesMerged()),
        parameters_((int)NbCoeffs, parameterSize_),
        velocity_(parameterSize_) {
    powersOfT_(0) = 1;
    for (size_type i = 1; i < NbPowerOfT; ++i)
      powersOfT_(i) = powersOfT_(i - 1) * length();
    if (se3Output) base_ = robot->configSpace()->vectorSpacesMerged();
  }
 
  Spline(const Spline& path);
 
  Spline(const Spline& path, const ConstraintSetPtr_t& constraints);
 
  void init(const Ptr_t& self) {
    Path::init(self);
    weak_ = self;
  }
 
  std::ostream& print(std::ostream& os) const;
 
  bool impl_compute(ConfigurationOut_t configuration, value_type t) const;
 
  void impl_derivative(vectorOut_t res, const value_type& t,
                       size_type order) const;
 
  void impl_paramDerivative(vectorOut_t res, const value_type& t) const;
 
  void impl_paramIntegrate(vectorIn_t dParam);
 
  void impl_velocityBound(vectorOut_t result, const value_type& t0,
                          const value_type& t1) const;
 
  size_type parameterSize_;
  DevicePtr_t robot_;
  LiegroupElement base_;
  ParameterMatrix_t parameters_;
 
 private:
  WkPtr_t weak_;
 
  mutable vector_t velocity_;
  mutable PowersOfT_t powersOfT_;
 
  friend class steeringMethod::Spline<_PolynomeBasis, _Order>;
 
 private:
  Spline() {}
  HPP_SERIALIZABLE();
};  // class Spline
}  //   namespace path
}  //   namespace core
}  // namespace hpp
#endif  // HPP_CORE_PATH_SPLINE_HH