diff --git a/benchmark/timings-geometry.cpp b/benchmark/timings-geometry.cpp
index 91537a9d390e5d5d1c357963df7193ed99e4892f..fd79b8672c7d81b679bf03b4df5e63c911b8b29c 100644
--- a/benchmark/timings-geometry.cpp
+++ b/benchmark/timings-geometry.cpp
@@ -72,10 +72,10 @@ int main()
se3::Model model = se3::urdf::buildModel(romeo_filename, se3::JointModelFreeFlyer());
se3::GeometryModel geom_model = se3::urdf::buildGeom(model, romeo_filename, package_dirs, COLLISION);
+ geom_model.addAllCollisionPairs();
Data data(model);
GeometryData geom_data(geom_model);
- geom_data.initializeListOfCollisionPairs();
std::vector<VectorXd> qs_romeo (NBT);
@@ -108,13 +108,13 @@ int main()
SMOOTH(NBT)
{
updateGeometryPlacements(model,data,geom_model,geom_data,qs_romeo[_smooth]);
- for (std::vector<se3::GeometryData::CollisionPair_t>::iterator it = geom_data.collision_pairs.begin(); it != geom_data.collision_pairs.end(); ++it)
+ for (std::vector<se3::CollisionPair>::iterator it = geom_model.collisionPairs.begin(); it != geom_model.collisionPairs.end(); ++it)
{
- geom_data.computeCollision(it->first, it->second);
+ geom_data.computeCollision(*it);
}
}
double collideTime = timer.toc(StackTicToc::US)/NBT - (update_col_time + geom_time);
- std::cout << "Collision test between two geometry objects (mean time) = \t" << collideTime / geom_data.nCollisionPairs
+ std::cout << "Collision test between two geometry objects (mean time) = \t" << collideTime / double(geom_model.collisionPairs.size())
<< StackTicToc::unitName(StackTicToc::US) << std::endl;
@@ -134,8 +134,8 @@ int main()
computeDistances(model,data,geom_model,geom_data,qs_romeo[_smooth]);
}
double computeDistancesTime = timer.toc(StackTicToc::US)/NBD - (update_col_time + geom_time);
- std::cout << "Compute distance between two geometry objects (mean time) = \t" << computeDistancesTime / geom_data.nCollisionPairs
- << " " << StackTicToc::unitName(StackTicToc::US) << " " << geom_data.nCollisionPairs << " col pairs" << std::endl;
+ std::cout << "Compute distance between two geometry objects (mean time) = \t" << computeDistancesTime / double(geom_model.collisionPairs.size())
+ << " " << StackTicToc::unitName(StackTicToc::US) << " " << geom_model.collisionPairs.size() << " col pairs" << std::endl;
@@ -244,7 +244,7 @@ hpp::model::HumanoidRobotPtr_t humanoidRobot =
computeDistances(geom_data);
}
computeDistancesTime = timer.toc(StackTicToc::US)/NBD ;
- std::cout << "Pinocchio - Compute distances (D) " << geom_data.nCollisionPairs << " col pairs\t" << computeDistancesTime
+ std::cout << "Pinocchio - Compute distances (D) " << geom_model.collisionPairs.size() << " col pairs\t" << computeDistancesTime
<< " " << StackTicToc::unitName(StackTicToc::US) << std::endl;
timer.tic();
@@ -262,7 +262,7 @@ hpp::model::HumanoidRobotPtr_t humanoidRobot =
computeDistances(model, data, geom_model, geom_data, qs_romeo_pino[_smooth]);
}
computeDistancesTime = timer.toc(StackTicToc::US)/NBD ;
- std::cout << "Pinocchio - Update + Compute distances (K+D) " << geom_data.nCollisionPairs << " col pairs\t" << computeDistancesTime
+ std::cout << "Pinocchio - Update + Compute distances (K+D) " << geom_model.collisionPairs.size() << " col pairs\t" << computeDistancesTime
<< " " << StackTicToc::unitName(StackTicToc::US) << std::endl;
diff --git a/src/algorithm/geometry.hxx b/src/algorithm/geometry.hxx
index b747f4df533a8c7f95a741a0bf1cf3be8da87435..afdb846074deab7b8c99aaf139b0fd414f4e0ec8 100644
--- a/src/algorithm/geometry.hxx
+++ b/src/algorithm/geometry.hxx
@@ -54,13 +54,18 @@ namespace se3
)
{
bool isColliding = false;
+ const GeometryModel & geomModel = data_geom.model_geom;
- for (std::size_t cpt = 0; cpt < data_geom.collision_pairs.size(); ++cpt)
+ for (std::size_t cpt = 0; cpt < geomModel.collisionPairs.size(); ++cpt)
{
- data_geom.collision_results[cpt] = data_geom.computeCollision(data_geom.collision_pairs[cpt].first, data_geom.collision_pairs[cpt].second);
- isColliding |= data_geom.collision_results[cpt].fcl_collision_result.isCollision();
- if(isColliding && stopAtFirstCollision)
- return true;
+ if(data_geom.activeCollisionPairs[cpt])
+ {
+ data_geom.collision_results[cpt]
+ = data_geom.computeCollision(geomModel.collisionPairs[cpt]);
+ isColliding |= data_geom.collision_results[cpt].fcl_collision_result.isCollision();
+ if(isColliding && stopAtFirstCollision)
+ return true;
+ }
}
return isColliding;
@@ -86,19 +91,23 @@ namespace se3
return computeDistances<true>(data_geom);
}
- template <bool ComputeShortest>
+ template <bool COMPUTE_SHORTEST>
inline std::size_t computeDistances(GeometryData & data_geom)
{
+ const GeometryModel & geomModel = data_geom.model_geom;
+ std::size_t min_index = geomModel.collisionPairs.size();
double min_dist = std::numeric_limits<double>::infinity();
- std::size_t min_index = data_geom.collision_pairs.size();
- for (std::size_t cpt = 0; cpt < data_geom.collision_pairs.size(); ++cpt)
+ for (std::size_t cpt = 0; cpt < geomModel.collisionPairs.size(); ++cpt)
{
- data_geom.distance_results[cpt] = data_geom.computeDistance(data_geom.collision_pairs[cpt].first,
- data_geom.collision_pairs[cpt].second);
- if (ComputeShortest && data_geom.distance_results[cpt].distance() < min_dist) {
- min_index = cpt;
- min_dist = data_geom.distance_results[cpt].distance();
- }
+ if(data_geom.activeCollisionPairs[cpt])
+ {
+ data_geom.distance_results[cpt] = data_geom.computeDistance(geomModel.collisionPairs[cpt]);
+ if (COMPUTE_SHORTEST && data_geom.distance_results[cpt].distance() < min_dist)
+ {
+ min_index = cpt;
+ min_dist = data_geom.distance_results[cpt].distance();
+ }
+ }
}
return min_index;
}
diff --git a/src/multibody/geometry.hpp b/src/multibody/geometry.hpp
index a48a41d41a99347eac1bc555bcc8f778136e5698..a3f4c2569fc2096cfb487434cefa2f5d020949eb 100644
--- a/src/multibody/geometry.hpp
+++ b/src/multibody/geometry.hpp
@@ -71,7 +71,7 @@ namespace se3
X.disp(os); return os;
}
}; // struct CollisionPair
-
+ typedef std::vector<CollisionPair> CollisionPairsVector_t;
/**
* @brief Result of distance computation between two CollisionObjects
@@ -81,6 +81,7 @@ namespace se3
typedef Model::Index Index;
typedef Model::GeomIndex GeomIndex;
+ DistanceResult() : fcl_distance_result(), object1(0), object2(0) {}
DistanceResult(fcl::DistanceResult dist_fcl, const GeomIndex co1, const GeomIndex co2)
: fcl_distance_result(dist_fcl), object1(co1), object2(co2)
{}
@@ -140,6 +141,7 @@ namespace se3
CollisionResult(fcl::CollisionResult coll_fcl, const GeomIndex co1, const GeomIndex co2)
: fcl_collision_result(coll_fcl), object1(co1), object2(co2)
{}
+ CollisionResult() : fcl_collision_result(), object1(0), object2(0) {}
bool operator == (const CollisionResult & other) const
{
@@ -252,7 +254,11 @@ struct GeometryObject
/// \brief Vector of GeometryObjects used for collision computations
std::vector<GeometryObject> geometryObjects;
-
+ ///
+ /// \brief Vector of collision pairs.
+ ///
+ CollisionPairsVector_t collisionPairs;
+
/// \brief A list of associated collision GeometryObjects to a given joint Id.
/// Inner objects can be seen as geometry objects that directly move when the associated joint moves
std::map < JointIndex, GeomIndexList > innerObjects;
@@ -264,9 +270,13 @@ struct GeometryObject
GeometryModel()
: ngeoms(0)
, geometryObjects()
+ , collisionPairs()
, innerObjects()
, outerObjects()
- {}
+ {
+ const std::size_t num_max_collision_pairs = (ngeoms * (ngeoms-1))/2;
+ collisionPairs.reserve(num_max_collision_pairs);
+ }
~GeometryModel() {};
@@ -317,6 +327,52 @@ struct GeometryObject
const std::string & getGeometryName(const GeomIndex index) const;
+ ///
+ /// \brief Add a collision pair into the vector of collision_pairs.
+ /// The method check before if the given CollisionPair is already included.
+ ///
+ /// \param[in] pair The CollisionPair to add.
+ ///
+ void addCollisionPair (const CollisionPair & pair);
+
+ ///
+ /// \brief Add all possible collision pairs.
+ ///
+ void addAllCollisionPairs();
+
+ ///
+ /// \brief Remove if exists the CollisionPair from the vector collision_pairs.
+ ///
+ /// \param[in] pair The CollisionPair to remove.
+ ///
+ void removeCollisionPair (const CollisionPair& pair);
+
+ ///
+ /// \brief Remove all collision pairs from collisionPairs. Same as collisionPairs.clear().
+ void removeAllCollisionPairs ();
+
+ ///
+ /// \brief Check if a collision pair exists in collisionPairs.
+ /// See also findCollisitionPair(const CollisionPair & pair).
+ ///
+ /// \param[in] pair The CollisionPair.
+ ///
+ /// \return True if the CollisionPair exists, false otherwise.
+ ///
+ bool existCollisionPair (const CollisionPair & pair) const;
+
+ ///
+ /// \brief Return the index of a given collision pair in collisionPairs.
+ ///
+ /// \param[in] pair The CollisionPair.
+ ///
+ /// \return The index of the CollisionPair in collisionPairs.
+ ///
+ Index findCollisionPair (const CollisionPair & pair) const;
+
+ /// \brief Display on std::cout the list of pairs (is it really useful?).
+ void displayCollisionPairs() const;
+
/**
* @brief Associate a GeometryObject of type COLLISION to a joint's inner objects list
*
@@ -340,9 +396,6 @@ struct GeometryObject
{
typedef Model::Index Index;
typedef Model::GeomIndex GeomIndex;
- typedef CollisionPair CollisionPair_t;
- typedef std::vector<CollisionPair_t> CollisionPairsVector_t;
-
///
/// \brief A const reference to the model storing all the geometries.
@@ -363,14 +416,8 @@ struct GeometryObject
std::vector<fcl::Transform3f> oMg_fcl;
///
/// \brief Vector of collision pairs.
- /// See addCollisionPair, removeCollisionPair to fill or remove elements in the vector.
///
- CollisionPairsVector_t collision_pairs;
-
- ///
- /// \brief Number of collision pairs stored in collision_pairs.
- ///
- Index nCollisionPairs;
+ std::vector<bool> activeCollisionPairs;
///
/// \brief Vector gathering the result of the distance computation for all the collision pairs.
@@ -388,119 +435,26 @@ struct GeometryObject
///
std::vector<double> radius;
- GeometryData(const GeometryModel & model_geom)
- : model_geom(model_geom)
+ GeometryData(const GeometryModel & modelGeom)
+ : model_geom(modelGeom)
, oMg(model_geom.ngeoms)
, oMg_fcl(model_geom.ngeoms)
- , collision_pairs()
- , nCollisionPairs(0)
+ , activeCollisionPairs()
, distance_results()
, collision_results()
, radius()
{
- const std::size_t num_max_collision_pairs = (model_geom.ngeoms * (model_geom.ngeoms-1))/2;
- collision_pairs.reserve(num_max_collision_pairs);
- distance_results.reserve(num_max_collision_pairs);
- collision_results.reserve(num_max_collision_pairs);
+ activeCollisionPairs.resize(modelGeom.collisionPairs.size());
+ distance_results.resize(modelGeom.collisionPairs.size());
+ collision_results.resize(modelGeom.collisionPairs.size());
}
~GeometryData() {};
- ///
- /// \brief Add a collision pair given by the index of the two colliding geometries into the vector of collision_pairs.
- /// The method check before if the given CollisionPair is already included.
- ///
- /// \param[in] co1 Index of the first colliding geometry.
- /// \param[in] co2 Index of the second colliding geometry.
- ///
- void addCollisionPair (const GeomIndex co1, const GeomIndex co2);
-
- ///
- /// \brief Add a collision pair into the vector of collision_pairs.
- /// The method check before if the given CollisionPair is already included.
- ///
- /// \param[in] pair The CollisionPair to add.
- ///
- void addCollisionPair (const CollisionPair_t & pair);
-
- ///
- /// \brief Add all possible collision pairs.
- ///
- void addAllCollisionPairs();
-
- ///
- /// \brief Remove if exists the collision pair given by the index of the two colliding geometries from the vector of collision_pairs.
- ///
- /// \param[in] co1 Index of the first colliding geometry.
- /// \param[in] co2 Index of the second colliding geometry.
- ///
- void removeCollisionPair (const GeomIndex co1, const GeomIndex co2);
-
- ///
- /// \brief Remove if exists the CollisionPair from the vector collision_pairs.
- ///
- /// \param[in] pair The CollisionPair to remove.
- ///
- void removeCollisionPair (const CollisionPair_t& pair);
-
- ///
- /// \brief Remove all collision pairs from collision_pairs. Same as collision_pairs.clear().
- void removeAllCollisionPairs ();
-
- ///
- /// \brief Check if a collision pair given by the index of the two colliding geometries exists in collision_pairs.
- /// See also findCollisitionPair(const GeomIndex co1, const GeomIndex co2).
- ///
- /// \param[in] co1 Index of the first colliding geometry.
- /// \param[in] co2 Index of the second colliding geometry.
- ///
- /// \return True if the CollisionPair exists, false otherwise.
- ///
- bool existCollisionPair (const GeomIndex co1, const GeomIndex co2) const ;
-
- ///
- /// \brief Check if a collision pair exists in collision_pairs.
- /// See also findCollisitionPair(const CollisionPair_t & pair).
- ///
- /// \param[in] pair The CollisionPair.
- ///
- /// \return True if the CollisionPair exists, false otherwise.
- ///
- bool existCollisionPair (const CollisionPair_t & pair) const;
-
- ///
- /// \brief Return the index in collision_pairs of a CollisionPair given by the index of the two colliding geometries.
- ///
- /// \param[in] co1 Index of the first colliding geometry.
- /// \param[in] co2 Index of the second colliding geometry.
- ///
- /// \return The index of the collision pair in collision_pairs.
- ///
- Index findCollisionPair (const GeomIndex co1, const GeomIndex co2) const;
-
- ///
- /// \brief Return the index of a given collision pair in collision_pairs.
- ///
- /// \param[in] pair The CollisionPair.
- ///
- /// \return The index of the CollisionPair in collision_pairs.
- ///
- Index findCollisionPair (const CollisionPair_t & pair) const;
-
- void initializeListOfCollisionPairs();
-
+ void activateCollisionPair(const Index pairId,const bool flag=true);
+ void deactivateCollisionPair(const Index pairId);
- ///
- /// \brief Compute the collision status between two collision objects given by their indexes.
- ///
- /// \param[in] co1 Index of the first collision object.
- /// \param[in] co2 Index of the second collision object.
- ///
- /// \return Return true is the collision objects are colliding.
- ///
- CollisionResult computeCollision(const GeomIndex co1, const GeomIndex co2) const;
-
///
/// \brief Compute the collision status between two collision objects of a given collision pair.
///
@@ -508,7 +462,7 @@ struct GeometryObject
///
/// \return Return true is the collision objects are colliding.
///
- CollisionResult computeCollision(const CollisionPair_t & pair) const;
+ CollisionResult computeCollision(const CollisionPair & pair) const;
///
/// \brief Compute the collision result of all the collision pairs according to
@@ -522,16 +476,6 @@ struct GeometryObject
///
bool isColliding() const;
- ///
- /// \brief Compute the minimal distance between two collision objects given by their indexes.
- ///
- /// \param[in] co1 Index of the first collision object.
- /// \param[in] co2 Index of the second collision object.
- ///
- /// \return An fcl struct containing the distance result.
- ///
- DistanceResult computeDistance(const GeomIndex co1, const GeomIndex co2) const;
-
///
/// \brief Compute the minimal distance between collision objects of a collison pair
///
@@ -539,7 +483,7 @@ struct GeometryObject
///
/// \return An fcl struct containing the distance result.
///
- DistanceResult computeDistance(const CollisionPair_t & pair) const;
+ DistanceResult computeDistance(const CollisionPair & pair) const;
///
/// \brief Compute the distance result for all collision pairs according to
@@ -550,13 +494,6 @@ struct GeometryObject
void resetDistances();
- void displayCollisionPairs() const
- {
- for (std::vector<CollisionPair_t>::const_iterator it = collision_pairs.begin(); it != collision_pairs.end(); ++it)
- {
- std::cout << it-> first << "\t" << it->second << std::endl;
- }
- }
friend std::ostream & operator<<(std::ostream & os, const GeometryData & data_geom);
}; // struct GeometryData
diff --git a/src/multibody/geometry.hxx b/src/multibody/geometry.hxx
index b8f08b879f511348de83c056614a56402f62ea98..4d8aef0c4cec927d65b11dba53d5fada45101439 100644
--- a/src/multibody/geometry.hxx
+++ b/src/multibody/geometry.hxx
@@ -118,127 +118,82 @@ namespace se3
inline std::ostream & operator<< (std::ostream & os, const GeometryData & data_geom)
{
- os << "Nb collision pairs = " << data_geom.nCollisionPairs << std::endl;
+ os << "Nb collision pairs = " << data_geom.activeCollisionPairs.size() << std::endl;
- for(GeometryData::Index i=0;i<(GeometryData::Index)(data_geom.nCollisionPairs);++i)
+ for(GeometryData::Index i=0;i<(GeometryData::Index)(data_geom.activeCollisionPairs.size());++i)
{
- os << "collision object in position " << data_geom.collision_pairs[i] << std::endl;
+ os << "collision object in position " << data_geom.model_geom.collisionPairs[i] << std::endl;
}
return os;
}
- inline void GeometryData::addCollisionPair (const GeomIndex co1, const GeomIndex co2)
+ inline void GeometryModel::addCollisionPair (const CollisionPair & pair)
{
- assert ( co1 != co2);
- assert ( co2 < model_geom.ngeoms);
- CollisionPair_t pair(co1, co2);
-
- addCollisionPair(pair);
- }
-
- inline void GeometryData::addCollisionPair (const CollisionPair_t & pair)
- {
- assert(pair.second < model_geom.ngeoms);
-
- if (!existCollisionPair(pair))
- {
- collision_pairs.push_back(pair);
- nCollisionPairs++;
-
- distance_results.push_back(DistanceResult( fcl::DistanceResult(), 0, 0));
- collision_results.push_back(CollisionResult( fcl::CollisionResult(), 0, 0));
- }
+ assert( (pair.first < ngeoms) && (pair.second < ngeoms) );
+ if (!existCollisionPair(pair)) { collisionPairs.push_back(pair); }
}
- inline void GeometryData::addAllCollisionPairs()
+ inline void GeometryModel::addAllCollisionPairs()
{
removeAllCollisionPairs();
- collision_pairs.reserve((model_geom.ngeoms * (model_geom.ngeoms-1))/2);
- for (Index i = 0; i < model_geom.ngeoms; ++i)
- for (Index j = i+1; j < model_geom.ngeoms; ++j)
- addCollisionPair(i,j);
+ for (Index i = 0; i < ngeoms; ++i)
+ for (Index j = i+1; j < ngeoms; ++j)
+ addCollisionPair(CollisionPair(i,j));
}
- inline void GeometryData::removeCollisionPair (const GeomIndex co1, const GeomIndex co2)
+ inline void GeometryModel::removeCollisionPair (const CollisionPair & pair)
{
- assert(co1 < co2);
- assert(co2 < model_geom.ngeoms);
- assert(existCollisionPair(co1,co2));
+ assert( (pair.first < ngeoms) && (pair.second < ngeoms) );
- removeCollisionPair (CollisionPair_t(co1,co2));
- }
-
- inline void GeometryData::removeCollisionPair (const CollisionPair_t & pair)
- {
- assert(pair.second < model_geom.ngeoms);
-
- CollisionPairsVector_t::iterator it = std::find(collision_pairs.begin(),
- collision_pairs.end(),
+ CollisionPairsVector_t::iterator it = std::find(collisionPairs.begin(),
+ collisionPairs.end(),
pair);
- if (it != collision_pairs.end())
- {
- collision_pairs.erase(it);
- nCollisionPairs--;
- }
+ if (it != collisionPairs.end()) { collisionPairs.erase(it); }
}
- inline void GeometryData::removeAllCollisionPairs ()
- {
- collision_pairs.clear();
- nCollisionPairs = 0;
- }
+ inline void GeometryModel::removeAllCollisionPairs () { collisionPairs.clear(); }
- inline bool GeometryData::existCollisionPair (const GeomIndex co1, const GeomIndex co2) const
+ inline bool GeometryModel::existCollisionPair (const CollisionPair & pair) const
{
- return existCollisionPair(CollisionPair_t(co1,co2));
- }
-
- inline bool GeometryData::existCollisionPair (const CollisionPair_t & pair) const
- {
- return (std::find(collision_pairs.begin(),
- collision_pairs.end(),
- pair) != collision_pairs.end());
- }
-
- inline GeometryData::Index GeometryData::findCollisionPair (const GeomIndex co1, const GeomIndex co2) const
- {
- return findCollisionPair(CollisionPair_t(co1,co2));
+ return (std::find(collisionPairs.begin(),
+ collisionPairs.end(),
+ pair) != collisionPairs.end());
}
- inline GeometryData::Index GeometryData::findCollisionPair (const CollisionPair_t & pair) const
+ inline GeometryModel::Index GeometryModel::findCollisionPair (const CollisionPair & pair) const
{
- CollisionPairsVector_t::const_iterator it = std::find(collision_pairs.begin(),
- collision_pairs.end(),
+ CollisionPairsVector_t::const_iterator it = std::find(collisionPairs.begin(),
+ collisionPairs.end(),
pair);
- return (Index) distance(collision_pairs.begin(), it);
+ return (Index) distance(collisionPairs.begin(), it);
}
-
-// std::vector<Index> GeometryData::findCollisionPairsSupportedBy(const JointIndex joint_id) const
-// {
-//// std::vector<Index> collision_pairs;
-//// for(CollisionPairsVector_t::const_iterator it = collision_pairs.begin();
-//// it != collision_pairs.end(); ++it)
-//// {
-//// if (geom_model.it->first )
-//// }
-// }
-
+ inline void GeometryModel::displayCollisionPairs() const
+ {
+ for (CollisionPairsVector_t::const_iterator it = collisionPairs.begin();
+ it != collisionPairs.end(); ++it)
+ {
+ std::cout << it-> first << "\t" << it->second << std::endl;
+ }
+ }
- inline void GeometryData::initializeListOfCollisionPairs()
+ inline void GeometryData::activateCollisionPair(const Index pairId,const bool flag)
{
- addAllCollisionPairs();
- assert(nCollisionPairs == collision_pairs.size());
+ assert( activeCollisionPairs.size() == model_geom.collisionPairs.size() );
+ assert( pairId < activeCollisionPairs.size() );
+ activeCollisionPairs[pairId] = flag;
}
- inline CollisionResult GeometryData::computeCollision(const GeomIndex co1, const GeomIndex co2) const
+ inline void GeometryData::deactivateCollisionPair(const Index pairId)
{
- return computeCollision(CollisionPair_t(co1,co2));
+ assert( activeCollisionPairs.size() == model_geom.collisionPairs.size() );
+ assert( pairId < activeCollisionPairs.size() );
+ activeCollisionPairs[pairId] = false;
}
-
- inline CollisionResult GeometryData::computeCollision(const CollisionPair_t & pair) const
+
+ inline CollisionResult GeometryData::computeCollision(const CollisionPair & pair) const
{
const Index & co1 = pair.first;
const Index & co2 = pair.second;
@@ -255,29 +210,27 @@ namespace se3
inline void GeometryData::computeAllCollisions()
{
- for(size_t i = 0; i<nCollisionPairs; ++i)
+ assert( activeCollisionPairs.size() == model_geom.collisionPairs.size() );
+ assert( collision_results .size() == model_geom.collisionPairs.size() );
+ for(size_t i = 0; i<model_geom.collisionPairs.size(); ++i)
{
- const CollisionPair_t & pair = collision_pairs[i];
- collision_results[i] = computeCollision(pair.first, pair.second);
+ if(activeCollisionPairs[i])
+ { collision_results[i] = computeCollision(model_geom.collisionPairs[i]); }
}
}
inline bool GeometryData::isColliding() const
{
- for(CollisionPairsVector_t::const_iterator it = collision_pairs.begin(); it != collision_pairs.end(); ++it)
+ for(size_t i = 0; i<model_geom.collisionPairs.size(); ++i)
{
- if (computeCollision(it->first, it->second).fcl_collision_result.isCollision())
+ if (activeCollisionPairs[i]
+ && computeCollision(model_geom.collisionPairs[i]).fcl_collision_result.isCollision())
return true;
}
return false;
}
- inline DistanceResult GeometryData::computeDistance(const GeomIndex co1, const GeomIndex co2) const
- {
- return computeDistance(CollisionPair_t(co1,co2));
- }
-
- inline DistanceResult GeometryData::computeDistance(const CollisionPair_t & pair) const
+ inline DistanceResult GeometryData::computeDistance(const CollisionPair & pair) const
{
const Index & co1 = pair.first;
const Index & co2 = pair.second;
@@ -293,10 +246,10 @@ namespace se3
inline void GeometryData::computeAllDistances ()
{
- for(size_t i = 0; i<nCollisionPairs; ++i)
+ for(size_t i = 0; i<activeCollisionPairs.size(); ++i)
{
- const CollisionPair_t & pair = collision_pairs[i];
- distance_results[i] = computeDistance(pair.first, pair.second);
+ if (activeCollisionPairs[i])
+ distance_results[i] = computeDistance(model_geom.collisionPairs[i]);
}
}
diff --git a/src/parsers/srdf.hpp b/src/parsers/srdf.hpp
index 010a678bb97bd6e3f595ced49e270c3ee3e7d0ae..6c03878f20636e2365a6b7006b34d93872cf411a 100644
--- a/src/parsers/srdf.hpp
+++ b/src/parsers/srdf.hpp
@@ -45,8 +45,7 @@ namespace se3
/// \param[in] verbose Verbosity mode (print removed collision pairs and undefined link inside the model).
///
inline void removeCollisionPairsFromSrdf(const Model& model,
- const GeometryModel & model_geom,
- GeometryData & data_geom,
+ GeometryModel & model_geom,
const std::string & filename,
const bool verbose) throw (std::invalid_argument)
{
@@ -104,7 +103,7 @@ namespace se3
it2 != innerObject2.end();
++it2)
{
- data_geom.removeCollisionPair(CollisionPair(*it1, *it2));
+ model_geom.removeCollisionPair(CollisionPair(*it1, *it2));
if(verbose)
std::cout << "Remove collision pair (" << joint_id1 << "," << joint_id2 << ")" << std::endl;
}
diff --git a/src/python/geometry-data.hpp b/src/python/geometry-data.hpp
index 35225c0c69b249a8b0798fee081c82dac5479634..a027275897cea34dba4669595267a1031c9a821d 100644
--- a/src/python/geometry-data.hpp
+++ b/src/python/geometry-data.hpp
@@ -65,7 +65,6 @@ namespace se3
{
typedef GeometryData::Index Index;
typedef GeometryData::GeomIndex GeomIndex;
- typedef GeometryData::CollisionPair_t CollisionPair_t;
typedef se3::DistanceResult DistanceResult;
typedef se3::CollisionResult CollisionResult;
typedef eigenpy::UnalignedEquivalent<SE3>::type SE3_fx;
@@ -92,16 +91,10 @@ namespace se3
(bp::arg("geometry_model"))),
"Initialize from the geometry model.")
- .add_property("nCollisionPairs", &GeometryDataPythonVisitor::nCollisionPairs)
-
.add_property("oMg",
bp::make_function(&GeometryDataPythonVisitor::oMg,
bp::return_internal_reference<>()),
"Vector of collision objects placement relative to the world.")
- .add_property("collision_pairs",
- bp::make_function(&GeometryDataPythonVisitor::collision_pairs,
- bp::return_internal_reference<>()),
- "Vector of collision pairs.")
.add_property("distance_results",
bp::make_function(&GeometryDataPythonVisitor::distance_results,
bp::return_internal_reference<>()),
@@ -110,29 +103,6 @@ namespace se3
bp::make_function(&GeometryDataPythonVisitor::collision_results,
bp::return_internal_reference<>()) )
- .def("addCollisionPair",&GeometryDataPythonVisitor::addCollisionPair,
- bp::args("co1 (index)","co2 (index)"),
- "Add a collision pair given by the index of the two collision objects."
- " Remark: co1 < co2")
- .def("addAllCollisionPairs",&GeometryDataPythonVisitor::addAllCollisionPairs,
- "Add all collision pairs.")
-
- .def("removeCollisionPair",&GeometryDataPythonVisitor::removeCollisionPair,
- bp::args("co1 (index)","co2 (index)"),
- "Remove a collision pair given by the index of the two collision objects."
- " Remark: co1 < co2")
- .def("removeAllCollisionPairs",&GeometryDataPythonVisitor::removeAllCollisionPairs,
- "Remove all collision pairs.")
-
- .def("existCollisionPair",&GeometryDataPythonVisitor::existCollisionPair,
- bp::args("co1 (index)","co2 (index)"),
- "Check if a collision pair given by the index of the two collision objects exists or not."
- " Remark: co1 < co2")
- .def("findCollisionPair", &GeometryDataPythonVisitor::findCollisionPair,
- bp::args("co1 (index)","co2 (index)"),
- "Return the index of a collision pair given by the index of the two collision objects exists or not."
- " Remark: co1 < co2")
-
.def("computeCollision",&GeometryDataPythonVisitor::computeCollision,
bp::args("co1 (index)","co2 (index)"),
"Check if the two collision objects of a collision pair are in collision."
@@ -160,50 +130,20 @@ namespace se3
return new GeometryDataHandler(new GeometryData(*geometry_model), true);
}
- static Index nCollisionPairs(const GeometryDataHandler & m ) { return m->nCollisionPairs; }
-
static std::vector<SE3> & oMg(GeometryDataHandler & m) { return m->oMg; }
- static std::vector<CollisionPair_t> & collision_pairs( GeometryDataHandler & m ) { return m->collision_pairs; }
static std::vector<DistanceResult> & distance_results( GeometryDataHandler & m ) { return m->distance_results; }
static std::vector<CollisionResult> & collision_results( GeometryDataHandler & m ) { return m->collision_results; }
- static void addCollisionPair (GeometryDataHandler & m, const GeomIndex co1, const GeomIndex co2)
- {
- m->addCollisionPair(co1, co2);
- }
- static void addAllCollisionPairs (GeometryDataHandler & m)
- {
- m->addAllCollisionPairs();
- }
-
- static void removeCollisionPair (GeometryDataHandler & m, const GeomIndex co1, const GeomIndex co2)
- {
- m->removeCollisionPair(co1, co2);
- }
- static void removeAllCollisionPairs (GeometryDataHandler & m)
- {
- m->removeAllCollisionPairs();
- }
-
- static bool existCollisionPair (const GeometryDataHandler & m, const GeomIndex co1, const GeomIndex co2)
- {
- return m->existCollisionPair(co1, co2);
- }
- static GeometryData::Index findCollisionPair (const GeometryDataHandler & m, const GeomIndex co1, const GeomIndex co2)
- {
- return m->findCollisionPair(co1, co2);
- }
-
static CollisionResult computeCollision(const GeometryDataHandler & m, const GeomIndex co1, const GeomIndex co2)
{
- return m->computeCollision(co1, co2);
+ return m->computeCollision(CollisionPair(co1, co2));
}
static bool isColliding(const GeometryDataHandler & m) { return m->isColliding(); }
static void computeAllCollisions(GeometryDataHandler & m) { m->computeAllCollisions(); }
static DistanceResult computeDistance(const GeometryDataHandler & m, const GeomIndex co1, const GeomIndex co2)
{
- return m->computeDistance(co1, co2);
+ return m->computeDistance(CollisionPair(co1, co2));
}
static void computeAllDistances(GeometryDataHandler & m) { m->computeAllDistances(); }
diff --git a/src/python/geometry-model.hpp b/src/python/geometry-model.hpp
index b9f1a67c4a72883d33ae4c07b780ce818753385c..8f01432fc2afe31df56b8c6635f71d29fa051b53 100644
--- a/src/python/geometry-model.hpp
+++ b/src/python/geometry-model.hpp
@@ -63,15 +63,40 @@ namespace se3
void visit(PyClass& cl) const
{
cl
- .add_property("ngeoms", &GeometryModelPythonVisitor::ngeoms)
-
- .def("getGeometryId",&GeometryModelPythonVisitor::getGeometryId)
- .def("existGeometryName",&GeometryModelPythonVisitor::existGeometryName)
- .def("getGeometryName",&GeometryModelPythonVisitor::getGeometryName)
- .add_property("geometryObjects",
- bp::make_function(&GeometryModelPythonVisitor::geometryObjects,
- bp::return_internal_reference<>()) )
- .def("__str__",&GeometryModelPythonVisitor::toString)
+ .add_property("ngeoms", &GeometryModelPythonVisitor::ngeoms)
+
+ .def("getGeometryId",&GeometryModelPythonVisitor::getGeometryId)
+ .def("existGeometryName",&GeometryModelPythonVisitor::existGeometryName)
+ .def("getGeometryName",&GeometryModelPythonVisitor::getGeometryName)
+ .add_property("geometryObjects",
+ bp::make_function(&GeometryModelPythonVisitor::geometryObjects,
+ bp::return_internal_reference<>()) )
+ .def("__str__",&GeometryModelPythonVisitor::toString)
+
+ .add_property("collision_pairs",
+ bp::make_function(&GeometryModelPythonVisitor::collision_pairs,
+ bp::return_internal_reference<>()),
+ "Vector of collision pairs.")
+ .def("addCollisionPair",&GeometryModelPythonVisitor::addCollisionPair,
+ bp::args("co1 (index)","co2 (index)"),
+ "Add a collision pair given by the index of the two collision objects."
+ " Remark: co1 < co2")
+ .def("addAllCollisionPairs",&GeometryModelPythonVisitor::addAllCollisionPairs,
+ "Add all collision pairs.")
+ .def("removeCollisionPair",&GeometryModelPythonVisitor::removeCollisionPair,
+ bp::args("co1 (index)","co2 (index)"),
+ "Remove a collision pair given by the index of the two collision objects."
+ " Remark: co1 < co2")
+ .def("removeAllCollisionPairs",&GeometryModelPythonVisitor::removeAllCollisionPairs,
+ "Remove all collision pairs.")
+ .def("existCollisionPair",&GeometryModelPythonVisitor::existCollisionPair,
+ bp::args("co1 (index)","co2 (index)"),
+ "Check if a collision pair given by the index of the two collision objects exists or not."
+ " Remark: co1 < co2")
+ .def("findCollisionPair", &GeometryModelPythonVisitor::findCollisionPair,
+ bp::args("co1 (index)","co2 (index)"),
+ "Return the index of a collision pair given by the index of the two collision objects exists or not."
+ " Remark: co1 < co2")
.def("BuildGeometryModel",&GeometryModelPythonVisitor::maker_default)
.staticmethod("BuildGeometryModel")
@@ -89,16 +114,33 @@ namespace se3
static std::vector<GeometryObject> & geometryObjects( GeometryModelHandler & m ) { return m->geometryObjects; }
+ static std::vector<CollisionPair> & collision_pairs( GeometryModelHandler & m )
+ { return m->collisionPairs; }
+
+ static void addCollisionPair (GeometryModelHandler & m, const GeomIndex co1, const GeomIndex co2)
+ { m->addCollisionPair(CollisionPair(co1, co2)); }
+
+ static void addAllCollisionPairs (GeometryModelHandler & m)
+ { m->addAllCollisionPairs(); }
+ static void removeCollisionPair (GeometryModelHandler & m, const GeomIndex co1, const GeomIndex co2)
+ { m->removeCollisionPair( CollisionPair(co1,co2) ); }
+
+ static void removeAllCollisionPairs (GeometryModelHandler & m)
+ { m->removeAllCollisionPairs(); }
+
+ static bool existCollisionPair (const GeometryModelHandler & m, const GeomIndex co1, const GeomIndex co2)
+ { return m->existCollisionPair(CollisionPair(co1,co2)); }
+ static GeometryModel::Index findCollisionPair (const GeometryModelHandler & m, const GeomIndex co1,
+ const GeomIndex co2)
+ { return m->findCollisionPair( CollisionPair(co1,co2) ); }
+
static GeometryModelHandler maker_default()
- {
- return GeometryModelHandler(new GeometryModel(), true);
- }
+ { return GeometryModelHandler(new GeometryModel(), true); }
-
static std::string toString(const GeometryModelHandler& m)
- { std::ostringstream s; s << *m; return s.str(); }
+ { std::ostringstream s; s << *m; return s.str(); }
/* --- Expose --------------------------------------------------------- */
static void expose()
diff --git a/src/python/parsers.hpp b/src/python/parsers.hpp
index 7464a2dbf8483b230f4f966477736a8351c2eac9..b54508dde7c7e3e3827490a2f04be38d5bc63748 100644
--- a/src/python/parsers.hpp
+++ b/src/python/parsers.hpp
@@ -54,32 +54,21 @@ namespace se3
#ifdef WITH_URDFDOM
- struct BuildModelVisitor : public boost::static_visitor<ModelHandler>
- {
- const std::string& _filename;
-
- BuildModelVisitor(const std::string& filename): _filename(filename){}
-
- template <typename JointModel> ModelHandler operator()(const JointModel & root_joint) const
- {
- Model * model = new Model();
- *model = se3::urdf::buildModel(_filename, root_joint);
- return ModelHandler(model,true);
- }
- };
static ModelHandler buildModelFromUrdf(const std::string & filename,
bp::object & root_joint_object
)
{
JointModelVariant root_joint = bp::extract<JointModelVariant> (root_joint_object);
- return boost::apply_visitor(BuildModelVisitor(filename), root_joint);
+ Model * model = new Model();
+ se3::urdf::buildModel(filename, root_joint, *model);
+ return ModelHandler(model,true);
}
static ModelHandler buildModelFromUrdf(const std::string & filename)
{
Model * model = new Model();
- *model = se3::urdf::buildModel(filename);
+ se3::urdf::buildModel(filename, *model);
return ModelHandler(model,true);
}
@@ -94,7 +83,8 @@ namespace se3
)
{
std::vector<std::string> hints;
- GeometryModel * geometry_model = new GeometryModel(se3::urdf::buildGeom(*model, filename,hints, type));
+ GeometryModel * geometry_model = new GeometryModel();
+ se3::urdf::buildGeom(*model, filename, type,*geometry_model,hints);
return GeometryModelHandler(geometry_model, true);
}
@@ -106,19 +96,19 @@ namespace se3
const GeometryType type
)
{
- GeometryModel * geometry_model = new GeometryModel(se3::urdf::buildGeom(*model, filename, package_dirs, type));
+ GeometryModel * geometry_model = new GeometryModel();
+ se3::urdf::buildGeom(*model, filename, type,*geometry_model,package_dirs);
return GeometryModelHandler(geometry_model, true);
}
static void removeCollisionPairsFromSrdf(ModelHandler & model,
GeometryModelHandler& geometry_model,
- GeometryDataHandler & geometry_data,
const std::string & filename,
bool verbose
)
{
- se3::srdf::removeCollisionPairsFromSrdf(*model, *geometry_model ,*geometry_data, filename, verbose);
+ se3::srdf::removeCollisionPairsFromSrdf(*model, *geometry_model, filename, verbose);
}
#endif // #ifdef WITH_HPP_FCL
@@ -185,7 +175,7 @@ namespace se3
"(remember to create the corresponding data structures).");
bp::def("removeCollisionPairsFromSrdf",removeCollisionPairsFromSrdf,
- bp::args("Model associated to GeometryData", "GeometryModel associated to a GeometryData", "GeometryData for which we want to remove pairs of collision", "srdf filename (string)", "verbosity"
+ bp::args("Model", "GeometryModel (where pairs are removed)","srdf filename (string)", "verbosity"
),
"Parse an srdf file in order to desactivate collision pairs for a specific GeometryData and GeometryModel ");
diff --git a/unittest/geom.cpp b/unittest/geom.cpp
index 19595a3f565c881a2f82c2f42ea050f4afd32968..059432e40331b31ba634a3292d005043f5fca5d0 100644
--- a/unittest/geom.cpp
+++ b/unittest/geom.cpp
@@ -164,7 +164,7 @@ BOOST_AUTO_TEST_CASE ( simple_boxes )
std::cout << model_geom;
std::cout << "------ DataGeom ------ " << std::endl;
std::cout << data_geom;
- BOOST_CHECK(data_geom.computeCollision(0,1).fcl_collision_result.isCollision() == true);
+ BOOST_CHECK(data_geom.computeCollision(CollisionPair(0,1)).fcl_collision_result.isCollision() == true);
Eigen::VectorXd q(model.nq);
q << 2, 0, 0,
@@ -172,21 +172,21 @@ BOOST_AUTO_TEST_CASE ( simple_boxes )
se3::updateGeometryPlacements(model, data, model_geom, data_geom, q);
std::cout << data_geom;
- BOOST_CHECK(data_geom.computeCollision(0,1).fcl_collision_result.isCollision() == false);
+ BOOST_CHECK(data_geom.computeCollision(CollisionPair(0,1)).fcl_collision_result.isCollision() == false);
q << 0.99, 0, 0,
0, 0, 0 ;
se3::updateGeometryPlacements(model, data, model_geom, data_geom, q);
std::cout << data_geom;
- BOOST_CHECK(data_geom.computeCollision(0,1).fcl_collision_result.isCollision() == true);
+ BOOST_CHECK(data_geom.computeCollision(CollisionPair(0,1)).fcl_collision_result.isCollision() == true);
q << 1.01, 0, 0,
0, 0, 0 ;
se3::updateGeometryPlacements(model, data, model_geom, data_geom, q);
std::cout << data_geom;
- BOOST_CHECK(data_geom.computeCollision(0,1).fcl_collision_result.isCollision() == false);
+ BOOST_CHECK(data_geom.computeCollision(CollisionPair(0,1)).fcl_collision_result.isCollision() == false);
}
BOOST_AUTO_TEST_CASE ( loading_model )
@@ -214,7 +214,7 @@ BOOST_AUTO_TEST_CASE ( loading_model )
1.5, -0.6, 0.5, 1.05, -0.4, -0.3, -0.2 ;
se3::updateGeometryPlacements(model, data, geometry_model, geometry_data, q);
- BOOST_CHECK(geometry_data.computeCollision(1,10).fcl_collision_result.isCollision() == false);
+ BOOST_CHECK(geometry_data.computeCollision(CollisionPair(1,10)).fcl_collision_result.isCollision() == false);
}
@@ -471,8 +471,9 @@ BOOST_AUTO_TEST_CASE ( hrp2_mesh_distance)
std::cout << "comparison between " << body1 << " and " << body2 << std::endl;
- se3::DistanceResult dist_pin = data_geom.computeDistance( geom.getGeometryId(body1),
- geom.getGeometryId(body2));
+ se3::DistanceResult dist_pin
+ = data_geom.computeDistance( CollisionPair(geom.getGeometryId(body1),
+ geom.getGeometryId(body2)) );
Distance_t distance_pin(dist_pin.fcl_distance_result);
distance_hpp.checkClose(distance_pin);