hpp-core: include/hpp/core/path.hh Source File

hpp-core 9.0.2 Implement basic classes for canonical path planning for kinematic chains.

Loading...
Searching...
No Matches
path.hh
Go to the documentation of this file.
1//
2// Copyright (c) 2014 CNRS
3// Authors: Florent Lamiraux
4//
5
6// Redistribution and use in source and binary forms, with or without
7// modification, are permitted provided that the following conditions are
8// met:
9//
10// 1. Redistributions of source code must retain the above copyright
11// notice, this list of conditions and the following disclaimer.
12//
13// 2. Redistributions in binary form must reproduce the above copyright
14// notice, this list of conditions and the following disclaimer in the
15// documentation and/or other materials provided with the distribution.
16//
17// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
18// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
19// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
20// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
21// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
22// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
23// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
24// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
25// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
26// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
28// DAMAGE.
29
30#ifndef HPP_CORE_PATH_HH
31#define HPP_CORE_PATH_HH
32
33#include <hpp/core/config.hh>
34#include <hpp/core/constraint-set.hh>
35#include <hpp/core/deprecated.hh>
36#include <hpp/core/fwd.hh>
37#include <hpp/core/projection-error.hh>
38#include <hpp/core/time-parameterization.hh>
39#include <hpp/util/exception.hh>
40#include <hpp/util/serialization-fwd.hh>
41
42namespace hpp {
43namespace core {
47
71 class HPP_CORE_DLLAPI Path {
72 public:
75
77 virtual ~Path() {}
78
80 virtual PathPtr_t copy() const = 0;
81
86 virtual PathPtr_t copy(const ConstraintSetPtr_t& constraints) const = 0;
87
89 template <class T>
90 shared_ptr<T> as(void) {
91 assert(HPP_DYNAMIC_PTR_CAST(T, weak_.lock()));
92 return HPP_STATIC_PTR_CAST(T, weak_.lock());
93 }
94
96 template <class T>
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());
100 }
101
103
106
113 PathPtr_t extract(const interval_t& subInterval) const;
114
116 PathPtr_t extract(const value_type& tmin, const value_type& tmax) const {
117 return extract(std::make_pair(tmin, tmax));
118 }
119
122 virtual PathPtr_t reverse() const;
123
125
128
130 Configuration_t eval(const value_type& time, bool& success) const {
131 return configAtParam(paramAtTime(time), success);
132 }
133
135 bool eval(ConfigurationOut_t result, const value_type& time) const {
136 value_type s = paramAtTime(time);
137 bool success = impl_compute(result, s);
138 if (!success) return false;
139 return applyConstraints(result, s);
140 }
141
143 bool at(const value_type& time, ConfigurationOut_t result) const {
144 return impl_compute(result, paramAtTime(time));
145 }
146
156 void derivative(vectorOut_t result, const value_type& time,
157 size_type order) const;
158
171 void velocityBound(vectorOut_t result, const value_type& t0,
172 const value_type& t1) const {
173 assert(result.size() == outputDerivativeSize());
174 assert(t0 <= t1);
175 impl_velocityBound(result, paramAtTime(std::max(t0, timeRange().first)),
176 paramAtTime(std::min(t1, timeRange().second)));
177 if (timeParam_) result *= timeParam_->derivativeBound(t0, t1);
178 }
179
182
184 size_type outputSize() const { return outputSize_; }
185
187 size_type outputDerivativeSize() const { return outputDerivativeSize_; }
188
190 const interval_t& timeRange() const { return timeRange_; }
191
193 virtual value_type length() const {
194 return timeRange_.second - timeRange_.first;
195 }
196
198 virtual Configuration_t initial() const = 0;
199
201 virtual Configuration_t end() const = 0;
202
204 const ConstraintSetPtr_t& constraints() const { return constraints_; }
205
207
212
217 const interval_t& paramRange() const { return paramRange_; }
218
220 const TimeParameterizationPtr_t& timeParameterization() const {
221 return timeParam_;
222 }
223
225 void timeParameterization(const TimeParameterizationPtr_t& tp,
226 const interval_t& tr) {
227 timeParam_ = tp;
228 timeRange(tr);
229 }
230
232
233 protected:
236 virtual std::ostream& print(std::ostream& os) const;
237
247 Path(const interval_t& interval, size_type outputSize,
248 size_type outputDerivativeSize, const ConstraintSetPtr_t& constraints);
249
255 Path(const interval_t& interval, size_type outputSize,
256 size_type outputDerivativeSize);
257
259 Path(const Path& path);
260
262 Path(const Path& path, const ConstraintSetPtr_t& constraints);
263
267 void init(const PathWkPtr_t& self);
268
270 interval_t paramRange_;
271
276 void constraints(const ConstraintSetPtr_t& constraint) {
277 constraints_ = constraint;
278 }
279
281 virtual void checkPath() const;
282
283 void timeRange(const interval_t& timeRange) {
284 timeRange_ = timeRange;
285 if (timeParam_) {
286 paramRange_.first = timeParam_->value(timeRange_.first);
287 paramRange_.second = timeParam_->value(timeRange_.second);
288 } else
289 paramRange_ = timeRange_;
290 }
291
292 value_type paramLength() const {
293 return paramRange_.second - paramRange_.first;
294 }
295
296 Configuration_t configAtParam(const value_type& param, bool& success) const {
297 Configuration_t result(outputSize());
298 success = impl_compute(result, param);
299 if (!success) return result;
300 success = applyConstraints(result, param);
301 return result;
302 }
303
307 virtual bool impl_compute(ConfigurationOut_t configuration,
308 value_type param) const = 0;
309
314 virtual void impl_derivative(vectorOut_t derivative, const value_type& param,
315 size_type order) const {
316 (void)derivative;
317 (void)param;
318 (void)order;
319 HPP_THROW_EXCEPTION(hpp::Exception, "not implemented");
320 }
321
325 virtual void impl_velocityBound(vectorOut_t bound, const value_type& param0,
326 const value_type& param1) const {
327 (void)bound;
328 (void)param0;
329 (void)param1;
330 HPP_THROW_EXCEPTION(hpp::Exception, "not implemented");
331 }
332
334 virtual PathPtr_t impl_extract(const interval_t& paramInterval) const;
335
336 private:
338 interval_t timeRange_;
339
340 value_type paramAtTime(const value_type& time) const {
341 if (timeParam_) {
342 return timeParam_->value(time);
343 }
344 return time;
345 }
346
347 bool applyConstraints(ConfigurationOut_t result,
348 const value_type& param) const;
349
351 size_type outputSize_;
353 size_type outputDerivativeSize_;
355 ConstraintSetPtr_t constraints_;
357 TimeParameterizationPtr_t timeParam_;
359 PathWkPtr_t weak_;
360 friend std::ostream& operator<<(std::ostream& os, const Path& path);
361 friend class ExtractedPath;
362
363 protected:
364 Path() {}
365
366 private:
367 HPP_SERIALIZABLE();
368}; // class Path
369 inline std::ostream& operator<<(std::ostream& os, const Path& path) {
370 return path.print(os);
371}
373
374} // namespace core
375} // namespace hpp
376#endif // HPP_CORE_PATH_HH
hpp::Exception
hpp::core::Path
Definition path.hh:71
hpp::core::Path::eval
Configuration_t eval(const value_type &time, bool &success) const
Configuration at time.
Definition path.hh:130
hpp::core::Path::outputSize
size_type outputSize() const
Get size of configuration space.
Definition path.hh:184
hpp::core::Path::init
void init(const PathWkPtr_t &self)
hpp::core::Path::print
virtual std::ostream & print(std::ostream &os) const
hpp::core::Path::paramLength
value_type paramLength() const
Definition path.hh:292
hpp::core::Path::outputDerivativeSize
size_type outputDerivativeSize() const
Get size of velocity.
Definition path.hh:187
hpp::core::Path::~Path
virtual ~Path()
Destructor.
Definition path.hh:77
hpp::core::Path::paramRange
const interval_t & paramRange() const
Definition path.hh:217
hpp::core::Path::impl_derivative
virtual void impl_derivative(vectorOut_t derivative, const value_type &param, size_type order) const
Definition path.hh:314
hpp::core::Path::extract
PathPtr_t extract(const interval_t &subInterval) const
hpp::core::Path::as
shared_ptr< T > as(void)
Static cast into a derived type.
Definition path.hh:90
hpp::core::Path::impl_compute
virtual bool impl_compute(ConfigurationOut_t configuration, value_type param) const =0
Function evaluation without applying constraints.
hpp::core::Path::timeParameterization
void timeParameterization(const TimeParameterizationPtr_t &tp, const interval_t &tr)
Set the time parameterization function.
Definition path.hh:225
hpp::core::Path::as
shared_ptr< const T > as(void) const
Static cast into a derived type.
Definition path.hh:97
hpp::core::Path::impl_extract
virtual PathPtr_t impl_extract(const interval_t &paramInterval) const
Virtual implementation of extract.
hpp::core::Path::Path
Path(const interval_t &interval, size_type outputSize, size_type outputDerivativeSize)
hpp::core::Path::at
bool at(const value_type &time, ConfigurationOut_t result) const
Get the configuration at a parameter without applying the constraints.
Definition path.hh:143
hpp::core::Path::paramRange_
interval_t paramRange_
Interval of parameters.
Definition path.hh:270
hpp::core::Path::copy
virtual PathPtr_t copy() const =0
Return a shared pointer to a copy of this.
hpp::core::Path::extract
PathPtr_t extract(const value_type &tmin, const value_type &tmax) const
Definition path.hh:116
hpp::core::Path::reverse
virtual PathPtr_t reverse() const
hpp::core::Path::copy
virtual PathPtr_t copy(const ConstraintSetPtr_t &constraints) const =0
hpp::core::Path::timeParameterization
const TimeParameterizationPtr_t & timeParameterization() const
Get the time parameterization function.
Definition path.hh:220
hpp::core::Path::constraints
void constraints(const ConstraintSetPtr_t &constraint)
Definition path.hh:276
hpp::core::Path::velocityBound
void velocityBound(vectorOut_t result, const value_type &t0, const value_type &t1) const
Definition path.hh:171
hpp::core::Path::timeRange
const interval_t & timeRange() const
Get interval of definition.
Definition path.hh:190
hpp::core::Path::derivative
void derivative(vectorOut_t result, const value_type &time, size_type order) const
hpp::core::Path::timeRange
void timeRange(const interval_t &timeRange)
Definition path.hh:283
hpp::core::Path::Path
Path(const Path &path, const ConstraintSetPtr_t &constraints)
Copy constructor with constraints.
hpp::core::Path::Path
Path()
Definition path.hh:364
hpp::core::Path::eval
bool eval(ConfigurationOut_t result, const value_type &time) const
Configuration at time.
Definition path.hh:135
hpp::core::Path::constraints
const ConstraintSetPtr_t & constraints() const
Get constraints the path is subject to.
Definition path.hh:204
hpp::core::Path::impl_velocityBound
virtual void impl_velocityBound(vectorOut_t bound, const value_type &param0, const value_type &param1) const
Definition path.hh:325
hpp::core::Path::Path
Path(const Path &path)
Copy constructor.
hpp::core::Path::end
virtual Configuration_t end() const =0
Get the final configuration.
hpp::core::Path::length
virtual value_type length() const
Get length of definition interval.
Definition path.hh:193
hpp::core::Path::configAtParam
Configuration_t configAtParam(const value_type &param, bool &success) const
Definition path.hh:296
hpp::core::Path::checkPath
virtual void checkPath() const
Should be called by child classes after having init.
hpp::core::Path::Path
Path(const interval_t &interval, size_type outputSize, size_type outputDerivativeSize, const ConstraintSetPtr_t &constraints)
hpp::core::Path::initial
virtual Configuration_t initial() const =0
Get the initial configuration.
operator<<
hpp::constraints::ComparisonTypes_t operator<<(hpp::constraints::internal::ReplicateCompType c1, hpp::constraints::internal::ReplicateCompType c2)
config.hh
HPP_CORE_DLLAPI
#define HPP_CORE_DLLAPI
Definition config.hh:88
constraint-set.hh
deprecated.hh
exception.hh
HPP_THROW_EXCEPTION
#define HPP_THROW_EXCEPTION(TYPE, MSG)
fwd.hh
hpp::core::operator<<
std::ostream & operator<<(std::ostream &os, const Constraint &constraint)
Definition constraint.hh:99
hpp
hpp::core::TimeParameterizationPtr_t
shared_ptr< TimeParameterization > TimeParameterizationPtr_t
Definition fwd.hh:189
hpp::core::value_type
pinocchio::value_type value_type
Definition fwd.hh:174
hpp::core::vectorOut_t
pinocchio::vectorOut_t vectorOut_t
Definition fwd.hh:222
hpp::core::ConfigurationOut_t
pinocchio::ConfigurationOut_t ConfigurationOut_t
Definition fwd.hh:109
hpp::core::interval_t
std::pair< value_type, value_type > interval_t
Definition fwd.hh:175
hpp::core::size_type
pinocchio::size_type size_type
Definition fwd.hh:173
hpp::core::Configuration_t
pinocchio::Configuration_t Configuration_t
Definition fwd.hh:107
hpp::core::ConstraintSetPtr_t
shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition fwd.hh:130
hpp::core::PathPtr_t
shared_ptr< Path > PathPtr_t
Definition fwd.hh:187
HPP_DYNAMIC_PTR_CAST
#define HPP_DYNAMIC_PTR_CAST(t, x)
HPP_STATIC_PTR_CAST
#define HPP_STATIC_PTR_CAST(t, x)
projection-error.hh
serialization-fwd.hh
HPP_SERIALIZABLE
#define HPP_SERIALIZABLE()
time-parameterization.hh