hpp-rbprm
4.10.0
Implementation of RB-PRM planner using hpp.
|
Go to the documentation of this file.
20 #ifndef HPP_RBPRM_BEZIER_PATH_HH
21 #define HPP_RBPRM_BEZIER_PATH_HH
23 # include <curves/bezier_curve.h>
24 # include <hpp/core/path.hh>
31 typedef curves::bezier_curve <double, double, true, Eigen::Vector3d >
bezier_t;
58 core::ConfigurationIn_t
init,
59 core::ConfigurationIn_t
end,
60 core::interval_t timeRange)
74 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,
75 core::ConfigurationIn_t
init,
76 core::ConfigurationIn_t
end,core::interval_t timeRange)
114 virtual core::PathPtr_t
copy ()
const
123 virtual core::PathPtr_t
copy (
const core::ConstraintSetPtr_t& constraints)
const
125 return createCopy (weak_.lock (), constraints);
132 virtual core::Configuration_t
initial ()
const;
135 virtual core::Configuration_t
end ()
const ;
138 core::Configuration_t
operator () (
const core::value_type& t)
const
140 core::Configuration_t result (outputSize ());
143 constraints()->apply (result);
154 return curve_->waypoints();
161 virtual std::ostream&
print (std::ostream &os)
const
163 os <<
"BezierPath:" << std::endl;
164 os <<
"interval: [ " << timeRange ().first <<
", "
165 << timeRange ().second <<
" ]" << std::endl;
166 os <<
"initial configuration: " <<
initial().transpose() << std::endl;
167 os <<
"final configuration: " <<
end().transpose () << std::endl;
168 os<<
"Curve of degree :"<<curve_->degree_<<std::endl;
169 os<<
"waypoints = "<<std::endl;
170 for(bezier_t::cit_point_t wpit = curve_->waypoints().begin() ; wpit != curve_->waypoints().end() ; ++wpit){
171 os<<(*wpit).transpose()<<std::endl;
178 core::ConfigurationIn_t
init,
179 core::ConfigurationIn_t
end,core::interval_t timeRange);
182 BezierPath (
const core::DevicePtr_t& robot, std::vector<bezier_t::point_t>::const_iterator wpBegin, std::vector<bezier_t::point_t>::const_iterator wpEnd,
183 core::ConfigurationIn_t
init,
184 core::ConfigurationIn_t
end, core::interval_t timeRange);
193 const core::ConstraintSetPtr_t& constraints);
198 parent_t::init (
self);
205 parent_t::init (
self);
209 virtual bool impl_compute (core::ConfigurationOut_t result,
210 core::value_type param)
const;
220 pinocchio::DevicePtr_t device_;
222 core::Configuration_t initial_;
223 core::Configuration_t end_;
224 BezierPathWkPtr_t weak_;
230 #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:123
HPP_PREDEF_CLASS(RbPrmFullBody)
void init(BezierPathPtr_t self)
Definition: bezier-path.hh:196
boost::shared_ptr< BezierPath > BezierPathPtr_t
Definition: bezier-path.hh:34
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:74
boost::shared_ptr< const BezierPath > BezierPathConstPtr_t
Definition: bezier-path.hh:35
bezier_t::t_point_t getWaypoints()
Definition: bezier-path.hh:153
static BezierPathPtr_t createCopy(const BezierPathPtr_t &path)
Definition: bezier-path.hh:87
bezier_Ptr getBezier()
Definition: bezier-path.hh:149
Path parent_t
Definition: bezier-path.hh:48
Definition: bezier-path.hh:44
core::Configuration_t operator()(const core::value_type &t) const
Definition: bezier-path.hh:138
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:56
virtual core::PathPtr_t copy() const
Definition: bezier-path.hh:114
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:161
virtual ~BezierPath()
Destructor.
Definition: bezier-path.hh:50
virtual bool impl_compute(core::ConfigurationOut_t result, core::value_type param) const
curves::bezier_curve< double, double, true, Eigen::Vector3d > bezier_t
Definition: bezier-path.hh:31
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:203
boost::shared_ptr< bezier_t > bezier_Ptr
Definition: bezier-path.hh:32