hpp-rbprm
4.10.1
Implementation of RB-PRM planner using hpp.
|
Go to the documentation of this file.
19 #ifndef HPP_RBPRM_BEZIER_PATH_HH
20 #define HPP_RBPRM_BEZIER_PATH_HH
22 #include <ndcurves/bezier_curve.h>
23 #include <hpp/core/path.hh>
30 typedef ndcurves::bezier_curve<double, double, true, Eigen::Vector3d>
bezier_t;
54 core::ConfigurationIn_t
end, core::interval_t timeRange) {
68 std::vector<bezier_t::point_t>::const_iterator wpBegin,
69 std::vector<bezier_t::point_t>::const_iterator wpEnd, core::ConfigurationIn_t
init,
70 core::ConfigurationIn_t
end, core::interval_t timeRange) {
108 virtual core::PathPtr_t
copy(
const core::ConstraintSetPtr_t& constraints)
const {
113 virtual core::Configuration_t
initial()
const;
116 virtual core::Configuration_t
end()
const;
118 core::Configuration_t
operator()(
const core::value_type& t)
const {
119 core::Configuration_t result(outputSize());
122 constraints()->apply(result);
133 virtual std::ostream&
print(std::ostream& os)
const {
134 os <<
"BezierPath:" << std::endl;
135 os <<
"interval: [ " << timeRange().first <<
", " << timeRange().second <<
" ]" << std::endl;
136 os <<
"initial configuration: " <<
initial().transpose() << std::endl;
137 os <<
"final configuration: " <<
end().transpose() << std::endl;
138 os <<
"Curve of degree :" << curve_->degree_ << std::endl;
139 os <<
"waypoints = " << std::endl;
140 for (bezier_t::cit_point_t wpit = curve_->waypoints().begin(); wpit != curve_->waypoints().end(); ++wpit) {
141 os << (*wpit).transpose() << std::endl;
148 core::ConfigurationIn_t
end, core::interval_t timeRange);
151 BezierPath(
const core::DevicePtr_t& robot, std::vector<bezier_t::point_t>::const_iterator wpBegin,
152 std::vector<bezier_t::point_t>::const_iterator wpEnd, core::ConfigurationIn_t
init,
153 core::ConfigurationIn_t
end, core::interval_t timeRange);
162 parent_t::init(
self);
168 parent_t::init(
self);
172 virtual bool impl_compute(core::ConfigurationOut_t result, core::value_type param)
const;
181 pinocchio::DevicePtr_t device_;
183 core::Configuration_t initial_;
184 core::Configuration_t end_;
185 BezierPathWkPtr_t weak_;
191 #endif // HPP_RBPRM_BEZIER_PATH_HH
virtual core::Configuration_t end() const
Get the final configuration.
virtual core::PathPtr_t copy(const core::ConstraintSetPtr_t &constraints) const
Definition: bezier-path.hh:108
HPP_PREDEF_CLASS(RbPrmFullBody)
void init(BezierPathPtr_t self)
Definition: bezier-path.hh:161
ndcurves::bezier_curve< double, double, true, Eigen::Vector3d > bezier_t
Definition: bezier-path.hh:30
boost::shared_ptr< BezierPath > BezierPathPtr_t
Definition: bezier-path.hh:33
static BezierPathPtr_t createCopy(const BezierPathPtr_t &path, const core::ConstraintSetPtr_t &constraints)
Definition: bezier-path.hh:90
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:67
boost::shared_ptr< const BezierPath > BezierPathConstPtr_t
Definition: bezier-path.hh:34
bezier_t::t_point_t getWaypoints()
Definition: bezier-path.hh:129
static BezierPathPtr_t createCopy(const BezierPathPtr_t &path)
Definition: bezier-path.hh:80
bezier_Ptr getBezier()
Definition: bezier-path.hh:127
Path parent_t
Definition: bezier-path.hh:45
Definition: bezier-path.hh:43
core::Configuration_t operator()(const core::value_type &t) const
Definition: bezier-path.hh:118
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:53
virtual core::PathPtr_t copy() const
Definition: bezier-path.hh:102
Definition: algorithm.hh:27
virtual core::Configuration_t initial() const
Get the initial configuration.
virtual std::ostream & print(std::ostream &os) const
Print path in a stream.
Definition: bezier-path.hh:133
virtual ~BezierPath()
Destructor.
Definition: bezier-path.hh:47
virtual bool impl_compute(core::ConfigurationOut_t result, core::value_type param) const
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
void initCopy(BezierPathPtr_t self)
Definition: bezier-path.hh:167
boost::shared_ptr< bezier_t > bezier_Ptr
Definition: bezier-path.hh:31