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

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

Loading...
Searching...
No Matches
interpolated-path.hh
Go to the documentation of this file.
1// Copyright (c) 2015 CNRS
2// Authors: Joseph Mirabel
3//
4
5// Redistribution and use in source and binary forms, with or without
6// modification, are permitted provided that the following conditions are
7// met:
8//
9// 1. Redistributions of source code must retain the above copyright
10// notice, this list of conditions and the following disclaimer.
11//
12// 2. Redistributions in binary form must reproduce the above copyright
13// notice, this list of conditions and the following disclaimer in the
14// documentation and/or other materials provided with the distribution.
15//
16// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
22// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
26// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
27// DAMAGE.
28
29#ifndef HPP_CORE_INTERPOLATED_PATH_HH
30#define HPP_CORE_INTERPOLATED_PATH_HH
31
32#include <hpp/core/config.hh>
33#include <hpp/core/fwd.hh>
34#include <hpp/core/path.hh>
35
36namespace hpp {
37namespace core {
40
52 class HPP_CORE_DLLAPI InterpolatedPath : public Path {
53 public:
54 typedef std::pair<const value_type, Configuration_t> InterpolationPoint_t;
55 typedef std::map<value_type, Configuration_t, std::less<value_type>,
56 Eigen::aligned_allocator<InterpolationPoint_t> >
57 InterpolationPoints_t;
58 typedef Path parent_t;
59
61 virtual ~InterpolatedPath() {}
62
67 static InterpolatedPathPtr_t create(const DevicePtr_t& device,
68 ConfigurationIn_t init,
69 ConfigurationIn_t end,
70 interval_t timeRange) {
71 InterpolatedPath* ptr = new InterpolatedPath(device, init, end, timeRange);
72 InterpolatedPathPtr_t shPtr(ptr);
73 ptr->init(shPtr);
74 return shPtr;
75 }
76
82 static InterpolatedPathPtr_t create(const DevicePtr_t& device,
83 ConfigurationIn_t init,
84 ConfigurationIn_t end,
85 interval_t timeRange,
86 ConstraintSetPtr_t constraints) {
87 InterpolatedPath* ptr =
88 new InterpolatedPath(device, init, end, timeRange, constraints);
89 InterpolatedPathPtr_t shPtr(ptr);
90 ptr->init(shPtr);
91 return shPtr;
92 }
93
98 static InterpolatedPathPtr_t create(const DevicePtr_t& device,
99 ConfigurationIn_t init,
100 ConfigurationIn_t end,
101 value_type length) {
102 return create(device, init, end, interval_t(0, length));
103 }
104
110 static InterpolatedPathPtr_t create(const DevicePtr_t& device,
111 ConfigurationIn_t init,
112 ConfigurationIn_t end, value_type length,
113 ConstraintSetPtr_t constraints) {
114 return create(device, init, end, interval_t(0, length), constraints);
115 }
116
119 static InterpolatedPathPtr_t createCopy(const InterpolatedPathPtr_t& path) {
120 InterpolatedPath* ptr = new InterpolatedPath(*path);
121 InterpolatedPathPtr_t shPtr(ptr);
122 ptr->initCopy(shPtr);
123 return shPtr;
124 }
125
131 static InterpolatedPathPtr_t create(const PathPtr_t& path,
132 const DevicePtr_t& device,
133 const std::size_t& nbSamples);
134
138 static InterpolatedPathPtr_t createCopy(
139 const InterpolatedPathPtr_t& path,
140 const ConstraintSetPtr_t& constraints) {
141 InterpolatedPath* ptr = new InterpolatedPath(*path, constraints);
142 InterpolatedPathPtr_t shPtr(ptr);
143 ptr->initCopy(shPtr);
144 return shPtr;
145 }
146
151 virtual PathPtr_t copy() const { return createCopy(weak_.lock()); }
152
157 virtual PathPtr_t copy(const ConstraintSetPtr_t& constraints) const {
158 return createCopy(weak_.lock(), constraints);
159 }
160
161 virtual PathPtr_t reverse() const;
162
164 DevicePtr_t device() const;
165
167 void insert(const value_type& time, ConfigurationIn_t config) {
168 configs_.insert(InterpolationPoint_t(time, config));
169 }
170
172 Configuration_t initial() const { return configs_.begin()->second; }
173
175 Configuration_t end() const { return configs_.rbegin()->second; }
176
177 const InterpolationPoints_t& interpolationPoints() const { return configs_; }
178
179 protected:
181 virtual std::ostream& print(std::ostream& os) const {
182 os << "InterpolatedPath:" << std::endl;
183 Path::print(os);
184 os << "initial configuration: " << initial().transpose() << std::endl;
185 os << "final configuration: " << end().transpose() << std::endl;
186 return os;
187 }
188
190 InterpolatedPath(const DevicePtr_t& robot, ConfigurationIn_t init,
191 ConfigurationIn_t end, interval_t timeRange);
192
194 InterpolatedPath(const DevicePtr_t& robot, ConfigurationIn_t init,
195 ConfigurationIn_t end, interval_t timeRange,
196 ConstraintSetPtr_t constraints);
197
199 InterpolatedPath(const PathPtr_t& path, const DevicePtr_t& device,
200 const std::size_t& nbSamples);
201
203 InterpolatedPath(const InterpolatedPath& path);
204
206 InterpolatedPath(const InterpolatedPath& path,
207 const ConstraintSetPtr_t& constraints);
208
209 void init(InterpolatedPathPtr_t self);
210
211 void initCopy(InterpolatedPathPtr_t self);
212
213 virtual bool impl_compute(ConfigurationOut_t result, value_type param) const;
215 virtual void impl_derivative(vectorOut_t result, const value_type& t,
216 size_type order) const;
217 virtual void impl_velocityBound(vectorOut_t result, const value_type& t0,
218 const value_type& t1) const;
219
222 PathPtr_t impl_extract(const interval_t& subInterval) const;
223
224 private:
225 DevicePtr_t device_;
226 InterpolationPoints_t configs_;
227 InterpolatedPathWkPtr_t weak_;
228}; // class InterpolatedPath
230} // namespace core
231} // namespace hpp
232#endif // HPP_CORE_INTERPOLATED_PATH_HH
hpp::core::InterpolatedPath
Definition interpolated-path.hh:52
hpp::core::InterpolatedPath::create
static InterpolatedPathPtr_t create(const DevicePtr_t &device, ConfigurationIn_t init, ConfigurationIn_t end, value_type length)
Definition interpolated-path.hh:98
hpp::core::InterpolatedPath::initial
Configuration_t initial() const
Get the initial configuration.
Definition interpolated-path.hh:172
hpp::core::InterpolatedPath::initCopy
void initCopy(InterpolatedPathPtr_t self)
hpp::core::InterpolatedPath::createCopy
static InterpolatedPathPtr_t createCopy(const InterpolatedPathPtr_t &path, const ConstraintSetPtr_t &constraints)
Definition interpolated-path.hh:138
hpp::core::InterpolatedPath::InterpolatedPath
InterpolatedPath(const InterpolatedPath &path, const ConstraintSetPtr_t &constraints)
Copy constructor with constraints.
hpp::core::InterpolatedPath::InterpolatedPath
InterpolatedPath(const PathPtr_t &path, const DevicePtr_t &device, const std::size_t &nbSamples)
DIscretization of a given path.
hpp::core::InterpolatedPath::insert
void insert(const value_type &time, ConfigurationIn_t config)
Insert interpolation point.
Definition interpolated-path.hh:167
hpp::core::InterpolatedPath::impl_extract
PathPtr_t impl_extract(const interval_t &subInterval) const
hpp::core::InterpolatedPath::create
static InterpolatedPathPtr_t create(const PathPtr_t &path, const DevicePtr_t &device, const std::size_t &nbSamples)
hpp::core::InterpolatedPath::~InterpolatedPath
virtual ~InterpolatedPath()
Destructor.
Definition interpolated-path.hh:61
hpp::core::InterpolatedPath::impl_velocityBound
virtual void impl_velocityBound(vectorOut_t result, const value_type &t0, const value_type &t1) const
hpp::core::InterpolatedPath::createCopy
static InterpolatedPathPtr_t createCopy(const InterpolatedPathPtr_t &path)
Definition interpolated-path.hh:119
hpp::core::InterpolatedPath::InterpolatedPath
InterpolatedPath(const InterpolatedPath &path)
Copy constructor.
hpp::core::InterpolatedPath::impl_derivative
virtual void impl_derivative(vectorOut_t result, const value_type &t, size_type order) const
Virtual implementation of derivative.
hpp::core::InterpolatedPath::print
virtual std::ostream & print(std::ostream &os) const
Print path in a stream.
Definition interpolated-path.hh:181
hpp::core::InterpolatedPath::parent_t
Path parent_t
Definition interpolated-path.hh:58
hpp::core::InterpolatedPath::create
static InterpolatedPathPtr_t create(const DevicePtr_t &device, ConfigurationIn_t init, ConfigurationIn_t end, interval_t timeRange, ConstraintSetPtr_t constraints)
Definition interpolated-path.hh:82
hpp::core::InterpolatedPath::copy
virtual PathPtr_t copy() const
Definition interpolated-path.hh:151
hpp::core::InterpolatedPath::init
void init(InterpolatedPathPtr_t self)
hpp::core::InterpolatedPath::device
DevicePtr_t device() const
Return the internal robot.
hpp::core::InterpolatedPath::InterpolationPoint_t
std::pair< const value_type, Configuration_t > InterpolationPoint_t
Definition interpolated-path.hh:54
hpp::core::InterpolatedPath::reverse
virtual PathPtr_t reverse() const
hpp::core::InterpolatedPath::interpolationPoints
const InterpolationPoints_t & interpolationPoints() const
Definition interpolated-path.hh:177
hpp::core::InterpolatedPath::InterpolatedPath
InterpolatedPath(const DevicePtr_t &robot, ConfigurationIn_t init, ConfigurationIn_t end, interval_t timeRange, ConstraintSetPtr_t constraints)
Constructor with constraints.
hpp::core::InterpolatedPath::InterpolationPoints_t
std::map< value_type, Configuration_t, std::less< value_type >, Eigen::aligned_allocator< InterpolationPoint_t > > InterpolationPoints_t
Definition interpolated-path.hh:57
hpp::core::InterpolatedPath::create
static InterpolatedPathPtr_t create(const DevicePtr_t &device, ConfigurationIn_t init, ConfigurationIn_t end, interval_t timeRange)
Definition interpolated-path.hh:67
hpp::core::InterpolatedPath::impl_compute
virtual bool impl_compute(ConfigurationOut_t result, value_type param) const
Function evaluation without applying constraints.
hpp::core::InterpolatedPath::create
static InterpolatedPathPtr_t create(const DevicePtr_t &device, ConfigurationIn_t init, ConfigurationIn_t end, value_type length, ConstraintSetPtr_t constraints)
Definition interpolated-path.hh:110
hpp::core::InterpolatedPath::end
Configuration_t end() const
Get the final configuration.
Definition interpolated-path.hh:175
hpp::core::InterpolatedPath::InterpolatedPath
InterpolatedPath(const DevicePtr_t &robot, ConfigurationIn_t init, ConfigurationIn_t end, interval_t timeRange)
Constructor.
hpp::core::InterpolatedPath::copy
virtual PathPtr_t copy(const ConstraintSetPtr_t &constraints) const
Definition interpolated-path.hh:157
hpp::core::Path
Definition path.hh:71
config.hh
HPP_CORE_DLLAPI
#define HPP_CORE_DLLAPI
Definition config.hh:88
fwd.hh
hpp
hpp::core::value_type
pinocchio::value_type value_type
Definition fwd.hh:174
hpp::core::InterpolatedPathPtr_t
shared_ptr< InterpolatedPath > InterpolatedPathPtr_t
Definition fwd.hh:211
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::ConfigurationIn_t
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition fwd.hh:108
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::DevicePtr_t
pinocchio::DevicePtr_t DevicePtr_t
Definition fwd.hh:134
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
path.hh