diff --git a/CMakeLists.txt b/CMakeLists.txt
index dcd569aa3d3f0ae44f6c63fe53620e17bf92c998..839e0c3d2b4259b47e16d093bb81ba33b44f3c66 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -212,6 +212,10 @@ SET(${PROJECT_NAME}_ALGORITHM_HEADERS
algorithm/default-check.hpp
)
+SET(${PROJECT_NAME}_CONTAINER_HEADERS
+ container/aligned-vector.hpp
+ )
+
SET(${PROJECT_NAME}_PARSERS_HEADERS
parsers/sample-models.hpp
parsers/utils.hpp
@@ -265,6 +269,7 @@ SET(HEADERS
${${PROJECT_NAME}_MULTIBODY_HEADERS}
${${PROJECT_NAME}_PARSERS_HEADERS}
${${PROJECT_NAME}_ALGORITHM_HEADERS}
+ ${${PROJECT_NAME}_CONTAINER_HEADERS}
exception.hpp
assert.hpp
)
@@ -280,6 +285,7 @@ MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/parsers")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/parsers/urdf")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/tools")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/algorithm")
+MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/container")
FOREACH(header ${HEADERS})
GET_FILENAME_COMPONENT(headerName ${header} NAME)
diff --git a/bindings/python/data.hpp b/bindings/python/data.hpp
index f403f4211f150e8cfadf95b6532148e7c6f30cc3..eb888e495b31c7d3b41f0c4c55afe535701754ce 100644
--- a/bindings/python/data.hpp
+++ b/bindings/python/data.hpp
@@ -70,19 +70,19 @@ namespace se3
{
cl
- .ADD_DATA_PROPERTY(std::vector<Motion>,a,"Body acceleration")
- .ADD_DATA_PROPERTY(std::vector<Motion>,a_gf,"Body acceleration containing also the gravity acceleration")
- .ADD_DATA_PROPERTY(std::vector<Motion>,v,"Body velocity")
- .ADD_DATA_PROPERTY(std::vector<Force>,f,"Body force")
- .ADD_DATA_PROPERTY(std::vector<SE3>,oMi,"Body absolute placement (wrt world)")
- .ADD_DATA_PROPERTY(std::vector<SE3>,oMf,"frames absolute placement (wrt world)")
- .ADD_DATA_PROPERTY(std::vector<SE3>,liMi,"Body relative placement (wrt parent)")
+ .ADD_DATA_PROPERTY(container::aligned_vector<Motion>,a,"Body acceleration")
+ .ADD_DATA_PROPERTY(container::aligned_vector<Motion>,a_gf,"Body acceleration containing also the gravity acceleration")
+ .ADD_DATA_PROPERTY(container::aligned_vector<Motion>,v,"Body velocity")
+ .ADD_DATA_PROPERTY(container::aligned_vector<Force>,f,"Body force")
+ .ADD_DATA_PROPERTY(container::aligned_vector<SE3>,oMi,"Body absolute placement (wrt world)")
+ .ADD_DATA_PROPERTY(container::aligned_vector<SE3>,oMf,"frames absolute placement (wrt world)")
+ .ADD_DATA_PROPERTY(container::aligned_vector<SE3>,liMi,"Body relative placement (wrt parent)")
.ADD_DATA_PROPERTY_CONST(Eigen::VectorXd,tau,"Joint forces")
.ADD_DATA_PROPERTY_CONST(Eigen::VectorXd,nle,"Non Linear Effects")
.ADD_DATA_PROPERTY_CONST(Eigen::VectorXd,ddq,"Joint accelerations")
- .ADD_DATA_PROPERTY(std::vector<Inertia>,Ycrb,"Inertia of the sub-tree composit rigid body")
+ .ADD_DATA_PROPERTY(container::aligned_vector<Inertia>,Ycrb,"Inertia of the sub-tree composit rigid body")
.ADD_DATA_PROPERTY_CONST(Eigen::MatrixXd,M,"Joint Inertia matrix")
- .ADD_DATA_PROPERTY_CONST(std::vector<Matrix6x>,Fcrb,"Spatial forces set, used in CRBA")
+ .ADD_DATA_PROPERTY_CONST(container::aligned_vector<Matrix6x>,Fcrb,"Spatial forces set, used in CRBA")
.ADD_DATA_PROPERTY(std::vector<int>,lastChild,"Index of the last child (for CRBA)")
.ADD_DATA_PROPERTY(std::vector<int>,nvSubtree,"Dimension of the subtree motion space (for CRBA)")
.ADD_DATA_PROPERTY_CONST(Eigen::MatrixXd,U,"Joint Inertia square root (upper triangle)")
@@ -90,7 +90,7 @@ namespace se3
.ADD_DATA_PROPERTY(std::vector<int>,parents_fromRow,"First previous non-zero row in M (used in Cholesky)")
.ADD_DATA_PROPERTY(std::vector<int>,nvSubtree_fromRow,"")
.ADD_DATA_PROPERTY_CONST(Matrix6x,J,"Jacobian of joint placement")
- .ADD_DATA_PROPERTY(std::vector<SE3>,iMf,"Body placement wrt to algorithm end effector.")
+ .ADD_DATA_PROPERTY(container::aligned_vector<SE3>,iMf,"Body placement wrt to algorithm end effector.")
.ADD_DATA_PROPERTY_CONST(Matrix6x,Ag,
"Centroidal matrix which maps from joint velocity to the centroidal momentum.")
@@ -99,9 +99,9 @@ namespace se3
.ADD_DATA_PROPERTY_CONST(Inertia,Ig,
"Centroidal Composite Rigid Body Inertia.")
- .ADD_DATA_PROPERTY(std::vector<Vector3>,com,"Subtree com position.")
- .ADD_DATA_PROPERTY(std::vector<Vector3>,vcom,"Subtree com velocity.")
- .ADD_DATA_PROPERTY(std::vector<Vector3>,acom,"Subtree com acceleration.")
+ .ADD_DATA_PROPERTY(container::aligned_vector<Vector3>,com,"Subtree com position.")
+ .ADD_DATA_PROPERTY(container::aligned_vector<Vector3>,vcom,"Subtree com velocity.")
+ .ADD_DATA_PROPERTY(container::aligned_vector<Vector3>,acom,"Subtree com acceleration.")
.ADD_DATA_PROPERTY(std::vector<double>,mass,"Subtree total mass.")
.ADD_DATA_PROPERTY_CONST(Matrix3x,Jcom,"Jacobian of center of mass.")
@@ -116,19 +116,19 @@ namespace se3
;
}
- IMPL_DATA_PROPERTY(std::vector<Motion>,a,"Body acceleration")
- IMPL_DATA_PROPERTY(std::vector<Motion>,a_gf,"Body acceleration containing also the gravity acceleration")
- IMPL_DATA_PROPERTY(std::vector<Motion>,v,"Body velocity")
- IMPL_DATA_PROPERTY(std::vector<Force>,f,"Body force")
- IMPL_DATA_PROPERTY(std::vector<SE3>,oMi,"Body absolute placement (wrt world)")
- IMPL_DATA_PROPERTY(std::vector<SE3>,oMf,"frames absolute placement (wrt world)")
- IMPL_DATA_PROPERTY(std::vector<SE3>,liMi,"Body relative placement (wrt parent)")
+ IMPL_DATA_PROPERTY(container::aligned_vector<Motion>,a,"Body acceleration")
+ IMPL_DATA_PROPERTY(container::aligned_vector<Motion>,a_gf,"Body acceleration containing also the gravity acceleration")
+ IMPL_DATA_PROPERTY(container::aligned_vector<Motion>,v,"Body velocity")
+ IMPL_DATA_PROPERTY(container::aligned_vector<Force>,f,"Body force")
+ IMPL_DATA_PROPERTY(container::aligned_vector<SE3>,oMi,"Body absolute placement (wrt world)")
+ IMPL_DATA_PROPERTY(container::aligned_vector<SE3>,oMf,"frames absolute placement (wrt world)")
+ IMPL_DATA_PROPERTY(container::aligned_vector<SE3>,liMi,"Body relative placement (wrt parent)")
IMPL_DATA_PROPERTY_CONST(Eigen::VectorXd,tau,"Joint forces")
IMPL_DATA_PROPERTY_CONST(Eigen::VectorXd,nle,"Non Linear Effects")
IMPL_DATA_PROPERTY_CONST(Eigen::VectorXd,ddq,"Joint acceleration")
- IMPL_DATA_PROPERTY(std::vector<Inertia>,Ycrb,"Inertia of the sub-tree composit rigid body")
+ IMPL_DATA_PROPERTY(container::aligned_vector<Inertia>,Ycrb,"Inertia of the sub-tree composit rigid body")
IMPL_DATA_PROPERTY_CONST(Eigen::MatrixXd,M,"Joint Inertia")
- IMPL_DATA_PROPERTY_CONST(std::vector<Matrix6x>,Fcrb,"Spatial forces set, used in CRBA")
+ IMPL_DATA_PROPERTY_CONST(container::aligned_vector<Matrix6x>,Fcrb,"Spatial forces set, used in CRBA")
IMPL_DATA_PROPERTY(std::vector<int>,lastChild,"Index of the last child (for CRBA)")
IMPL_DATA_PROPERTY(std::vector<int>,nvSubtree,"Dimension of the subtree motion space (for CRBA)")
IMPL_DATA_PROPERTY_CONST(Eigen::MatrixXd,U,"Joint Inertia square root (upper triangle)")
@@ -136,7 +136,7 @@ namespace se3
IMPL_DATA_PROPERTY(std::vector<int>,parents_fromRow,"First previous non-zero row in M (used in Cholesky)")
IMPL_DATA_PROPERTY(std::vector<int>,nvSubtree_fromRow,"")
IMPL_DATA_PROPERTY_CONST(Matrix6x,J,"Jacobian of joint placement")
- IMPL_DATA_PROPERTY(std::vector<SE3>,iMf,"Body placement wrt to algorithm end effector.")
+ IMPL_DATA_PROPERTY(container::aligned_vector<SE3>,iMf,"Body placement wrt to algorithm end effector.")
IMPL_DATA_PROPERTY_CONST(Matrix6x,Ag,
"Centroidal matrix which maps from joint velocity to the centroidal momentum.")
@@ -145,9 +145,9 @@ namespace se3
IMPL_DATA_PROPERTY_CONST(Inertia,Ig,
"Centroidal Composite Rigid Body Inertia.")
- IMPL_DATA_PROPERTY(std::vector<Vector3>,com,"Subtree com position.")
- IMPL_DATA_PROPERTY(std::vector<Vector3>,vcom,"Subtree com velocity.")
- IMPL_DATA_PROPERTY(std::vector<Vector3>,acom,"Subtree com acceleration.")
+ IMPL_DATA_PROPERTY(container::aligned_vector<Vector3>,com,"Subtree com position.")
+ IMPL_DATA_PROPERTY(container::aligned_vector<Vector3>,vcom,"Subtree com velocity.")
+ IMPL_DATA_PROPERTY(container::aligned_vector<Vector3>,acom,"Subtree com acceleration.")
IMPL_DATA_PROPERTY(std::vector<double>,mass,"Subtree total mass.")
IMPL_DATA_PROPERTY_CONST(Matrix3x,Jcom,"Jacobian of center of mass.")
@@ -168,8 +168,8 @@ namespace se3
.def(DataPythonVisitor());
bp::to_python_converter< DataHandler::SmartPtr_t,DataPythonVisitor >();
- bp::class_< std::vector<Vector3> >("StdVec_vec3d")
- .def(bp::vector_indexing_suite< std::vector<Vector3>, true >());
+ bp::class_< container::aligned_vector<Vector3> >("StdVec_vec3d")
+ .def(bp::vector_indexing_suite< container::aligned_vector<Vector3>, true >());
}
};
diff --git a/bindings/python/frame.hpp b/bindings/python/frame.hpp
index c255ef3b349218c80d1c4c600f55af270675fec5..5b3c73a3ed681d7d9de50f8e4d43223e755efdd7 100644
--- a/bindings/python/frame.hpp
+++ b/bindings/python/frame.hpp
@@ -26,6 +26,7 @@
#include "pinocchio/bindings/python/se3.hpp"
#include "pinocchio/multibody/frame.hpp"
#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/container/aligned-vector.hpp"
namespace se3
{
@@ -92,7 +93,7 @@ namespace se3
// bp::to_python_converter< Frame,FramePythonVisitor >();
bp::class_< std::vector<Frame> >("StdVec_Frame")
- .def(bp::vector_indexing_suite< std::vector<Frame> >());
+ .def(bp::vector_indexing_suite< container::aligned_vector<Frame> >());
}
diff --git a/bindings/python/geometry-data.hpp b/bindings/python/geometry-data.hpp
index 1510b6139f33bdcc04f15d12c9841cdad4d6e735..c709e61651b19628e6a079f963299f2ad4ced13c 100644
--- a/bindings/python/geometry-data.hpp
+++ b/bindings/python/geometry-data.hpp
@@ -157,7 +157,7 @@ namespace se3
return new GeometryDataHandler(new GeometryData(*geometry_model), true);
}
- static std::vector<SE3> & oMg(GeometryDataHandler & m) { return m->oMg; }
+ static container::aligned_vector<SE3> & oMg(GeometryDataHandler & m) { return m->oMg; }
#ifdef WITH_HPP_FCL
static std::vector<bool> & activeCollisionPairs(GeometryDataHandler & m) { return m->activeCollisionPairs; }
static fcl::DistanceRequest & distanceRequest( GeometryDataHandler & m ) { return m->distanceRequest; }
diff --git a/bindings/python/geometry-model.hpp b/bindings/python/geometry-model.hpp
index 7fa8a1ece64c6ce96ca1cf074b6d590ed1a07ce5..187a70d5ab8df86a1768e804d0866fe508da89b8 100644
--- a/bindings/python/geometry-model.hpp
+++ b/bindings/python/geometry-model.hpp
@@ -112,7 +112,7 @@ namespace se3
{ return gmodelPtr->existGeometryName(name);}
- static std::vector<GeometryObject> & geometryObjects( GeometryModelHandler & m ) { return m->geometryObjects; }
+ static container::aligned_vector<GeometryObject> & geometryObjects( GeometryModelHandler & m ) { return m->geometryObjects; }
static GeomIndex addGeometryObject( GeometryModelHandler & m, GeometryObject gobject, const ModelHandler & model, const bool autofillJointParent)
{ return m-> addGeometryObject(gobject, *model, autofillJointParent); }
diff --git a/bindings/python/model.hpp b/bindings/python/model.hpp
index a9136c0cdfef877a4edadc75065ef01c4cad7a99..f5e20bd661fd692a4c99397042cb7169b27a9709 100644
--- a/bindings/python/model.hpp
+++ b/bindings/python/model.hpp
@@ -163,8 +163,8 @@ namespace se3
static int njoints( ModelHandler & m ) { return m->njoints; }
static int nbodies( ModelHandler & m ) { return m->nbodies; }
static int nframes( ModelHandler & m ) { return m->nframes; }
- static std::vector<Inertia> & inertias( ModelHandler & m ) { return m->inertias; }
- static std::vector<SE3> & jointPlacements( ModelHandler & m ) { return m->jointPlacements; }
+ static container::aligned_vector<Inertia> & inertias( ModelHandler & m ) { return m->inertias; }
+ static container::aligned_vector<SE3> & jointPlacements( ModelHandler & m ) { return m->jointPlacements; }
static JointModelVector & joints( ModelHandler & m ) { return m->joints; }
static std::vector<Model::JointIndex> & parents( ModelHandler & m ) { return m->parents; }
static std::vector<std::string> & names ( ModelHandler & m ) { return m->names; }
@@ -173,7 +173,7 @@ namespace se3
static Eigen::VectorXd velocityLimit(ModelHandler & m) {return m->velocityLimit;}
static Eigen::VectorXd lowerPositionLimit(ModelHandler & m) {return m->lowerPositionLimit;}
static Eigen::VectorXd upperPositionLimit(ModelHandler & m) {return m->upperPositionLimit;}
- static std::vector<Frame> & frames ( ModelHandler & m ) {return m->frames; }
+ static container::aligned_vector<Frame> & frames ( ModelHandler & m ) {return m->frames; }
static std::vector<Model::IndexVector> & subtrees(ModelHandler & m) { return m->subtrees; }
static Motion gravity( ModelHandler & m ) { return m->gravity; }
diff --git a/src/container/aligned-vector.hpp b/src/container/aligned-vector.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..670325f89477d79850f4bea8a15365d478c0d27d
--- /dev/null
+++ b/src/container/aligned-vector.hpp
@@ -0,0 +1,60 @@
+//
+// Copyright (c) 2016 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_container_aligned_vector_hpp__
+#define __se3_container_aligned_vector_hpp__
+
+#include <vector>
+#include <Eigen/StdVector>
+
+namespace se3
+{
+ namespace container
+ {
+ ///
+ /// \brief Specialization of an std::vector with an aligned allocator. This specialization might be used when the type T is or contains some Eigen members.
+ ///
+ /// \tparam T Type of the elements.
+ ///
+ template<typename T>
+ class aligned_vector : public std::vector<T, Eigen::aligned_allocator<T> >
+ {
+ typedef ::std::vector<T, Eigen::aligned_allocator<T> > vector_base;
+ public:
+ typedef T value_type;
+ typedef typename vector_base::allocator_type allocator_type;
+ typedef typename vector_base::size_type size_type;
+ typedef typename vector_base::iterator iterator;
+
+ explicit aligned_vector(const allocator_type & a = allocator_type()) : vector_base(a) {}
+ template<typename InputIterator>
+ aligned_vector(InputIterator first, InputIterator last, const allocator_type& a = allocator_type())
+ : vector_base(first, last, a) {}
+ aligned_vector(const aligned_vector & c) : vector_base(c) {}
+ explicit aligned_vector(size_type num, const value_type & val = value_type())
+ : vector_base(num, val) {}
+ aligned_vector(iterator start, iterator end) : vector_base(start, end) {}
+ aligned_vector & operator=(const aligned_vector& x)
+ { vector_base::operator=(x); return *this; }
+
+ }; // struct aligned_vector
+
+ } // namespace container
+
+} // namespace se3
+
+#endif // ifndef __se3_container_aligned_vector_hpp__
diff --git a/src/multibody/fcl.hpp b/src/multibody/fcl.hpp
index 1587e4b5a09b248ba39612c4803ed48e79431e75..4c1288256fbfaed6a3686c2b044d8b0cbd0afb8f 100644
--- a/src/multibody/fcl.hpp
+++ b/src/multibody/fcl.hpp
@@ -19,6 +19,7 @@
#define __se3_fcl_hpp__
#include "pinocchio/multibody/fwd.hpp"
+#include "pinocchio/container/aligned-vector.hpp"
#ifdef WITH_HPP_FCL
#include <hpp/fcl/collision_object.h>
@@ -84,7 +85,8 @@ enum GeometryType
struct GeometryObject
{
-
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
/// \brief Name of the geometry object
std::string name;
diff --git a/src/multibody/frame.hpp b/src/multibody/frame.hpp
index c440277efc64bdc7f9c4cd352512347967a04bc6..4d0b01bc1e26d150f609bb58e603c1f279f686a2 100644
--- a/src/multibody/frame.hpp
+++ b/src/multibody/frame.hpp
@@ -22,6 +22,7 @@
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/multibody/fwd.hpp"
#include "pinocchio/tools/string-generator.hpp"
+#include "pinocchio/container/aligned-vector.hpp"
#include <Eigen/StdVector>
#include <string>
@@ -109,6 +110,4 @@ namespace se3
} // namespace se3
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::Frame)
-
#endif // ifndef __se3_frame_hpp__
diff --git a/src/multibody/geometry.hpp b/src/multibody/geometry.hpp
index e9b00f5578b4b1246ec3e8ddf2c79d3af9c5f39b..f3fddd215f570105706eaebe8bf1add296c7e3e8 100644
--- a/src/multibody/geometry.hpp
+++ b/src/multibody/geometry.hpp
@@ -21,6 +21,7 @@
#include "pinocchio/multibody/fcl.hpp"
#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/container/aligned-vector.hpp"
#include <iostream>
@@ -36,12 +37,13 @@ namespace se3
struct GeometryModel
{
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// \brief The number of GeometryObjects
Index ngeoms;
/// \brief Vector of GeometryObjects used for collision computations
- std::vector<GeometryObject> geometryObjects;
+ container::aligned_vector<GeometryObject> geometryObjects;
///
/// \brief Vector of collision pairs.
///
@@ -155,6 +157,8 @@ namespace se3
struct GeometryData
{
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
///
/// \brief Vector gathering the SE3 placements of the geometry objects relative to the world.
/// See updateGeometryPlacements to update the placements.
@@ -162,7 +166,7 @@ namespace se3
/// oMg is used for pinocchio (kinematics) computation but is translated to fcl type
/// for fcl (collision) computation. The copy is done in collisionObjects[i]->setTransform(.)
///
- std::vector<se3::SE3> oMg;
+ container::aligned_vector<se3::SE3> oMg;
#ifdef WITH_HPP_FCL
///
diff --git a/src/multibody/geometry.hxx b/src/multibody/geometry.hxx
index 1d706b5939abc65116b69a865b0a9bd8e1bd7f46..ffba5feecbd585c629bc8206d1f304a345e475a7 100644
--- a/src/multibody/geometry.hxx
+++ b/src/multibody/geometry.hxx
@@ -74,7 +74,7 @@ namespace se3
inline GeomIndex GeometryModel::getGeometryId(const std::string & name) const
{
- std::vector<GeometryObject>::const_iterator it = std::find_if(geometryObjects.begin(),
+ container::aligned_vector<GeometryObject>::const_iterator it = std::find_if(geometryObjects.begin(),
geometryObjects.end(),
boost::bind(&GeometryObject::name, _1) == name
);
diff --git a/src/multibody/joint/joint.hpp b/src/multibody/joint/joint.hpp
index f5f829301131c516cfcd5905920d0328f60389e2..258c2e9942ea935fc86ef9559ff4042f40506950 100644
--- a/src/multibody/joint/joint.hpp
+++ b/src/multibody/joint/joint.hpp
@@ -21,6 +21,7 @@
#include "pinocchio/assert.hpp"
#include "pinocchio/multibody/joint/joint-variant.hpp"
#include "pinocchio/multibody/joint/joint-basic-visitors.hxx"
+#include "pinocchio/container/aligned-vector.hpp"
namespace se3
{
@@ -150,8 +151,8 @@ namespace se3
void setIndexes(JointIndex id,int nq,int nv) { ::se3::setIndexes(*this,id, nq, nv); }
};
- typedef std::vector<JointData, Eigen::aligned_allocator<JointData> > JointDataVector;
- typedef std::vector<JointModel, Eigen::aligned_allocator<JointModel> > JointModelVector;
+ typedef container::aligned_vector<JointData> JointDataVector;
+ typedef container::aligned_vector<JointModel> JointModelVector;
} // namespace se3
diff --git a/src/multibody/model.hpp b/src/multibody/model.hpp
index 6570d6d21f33fde3dc84854c9a461e4ab63a336e..897740569c6f53a22a0b2fdb471d1c89c00efbb5 100644
--- a/src/multibody/model.hpp
+++ b/src/multibody/model.hpp
@@ -29,16 +29,11 @@
#include "pinocchio/multibody/joint/joint.hpp"
#include "pinocchio/deprecated.hh"
#include "pinocchio/tools/string-generator.hpp"
+#include "pinocchio/container/aligned-vector.hpp"
#include <iostream>
#include <Eigen/Cholesky>
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::Inertia)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::Force)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,6,Eigen::Dynamic>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,6,6>)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::SE3::Vector3)
-
namespace se3
{
struct Model
@@ -66,10 +61,10 @@ namespace se3
int nframes;
/// \brief Spatial inertias of the body <i> expressed in the supporting joint frame <i>.
- std::vector<Inertia> inertias;
+ container::aligned_vector<Inertia> inertias;
/// \brief Placement (SE3) of the input of joint <i> regarding to the parent joint output <li>.
- std::vector<SE3> jointPlacements;
+ container::aligned_vector<SE3> jointPlacements;
/// \brief Model of joint <i>, encapsulated in a JointModelAccessor.
JointModelVector joints;
@@ -94,7 +89,7 @@ namespace se3
Eigen::VectorXd upperPositionLimit;
/// \brief Vector of operational frames registered on the model.
- std::vector<Frame> frames;
+ container::aligned_vector<Frame> frames;
/// \brief Vector of subtrees.
/// subtree[j] corresponds to the subtree supported by the joint j.
@@ -365,23 +360,23 @@ namespace se3
JointDataVector joints;
/// \brief Vector of joint accelerations.
- std::vector<Motion> a;
+ container::aligned_vector<Motion> a;
/// \brief Vector of joint accelerations due to the gravity field.
- std::vector<Motion> a_gf;
+ container::aligned_vector<Motion> a_gf;
/// \brief Vector of joint velocities.
- std::vector<Motion> v;
+ container::aligned_vector<Motion> v;
/// \brief Vector of body forces. For each body, the force represents the sum of
/// all external forces acting on the body.
- std::vector<Force> f;
+ container::aligned_vector<Force> f;
/// \brief Vector of absolute joint placements (wrt the world).
- std::vector<SE3> oMi;
+ container::aligned_vector<SE3> oMi;
/// \brief Vector of relative joint placements (wrt the body parent).
- std::vector<SE3> liMi;
+ container::aligned_vector<SE3> liMi;
/// \brief Vector of joint torques (dim model.nv).
Eigen::VectorXd tau;
@@ -392,10 +387,10 @@ namespace se3
Eigen::VectorXd nle;
/// \brief Vector of absolute operationnel frame placements (wrt the world).
- std::vector<SE3> oMf;
+ container::aligned_vector<SE3> oMf;
/// \brief Vector of sub-tree composite rigid body inertias, i.e. the apparent inertia of the subtree supported by the joint.
- std::vector<Inertia> Ycrb;
+ container::aligned_vector<Inertia> Ycrb;
/// \brief The joint space inertia matrix (a square matrix of dim model.nv).
Eigen::MatrixXd M;
@@ -405,7 +400,7 @@ namespace se3
// ABA internal data
/// \brief Inertia matrix of the subtree expressed as dense matrix [ABA]
- std::vector<Inertia::Matrix6> Yaba;
+ container::aligned_vector<Inertia::Matrix6> Yaba;
/// \brief Intermediate quantity corresponding to apparent torque [ABA]
Eigen::VectorXd u; // Joint Inertia
@@ -425,7 +420,7 @@ namespace se3
Inertia Ig;
/// \brief Spatial forces set, used in CRBA and CCRBA
- std::vector<Matrix6x> Fcrb;
+ container::aligned_vector<Matrix6x> Fcrb;
/// \brief Index of the last child (for CRBA)
std::vector<int> lastChild;
@@ -452,16 +447,16 @@ namespace se3
Matrix6x J;
/// \brief Vector of joint placements wrt to algorithm end effector.
- std::vector<SE3> iMf;
+ container::aligned_vector<SE3> iMf;
/// \brief Vector of subtree center of mass positions expressed in the root joint of the subtree. In other words, com[j] is the CoM position of the subtree supported by joint \f$ j \f$ and expressed in the joint frame \f$ j \f$. The element com[0] corresponds to the center of mass position of the whole model and expressed in the global frame.
- std::vector<Eigen::Vector3d> com;
+ container::aligned_vector<Eigen::Vector3d> com;
/// \brief Vector of subtree center of mass linear velocities expressed in the root joint of the subtree. In other words, vcom[j] is the CoM linear velocity of the subtree supported by joint \f$ j \f$ and expressed in the joint frame \f$ j \f$. The element vcom[0] corresponds to the velocity of the CoM of the whole model expressed in the global frame.
- std::vector<Eigen::Vector3d> vcom;
+ container::aligned_vector<Eigen::Vector3d> vcom;
/// \brief Vector of subtree center of mass linear accelerations expressed in the root joint of the subtree. In other words, acom[j] is the CoM linear acceleration of the subtree supported by joint \f$ j \f$ and expressed in the joint frame \f$ j \f$. The element acom[0] corresponds to the acceleration of the CoM of the whole model expressed in the global frame.
- std::vector<Eigen::Vector3d> acom;
+ container::aligned_vector<Eigen::Vector3d> acom;
/// \brief Vector of subtree mass. In other words, mass[j] is the mass of the subtree supported by joint \f$ j \f$. The element mass[0] corrresponds to the total mass of the model.
std::vector<double> mass;
diff --git a/src/multibody/model.hxx b/src/multibody/model.hxx
index 8104ef1756aa701f495c624c5047315e7e3da71e..d425ff6332f118f1782361285b5bdf34734d9aa2 100644
--- a/src/multibody/model.hxx
+++ b/src/multibody/model.hxx
@@ -188,7 +188,7 @@ namespace se3
inline Model::FrameIndex Model::getFrameId ( const std::string & name, const FrameType & type ) const
{
- std::vector<Frame>::const_iterator it = std::find_if( frames.begin()
+ container::aligned_vector<Frame>::const_iterator it = std::find_if( frames.begin()
, frames.end()
, details::FilterFrame (name, type)
);