hpp-rbprm 4.14.0
Implementation of RB-PRM planner using hpp.
Loading...
Searching...
No Matches
bezier-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_BEZIER_PATH_HH
20#define HPP_RBPRM_BEZIER_PATH_HH
21
22#include <ndcurves/bezier_curve.h>
23
24#include <hpp/core/path.hh>
25#include <map>
26#include <vector>
27
28namespace hpp {
29namespace rbprm {
30
31typedef ndcurves::bezier_curve<double, double, true, Eigen::Vector3d> bezier_t;
32typedef shared_ptr<bezier_t> bezier_Ptr;
34typedef shared_ptr<BezierPath> BezierPathPtr_t;
35typedef shared_ptr<const BezierPath> BezierPathConstPtr_t;
36
44
45class BezierPath : public core::Path {
46 public:
47 typedef Path parent_t;
49 virtual ~BezierPath() {}
50
55 static BezierPathPtr_t create(const core::DevicePtr_t& device,
56 const bezier_Ptr& curve,
57 core::ConfigurationIn_t init,
58 core::ConfigurationIn_t end,
59 core::interval_t timeRange) {
60 BezierPath* ptr = new BezierPath(device, curve, init, end, timeRange);
61 BezierPathPtr_t shPtr(ptr);
62 ptr->init(shPtr);
63 ptr->checkPath();
64 return shPtr;
65 }
66
73 const core::DevicePtr_t& device,
74 std::vector<bezier_t::point_t>::const_iterator wpBegin,
75 std::vector<bezier_t::point_t>::const_iterator wpEnd,
76 core::ConfigurationIn_t init, core::ConfigurationIn_t end,
77 core::interval_t timeRange) {
78 BezierPath* ptr =
79 new BezierPath(device, wpBegin, wpEnd, init, end, timeRange);
80 BezierPathPtr_t shPtr(ptr);
81 ptr->init(shPtr);
82 ptr->checkPath();
83 return shPtr;
84 }
85
89 BezierPath* ptr = new BezierPath(*path);
90 BezierPathPtr_t shPtr(ptr);
91 ptr->initCopy(shPtr);
92 return shPtr;
93 }
94
99 const BezierPathPtr_t& path,
100 const core::ConstraintSetPtr_t& constraints) {
101 BezierPath* ptr = new BezierPath(*path, constraints);
102 BezierPathPtr_t shPtr(ptr);
103 ptr->initCopy(shPtr);
104 ptr->checkPath();
105 return shPtr;
106 }
107
112 virtual core::PathPtr_t copy() const { return createCopy(weak_.lock()); }
113
118 virtual core::PathPtr_t copy(
119 const core::ConstraintSetPtr_t& constraints) const {
120 return createCopy(weak_.lock(), constraints);
121 }
122
124 virtual core::Configuration_t initial() const;
125
127 virtual core::Configuration_t end() const;
128
129 core::Configuration_t operator()(const core::value_type& t) const {
130 core::Configuration_t result(outputSize());
131 impl_compute(result, t);
132 if (constraints()) {
133 constraints()->apply(result);
134 }
135 return result;
136 }
137
138 bezier_Ptr getBezier() { return curve_; }
139
140 bezier_t::t_point_t getWaypoints() { return curve_->waypoints(); }
141
142 protected:
144 virtual std::ostream& print(std::ostream& os) const {
145 os << "BezierPath:" << std::endl;
146 os << "interval: [ " << timeRange().first << ", " << timeRange().second
147 << " ]" << std::endl;
148 os << "initial configuration: " << initial().transpose() << std::endl;
149 os << "final configuration: " << end().transpose() << std::endl;
150 os << "Curve of degree :" << curve_->degree_ << std::endl;
151 os << "waypoints = " << std::endl;
152 for (bezier_t::cit_point_t wpit = curve_->waypoints().begin();
153 wpit != curve_->waypoints().end(); ++wpit) {
154 os << (*wpit).transpose() << std::endl;
155 }
156 return os;
157 }
158
160 BezierPath(const core::DevicePtr_t& robot, const bezier_Ptr& curve,
161 core::ConfigurationIn_t init, core::ConfigurationIn_t end,
162 core::interval_t timeRange);
163
165 BezierPath(const core::DevicePtr_t& robot,
166 std::vector<bezier_t::point_t>::const_iterator wpBegin,
167 std::vector<bezier_t::point_t>::const_iterator wpEnd,
168 core::ConfigurationIn_t init, core::ConfigurationIn_t end,
169 core::interval_t timeRange);
170
172 BezierPath(const BezierPath& path);
173
176 const core::ConstraintSetPtr_t& constraints);
177
179 parent_t::init(self);
180 weak_ = self;
181 checkPath();
182 }
183
185 parent_t::init(self);
186 weak_ = self;
187 }
188
189 virtual bool impl_compute(core::ConfigurationOut_t result,
190 core::value_type param) const;
191
192 /*
194 virtual void impl_derivative (core::vectorOut_t result, const
195 core::value_type& t, core::size_type order) const;
196 */
197
198 private:
199 pinocchio::DevicePtr_t device_;
200 bezier_Ptr curve_;
201 core::Configuration_t initial_;
202 core::Configuration_t end_;
203 BezierPathWkPtr_t weak_;
204
205}; // class Bezier Path
206} // namespace rbprm
207} // namespace hpp
208
209#endif // HPP_RBPRM_BEZIER_PATH_HH
Definition: bezier-path.hh:45
static BezierPathPtr_t create(const core::DevicePtr_t &device, const bezier_Ptr &curve, core::ConfigurationIn_t init, core::ConfigurationIn_t end, core::interval_t timeRange)
Definition: bezier-path.hh:55
virtual core::PathPtr_t copy(const core::ConstraintSetPtr_t &constraints) const
Definition: bezier-path.hh:118
BezierPath(const BezierPath &path)
Copy constructor.
virtual bool impl_compute(core::ConfigurationOut_t result, core::value_type param) const
static BezierPathPtr_t createCopy(const BezierPathPtr_t &path, const core::ConstraintSetPtr_t &constraints)
Definition: bezier-path.hh:98
virtual std::ostream & print(std::ostream &os) const
Print path in a stream.
Definition: bezier-path.hh:144
virtual ~BezierPath()
Destructor.
Definition: bezier-path.hh:49
virtual core::PathPtr_t copy() const
Definition: bezier-path.hh:112
static BezierPathPtr_t createCopy(const BezierPathPtr_t &path)
Definition: bezier-path.hh:88
bezier_t::t_point_t getWaypoints()
Definition: bezier-path.hh:140
static BezierPathPtr_t create(const core::DevicePtr_t &device, std::vector< bezier_t::point_t >::const_iterator wpBegin, std::vector< bezier_t::point_t >::const_iterator wpEnd, core::ConfigurationIn_t init, core::ConfigurationIn_t end, core::interval_t timeRange)
Definition: bezier-path.hh:72
bezier_Ptr getBezier()
Definition: bezier-path.hh:138
virtual core::Configuration_t end() const
Get the final configuration.
core::Configuration_t operator()(const core::value_type &t) const
Definition: bezier-path.hh:129
BezierPath(const core::DevicePtr_t &robot, const bezier_Ptr &curve, core::ConfigurationIn_t init, core::ConfigurationIn_t end, core::interval_t timeRange)
constructor with curve
Path parent_t
Definition: bezier-path.hh:47
void init(BezierPathPtr_t self)
Definition: bezier-path.hh:178
virtual core::Configuration_t initial() const
Get the initial configuration.
BezierPath(const core::DevicePtr_t &robot, std::vector< bezier_t::point_t >::const_iterator wpBegin, std::vector< bezier_t::point_t >::const_iterator wpEnd, core::ConfigurationIn_t init, core::ConfigurationIn_t end, core::interval_t timeRange)
constructor with waypoints
void initCopy(BezierPathPtr_t self)
Definition: bezier-path.hh:184
BezierPath(const BezierPath &path, const core::ConstraintSetPtr_t &constraints)
Copy constructor with constraints.
HPP_PREDEF_CLASS(RbPrmFullBody)
ndcurves::bezier_curve< double, double, true, Eigen::Vector3d > bezier_t
Definition: bezier-path.hh:31
shared_ptr< BezierPath > BezierPathPtr_t
Definition: bezier-path.hh:34
shared_ptr< const BezierPath > BezierPathConstPtr_t
Definition: bezier-path.hh:35
shared_ptr< bezier_t > bezier_Ptr
Definition: bezier-path.hh:32
Definition: algorithm.hh:26