19 #ifndef HPP_CORE_PATH_HH 20 # define HPP_CORE_PATH_HH 22 # include <boost/concept_check.hpp> 23 # include <hpp/util/exception.hh> 24 # include <hpp/util/serialization-fwd.hh> 80 template <
class T> boost::shared_ptr<T>
as (
void)
82 assert (HPP_DYNAMIC_PTR_CAST (T, weak_.lock ()));
83 return HPP_STATIC_PTR_CAST (T, weak_.lock ());
87 template <
class T> boost::shared_ptr<const T>
as (
void)
const 89 assert (HPP_DYNAMIC_PTR_CAST (
const T, weak_.lock ()));
90 return HPP_STATIC_PTR_CAST (
const T, weak_.lock ());
109 return extract (std::make_pair(tmin, tmax));
126 return (*
this) (time, unused);
131 return configAtParam (paramAtTime(time), success);
138 bool success = impl_compute (result, s);
139 if (!success)
return false;
140 return applyConstraints (result, s);
145 return this->operator() (time, success);
151 return this->operator() (result, time);
157 return impl_compute (result, paramAtTime(time));
185 assert(result.size() == outputDerivativeSize());
187 impl_velocityBound (result,
188 paramAtTime (std::max(t0, timeRange().first )),
189 paramAtTime (std::min(t1, timeRange().second)));
191 result *= timeParam_->derivativeBound (t0, t1);
206 return outputDerivativeSize_;
218 return timeRange_.second - timeRange_.first;
262 virtual std::ostream& print (std::ostream &os)
const;
294 void init (
const PathWkPtr_t&
self);
304 constraints_ = constraint;
308 virtual void checkPath ()
const;
312 timeRange_ = timeRange;
314 paramRange_.first = timeParam_->value(timeRange_.first );
315 paramRange_.second = timeParam_->value(timeRange_.second);
317 paramRange_ = timeRange_;
327 return paramRange_.second - paramRange_.first;
333 success = impl_compute (result, param);
334 if (!success)
return result;
335 success = applyConstraints (result, param);
356 HPP_THROW_EXCEPTION (hpp::Exception,
"not implemented");
369 HPP_THROW_EXCEPTION (hpp::Exception,
"not implemented");
382 return timeParam_->value (time);
399 friend std::ostream&
operator<< (std::ostream& os,
const Path& path);
400 friend class ExtractedPath;
409 return path.
print (os);
415 #endif // HPP_CORE_PATH_HH boost::shared_ptr< Path > PathPtr_t
Definition: fwd.hh:170
virtual ~Path()
Destructor.
Definition: path.hh:68
size_type outputSize() const
Get size of configuration space.
Definition: path.hh:198
value_type paramLength() const
Definition: path.hh:325
Definition: basic-configuration-shooter.hh:26
size_type outputDerivativeSize() const
Get size of velocity.
Definition: path.hh:204
const interval_t & paramRange() const
Definition: path.hh:244
boost::shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition: fwd.hh:110
std::pair< value_type, value_type > interval_t
Definition: fwd.hh:158
void velocityBound(vectorOut_t result, const value_type &t0, const value_type &t1) const
Definition: path.hh:183
PathPtr_t extract(const value_type &tmin, const value_type &tmax) const
Definition: path.hh:107
pinocchio::ConfigurationOut_t ConfigurationOut_t
Definition: fwd.hh:98
pinocchio::size_type size_type
Definition: fwd.hh:156
void timeRange(const interval_t &timeRange)
Definition: path.hh:310
void timeParameterization(const TimeParameterizationPtr_t &tp, const interval_t &tr)
Set the time parameterization function.
Definition: path.hh:250
virtual value_type length() const
Get length of definition interval.
Definition: path.hh:216
const ConstraintSetPtr_t & constraints() const
Get constraints the path is subject to.
Definition: path.hh:228
Configuration_t configAtParam(const value_type ¶m, bool &success) const
Definition: path.hh:330
boost::shared_ptr< const T > as(void) const
Static cast into a derived type.
Definition: path.hh:87
interval_t paramRange_
Interval of parameters.
Definition: path.hh:297
Path()
Definition: path.hh:403
const interval_t & timeRange() const
Get interval of definition.
Definition: path.hh:210
Configuration_t eval(const value_type &time, bool &success) const
Definition: path.hh:143
bool at(const value_type &time, ConfigurationOut_t result) const
Get the configuration at a parameter without applying the constraints.
Definition: path.hh:155
pinocchio::value_type value_type
Definition: fwd.hh:157
boost::shared_ptr< TimeParameterization > TimeParameterizationPtr_t
Definition: fwd.hh:172
virtual void impl_derivative(vectorOut_t derivative, const value_type ¶m, size_type order) const
Definition: path.hh:349
virtual std::ostream & print(std::ostream &os) const
void constraints(const ConstraintSetPtr_t &constraint)
Definition: path.hh:303
constraints::Implicit NumericalConstraint HPP_CORE_DEPRECATED
Definition: fwd.hh:347
const TimeParameterizationPtr_t & timeParameterization() const
Definition: path.hh:320
bool eval(ConfigurationOut_t result, const value_type &time) const
Definition: path.hh:148
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:203
virtual void impl_velocityBound(vectorOut_t bound, const value_type ¶m0, const value_type ¶m1) const
Definition: path.hh:362
std::ostream & operator<<(std::ostream &os, const Constraint &constraint)
Definition: constraint.hh:101
#define HPP_CORE_DLLAPI
Definition: config.hh:64
pinocchio::Configuration_t Configuration_t
Definition: fwd.hh:96
boost::shared_ptr< T > as(void)
Static cast into a derived type.
Definition: path.hh:80