Commit 0e3a6e13 authored by jcarpent's avatar jcarpent
Browse files

[Alignment] Introduce aligned_vector to specialize std::vector with aligned types

parent 2f7caa55
......@@ -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)
......
......@@ -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 >());
}
};
......
......@@ -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> >());
}
......
......@@ -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; }
......
......@@ -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); }
......
......@@ -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; }
......
//
// 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__
......@@ -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;
......
......@@ -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__
......@@ -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
///
......
......@@ -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
);
......
......@@ -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
......
......@@ -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;
......
......@@ -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)
);
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment