From e5dfbf4c931497b925dd7c0f181dbf191ba250e6 Mon Sep 17 00:00:00 2001
From: Valenza Florian <fvalenza@laas.fr>
Date: Wed, 13 Jul 2016 18:19:51 +0200
Subject: [PATCH] [C++][Python] Removed references to kinematic model/data in
geometry model/data. + fixed unallocated memory map ( due to using reserve
keyword instead of resize)
---
benchmark/timings-geometry.cpp | 2 +-
src/multibody/geometry.hpp | 19 +++++--------------
src/multibody/geometry.hxx | 3 +++
src/parsers/srdf.hpp | 4 ++--
src/parsers/urdf/geometry.hxx | 2 +-
src/python/geometry-data.hpp | 8 ++++----
src/python/geometry-model.hpp | 4 ++--
src/python/parsers.hpp | 7 ++++---
src/python/robot_wrapper.py | 2 +-
unittest/geom.cpp | 10 +++++-----
10 files changed, 28 insertions(+), 33 deletions(-)
diff --git a/benchmark/timings-geometry.cpp b/benchmark/timings-geometry.cpp
index 98dd427f4..db0584db5 100644
--- a/benchmark/timings-geometry.cpp
+++ b/benchmark/timings-geometry.cpp
@@ -74,7 +74,7 @@ int main()
se3::GeometryModel geom_model = se3::urdf::buildGeom(model, romeo_filename, package_dirs, COLLISION);
Data data(model);
- GeometryData geom_data(data, geom_model);
+ GeometryData geom_data(geom_model);
geom_data.initializeListOfCollisionPairs();
diff --git a/src/multibody/geometry.hpp b/src/multibody/geometry.hpp
index f30608066..f77a1e341 100644
--- a/src/multibody/geometry.hpp
+++ b/src/multibody/geometry.hpp
@@ -244,9 +244,7 @@ struct GeometryObject
typedef std::vector<GeomIndex> GeomIndexList;
- /// \brief A const reference to the reference model.
- const se3::Model & model;
-
+
/// \brief The number of GeometryObjects
Index ngeoms;
@@ -261,9 +259,8 @@ struct GeometryObject
/// Outer objects can be seen as geometry objects that may often be obstacles to the Inner objects of given joint
std::map < JointIndex, GeomIndexList > outerObjects;
- GeometryModel(const se3::Model & model)
- : model(model)
- , ngeoms(0)
+ GeometryModel()
+ : ngeoms(0)
, geometryObjects()
, innerObjects()
, outerObjects()
@@ -344,11 +341,6 @@ struct GeometryObject
typedef CollisionPair CollisionPair_t;
typedef std::vector<CollisionPair_t> CollisionPairsVector_t;
- ///
- /// \brief A const reference to the data associated to the robot model.
- /// See class Data.
- ///
- const Data & data_ref;
///
/// \brief A const reference to the model storing all the geometries.
@@ -388,9 +380,8 @@ struct GeometryObject
///
std::vector <CollisionResult> collision_results;
- GeometryData(const Data & data, const GeometryModel & model_geom)
- : data_ref(data)
- , model_geom(model_geom)
+ GeometryData(const GeometryModel & model_geom)
+ : model_geom(model_geom)
, oMg(model_geom.ngeoms)
, oMg_fcl(model_geom.ngeoms)
, collision_pairs()
diff --git a/src/multibody/geometry.hxx b/src/multibody/geometry.hxx
index 6b714c52c..05b71ccdc 100644
--- a/src/multibody/geometry.hxx
+++ b/src/multibody/geometry.hxx
@@ -145,6 +145,9 @@ namespace se3
{
collision_pairs.push_back(pair);
nCollisionPairs++;
+
+ distance_results.push_back(DistanceResult( fcl::DistanceResult(), 0, 0));
+ collision_results.push_back(CollisionResult( fcl::CollisionResult(), 0, 0));
}
}
diff --git a/src/parsers/srdf.hpp b/src/parsers/srdf.hpp
index 52736cd1c..aa82fa354 100644
--- a/src/parsers/srdf.hpp
+++ b/src/parsers/srdf.hpp
@@ -41,7 +41,8 @@ namespace se3
/// \param[in] filename The complete path to the SRDF file.
/// \param[in] verbose Verbosity mode.
///
- inline void removeCollisionPairsFromSrdf(const GeometryModel & model_geom,
+ inline void removeCollisionPairsFromSrdf(const Model& model,
+ const GeometryModel & model_geom,
GeometryData & data_geom,
const std::string & filename,
const bool verbose) throw (std::invalid_argument)
@@ -68,7 +69,6 @@ namespace se3
read_xml(srdf_stream, pt);
// Iterate over collision pairs
- const se3::Model & model = model_geom.model;
BOOST_FOREACH(const ptree::value_type & v, pt.get_child("robot"))
{
if (v.first == "disable_collisions")
diff --git a/src/parsers/urdf/geometry.hxx b/src/parsers/urdf/geometry.hxx
index 90130f921..006586aeb 100644
--- a/src/parsers/urdf/geometry.hxx
+++ b/src/parsers/urdf/geometry.hxx
@@ -217,7 +217,7 @@ namespace se3
throw std::invalid_argument(exception_message);
}
- GeometryModel model_geom(model);
+ GeometryModel model_geom;
std::vector<std::string> hint_directories(package_dirs);
diff --git a/src/python/geometry-data.hpp b/src/python/geometry-data.hpp
index 7d68d3435..35225c0c6 100644
--- a/src/python/geometry-data.hpp
+++ b/src/python/geometry-data.hpp
@@ -89,8 +89,8 @@ namespace se3
.def("__init__",
bp::make_constructor(&GeometryDataPythonVisitor::makeDefault,
bp::default_call_policies(),
- (bp::arg("data"),bp::arg("geometry_model"))),
- "Initialize from data and the geometry model.")
+ (bp::arg("geometry_model"))),
+ "Initialize from the geometry model.")
.add_property("nCollisionPairs", &GeometryDataPythonVisitor::nCollisionPairs)
@@ -155,9 +155,9 @@ namespace se3
;
}
- static GeometryDataHandler* makeDefault(const DataHandler & data, const GeometryModelHandler & geometry_model)
+ static GeometryDataHandler* makeDefault(const GeometryModelHandler & geometry_model)
{
- return new GeometryDataHandler(new GeometryData(*data, *geometry_model), true);
+ return new GeometryDataHandler(new GeometryData(*geometry_model), true);
}
static Index nCollisionPairs(const GeometryDataHandler & m ) { return m->nCollisionPairs; }
diff --git a/src/python/geometry-model.hpp b/src/python/geometry-model.hpp
index 3c5698bbc..b9f1a67c4 100644
--- a/src/python/geometry-model.hpp
+++ b/src/python/geometry-model.hpp
@@ -91,9 +91,9 @@ namespace se3
- static GeometryModelHandler maker_default(const ModelHandler & model)
+ static GeometryModelHandler maker_default()
{
- return GeometryModelHandler(new GeometryModel(*model), true);
+ return GeometryModelHandler(new GeometryModel(), true);
}
diff --git a/src/python/parsers.hpp b/src/python/parsers.hpp
index 485d438e7..7464a2dbf 100644
--- a/src/python/parsers.hpp
+++ b/src/python/parsers.hpp
@@ -111,13 +111,14 @@ namespace se3
return GeometryModelHandler(geometry_model, true);
}
- static void removeCollisionPairsFromSrdf(GeometryModelHandler & model,
+ static void removeCollisionPairsFromSrdf(ModelHandler & model,
+ GeometryModelHandler& geometry_model,
GeometryDataHandler & geometry_data,
const std::string & filename,
bool verbose
)
{
- se3::srdf::removeCollisionPairsFromSrdf(*model, *geometry_data, filename, verbose);
+ se3::srdf::removeCollisionPairsFromSrdf(*model, *geometry_model ,*geometry_data, filename, verbose);
}
#endif // #ifdef WITH_HPP_FCL
@@ -184,7 +185,7 @@ namespace se3
"(remember to create the corresponding data structures).");
bp::def("removeCollisionPairsFromSrdf",removeCollisionPairsFromSrdf,
- bp::args("GeometryModel associated to a GeometryData", "GeometryData for which we want to remove pairs of collision", "srdf filename (string)", "verbosity"
+ 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"
),
"Parse an srdf file in order to desactivate collision pairs for a specific GeometryData and GeometryModel ");
diff --git a/src/python/robot_wrapper.py b/src/python/robot_wrapper.py
index 1b5cf3742..99d070611 100644
--- a/src/python/robot_wrapper.py
+++ b/src/python/robot_wrapper.py
@@ -51,7 +51,7 @@ class RobotWrapper(object):
utils.fromListToVectorOfString(package_dirs), se3.GeometryType.COLLISION)
self.visual_model = se3.buildGeomFromUrdf(self.model, filename,
utils.fromListToVectorOfString(package_dirs), se3.GeometryType.VISUAL)
- self.collision_data = se3.GeometryData(self.data, self.collision_model)
+ self.collision_data = se3.GeometryData(self.collision_model)
self.v0 = utils.zero(self.nv)
self.q0 = utils.zero(self.nq)
diff --git a/unittest/geom.cpp b/unittest/geom.cpp
index 82cc738eb..55cf86d7a 100644
--- a/unittest/geom.cpp
+++ b/unittest/geom.cpp
@@ -144,7 +144,7 @@ BOOST_AUTO_TEST_SUITE ( GeomTest )
BOOST_AUTO_TEST_CASE ( simple_boxes )
{
se3::Model model;
- se3::GeometryModel model_geom(model);
+ se3::GeometryModel model_geom;
using namespace se3;
@@ -162,7 +162,7 @@ BOOST_AUTO_TEST_CASE ( simple_boxes )
model_geom.addGeometryObject(model.getJointId("planar2_joint"),box2, SE3::Identity(), "ff2_collision_object", "");
se3::Data data(model);
- se3::GeometryData data_geom(data, model_geom);
+ se3::GeometryData data_geom(model_geom);
std::cout << "------ Model ------ " << std::endl;
std::cout << model;
@@ -212,7 +212,7 @@ BOOST_AUTO_TEST_CASE ( loading_model )
GeometryModel geometry_model = se3::urdf::buildGeom(model, filename, package_dirs, se3::COLLISION);
Data data(model);
- GeometryData geometry_data(data, geometry_model);
+ GeometryData geometry_data(geometry_model);
Eigen::VectorXd q(model.nq);
q << 0, 0, 0.840252, 0, 0, 0, 1, 0, 0, -0.3490658, 0.6981317, -0.3490658, 0, 0, 0, -0.3490658,
@@ -254,7 +254,7 @@ BOOST_AUTO_TEST_CASE ( romeo_joints_meshes_positions )
Data data(model);
- GeometryData data_geom(data, geom);
+ GeometryData data_geom(geom);
// Configuration to be tested
@@ -362,7 +362,7 @@ BOOST_AUTO_TEST_CASE ( hrp2_mesh_distance)
Data data(model);
- GeometryData data_geom(data, geom);
+ GeometryData data_geom(geom);
// Configuration to be tested
--
GitLab