core: use Allocator to support aligned vectors

stack-of-tasks · Apr 10, 2021 · adef26a · adef26a
1 parent 2e79ded
commit adef26a
Show file tree

Hide file tree

Showing 3 changed files with 29 additions and 22 deletions.
diff --git a/bindings/python/algorithm/expose-model.cpp b/bindings/python/algorithm/expose-model.cpp
@@ -50,25 +50,26 @@ namespace pinocchio
     template <typename Scalar, int Options,
               template <typename, int> class JointCollectionTpl,
               typename ConfigVectorType>
-    bp::tuple buildReducedModel(
-        const ModelTpl<Scalar, Options, JointCollectionTpl> &model,
-        const std::vector<GeometryModel> &list_of_geom_models,
-        const std::vector<JointIndex> &list_of_joints_to_lock,
-        const Eigen::MatrixBase<ConfigVectorType> &reference_configuration) {
+    bp::tuple buildReducedModel(const ModelTpl<Scalar, Options, JointCollectionTpl> &model,
+                                const std::vector<GeometryModel,Eigen::aligned_allocator<GeometryModel> > &list_of_geom_models,
+                                const std::vector<JointIndex> &list_of_joints_to_lock,
+                                const Eigen::MatrixBase<ConfigVectorType> &reference_configuration)
+  {
       typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
-      std::vector<GeometryModel> reduced_geom_models;
+      std::vector<GeometryModel,Eigen::aligned_allocator<GeometryModel> > reduced_geom_models;
       Model reduced_model;
       buildReducedModel(model, list_of_geom_models, list_of_joints_to_lock,
-                          reference_configuration, reduced_model,
-                          reduced_geom_models);
+                        reference_configuration, reduced_model,
+                        reduced_geom_models);
       return bp::make_tuple(reduced_model, reduced_geom_models);
     }
 
     void exposeModelAlgo()
     {
       using namespace Eigen;
 
-      StdVectorPythonVisitor<GeometryModel>::expose("StdVec_GeometryModel");
+      typedef std::vector<GeometryModel,Eigen::aligned_allocator<GeometryModel> > GeometryModelVector;
+      StdVectorPythonVisitor<GeometryModel,GeometryModelVector::allocator_type>::expose("StdVec_GeometryModel");
 
       bp::def("appendModel",
               (Model (*)(const Model &, const Model &, const FrameIndex, const SE3 &))&appendModel<double,0,JointCollectionDefaultTpl>,
@@ -105,30 +106,34 @@ namespace pinocchio
               "\treference_configuration: reference configuration to compute the placement of the lock joints\n");
 
       bp::def("buildReducedModel",
-              (bp::tuple (*)(const Model &, const GeometryModel &, const std::vector<JointIndex> &, const Eigen::MatrixBase<VectorXd> &))
+              (bp::tuple (*)(const Model &,
+                             const GeometryModel &,
+                             const std::vector<JointIndex> &,
+                             const Eigen::MatrixBase<VectorXd> &))
               &buildReducedModel<double,0,JointCollectionDefaultTpl,VectorXd>,
               bp::args("model",
                        "geom_model",
                        "list_of_joints_to_lock",
                        "reference_configuration"),
-              "Build a reduced model and a rededuced geometry model  from a given input model,"
-              "a given input geometry model and a list of joint to lock.\n\n"
+              "Build a reduced model and a rededuced geometry model from a given input model,"
+              "an input geometry model and a list of joint to lock.\n\n"
               "Parameters:\n"
               "\tmodel: input kinematic modell to reduce\n"
               "\tgeom_model: input geometry model to reduce\n"
               "\tlist_of_joints_to_lock: list of joint indexes to lock\n"
               "\treference_configuration: reference configuration to compute the placement of the lock joints\n");
 
       bp::def("buildReducedModel",
-              (bp::tuple(*)(const Model &, const std::vector<GeometryModel> &,
+              (bp::tuple(*)(const Model &,
+                            const std::vector<GeometryModel,Eigen::aligned_allocator<GeometryModel> > &,
                             const std::vector<JointIndex> &,
-                            const Eigen::MatrixBase<VectorXd> &)) &
-                  buildReducedModel<double, 0, JointCollectionDefaultTpl, VectorXd>,
+                            const Eigen::MatrixBase<VectorXd> &))
+              buildReducedModel<double, 0, JointCollectionDefaultTpl, VectorXd>,
               bp::args("model", "list_of_geom_models", "list_of_joints_to_lock",
-                      "reference_configuration"),
-              "Build a reduced model and a reduced geometry model from a given "
+                       "reference_configuration"),
+              "Build a reduced model and the related reduced geometry models from a given "
               "input model,"
-              "a given input geometry model and a list of joint to lock.\n\n"
+              "a list of input geometry model and a list of joint to lock.\n\n"
               "Parameters:\n"
               "\tmodel: input kinematic model to reduce\n"
               "\tlist_of_geom_models: input geometry models to reduce\n"

diff --git a/src/algorithm/model.hpp b/src/algorithm/model.hpp
@@ -160,14 +160,15 @@ namespace pinocchio
    */
   template <typename Scalar, int Options,
             template <typename, int> class JointCollectionTpl,
+            typename GeometryModelAllocator,
             typename ConfigVectorType>
   void buildReducedModel(
       const ModelTpl<Scalar, Options, JointCollectionTpl> &model,
-      const std::vector<GeometryModel> &list_of_geom_models,
+      const std::vector<GeometryModel,GeometryModelAllocator> &list_of_geom_models,
       const std::vector<JointIndex> &list_of_joints_to_lock,
       const Eigen::MatrixBase<ConfigVectorType> &reference_configuration,
       ModelTpl<Scalar, Options, JointCollectionTpl> &reduced_model,
-      std::vector<GeometryModel> &list_of_reduced_geom_models);
+      std::vector<GeometryModel,GeometryModelAllocator> &list_of_reduced_geom_models);
 
 } // namespace pinocchio
 

diff --git a/src/algorithm/model.hxx b/src/algorithm/model.hxx
@@ -489,14 +489,15 @@ namespace pinocchio
 
   template <typename Scalar, int Options,
             template <typename, int> class JointCollectionTpl,
+            typename GeometryModelAllocator,
             typename ConfigVectorType>
   void buildReducedModel(
       const ModelTpl<Scalar, Options, JointCollectionTpl> &input_model,
-      const std::vector<GeometryModel> &list_of_geom_models,
+      const std::vector<GeometryModel,GeometryModelAllocator> &list_of_geom_models,
       const std::vector<JointIndex> &list_of_joints_to_lock,
       const Eigen::MatrixBase<ConfigVectorType> &reference_configuration,
       ModelTpl<Scalar, Options, JointCollectionTpl> &reduced_model,
-      std::vector<GeometryModel> &list_of_reduced_geom_models) {
+      std::vector<GeometryModel,GeometryModelAllocator> &list_of_reduced_geom_models) {
 
     typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
     buildReducedModel(input_model, list_of_joints_to_lock, reference_configuration, reduced_model);