From 073f585a2e6df7550541b1ec9e18fcd72d191e7f Mon Sep 17 00:00:00 2001
From: Valenza Florian <fvalenza@laas.fr>
Date: Thu, 14 Jan 2016 12:20:46 +0100
Subject: [PATCH] [C++] computeDistances and computeCollisions moved out of
GeometryData. Added a vector of boolean handling the state of whether or not
the pairs are in collision in GeometryData
---
CMakeLists.txt | 3 +
benchmark/timings-geometry.cpp | 17 ++--
src/algorithm/collisions.hpp | 151 +++++++++++++++++++++++++++++++++
src/algorithm/kinematics.hpp | 32 +------
src/multibody/geometry.hpp | 8 +-
src/multibody/geometry.hxx | 20 -----
src/python/algorithms.hpp | 66 +++++++++++++-
src/python/geometry-data.hpp | 11 +--
unittest/geom.cpp | 13 +--
9 files changed, 243 insertions(+), 78 deletions(-)
create mode 100644 src/algorithm/collisions.hpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 71bea9b02..416197110 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -193,6 +193,9 @@ IF(HPP_FCL_FOUND)
multibody/geometry.hpp
multibody/geometry.hxx
)
+ LIST(APPEND ${PROJECT_NAME}_ALGORITHM_HEADERS
+ algorithm/collisions.hpp
+ )
ENDIF(HPP_FCL_FOUND)
IF(LUA5_1_FOUND)
diff --git a/benchmark/timings-geometry.cpp b/benchmark/timings-geometry.cpp
index 06e96b7e2..1394f331b 100644
--- a/benchmark/timings-geometry.cpp
+++ b/benchmark/timings-geometry.cpp
@@ -11,6 +11,7 @@
#include "pinocchio/algorithm/center-of-mass.hpp"
#include "pinocchio/simulation/compute-all-terms.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
+#include "pinocchio/algorithm/collisions.hpp"
#include "pinocchio/multibody/parser/urdf.hpp"
#include "pinocchio/multibody/parser/sample-models.hpp"
@@ -78,7 +79,7 @@ int main()
timer.tic();
SMOOTH(NBT)
{
- updateCollisionGeometry<true>(romeo_model,romeo_data,romeo_model_geom,romeo_data_geom,qs_romeo[_smooth]);
+ updateCollisionGeometry(romeo_model,romeo_data,romeo_model_geom,romeo_data_geom,qs_romeo[_smooth], true);
}
double update_col_time = timer.toc(StackTicToc::US)/NBT - geom_time;
std::cout << "Update Collision Geometry < false > = \t" << update_col_time << " " << StackTicToc::unitName(StackTicToc::US) << std::endl;
@@ -86,7 +87,7 @@ int main()
timer.tic();
SMOOTH(NBT)
{
- updateCollisionGeometry<true>(romeo_model,romeo_data,romeo_model_geom,romeo_data_geom,qs_romeo[_smooth]);
+ updateCollisionGeometry(romeo_model,romeo_data,romeo_model_geom,romeo_data_geom,qs_romeo[_smooth], true);
for (std::vector<se3::GeometryData::CollisionPair_t>::iterator it = romeo_data_geom.collision_pairs.begin(); it != romeo_data_geom.collision_pairs.end(); ++it)
{
romeo_data_geom.collide(it->first, it->second);
@@ -100,8 +101,7 @@ int main()
timer.tic();
SMOOTH(NBT)
{
- updateCollisionGeometry<true>(romeo_model,romeo_data,romeo_model_geom,romeo_data_geom,qs_romeo[_smooth]);
- romeo_data_geom.isColliding();
+ computeCollisions(romeo_model,romeo_data,romeo_model_geom,romeo_data_geom,qs_romeo[_smooth], true);
}
double is_colliding_time = timer.toc(StackTicToc::US)/NBT - (update_col_time + geom_time);
std::cout << "Collision Test : robot in collision? = \t" << is_colliding_time
@@ -111,8 +111,7 @@ int main()
timer.tic();
SMOOTH(1000)
{
- updateCollisionGeometry<true>(romeo_model,romeo_data,romeo_model_geom,romeo_data_geom,qs_romeo[_smooth]);
- romeo_data_geom.computeDistances();
+ computeDistances(romeo_model,romeo_data,romeo_model_geom,romeo_data_geom,qs_romeo[_smooth]);
}
double computeDistancesTime = timer.toc(StackTicToc::US)/1000 - (update_col_time + geom_time);
std::cout << "Compute distance between two geometry objects (mean time) = \t" << computeDistancesTime / romeo_data_geom.nCollisionPairs
@@ -174,8 +173,7 @@ int main()
timer.tic();
SMOOTH(NBD)
{
- updateCollisionGeometry<true>(model_hrp2_pino,data_hrp2_pino,geometry_model_hrp2_pino,data_geom_hrp2_pino,qs_hrp2_pino[_smooth]);
- data_geom_hrp2_pino.isColliding();
+ computeCollisions(model_hrp2_pino,data_hrp2_pino,geometry_model_hrp2_pino,data_geom_hrp2_pino,qs_hrp2_pino[_smooth], true);
}
double is_hrp2_colliding_time_pino = timer.toc(StackTicToc::US)/NBD;
std::cout << "Pinocchio - Collision Test : update + robot in collision? = \t" << is_hrp2_colliding_time_pino
@@ -196,8 +194,7 @@ int main()
timer.tic();
SMOOTH(NBD)
{
- se3::updateCollisionGeometry<true>(model_hrp2_pino, data_hrp2_pino, geometry_model_hrp2_pino, data_geom_hrp2_pino, qs_hrp2_pino[_smooth]);
- data_geom_hrp2_pino.computeDistances();
+ computeDistances(model_hrp2_pino, data_hrp2_pino, geometry_model_hrp2_pino, data_geom_hrp2_pino, qs_hrp2_pino[_smooth]);
}
computeDistancesTime = timer.toc(StackTicToc::US)/NBD ;
std::cout << "Pinocchio - Update + Compute distances" << data_geom_hrp2_pino.nCollisionPairs << " col pairs\t" << computeDistancesTime
diff --git a/src/algorithm/collisions.hpp b/src/algorithm/collisions.hpp
new file mode 100644
index 000000000..fc4f9df8c
--- /dev/null
+++ b/src/algorithm/collisions.hpp
@@ -0,0 +1,151 @@
+//
+// Copyright (c) 2015 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Pinocchio is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef __se3_collisions_hpp__
+#define __se3_collisions_hpp__
+
+#include "pinocchio/multibody/visitor.hpp"
+#include "pinocchio/multibody/model.hpp"
+
+#include "pinocchio/algorithm/kinematics.hpp"
+#include "pinocchio/multibody/geometry.hpp"
+
+namespace se3
+{
+
+
+ inline void updateCollisionGeometry(const Model & model,
+ Data & data,
+ const GeometryModel& geom,
+ GeometryData& data_geom,
+ const Eigen::VectorXd & q,
+ const bool computeGeometry = false
+ );
+
+ inline bool computeCollisions(const Model & model,
+ Data & data,
+ const GeometryModel & model_geom,
+ GeometryData & data_geom,
+ const Eigen::VectorXd & q,
+ const bool stopAtFirstCollision = false
+ );
+
+ inline bool computeCollisions(GeometryData & data_geom,
+ const bool stopAtFirstCollision = false
+ );
+
+ inline void computeDistances(GeometryData & data_geom);
+
+ inline void computeDistances(const Model & model,
+ Data & data,
+ const GeometryModel & model_geom,
+ GeometryData & data_geom,
+ const Eigen::VectorXd & q
+ );
+
+} // namespace se3
+
+/* --- Details -------------------------------------------------------------------- */
+namespace se3
+{
+
+
+inline void updateCollisionGeometry(const Model & model,
+ Data & data,
+ const GeometryModel & model_geom,
+ GeometryData & data_geom,
+ const Eigen::VectorXd & q,
+ const bool computeGeometry
+ )
+{
+ using namespace se3;
+
+ if (computeGeometry) geometry(model, data, q);
+ for (GeometryData::Index i=0; i < (GeometryData::Index) data_geom.model_geom.ngeom; ++i)
+ {
+ const Model::Index & parent = model_geom.geom_parents[i];
+ data_geom.oMg[i] = (data.oMi[parent] * model_geom.geometryPlacement[i]);
+ data_geom.oMg_fcl[i] = toFclTransform3f(data_geom.oMg[i]);
+ }
+}
+
+inline bool computeCollisions(GeometryData & data_geom,
+ const bool stopAtFirstCollision
+ )
+{
+ using namespace se3;
+
+ bool isColliding = false;
+
+ for (std::size_t cpt = 0; cpt < data_geom.collision_pairs.size(); ++cpt)
+ {
+ data_geom.collisions[cpt] = data_geom.collide(data_geom.collision_pairs[cpt].first, data_geom.collision_pairs[cpt].second);
+ isColliding = data_geom.collisions[cpt];
+ if(isColliding && stopAtFirstCollision)
+ {
+ return true;
+ }
+ }
+ return isColliding;
+}
+
+// WARNING, if stopAtFirstcollision = true, then the collisions vector will not be fulfilled.
+inline bool computeCollisions(const Model & model,
+ Data & data,
+ const GeometryModel & model_geom,
+ GeometryData & data_geom,
+ const Eigen::VectorXd & q,
+ const bool stopAtFirstCollision
+ )
+{
+ using namespace se3;
+
+
+ updateCollisionGeometry (model, data, model_geom, data_geom, q, true);
+
+
+ return computeCollisions(data_geom, stopAtFirstCollision);
+
+}
+
+
+inline void computeDistances(GeometryData & data_geom)
+{
+ for (std::size_t cpt = 0; cpt < data_geom.collision_pairs.size(); ++cpt)
+ {
+ data_geom.distances[cpt] = DistanceResult(data_geom.computeDistance(data_geom.collision_pairs[cpt].first, data_geom.collision_pairs[cpt].second),
+ data_geom.collision_pairs[cpt].first,
+ data_geom.collision_pairs[cpt].second
+ );
+ }
+}
+
+inline void computeDistances(const Model & model,
+ Data & data,
+ const GeometryModel & model_geom,
+ GeometryData & data_geom,
+ const Eigen::VectorXd & q
+ )
+{
+ updateCollisionGeometry (model, data, model_geom, data_geom, q, true);
+
+ computeDistances(data_geom);
+}
+} // namespace se3
+
+#endif // ifndef __se3_collisions_hpp__
+
diff --git a/src/algorithm/kinematics.hpp b/src/algorithm/kinematics.hpp
index 30e028ee3..3181e00fb 100644
--- a/src/algorithm/kinematics.hpp
+++ b/src/algorithm/kinematics.hpp
@@ -20,9 +20,6 @@
#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/multibody/model.hpp"
-#ifdef WITH_HPP_FCL
- #include "pinocchio/multibody/geometry.hpp"
-#endif
namespace se3
{
@@ -30,14 +27,7 @@ namespace se3
Data & data,
const Eigen::VectorXd & q);
- #ifdef WITH_HPP_FCL
- template < bool computeGeometry >
- inline void updateCollisionGeometry(const Model & model,
- Data & data,
- const GeometryModel& geom,
- GeometryData& data_geom,
- const Eigen::VectorXd & q);
- #endif
+
inline void kinematics(const Model & model,
Data & data,
@@ -102,27 +92,7 @@ namespace se3
}
}
-#ifdef WITH_HPP_FCL
-
- template < bool computeGeometry >
- inline void updateCollisionGeometry(const Model & model,
- Data & data,
- const GeometryModel & model_geom,
- GeometryData & data_geom,
- const Eigen::VectorXd & q)
- {
- using namespace se3;
-
- if (computeGeometry) geometry(model, data, q);
- for (GeometryData::Index i=0; i < (GeometryData::Index) data_geom.model_geom.ngeom; ++i)
- {
- const Model::Index & parent = model_geom.geom_parents[i];
- data_geom.oMg[i] = (data.oMi[parent] * model_geom.geometryPlacement[i]);
- data_geom.oMg_fcl[i] = toFclTransform3f(data_geom.oMg[i]);
- }
- }
-#endif
struct KinematicsStep : public fusion::JointVisitor<KinematicsStep>
{
typedef boost::fusion::vector< const se3::Model&,
diff --git a/src/multibody/geometry.hpp b/src/multibody/geometry.hpp
index 9f7851de7..886209e31 100644
--- a/src/multibody/geometry.hpp
+++ b/src/multibody/geometry.hpp
@@ -139,8 +139,10 @@ namespace se3
std::vector<fcl::Transform3f> oMg_fcl;
std::vector < CollisionPair_t > collision_pairs;
- std::size_t nCollisionPairs;
+ Index nCollisionPairs;
+
std::vector < DistanceResult > distances;
+ std::vector < bool > collisions;
GeometryData(Data& data, GeometryModel& model_geom)
: data_ref(data)
@@ -150,9 +152,11 @@ namespace se3
, collision_pairs()
, nCollisionPairs(0)
, distances()
+ , collisions()
{
initializeListOfCollisionPairs();
distances.resize(nCollisionPairs, DistanceResult( fcl::DistanceResult(), 0, 0) );
+ collisions.resize(nCollisionPairs, false );
}
@@ -169,11 +173,9 @@ namespace se3
void initializeListOfCollisionPairs();
bool collide(Index co1, Index co2) const;
- bool isColliding() const;
fcl::DistanceResult computeDistance(Index co1, Index co2) const;
void resetDistances();
- void computeDistances ();
std::vector < DistanceResult > distanceResults(); //TODO : to keep or not depending of public or not for distances
diff --git a/src/multibody/geometry.hxx b/src/multibody/geometry.hxx
index 1812367d8..eba1fa6f9 100644
--- a/src/multibody/geometry.hxx
+++ b/src/multibody/geometry.hxx
@@ -195,17 +195,6 @@ namespace se3
return false;
}
- inline bool GeometryData::isColliding() const
- {
- for (std::vector<CollisionPair_t>::const_iterator it = collision_pairs.begin(); it != collision_pairs.end(); ++it)
- {
- if (collide(it->first, it->second))
- {
- return true;
- }
- }
- return false;
- }
inline fcl::DistanceResult GeometryData::computeDistance(Index co1, Index co2) const
{
@@ -222,15 +211,6 @@ namespace se3
std::fill(distances.begin(), distances.end(), DistanceResult( fcl::DistanceResult(), 0, 0) );
}
- inline void GeometryData::computeDistances ()
- {
- std::size_t cpt = 0;
- for (std::vector<CollisionPair_t>::iterator it = collision_pairs.begin(); it != collision_pairs.end(); ++it)
- {
- distances[cpt] = DistanceResult(computeDistance(it->first, it->second), it->first, it->second);
- cpt++;
- }
- }
} // namespace se3
diff --git a/src/python/algorithms.hpp b/src/python/algorithms.hpp
index c971a6cc4..3c62c05fe 100644
--- a/src/python/algorithms.hpp
+++ b/src/python/algorithms.hpp
@@ -34,6 +34,13 @@
#include "pinocchio/simulation/compute-all-terms.hpp"
+#ifdef WITH_HPP_FCL
+#include "pinocchio/multibody/geometry.hpp"
+#include "pinocchio/python/geometry-model.hpp"
+#include "pinocchio/python/geometry-data.hpp"
+#include "pinocchio/algorithm/collisions.hpp"
+#endif
+
namespace se3
{
namespace python
@@ -147,6 +154,39 @@ namespace se3
jointLimits(*model,*data,q);
}
+#ifdef WITH_HPP_FCL
+ static bool computeCollisions_proxy(GeometryDataHandler & data_geom,
+ const bool stopAtFirstCollision)
+ {
+ return computeCollisions(*data_geom, stopAtFirstCollision);
+ }
+
+ static bool computeGeometryAndCollisions_proxy(const ModelHandler & model,
+ DataHandler & data,
+ const GeometryModelHandler & model_geom,
+ GeometryDataHandler & data_geom,
+ const VectorXd_fx & q,
+ const bool & stopAtFirstCollision)
+ {
+ return computeCollisions(*model,*data,*model_geom, *data_geom, q, stopAtFirstCollision);
+ }
+
+ static void computeDistances_proxy(GeometryDataHandler & data_geom)
+ {
+ computeDistances(*data_geom);
+ }
+
+ static void computeGeometryAndDistances_proxy( const ModelHandler & model,
+ DataHandler & data,
+ const GeometryModelHandler & model_geom,
+ GeometryDataHandler & data_geom,
+ const Eigen::VectorXd & q
+ )
+ {
+ computeDistances(*model, *data, *model_geom, *data_geom, q);
+ }
+
+#endif
/* --- Expose --------------------------------------------------------- */
@@ -236,8 +276,32 @@ namespace se3
"Configuration q (size Model::nq)"),
"Compute the maximum limits of all the joints of the model "
"and put the results in data.");
- }
+#ifdef WITH_HPP_FCL
+ bp::def("computeCollisions",computeCollisions_proxy,
+ bp::args("GeometryData","bool"),
+ "Determine if collisions pairs are effectively in collision."
+ );
+
+ bp::def("computeGeometryAndCollisions",computeGeometryAndCollisions_proxy,
+ bp::args("Model","Data","GeometryModel","GeometryData","Configuration q (size Model::nq)", "bool"),
+ "Update the geometry for a given configuration and"
+ "determine if collision pars are effectively in collision"
+ );
+
+ bp::def("computeDistances",computeDistances_proxy,
+ bp::args("GeometryData"),
+ "Compute the distance between each collision pairs."
+ );
+
+ bp::def("computeGeometryAndDistances",computeGeometryAndDistances_proxy,
+ bp::args("Model","Data","GeometryModel","GeometryData","Configuration q (size Model::nq)"),
+ "Update the geometry for a given configuration and"
+ "compute the distance between each collision pairs"
+ );
+
+ }
+#endif
};
}} // namespace se3::python
diff --git a/src/python/geometry-data.hpp b/src/python/geometry-data.hpp
index a7eb6c4e3..5cf499619 100644
--- a/src/python/geometry-data.hpp
+++ b/src/python/geometry-data.hpp
@@ -75,14 +75,14 @@ namespace se3
.add_property("distances",
bp::make_function(&GeometryDataPythonVisitor::distances,
bp::return_internal_reference<>()) )
+ .add_property("collisions",
+ bp::make_function(&GeometryDataPythonVisitor::collisions,
+ bp::return_internal_reference<>()) )
.def("addCollisionPair",&GeometryDataPythonVisitor::isCollisionPair)
.def("removeCollisionPair",&GeometryDataPythonVisitor::isCollisionPair)
.def("isCollisionPair",&GeometryDataPythonVisitor::isCollisionPair)
.def("collide",&GeometryDataPythonVisitor::collide)
- .def("isColliding",&GeometryDataPythonVisitor::isColliding)
- .def("computeDistances",&GeometryDataPythonVisitor::computeDistances)
-
.def("__str__",&GeometryDataPythonVisitor::toString)
@@ -95,16 +95,13 @@ namespace se3
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> & distances( GeometryDataHandler & m ) { return m->distances; }
+ static std::vector<bool> & collisions( GeometryDataHandler & m ) { return m->collisions; }
static void addCollisionPair (GeometryDataHandler & m, Index co1, Index co2) { m -> addCollisionPair(co1, co2);}
static void removeCollisionPair (GeometryDataHandler & m, Index co1, Index co2) { m -> removeCollisionPair(co1, co2);}
static bool isCollisionPair (const GeometryDataHandler & m, Index co1, Index co2) { return m -> isCollisionPair(co1, co2);}
static bool collide(const GeometryDataHandler & m, Index co1, Index co2) { return m -> collide(co1, co2); };
- static bool isColliding(const GeometryDataHandler & m ) { return m -> isColliding(); };
-
- static void computeDistances (GeometryDataHandler & m) { m -> computeDistances(); }
-
diff --git a/unittest/geom.cpp b/unittest/geom.cpp
index 50404e473..ae59536b4 100644
--- a/unittest/geom.cpp
+++ b/unittest/geom.cpp
@@ -34,6 +34,7 @@
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/parser/sample-models.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
+#include "pinocchio/algorithm/collisions.hpp"
#include "pinocchio/multibody/parser/urdf.hpp"
#include "pinocchio/spatial/explog.hpp"
@@ -164,21 +165,21 @@ BOOST_AUTO_TEST_CASE ( simple_boxes )
q << 2, 0, 0,
0, 0, 0 ;
- se3::updateCollisionGeometry<true>(model, data, model_geom, data_geom, q);
+ se3::updateCollisionGeometry(model, data, model_geom, data_geom, q, true);
std::cout << data_geom;
assert(data_geom.collide(0,1) == false && "");
q << 0.99, 0, 0,
0, 0, 0 ;
- se3::updateCollisionGeometry<true>(model, data, model_geom, data_geom, q);
+ se3::updateCollisionGeometry(model, data, model_geom, data_geom, q, true);
std::cout << data_geom;
assert(data_geom.collide(0,1) == true && "");
q << 1.01, 0, 0,
0, 0, 0 ;
- se3::updateCollisionGeometry<true>(model, data, model_geom, data_geom, q);
+ se3::updateCollisionGeometry(model, data, model_geom, data_geom, q, true);
std::cout << data_geom;
assert(data_geom.collide(0,1) == false && "");
}
@@ -204,7 +205,7 @@ BOOST_AUTO_TEST_CASE ( loading_model )
0.6981317, -0.3490658, 0, 0, 1.5, 0.6, -0.5, -1.05, -0.4, -0.3, -0.2, 0, 0, 0, 0,
1.5, -0.6, 0.5, 1.05, -0.4, -0.3, -0.2 ;
- se3::updateCollisionGeometry<true>(robot.first, data, robot.second, data_geom, q);
+ se3::updateCollisionGeometry(robot.first, data, robot.second, data_geom, q, true);
assert(data_geom.collide(1,10) == false && "");
}
@@ -244,7 +245,7 @@ BOOST_AUTO_TEST_CASE ( hrp2_joints_meshes_positions )
assert(q_pino.size() == robot.first.nq && "wrong config size");
- se3::updateCollisionGeometry<true>(robot.first, data, robot.second, data_geom, q_pino);
+ se3::updateCollisionGeometry(robot.first, data, robot.second, data_geom, q_pino, true);
/// ************* HPP ************* ///
@@ -344,7 +345,7 @@ BOOST_AUTO_TEST_CASE ( hrp2_mesh_distance)
assert(q_pino.size() == robot.first.nq && "wrong config size");
- se3::updateCollisionGeometry<true>(robot.first, data, robot.second, data_geom, q_pino);
+ se3::updateCollisionGeometry(robot.first, data, robot.second, data_geom, q_pino, true);
/// ************* HPP ************* ///
--
GitLab