hpp-rbprm 4.15.1
Implementation of RB-PRM planner using hpp.
Loading...
Searching...
No Matches
timed-parabola-path.hh
Go to the documentation of this file.
1//
2// Copyright (c) 2017 CNRS
3// Authors: Pierre Fernbach
4//
5// This file is part of hpp-core
6// hpp-core is free software: you can redistribute it
7// and/or modify it under the terms of the GNU Lesser General Public
8// License as published by the Free Software Foundation, either version
9// 3 of the License, or (at your option) any later version.
10//
11// hpp-core is distributed in the hope that it will be
12// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
13// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14// General Lesser Public License for more details. You should have
15// received a copy of the GNU Lesser General Public License along with
16// hpp-core If not, see
17// <http://www.gnu.org/licenses/>.
18
19#ifndef HPP_RBPRM_TIMED_PARABOLA_PATH_HH
20#define HPP_RBPRM_TIMED_PARABOLA_PATH_HH
21
23
24namespace hpp {
25namespace rbprm {
26
27// forward declaration of class
29// Planner objects are manipulated only via shared pointers
30typedef shared_ptr<TimedParabolaPath> TimedParabolaPathPtr_t;
31
36 public:
39 virtual ~TimedParabolaPath() {}
40
45 static TimedParabolaPathPtr_t create(const core::DevicePtr_t& device,
46 core::ConfigurationIn_t init,
47 core::ConfigurationIn_t end,
48 ParabolaPathPtr_t parabolaPath) {
50 new TimedParabolaPath(device, init, end, parabolaPath);
51 TimedParabolaPathPtr_t shPtr(ptr);
52 ptr->init(shPtr);
53 return shPtr;
54 }
55
60 static TimedParabolaPathPtr_t create(const core::DevicePtr_t& device,
61 core::ConfigurationIn_t init,
62 core::ConfigurationIn_t end,
63 core::value_type length,
64 core::vector_t coefficients) {
67 TimedParabolaPathPtr_t shPtr(ptr);
68 ptr->init(shPtr);
69 return shPtr;
70 }
71
78 static TimedParabolaPathPtr_t create(const core::DevicePtr_t& device,
79 core::ConfigurationIn_t init,
80 core::ConfigurationIn_t end,
81 core::value_type length,
82 core::vector_t coefficients,
83 core::vector_t V0, core::vector_t Vimp,
84 std::vector<std::string> initialROMnames,
85 std::vector<std::string> endROMnames) {
88 initialROMnames, endROMnames);
89 TimedParabolaPathPtr_t shPtr(ptr);
90 ptr->init(shPtr);
91 return shPtr;
92 }
93
97 TimedParabolaPath* ptr = new TimedParabolaPath(*path);
98 TimedParabolaPathPtr_t shPtr(ptr);
99 ptr->init(shPtr);
100 return shPtr;
101 }
102
108 const TimedParabolaPathPtr_t& path,
109 const core::ConstraintSetPtr_t& /*constraints*/) {
110 // TimedParabolaPath* ptr = new TimedParabolaPath (*path, constraints);
111 TimedParabolaPath* ptr = new TimedParabolaPath(*path);
112 TimedParabolaPathPtr_t shPtr(ptr);
113 ptr->init(shPtr);
114 return shPtr;
115 }
116
121 virtual core::PathPtr_t copy() const { return createCopy(weak_.lock()); }
122
127 virtual core::PathPtr_t copy(
128 const core::ConstraintSetPtr_t& constraints) const {
129 return createCopy(weak_.lock(), constraints);
130 }
131
136 virtual core::PathPtr_t extract(const core::interval_t& subInterval) const;
137
140 virtual core::PathPtr_t reverse() const;
141
146 void initialConfig(core::ConfigurationIn_t initial) {
147 assert(initial.size() == initial_.size());
148 initial_ = initial;
149 }
150
155 void endConfig(core::ConfigurationIn_t end) {
156 assert(end.size() == end_.size());
157 end_ = end;
158 }
159
161 core::DevicePtr_t device() const;
162
164 core::Configuration_t initial() const { return initial_; }
165
167 core::Configuration_t end() const { return end_; }
168
170 virtual core::value_type length() const { return length_; }
171
172 protected:
174 TimedParabolaPath(const core::DevicePtr_t& robot,
175 core::ConfigurationIn_t init, core::ConfigurationIn_t end,
176 ParabolaPathPtr_t parabolaPath);
177
179 TimedParabolaPath(const core::DevicePtr_t& robot,
180 core::ConfigurationIn_t init, core::ConfigurationIn_t end,
181 core::value_type length, core::vector_t coefficients);
182
184 TimedParabolaPath(const core::DevicePtr_t& robot,
185 core::ConfigurationIn_t init, core::ConfigurationIn_t end,
186 core::value_type length, core::vector_t coefs,
187 core::vector_t V0, core::vector_t Vimp,
188 std::vector<std::string> initialROMnames,
189 std::vector<std::string> endROMnames);
190
193
195 parent_t::init(self);
196 weak_ = self;
197 }
198
200 virtual bool impl_compute(core::ConfigurationOut_t result,
201 core::value_type t) const;
202
203 virtual double computeTimedLength(double x_theta, double v0, double alpha0);
204 virtual double computeTimedLength(ParabolaPathPtr_t parabolaPath);
205
207 virtual std::ostream& print(std::ostream& os) const {
208 os << "TimedParabolaPath:" << std::endl;
209 os << "interval: [ " << timeRange().first << ", " << timeRange().second
210 << " ]" << std::endl;
211 os << "initial configuration: " << initial_.transpose() << std::endl;
212 os << "final configuration: " << end_.transpose() << std::endl;
213 return os;
214 }
215
216 private:
217 core::DevicePtr_t device_;
218 core::Configuration_t initial_;
219 core::Configuration_t end_;
220 TimedParabolaPathWkPtr_t weak_;
221 ParabolaPathPtr_t parabolaPath_;
222 mutable core::value_type length_;
223
224}; // class TimedParabolaPath
225} // namespace rbprm
226} // namespace hpp
227
228#endif // HPP_RBPRM_TIMED_PARABOLA_PATH_HH
Definition: parabola-path.hh:44
core::vector_t coefficients() const
Get path coefficients.
Definition: parabola-path.hh:172
void init(ParabolaPathPtr_t self)
Definition: parabola-path.hh:217
Definition: timed-parabola-path.hh:35
virtual core::PathPtr_t extract(const core::interval_t &subInterval) const
virtual ~TimedParabolaPath()
Destructor.
Definition: timed-parabola-path.hh:39
ParabolaPath parent_t
Definition: timed-parabola-path.hh:37
virtual core::PathPtr_t copy(const core::ConstraintSetPtr_t &constraints) const
Definition: timed-parabola-path.hh:127
virtual double computeTimedLength(ParabolaPathPtr_t parabolaPath)
TimedParabolaPath(const TimedParabolaPath &path)
Copy constructor.
static TimedParabolaPathPtr_t create(const core::DevicePtr_t &device, core::ConfigurationIn_t init, core::ConfigurationIn_t end, core::value_type length, core::vector_t coefficients)
Definition: timed-parabola-path.hh:60
static TimedParabolaPathPtr_t create(const core::DevicePtr_t &device, core::ConfigurationIn_t init, core::ConfigurationIn_t end, ParabolaPathPtr_t parabolaPath)
Definition: timed-parabola-path.hh:45
core::Configuration_t end() const
Get the final configuration.
Definition: timed-parabola-path.hh:167
core::DevicePtr_t device() const
Return the internal robot.
virtual double computeTimedLength(double x_theta, double v0, double alpha0)
virtual bool impl_compute(core::ConfigurationOut_t result, core::value_type t) const
Param is the time.
static TimedParabolaPathPtr_t createCopy(const TimedParabolaPathPtr_t &path)
Definition: timed-parabola-path.hh:96
virtual std::ostream & print(std::ostream &os) const
Print path in a stream.
Definition: timed-parabola-path.hh:207
TimedParabolaPath(const core::DevicePtr_t &robot, core::ConfigurationIn_t init, core::ConfigurationIn_t end, core::value_type length, core::vector_t coefficients)
Constructor.
void endConfig(core::ConfigurationIn_t end)
Definition: timed-parabola-path.hh:155
void init(TimedParabolaPathPtr_t self)
Definition: timed-parabola-path.hh:194
void initialConfig(core::ConfigurationIn_t initial)
Definition: timed-parabola-path.hh:146
core::Configuration_t initial() const
Get the initial configuration.
Definition: timed-parabola-path.hh:164
static TimedParabolaPathPtr_t create(const core::DevicePtr_t &device, core::ConfigurationIn_t init, core::ConfigurationIn_t end, core::value_type length, core::vector_t coefficients, core::vector_t V0, core::vector_t Vimp, std::vector< std::string > initialROMnames, std::vector< std::string > endROMnames)
Definition: timed-parabola-path.hh:78
static TimedParabolaPathPtr_t createCopy(const TimedParabolaPathPtr_t &path, const core::ConstraintSetPtr_t &)
Definition: timed-parabola-path.hh:107
virtual core::PathPtr_t copy() const
Definition: timed-parabola-path.hh:121
virtual core::PathPtr_t reverse() const
TimedParabolaPath(const core::DevicePtr_t &robot, core::ConfigurationIn_t init, core::ConfigurationIn_t end, core::value_type length, core::vector_t coefs, core::vector_t V0, core::vector_t Vimp, std::vector< std::string > initialROMnames, std::vector< std::string > endROMnames)
Constructor with velocities and ROMnames.
TimedParabolaPath(const core::DevicePtr_t &robot, core::ConfigurationIn_t init, core::ConfigurationIn_t end, ParabolaPathPtr_t parabolaPath)
Constructor.
virtual core::value_type length() const
Get previously computed length.
Definition: timed-parabola-path.hh:170
HPP_PREDEF_CLASS(RbPrmFullBody)
shared_ptr< ParabolaPath > ParabolaPathPtr_t
Definition: parabola-path.hh:33
shared_ptr< TimedParabolaPath > TimedParabolaPathPtr_t
Definition: timed-parabola-path.hh:30
Definition: algorithm.hh:26