path.hh

Go to the documentation of this file.

//
// Copyright (c) 2014 CNRS
// Authors: Florent Lamiraux
//
 
// 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_HH
#define HPP_CORE_PATH_HH
 
#include <hpp/core/config.hh>
#include <hpp/core/constraint-set.hh>
#include <hpp/core/deprecated.hh>
#include <hpp/core/fwd.hh>
#include <hpp/core/projection-error.hh>
#include <hpp/core/time-parameterization.hh>
#include <hpp/util/exception.hh>
#include <hpp/util/serialization-fwd.hh>
 
namespace hpp {
namespace core {
 
class HPP_CORE_DLLAPI Path {
 public:
 
  virtual ~Path() {}
 
  virtual PathPtr_t copy() const = 0;
 
  virtual PathPtr_t copy(const ConstraintSetPtr_t& constraints) const = 0;
 
  template <class T>
  shared_ptr<T> as(void) {
    assert(HPP_DYNAMIC_PTR_CAST(T, weak_.lock()));
    return HPP_STATIC_PTR_CAST(T, weak_.lock());
  }
 
  template <class T>
  shared_ptr<const T> as(void) const {
    assert(HPP_DYNAMIC_PTR_CAST(const T, weak_.lock()));
    return HPP_STATIC_PTR_CAST(const T, weak_.lock());
  }
 
 
 
  PathPtr_t extract(const interval_t& subInterval) const;
 
  PathPtr_t extract(const value_type& tmin, const value_type& tmax) const {
    return extract(std::make_pair(tmin, tmax));
  }
 
  virtual PathPtr_t reverse() const;
 
 
 
  Configuration_t eval(const value_type& time, bool& success) const {
    return configAtParam(paramAtTime(time), success);
  }
 
  bool eval(ConfigurationOut_t result, const value_type& time) const {
    value_type s = paramAtTime(time);
    bool success = impl_compute(result, s);
    if (!success) return false;
    return applyConstraints(result, s);
  }
 
  bool at(const value_type& time, ConfigurationOut_t result) const {
    return impl_compute(result, paramAtTime(time));
  }
 
  void derivative(vectorOut_t result, const value_type& time,
                  size_type order) const;
 
  void velocityBound(vectorOut_t result, const value_type& t0,
                     const value_type& t1) const {
    assert(result.size() == outputDerivativeSize());
    assert(t0 <= t1);
    impl_velocityBound(result, paramAtTime(std::max(t0, timeRange().first)),
                       paramAtTime(std::min(t1, timeRange().second)));
    if (timeParam_) result *= timeParam_->derivativeBound(t0, t1);
  }
 
 
  size_type outputSize() const { return outputSize_; }
 
  size_type outputDerivativeSize() const { return outputDerivativeSize_; }
 
  const interval_t& timeRange() const { return timeRange_; }
 
  virtual value_type length() const {
    return timeRange_.second - timeRange_.first;
  }
 
  virtual Configuration_t initial() const = 0;
 
  virtual Configuration_t end() const = 0;
 
  const ConstraintSetPtr_t& constraints() const { return constraints_; }
 
 
 
  const interval_t& paramRange() const { return paramRange_; }
 
  const TimeParameterizationPtr_t& timeParameterization() const {
    return timeParam_;
  }
 
  void timeParameterization(const TimeParameterizationPtr_t& tp,
                            const interval_t& tr) {
    timeParam_ = tp;
    timeRange(tr);
  }
 
 
 protected:
  virtual std::ostream& print(std::ostream& os) const;
 
  Path(const interval_t& interval, size_type outputSize,
       size_type outputDerivativeSize, const ConstraintSetPtr_t& constraints);
 
  Path(const interval_t& interval, size_type outputSize,
       size_type outputDerivativeSize);
 
  Path(const Path& path);
 
  Path(const Path& path, const ConstraintSetPtr_t& constraints);
 
  void init(const PathWkPtr_t& self);
 
  interval_t paramRange_;
 
  void constraints(const ConstraintSetPtr_t& constraint) {
    constraints_ = constraint;
  }
 
  virtual void checkPath() const;
 
  void timeRange(const interval_t& timeRange) {
    timeRange_ = timeRange;
    if (timeParam_) {
      paramRange_.first = timeParam_->value(timeRange_.first);
      paramRange_.second = timeParam_->value(timeRange_.second);
    } else
      paramRange_ = timeRange_;
  }
 
  value_type paramLength() const {
    return paramRange_.second - paramRange_.first;
  }
 
  Configuration_t configAtParam(const value_type& param, bool& success) const {
    Configuration_t result(outputSize());
    success = impl_compute(result, param);
    if (!success) return result;
    success = applyConstraints(result, param);
    return result;
  }
 
  virtual bool impl_compute(ConfigurationOut_t configuration,
                            value_type param) const = 0;
 
  virtual void impl_derivative(vectorOut_t derivative, const value_type& param,
                               size_type order) const {
    (void)derivative;
    (void)param;
    (void)order;
    HPP_THROW_EXCEPTION(hpp::Exception, "not implemented");
  }
 
  virtual void impl_velocityBound(vectorOut_t bound, const value_type& param0,
                                  const value_type& param1) const {
    (void)bound;
    (void)param0;
    (void)param1;
    HPP_THROW_EXCEPTION(hpp::Exception, "not implemented");
  }
 
  virtual PathPtr_t impl_extract(const interval_t& paramInterval) const;
 
 private:
  interval_t timeRange_;
 
  value_type paramAtTime(const value_type& time) const {
    if (timeParam_) {
      return timeParam_->value(time);
    }
    return time;
  }
 
  bool applyConstraints(ConfigurationOut_t result,
                        const value_type& param) const;
 
  size_type outputSize_;
  size_type outputDerivativeSize_;
  ConstraintSetPtr_t constraints_;
  TimeParameterizationPtr_t timeParam_;
  PathWkPtr_t weak_;
  friend std::ostream& operator<<(std::ostream& os, const Path& path);
  friend class ExtractedPath;
 
 protected:
  Path() {}
 
 private:
  HPP_SERIALIZABLE();
};  // class Path
inline std::ostream& operator<<(std::ostream& os, const Path& path) {
  return path.print(os);
}
 
}  //   namespace core
}  // namespace hpp
#endif  // HPP_CORE_PATH_HH