Commit c3e10aa6 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

[ContinuousValidation] Update internal API to prepare multithreading

parent 6575624e
......@@ -80,8 +80,6 @@ namespace hpp {
/// care about obstacles.
virtual void addObstacle (const CollisionObjectConstPtr_t& object);
virtual void setPath(const PathPtr_t &path, bool reverse);
/// Remove a collision pair between a joint and an obstacle
/// \param joint the joint that holds the inner objects,
/// \param obstacle the obstacle to remove.
......@@ -100,6 +98,11 @@ namespace hpp {
virtual ~ContinuousValidation ();
protected:
typedef continuousValidation::BodyPairCollisions_t BodyPairCollisions_t;
static void setPath(BodyPairCollisions_t& bodyPairCollisions,
const PathPtr_t &path, bool reverse);
/// Constructor
/// \param robot the robot for which validation is performed,
/// \param tolerance maximal penetration allowed.
......@@ -107,6 +110,7 @@ namespace hpp {
const value_type& tolerance);
/// Validate interval centered on a path parameter
/// \param bodyPairCollisions collision to consider
/// \param config Configuration at abscissa t on the path.
/// \param t parameter value in the path interval of definition
/// \retval interval interval over which the path is collision-free,
......@@ -115,10 +119,12 @@ namespace hpp {
/// value, false if the body pair is in collision.
/// \note object should be in the positions defined by the configuration
/// of parameter t on the path.
virtual bool validateConfiguration (const Configuration_t& config,
const value_type& t,
interval_t& interval,
PathValidationReportPtr_t& report);
virtual bool validateConfiguration (BodyPairCollisions_t& bodyPairCollisions,
const Configuration_t& config,
const value_type& t,
interval_t& interval,
PathValidationReportPtr_t& report);
DevicePtr_t robot_;
value_type tolerance_;
......@@ -126,9 +132,9 @@ namespace hpp {
void init (ContinuousValidationWkPtr_t weak);
// all BodyPairValidation to validate
continuousValidation::BodyPairCollisions_t bodyPairCollisions_;
BodyPairCollisions_t bodyPairCollisions_;
// BodyPairCollision for which collision is disabled
continuousValidation::BodyPairCollisions_t disabledBodyPairCollisions_;
BodyPairCollisions_t disabledBodyPairCollisions_;
value_type stepSize_;
// Initializer as a delegate
continuousValidation::InitializerPtr_t initializer_;
......@@ -136,8 +142,9 @@ namespace hpp {
// Weak pointer to itself
ContinuousValidationWkPtr_t weak_;
virtual bool validateStraightPath (const PathPtr_t& path,
bool reverse, PathPtr_t& validPart, PathValidationReportPtr_t& report) = 0;
virtual bool validateStraightPath (BodyPairCollisions_t& bodyPairCollisions,
const PathPtr_t& path, bool reverse, PathPtr_t& validPart,
PathValidationReportPtr_t& report) = 0;
/// Validate a set of intervals for a given parameter along a path
///
......
......@@ -80,11 +80,13 @@ namespace hpp {
private:
// Weak pointer to itself
DichotomyWkPtr_t weak_;
bool validateStraightPath (const PathPtr_t& path, bool reverse,
bool validateStraightPath (BodyPairCollisions_t& bodyPairCollisions,
const PathPtr_t& path, bool reverse,
PathPtr_t& validPart,
PathValidationReportPtr_t& report);
template <bool reverse>
bool validateStraightPath (const PathPtr_t& path,
bool validateStraightPath (BodyPairCollisions_t& bodyPairCollisions,
const PathPtr_t& path,
PathPtr_t& validPart,
PathValidationReportPtr_t& report);
}; // class Dichotomy
......
......@@ -79,11 +79,13 @@ namespace hpp {
private:
// Weak pointer to itself
ProgressiveWkPtr_t weak_;
bool validateStraightPath (const PathPtr_t& path, bool reverse,
bool validateStraightPath (BodyPairCollisions_t& bodyPairCollisions,
const PathPtr_t& path, bool reverse,
PathPtr_t& validPart,
PathValidationReportPtr_t& report);
template <bool reverse>
bool validateStraightPath (const PathPtr_t& path,
bool validateStraightPath (BodyPairCollisions_t& bodyPairCollisions,
const PathPtr_t& path,
PathPtr_t& validPart,
PathValidationReportPtr_t& report);
}; // class Progressive
......
......@@ -34,14 +34,16 @@ namespace hpp {
using continuousValidation::SolidSolidCollision;
/// Validate interval centered on a path parameter
/// \param bodyPairCollisions a reference to the pair with smallest interval.
/// \param config Configuration at abscissa tmin on the path.
/// \param t parameter value in the path interval of definition
/// \retval interval interval validated for all validation elements
/// \retval report reason why the interval is not valid,
/// \return true if the configuration is collision free for this parameter
/// value, false otherwise.
bool ContinuousValidation::validateConfiguration(const Configuration_t &config, const value_type &t,
interval_t &interval, PathValidationReportPtr_t &report)
bool ContinuousValidation::validateConfiguration(BodyPairCollisions_t& bodyPairCollisions,
const Configuration_t &config, const value_type &t,
interval_t &interval, PathValidationReportPtr_t &report)
{
interval.first = -std::numeric_limits <value_type>::infinity ();
interval.second = std::numeric_limits <value_type>::infinity ();
......@@ -49,15 +51,15 @@ namespace hpp {
robot.currentConfiguration (config);
robot.computeForwardKinematics();
robot.updateGeometryPlacements();
BodyPairCollisions_t::iterator smallestInterval = bodyPairCollisions_.begin();
BodyPairCollisions_t::iterator smallestInterval = bodyPairCollisions.begin();
if (!validateIntervals<BodyPairCollisions_t, CollisionValidationReportPtr_t>
(bodyPairCollisions_, t, interval, report,
(bodyPairCollisions, t, interval, report,
smallestInterval, robot.d()))
return false;
// Put the smallest interval first so that, at next iteration,
// collision pairs with large interval are not computed.
if (bodyPairCollisions_.size() > 1 && smallestInterval != bodyPairCollisions_.begin())
std::iter_swap (bodyPairCollisions_.begin(), smallestInterval);
if (bodyPairCollisions.size() > 1 && smallestInterval != bodyPairCollisions.begin())
std::iter_swap (bodyPairCollisions.begin(), smallestInterval);
return true;
}
......@@ -117,7 +119,7 @@ namespace hpp {
return true;
}
}
return validateStraightPath(path, reverse, validPart, report);
return validateStraightPath(bodyPairCollisions_, path, reverse, validPart, report);
}
void ContinuousValidation::addObstacle(const CollisionObjectConstPtr_t &object)
......@@ -134,10 +136,11 @@ namespace hpp {
}
}
void ContinuousValidation::setPath(const PathPtr_t &path, bool reverse)
void ContinuousValidation::setPath(BodyPairCollisions_t& bodyPairCollisions,
const PathPtr_t &path, bool reverse)
{
for (BodyPairCollisions_t::iterator itPair = bodyPairCollisions_.begin ();
itPair != bodyPairCollisions_.end (); ++itPair) {
for (BodyPairCollisions_t::iterator itPair = bodyPairCollisions.begin ();
itPair != bodyPairCollisions.end (); ++itPair) {
(*itPair)->path (path, reverse);
}
}
......@@ -146,8 +149,7 @@ namespace hpp {
{
assert (joint);
bool removed = false;
for (BodyPairCollisions_t::iterator itPair =
bodyPairCollisions_.begin();
for (BodyPairCollisions_t::iterator itPair = bodyPairCollisions_.begin();
itPair != bodyPairCollisions_.end(); ++itPair)
{
// If jointA == joint and jointB is the root joint.
......
......@@ -47,22 +47,22 @@ namespace hpp {
}
bool Dichotomy::validateStraightPath
(const PathPtr_t& path, bool reverse, PathPtr_t& validPart,
(BodyPairCollisions_t& bpc, const PathPtr_t& path, bool reverse, PathPtr_t& validPart,
PathValidationReportPtr_t& report)
{
if (reverse) return validateStraightPath<true> (path, validPart, report);
else return validateStraightPath<false>(path, validPart, report);
if (reverse) return validateStraightPath<true> (bpc, path, validPart, report);
else return validateStraightPath<false>(bpc, path, validPart, report);
}
template <bool reverse>
bool Dichotomy::validateStraightPath
(const PathPtr_t& path, PathPtr_t& validPart,
(BodyPairCollisions_t& bodyPairCollisions, const PathPtr_t& path, PathPtr_t& validPart,
PathValidationReportPtr_t& report)
{
// start by validating end of path
bool finished = false;
bool valid = true;
setPath(path, reverse);
setPath(bodyPairCollisions, path, reverse);
Intervals validSubset;
const interval_t& tr (path->timeRange());
value_type t = first(tr, reverse);
......@@ -74,7 +74,7 @@ namespace hpp {
PathValidationReportPtr_t pathReport;
interval_t interval;
if (!success ||
!validateConfiguration (q, t, interval, pathReport)) {
!validateConfiguration (bodyPairCollisions, q, t, interval, pathReport)) {
report = pathReport;
valid = false;
} else {
......
......@@ -57,17 +57,17 @@ namespace hpp {
}
bool Progressive::validateStraightPath
(const PathPtr_t& path, bool reverse, PathPtr_t& validPart,
PathValidationReportPtr_t& report)
(BodyPairCollisions_t& bpc, const PathPtr_t& path,
bool reverse, PathPtr_t& validPart, PathValidationReportPtr_t& report)
{
if (reverse) return validateStraightPath<true> (path, validPart, report);
else return validateStraightPath<false>(path, validPart, report);
if (reverse) return validateStraightPath<true> (bpc, path, validPart, report);
else return validateStraightPath<false>(bpc, path, validPart, report);
}
template <bool reverse>
bool Progressive::validateStraightPath
(const PathPtr_t& path, PathPtr_t& validPart,
PathValidationReportPtr_t& report)
(BodyPairCollisions_t& bodyPairCollisions, const PathPtr_t& path,
PathPtr_t& validPart, PathValidationReportPtr_t& report)
{
// for each IntervalValidation
// - set path,
......@@ -78,7 +78,7 @@ namespace hpp {
PathValidationReportPtr_t pathReport;
interval_t interval;
setPath(path, reverse);
setPath(bodyPairCollisions, path, reverse);
const value_type tmin = tr.first;
const value_type tmax = tr.second;
......@@ -92,7 +92,7 @@ namespace hpp {
bool success = (*path) (q, t);
value_type tprev = t;
if (!success ||
!validateConfiguration (q, t, interval, pathReport)) {
!validateConfiguration (bodyPairCollisions, q, t, interval, pathReport)) {
report = pathReport;
valid = false;
} else {
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment