30#ifndef HPP_CORE_PATH_HH
31#define HPP_CORE_PATH_HH
39#include <hpp/util/exception.hh>
40#include <hpp/util/serialization-fwd.hh>
90 shared_ptr<T>
as(
void) {
91 assert(HPP_DYNAMIC_PTR_CAST(T, weak_.lock()));
92 return HPP_STATIC_PTR_CAST(T, weak_.lock());
97 shared_ptr<const T>
as(
void)
const {
98 assert(HPP_DYNAMIC_PTR_CAST(
const T, weak_.lock()));
99 return HPP_STATIC_PTR_CAST(
const T, weak_.lock());
117 return extract(std::make_pair(tmin, tmax));
132 bool& success)
const {
133 return eval(time, success);
139 return eval(result, time);
144 return configAtParam(paramAtTime(time), success);
150 bool success = impl_compute(result, s);
151 if (!success)
return false;
152 return applyConstraints(result, s);
157 return impl_compute(result, paramAtTime(time));
186 assert(result.size() == outputDerivativeSize());
188 impl_velocityBound(result, paramAtTime(std::max(t0, timeRange().first)),
189 paramAtTime(std::min(t1, timeRange().second)));
190 if (timeParam_) result *= timeParam_->derivativeBound(t0, t1);
207 return timeRange_.second - timeRange_.first;
244 virtual std::ostream&
print(std::ostream& os)
const;
275 void init(
const PathWkPtr_t& self);
285 constraints_ = constraint;
292 timeRange_ = timeRange;
294 paramRange_.first = timeParam_->value(timeRange_.first);
295 paramRange_.second = timeParam_->value(timeRange_.second);
297 paramRange_ = timeRange_;
305 return paramRange_.second - paramRange_.first;
310 success = impl_compute(result, param);
311 if (!success)
return result;
312 success = applyConstraints(result, param);
331 HPP_THROW_EXCEPTION(hpp::Exception,
"not implemented");
342 HPP_THROW_EXCEPTION(hpp::Exception,
"not implemented");
354 return timeParam_->value(time);
359 bool applyConstraints(ConfigurationOut_t result,
360 const value_type& param)
const;
363 size_type outputSize_;
365 size_type outputDerivativeSize_;
367 ConstraintSetPtr_t constraints_;
369 TimeParameterizationPtr_t timeParam_;
372 friend std::ostream& operator<<(std::ostream& os,
const Path& path);
373 friend class ExtractedPath;
382 return path.
print(os);
Configuration_t eval(const value_type &time, bool &success) const
Configuration at time.
Definition: path.hh:143
HPP_CORE_DEPRECATED Configuration_t operator()(const value_type &time, bool &success) const
Definition: path.hh:131
size_type outputSize() const
Get size of configuration space.
Definition: path.hh:197
void init(const PathWkPtr_t &self)
virtual std::ostream & print(std::ostream &os) const
value_type paramLength() const
Definition: path.hh:304
size_type outputDerivativeSize() const
Get size of velocity.
Definition: path.hh:200
virtual ~Path()
Destructor.
Definition: path.hh:77
const interval_t & paramRange() const
Definition: path.hh:230
virtual void impl_derivative(vectorOut_t derivative, const value_type ¶m, size_type order) const
Definition: path.hh:326
PathPtr_t extract(const interval_t &subInterval) const
shared_ptr< T > as(void)
Static cast into a derived type.
Definition: path.hh:90
virtual bool impl_compute(ConfigurationOut_t configuration, value_type param) const =0
Function evaluation without applying constraints.
void timeParameterization(const TimeParameterizationPtr_t &tp, const interval_t &tr)
Set the time parameterization function.
Definition: path.hh:233
shared_ptr< const T > as(void) const
Static cast into a derived type.
Definition: path.hh:97
bool operator()(ConfigurationOut_t result, const value_type &time) const
Definition: path.hh:138
virtual PathPtr_t impl_extract(const interval_t ¶mInterval) const
Virtual implementation of extract.
Path(const interval_t &interval, size_type outputSize, size_type outputDerivativeSize)
bool at(const value_type &time, ConfigurationOut_t result) const
Get the configuration at a parameter without applying the constraints.
Definition: path.hh:156
interval_t paramRange_
Interval of parameters.
Definition: path.hh:278
virtual PathPtr_t copy() const =0
Return a shared pointer to a copy of this.
PathPtr_t extract(const value_type &tmin, const value_type &tmax) const
Definition: path.hh:116
virtual PathPtr_t reverse() const
virtual PathPtr_t copy(const ConstraintSetPtr_t &constraints) const =0
const TimeParameterizationPtr_t & timeParameterization() const
Definition: path.hh:300
void constraints(const ConstraintSetPtr_t &constraint)
Definition: path.hh:284
void velocityBound(vectorOut_t result, const value_type &t0, const value_type &t1) const
Definition: path.hh:184
const interval_t & timeRange() const
Get interval of definition.
Definition: path.hh:203
void derivative(vectorOut_t result, const value_type &time, size_type order) const
void timeRange(const interval_t &timeRange)
Definition: path.hh:291
Path(const Path &path, const ConstraintSetPtr_t &constraints)
Copy constructor with constraints.
Path()
Definition: path.hh:376
bool eval(ConfigurationOut_t result, const value_type &time) const
Configuration at time.
Definition: path.hh:148
const ConstraintSetPtr_t & constraints() const
Get constraints the path is subject to.
Definition: path.hh:217
virtual void impl_velocityBound(vectorOut_t bound, const value_type ¶m0, const value_type ¶m1) const
Definition: path.hh:337
Path(const Path &path)
Copy constructor.
virtual Configuration_t end() const =0
Get the final configuration.
virtual value_type length() const
Get length of definition interval.
Definition: path.hh:206
Configuration_t configAtParam(const value_type ¶m, bool &success) const
Definition: path.hh:308
virtual void checkPath() const
Should be called by child classes after having init.
Path(const interval_t &interval, size_type outputSize, size_type outputDerivativeSize, const ConstraintSetPtr_t &constraints)
virtual Configuration_t initial() const =0
Get the initial configuration.
#define HPP_CORE_DLLAPI
Definition: config.hh:64
#define HPP_CORE_DEPRECATED
Definition: deprecated.hh:37
std::ostream & operator<<(std::ostream &os, const Constraint &constraint)
Definition: constraint.hh:99
shared_ptr< TimeParameterization > TimeParameterizationPtr_t
Definition: fwd.hh:189
pinocchio::value_type value_type
Definition: fwd.hh:174
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:222
pinocchio::ConfigurationOut_t ConfigurationOut_t
Definition: fwd.hh:108
std::pair< value_type, value_type > interval_t
Definition: fwd.hh:175
pinocchio::size_type size_type
Definition: fwd.hh:173
pinocchio::Configuration_t Configuration_t
Definition: fwd.hh:106
shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition: fwd.hh:130
shared_ptr< Path > PathPtr_t
Definition: fwd.hh:187
Definition: bi-rrt-planner.hh:35