19 #ifndef HPP_RBPRM_BEZIER_PATH_HH 20 #define HPP_RBPRM_BEZIER_PATH_HH 22 #include <ndcurves/bezier_curve.h> 24 #include <hpp/core/path.hh> 31 typedef ndcurves::bezier_curve<double, double, true, Eigen::Vector3d>
bezier_t;
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) {
61 BezierPathPtr_t shPtr(ptr);
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) {
79 new BezierPath(device, wpBegin, wpEnd, init, end, timeRange);
80 BezierPathPtr_t shPtr(ptr);
88 static BezierPathPtr_t
createCopy(
const BezierPathPtr_t& path) {
90 BezierPathPtr_t shPtr(ptr);
99 const BezierPathPtr_t& path,
100 const core::ConstraintSetPtr_t& constraints) {
102 BezierPathPtr_t shPtr(ptr);
119 const core::ConstraintSetPtr_t& constraints)
const {
124 virtual core::Configuration_t
initial()
const;
127 virtual core::Configuration_t
end()
const;
129 core::Configuration_t
operator()(
const core::value_type& t)
const {
130 core::Configuration_t result(outputSize());
133 constraints()->apply(result);
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;
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);
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);
176 const core::ConstraintSetPtr_t& constraints);
178 void init(BezierPathPtr_t
self) {
179 parent_t::init(
self);
185 parent_t::init(
self);
189 virtual bool impl_compute(core::ConfigurationOut_t result,
190 core::value_type param)
const;
199 pinocchio::DevicePtr_t device_;
201 core::Configuration_t initial_;
202 core::Configuration_t end_;
203 BezierPathWkPtr_t weak_;
209 #endif // HPP_RBPRM_BEZIER_PATH_HH Definition: bezier-path.hh:45
ndcurves::bezier_curve< double, double, true, Eigen::Vector3d > bezier_t
Definition: bezier-path.hh:31
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
shared_ptr< BezierPath > BezierPathPtr_t
Definition: bezier-path.hh:34
Definition: algorithm.hh:26
bezier_Ptr getBezier()
Definition: bezier-path.hh:138
virtual core::Configuration_t end() const
Get the final configuration.
void init(BezierPathPtr_t self)
Definition: bezier-path.hh:178
virtual core::PathPtr_t copy(const core::ConstraintSetPtr_t &constraints) const
Definition: bezier-path.hh:118
virtual core::Configuration_t initial() const
Get the initial configuration.
static BezierPathPtr_t createCopy(const BezierPathPtr_t &path)
Definition: bezier-path.hh:88
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
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
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
HPP_PREDEF_CLASS(RbPrmFullBody)
bezier_t::t_point_t getWaypoints()
Definition: bezier-path.hh:140
virtual bool impl_compute(core::ConfigurationOut_t result, core::value_type param) const
Path parent_t
Definition: bezier-path.hh:47
virtual ~BezierPath()
Destructor.
Definition: bezier-path.hh:49
core::Configuration_t operator()(const core::value_type &t) const
Definition: bezier-path.hh:129
virtual core::PathPtr_t copy() const
Definition: bezier-path.hh:112
shared_ptr< const BezierPath > BezierPathConstPtr_t
Definition: bezier-path.hh:35
shared_ptr< bezier_t > bezier_Ptr
Definition: bezier-path.hh:32
void initCopy(BezierPathPtr_t self)
Definition: bezier-path.hh:184