19 #ifndef HPP_CORE_CONTINUOUS_VALIDATION_HH
20 # define HPP_CORE_CONTINUOUS_VALIDATION_HH
22 # include <hpp/pinocchio/pool.hh>
33 using continuousValidation::IntervalValidation;
168 const std::string& body_b,
191 template <
class Delegate>
void add(
const Delegate& delegate);
195 template <
class Delegate>
void reset();
260 typename IntervalValidations_t::iterator& smallestInterval,
261 pinocchio::DeviceData& data)
263 typename IntervalValidations_t::iterator itMin = validations.begin();
264 for (IntervalValidations_t::iterator itVal (validations.begin());
265 itVal != validations.end(); ++itVal)
271 if (!(*itVal)->validateConfiguration(t, tmpInt, report, data))
274 pathReport->configurationReport = report;
275 pathReport->parameter = t;
280 if (interval.second > tmpInt.second)
283 smallestInterval = itVal;
285 interval.first = std::max(interval.first, tmpInt.first);
286 interval.second = std::min(interval.second, tmpInt.second);
287 assert((*itVal)->path()->length() == 0 || interval.second > interval.first);
288 assert(interval.first <= t);
289 assert(t <= interval.second);
299 void init (ContinuousValidationWkPtr_t weak);
311 ContinuousValidationWkPtr_t weak_;
313 virtual bool validateStraightPath
318 std::vector<Initialize> initialize_;
319 std::vector<AddObstacle> addObstacle_;
323 void ContinuousValidation::add<ContinuousValidation::AddObstacle>
327 void ContinuousValidation::reset<ContinuousValidation::AddObstacle>();
330 void ContinuousValidation::add<ContinuousValidation::Initialize>
334 void ContinuousValidation::reset<ContinuousValidation::Initialize>();
337 (
const Delegate& delegate)
340 "No delegate of this type in class ContinuousValidation.");
345 "No delegate of this type in class ContinuousValidation.");
Definition: continuous-validation.hh:113
ContinuousValidation * owner_
Definition: continuous-validation.hh:124
DeviceWkPtr_t robot_
Definition: continuous-validation.hh:125
AddObstacle(ContinuousValidation &owner)
virtual void doExecute(const CollisionObjectConstPtr_t &object) const
virtual ~AddObstacle()
Definition: continuous-validation.hh:122
ContinuousValidation & owner() const
Definition: continuous-validation.hh:121
Definition: continuous-validation.hh:95
virtual ~Initialize()
Definition: continuous-validation.hh:105
ContinuousValidation * owner_
Definition: continuous-validation.hh:107
Initialize(ContinuousValidation &owner)
virtual void doExecute() const
ContinuousValidation & owner() const
Definition: continuous-validation.hh:104
Definition: continuous-validation.hh:89
void init(ContinuousValidationWkPtr_t weak)
Store weak pointer to itself.
static void setPath(IntervalValidations_t &intervalValidations, const PathPtr_t &path, bool reverse)
virtual bool validate(const PathPtr_t &path, bool reverse, PathPtr_t &validPart, PathValidationReportPtr_t &report)
virtual void addObstacle(const CollisionObjectConstPtr_t &object)
IntervalValidations_t intervalValidations_
All BodyPairValidation to validate.
Definition: continuous-validation.hh:302
continuousValidation::IntervalValidations_t IntervalValidations_t
Definition: continuous-validation.hh:217
virtual void setSecurityMargins(const matrix_t &securityMatrix)
bool validateIntervals(IntervalValidations_t &validations, const value_type &t, interval_t &interval, PathValidationReportPtr_t &pathReport, typename IntervalValidations_t::iterator &smallestInterval, pinocchio::DeviceData &data)
Definition: continuous-validation.hh:258
virtual bool validateConfiguration(IntervalValidations_t &intervalValidations, const Configuration_t &config, const value_type &t, interval_t &interval, PathValidationReportPtr_t &report)
virtual ~ContinuousValidation()
ContinuousValidation(const DevicePtr_t &robot, const value_type &tolerance)
pinocchio::Pool< IntervalValidations_t > bodyPairCollisionPool_
Definition: continuous-validation.hh:306
void addIntervalValidation(const IntervalValidationPtr_t &intervalValidation)
void filterCollisionPairs(const RelativeMotion::matrix_type &relMotion)
void reset()
Definition: continuous-validation.hh:342
DevicePtr_t robot() const
Definition: continuous-validation.hh:207
IntervalValidations_t disabledBodyPairCollisions_
BodyPairCollision for which collision is disabled.
Definition: continuous-validation.hh:304
value_type tolerance_
Definition: continuous-validation.hh:296
virtual void setSecurityMarginBetweenBodies(const std::string &body_a, const std::string &body_b, const value_type &margin)
value_type tolerance() const
Get tolerance value.
Definition: continuous-validation.hh:202
value_type stepSize_
Definition: continuous-validation.hh:308
DevicePtr_t robot_
Definition: continuous-validation.hh:295
void add(const Delegate &delegate)
Definition: continuous-validation.hh:337
virtual void removeObstacleFromJoint(const JointPtr_t &joint, const CollisionObjectConstPtr_t &obstacle)
Definition: obstacle-user.hh:37
Definition: path-validation.hh:36
#define HPP_CORE_DLLAPI
Definition: config.hh:64
shared_ptr< IntervalValidation > IntervalValidationPtr_t
Definition: fwd.hh:243
std::vector< IntervalValidationPtr_t > IntervalValidations_t
Definition: fwd.hh:244
pinocchio::DeviceWkPtr_t DeviceWkPtr_t
Definition: fwd.hh:115
pinocchio::value_type value_type
Definition: fwd.hh:157
pinocchio::JointPtr_t JointPtr_t
Definition: fwd.hh:133
std::pair< value_type, value_type > interval_t
Definition: fwd.hh:158
shared_ptr< ValidationReport > ValidationReportPtr_t
Definition: fwd.hh:207
pinocchio::Configuration_t Configuration_t
Definition: fwd.hh:96
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:114
shared_ptr< Path > PathPtr_t
Definition: fwd.hh:170
shared_ptr< PathValidationReport > PathValidationReportPtr_t
Definition: fwd.hh:299
pinocchio::CollisionObjectConstPtr_t CollisionObjectConstPtr_t
Definition: fwd.hh:90
pinocchio::matrix_t matrix_t
Definition: fwd.hh:145
Definition: bi-rrt-planner.hh:24
Definition: path-validation-report.hh:36
Eigen::Matrix< RelativeMotionType, Eigen::Dynamic, Eigen::Dynamic > matrix_type
Definition: relative-motion.hh:51