From dfe234fd534285988f93393b6e40eb062444095d Mon Sep 17 00:00:00 2001 From: Valenza Florian <fvalenza@laas.fr> Date: Wed, 13 Jul 2016 11:50:03 +0200 Subject: [PATCH] [C++] Renamed members of GeometryData --- src/algorithm/collisions.hpp | 4 ++-- src/multibody/geometry.hpp | 8 ++++---- src/multibody/geometry.hxx | 8 ++++---- src/parsers/utils.hpp | 2 +- src/python/geometry-data.hpp | 6 +++--- src/python/robot_wrapper.py | 2 +- unittest/geom.cpp | 2 +- 7 files changed, 16 insertions(+), 16 deletions(-) diff --git a/src/algorithm/collisions.hpp b/src/algorithm/collisions.hpp index b6c6ad0f3..5e4ba7bd3 100644 --- a/src/algorithm/collisions.hpp +++ b/src/algorithm/collisions.hpp @@ -105,8 +105,8 @@ namespace se3 for (GeometryData::GeomIndex i=0; i < (GeometryData::GeomIndex) data_geom.model_geom.ngeoms; ++i) { const Model::JointIndex & parent = model_geom.geometry_objects[i].parent; - data_geom.oMg_geometries[i] = (data.oMi[parent] * model_geom.geometry_objects[i].placement); - data_geom.oMg_fcl_geometries[i] = toFclTransform3f(data_geom.oMg_geometries[i]); + data_geom.oMg[i] = (data.oMi[parent] * model_geom.geometry_objects[i].placement); + data_geom.oMg_fcl[i] = toFclTransform3f(data_geom.oMg[i]); } } diff --git a/src/multibody/geometry.hpp b/src/multibody/geometry.hpp index ee211562a..ec442535b 100644 --- a/src/multibody/geometry.hpp +++ b/src/multibody/geometry.hpp @@ -366,13 +366,13 @@ struct GeometryObject /// \brief Vector gathering the SE3 placements of the geometry objects relative to the world. /// See updateGeometryPlacements to update the placements. /// - std::vector<se3::SE3> oMg_geometries; + std::vector<se3::SE3> oMg; /// /// \brief Same as oMg but using fcl::Transform3f to store placement. /// This pre-allocation avoids dynamic allocation during collision checking or distance computations. /// - std::vector<fcl::Transform3f> oMg_fcl_geometries; + std::vector<fcl::Transform3f> oMg_fcl; /// /// \brief Vector of collision pairs. /// See addCollisionPair, removeCollisionPair to fill or remove elements in the vector. @@ -397,8 +397,8 @@ struct GeometryObject GeometryData(const Data & data, const GeometryModel & model_geom) : data_ref(data) , model_geom(model_geom) - , oMg_geometries(model_geom.ngeoms) - , oMg_fcl_geometries(model_geom.ngeoms) + , oMg(model_geom.ngeoms) + , oMg_fcl(model_geom.ngeoms) , collision_pairs() , nCollisionPairs(0) , distance_results() diff --git a/src/multibody/geometry.hxx b/src/multibody/geometry.hxx index c36fe8e17..d4012904a 100644 --- a/src/multibody/geometry.hxx +++ b/src/multibody/geometry.hxx @@ -256,8 +256,8 @@ namespace se3 fcl::CollisionRequest collisionRequest (1, false, false, 1, false, true, fcl::GST_INDEP); fcl::CollisionResult collisionResult; - fcl::collide (model_geom.geometry_objects[co1].collision_object.collisionGeometry().get(), oMg_fcl_geometries[co1], - model_geom.geometry_objects[co2].collision_object.collisionGeometry().get(), oMg_fcl_geometries[co2], + fcl::collide (model_geom.geometry_objects[co1].collision_object.collisionGeometry().get(), oMg_fcl[co1], + model_geom.geometry_objects[co2].collision_object.collisionGeometry().get(), oMg_fcl[co2], collisionRequest, collisionResult); return CollisionResult (collisionResult, co1, co2); @@ -294,8 +294,8 @@ namespace se3 fcl::DistanceRequest distanceRequest (true, 0, 0, fcl::GST_INDEP); fcl::DistanceResult result; - fcl::distance ( model_geom.geometry_objects[co1].collision_object.collisionGeometry().get(), oMg_fcl_geometries[co1], - model_geom.geometry_objects[co2].collision_object.collisionGeometry().get(), oMg_fcl_geometries[co2], + fcl::distance ( model_geom.geometry_objects[co1].collision_object.collisionGeometry().get(), oMg_fcl[co1], + model_geom.geometry_objects[co2].collision_object.collisionGeometry().get(), oMg_fcl[co2], distanceRequest, result); return DistanceResult (result, co1, co2); diff --git a/src/parsers/utils.hpp b/src/parsers/utils.hpp index 87e33ab4b..9162336ae 100644 --- a/src/parsers/utils.hpp +++ b/src/parsers/utils.hpp @@ -96,4 +96,4 @@ namespace se3 } // namespace se3 -#endif // __se3_parsers_utils_hpp__ \ No newline at end of file +#endif // __se3_parsers_utils_hpp__ diff --git a/src/python/geometry-data.hpp b/src/python/geometry-data.hpp index 8f4d53b74..1e1478711 100644 --- a/src/python/geometry-data.hpp +++ b/src/python/geometry-data.hpp @@ -94,8 +94,8 @@ namespace se3 .add_property("nCollisionPairs", &GeometryDataPythonVisitor::nCollisionPairs) - .add_property("oMg_geometries", - bp::make_function(&GeometryDataPythonVisitor::oMg_geometries, + .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", @@ -165,7 +165,7 @@ namespace se3 static Index nCollisionPairs(const GeometryDataHandler & m ) { return m->nCollisionPairs; } - static std::vector<SE3> & oMg_geometries(GeometryDataHandler & m) { return m->oMg_geometries; } + 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; } diff --git a/src/python/robot_wrapper.py b/src/python/robot_wrapper.py index 9579d584e..d5dde02f8 100644 --- a/src/python/robot_wrapper.py +++ b/src/python/robot_wrapper.py @@ -188,7 +188,7 @@ class RobotWrapper(object): for visual in self.visual_model.geometry_objects : - M = self.collision_data.oMg_geometries[self.visual_model.getGeometryId(visual.name)] + M = self.collision_data.oMg[self.visual_model.getGeometryId(visual.name)] pinocchioConf = utils.se3ToXYZQUAT(M) viewerConf = utils.XYZQUATToViewerConfiguration(pinocchioConf) self.viewer.gui.applyConfiguration(self.viewerNodeNames(visual), viewerConf) diff --git a/unittest/geom.cpp b/unittest/geom.cpp index 18f24bf55..9198405e0 100644 --- a/unittest/geom.cpp +++ b/unittest/geom.cpp @@ -449,7 +449,7 @@ GeometryPositionsMap_t fillPinocchioGeometryPositions(const se3::GeometryData & GeometryPositionsMap_t result; for (std::size_t i = 0; i < data_geom.model_geom.ngeoms ; ++i) { - result[data_geom.model_geom.getGeometryName(i)] = data_geom.oMg_geometries[i]; + result[data_geom.model_geom.getGeometryName(i)] = data_geom.oMg[i]; } return result; } -- GitLab