hpp-rbprm 4.15.1
Implementation of RB-PRM planner using hpp.
Loading...
Searching...
No Matches
parabola-path.hh
Go to the documentation of this file.
1//
2// Copyright (c) 2015 CNRS
3// Authors: Mylene Campana
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_PARABOLA_PATH_HH
20#define HPP_RBPRM_PARABOLA_PATH_HH
21
22#include <hpp/core/config.hh>
23#include <hpp/core/fwd.hh>
24#include <hpp/core/path.hh>
25
26namespace hpp {
27namespace rbprm {
28using core::size_type;
29
30// forward declaration of class
32// Planner objects are manipulated only via shared pointers
33typedef shared_ptr<ParabolaPath> ParabolaPathPtr_t;
34
44class ParabolaPath : public core::Path {
45 public:
46 typedef Path parent_t;
48 virtual ~ParabolaPath() {}
49
54 static ParabolaPathPtr_t create(const core::DevicePtr_t& device,
55 core::ConfigurationIn_t init,
56 core::ConfigurationIn_t end,
57 core::value_type length,
58 core::vector_t coefficients) {
59 ParabolaPath* ptr =
61 ParabolaPathPtr_t shPtr(ptr);
62 ptr->init(shPtr);
63 return shPtr;
64 }
65
72 static ParabolaPathPtr_t create(const core::DevicePtr_t& device,
73 core::ConfigurationIn_t init,
74 core::ConfigurationIn_t end,
75 core::value_type length,
76 core::vector_t coefficients,
77 core::vector_t V0, core::vector_t Vimp,
78 std::vector<std::string> initialROMnames,
79 std::vector<std::string> endROMnames) {
80 ParabolaPath* ptr =
82 initialROMnames, endROMnames);
83 ParabolaPathPtr_t shPtr(ptr);
84 ptr->init(shPtr);
85 return shPtr;
86 }
87
91 ParabolaPath* ptr = new ParabolaPath(*path);
92 ParabolaPathPtr_t shPtr(ptr);
93 ptr->init(shPtr);
94 return shPtr;
95 }
96
102 const ParabolaPathPtr_t& path,
103 const core::ConstraintSetPtr_t& /*constraints*/) {
104 // ParabolaPath* ptr = new ParabolaPath (*path, constraints);
105 ParabolaPath* ptr = new ParabolaPath(*path);
106 ParabolaPathPtr_t shPtr(ptr);
107 ptr->init(shPtr);
108 return shPtr;
109 }
110
115 virtual core::PathPtr_t copy() const { return createCopy(weak_.lock()); }
116
121 virtual core::PathPtr_t copy(
122 const core::ConstraintSetPtr_t& constraints) const {
123 return createCopy(weak_.lock(), constraints);
124 }
125
130 virtual core::PathPtr_t extract(const core::interval_t& subInterval) const;
131
134 virtual core::PathPtr_t reverse() const;
135
140 void initialConfig(core::ConfigurationIn_t initial) {
141 assert(initial.size() == initial_.size());
142 initial_ = initial;
143 }
144
149 void endConfig(core::ConfigurationIn_t end) {
150 assert(end.size() == end_.size());
151 end_ = end;
152 }
153
155 core::DevicePtr_t device() const;
156
158 core::Configuration_t initial() const { return initial_; }
159
161 core::Configuration_t end() const { return end_; }
162
164 virtual core::value_type length() const { return length_; }
165
167 void coefficients(core::vector_t coefs) const {
168 for (size_type i = 0; i < coefs.size(); i++) coefficients_(i) = coefs(i);
169 }
170
172 core::vector_t coefficients() const { return coefficients_; }
173
174 virtual core::value_type computeLength(
175 const core::ConfigurationIn_t q1, const core::ConfigurationIn_t q2) const;
176
178 core::vector_t evaluateVelocity(const core::value_type t) const;
179
180 core::value_type alpha_; // chosen alpha in interval
181 core::value_type alphaMin_; // min bound of alpha interval
182 core::value_type alphaMax_; // max bound of alpha interval
183 core::value_type Xtheta_;
184 core::value_type Z_;
185 core::vector_t V0_; // initial velocity
186 core::vector_t Vimp_; // final velocity
187 std::vector<std::string> initialROMnames_; // active ROM list at begining
188 std::vector<std::string> endROMnames_; // active ROM list at end
189
190 protected:
192 virtual std::ostream& print(std::ostream& os) const {
193 os << "ParabolaPath:" << std::endl;
194 os << "interval: [ " << timeRange().first << ", " << timeRange().second
195 << " ]" << std::endl;
196 os << "initial configuration: " << initial_.transpose() << std::endl;
197 os << "final configuration: " << end_.transpose() << std::endl;
198 return os;
199 }
201 ParabolaPath(const core::DevicePtr_t& robot, core::ConfigurationIn_t init,
202 core::ConfigurationIn_t end, core::value_type length,
203 core::vector_t coefficients);
204
206 ParabolaPath(const core::DevicePtr_t& device, core::ConfigurationIn_t init,
207 core::ConfigurationIn_t end, core::value_type length,
208 core::vector_t coefs, core::vector_t V0_, core::vector_t Vimp,
209 std::vector<std::string> initialROMnames,
210 std::vector<std::string> endROMnames);
211
214
215 core::value_type lengthFunction(const core::value_type x) const;
216
218 parent_t::init(self);
219 weak_ = self;
220 }
221
222 /*void initCopy (ParabolaPathPtr_t self)
223 {
224 parent_t::initCopy (self);
225 weak_ = self;
226 }*/
227
233 virtual bool impl_compute(core::ConfigurationOut_t result,
234 core::value_type param) const;
235
236 private:
237 core::DevicePtr_t device_;
238 core::Configuration_t initial_;
239 core::Configuration_t end_;
240 ParabolaPathWkPtr_t weak_;
241 mutable core::vector_t coefficients_; // parabola coefficients
242 mutable core::value_type length_;
243}; // class ParabolaPath
244} // namespace rbprm
245} // namespace hpp
246#endif // HPP_CORE_PARABOLA_PATH_HH
Definition: parabola-path.hh:44
void coefficients(core::vector_t coefs) const
Set the three parabola coefficients.
Definition: parabola-path.hh:167
static ParabolaPathPtr_t createCopy(const ParabolaPathPtr_t &path)
Definition: parabola-path.hh:90
std::vector< std::string > initialROMnames_
Definition: parabola-path.hh:187
core::vector_t evaluateVelocity(const core::value_type t) const
Evaluate velocity vector at path abcissa t.
static ParabolaPathPtr_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: parabola-path.hh:72
core::DevicePtr_t device() const
Return the internal robot.
core::value_type alphaMin_
Definition: parabola-path.hh:181
std::vector< std::string > endROMnames_
Definition: parabola-path.hh:188
static ParabolaPathPtr_t create(const core::DevicePtr_t &device, core::ConfigurationIn_t init, core::ConfigurationIn_t end, core::value_type length, core::vector_t coefficients)
Definition: parabola-path.hh:54
core::value_type Z_
Definition: parabola-path.hh:184
core::value_type alphaMax_
Definition: parabola-path.hh:182
ParabolaPath(const core::DevicePtr_t &robot, core::ConfigurationIn_t init, core::ConfigurationIn_t end, core::value_type length, core::vector_t coefficients)
Constructor.
virtual bool impl_compute(core::ConfigurationOut_t result, core::value_type param) const
core::vector_t coefficients() const
Get path coefficients.
Definition: parabola-path.hh:172
ParabolaPath(const core::DevicePtr_t &device, 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.
void initialConfig(core::ConfigurationIn_t initial)
Definition: parabola-path.hh:140
core::vector_t V0_
Definition: parabola-path.hh:185
virtual core::PathPtr_t copy() const
Definition: parabola-path.hh:115
virtual core::value_type length() const
Get previously computed length.
Definition: parabola-path.hh:164
virtual core::PathPtr_t copy(const core::ConstraintSetPtr_t &constraints) const
Definition: parabola-path.hh:121
void init(ParabolaPathPtr_t self)
Definition: parabola-path.hh:217
ParabolaPath(const ParabolaPath &path)
Copy constructor.
core::value_type Xtheta_
Definition: parabola-path.hh:183
core::value_type alpha_
Definition: parabola-path.hh:180
virtual core::PathPtr_t reverse() const
core::vector_t Vimp_
Definition: parabola-path.hh:186
virtual std::ostream & print(std::ostream &os) const
Print path in a stream.
Definition: parabola-path.hh:192
virtual ~ParabolaPath()
Destructor.
Definition: parabola-path.hh:48
core::value_type lengthFunction(const core::value_type x) const
static ParabolaPathPtr_t createCopy(const ParabolaPathPtr_t &path, const core::ConstraintSetPtr_t &)
Definition: parabola-path.hh:101
virtual core::PathPtr_t extract(const core::interval_t &subInterval) const
Path parent_t
Definition: parabola-path.hh:46
core::Configuration_t initial() const
Get the initial configuration.
Definition: parabola-path.hh:158
core::Configuration_t end() const
Get the final configuration.
Definition: parabola-path.hh:161
void endConfig(core::ConfigurationIn_t end)
Definition: parabola-path.hh:149
virtual core::value_type computeLength(const core::ConfigurationIn_t q1, const core::ConfigurationIn_t q2) const
HPP_PREDEF_CLASS(RbPrmFullBody)
shared_ptr< ParabolaPath > ParabolaPathPtr_t
Definition: parabola-path.hh:33
Definition: algorithm.hh:26