Skip to content

Commit

Permalink
std::vector uses proper Eigen allocator alignment
Browse files Browse the repository at this point in the history 
jrl-umi3218/Tasks#23
  • Loading branch information
@ahundt
ahundt committed Feb 21, 2017
1 parent 88c611b commit ac6b40a
Show file tree
Hide file tree
Showing 11 changed files with 47 additions and 44 deletions.
4 changes: 2 additions & 2 deletions binding/python/generate.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -268,7 +268,7 @@ def build_mbc(mbc):
mbc.add_instance_attribute('jointVelocity', 'std::vector<sva::MotionVecd>')
mbc.add_instance_attribute('jointTorque', 'std::vector<std::vector<double> >')

mbc.add_instance_attribute('motionSubspace', 'std::vector<Eigen::MatrixXd>',
mbc.add_instance_attribute('motionSubspace', 'std::vector<Eigen::MatrixXd, Eigen::aligned_allocator<Eigen::MatrixXd> >',
getter='python_motionSubspace',
setter='python_motionSubspace')

Expand Down Expand Up @@ -825,7 +825,7 @@ def build_idim(mod, idim):
rbd.add_container('std::vector<sva::MotionVecd>', 'sva::MotionVecd', 'vector')
rbd.add_container('std::vector<sva::ForceVecd>', 'sva::ForceVecd', 'vector')
rbd.add_container('std::vector<sva::RBInertiad>', 'sva::RBInertiad', 'vector')
rbd.add_container('std::vector<Eigen::MatrixXd>', 'Eigen::MatrixXd', 'vector')
rbd.add_container('std::vector<Eigen::MatrixXd, Eigen::aligned_allocator<Eigen::MatrixXd> >', 'Eigen::MatrixXd', 'vector')

# build map type
rbd.add_container('std::map<std::string, std::string>',
Expand Down
1 change: 1 addition & 0 deletions src/CoM.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@

// Eigen
#include <Eigen/Core>
#include <Eigen/StdVector>

// RBDyn
#include "Jacobian.h"
Expand Down
3 changes: 2 additions & 1 deletion src/FD.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@

// Eigen
#include <Eigen/Core>
#include <Eigen/StdVector>

// SpaceVecAlg
#include <SpaceVecAlg/SpaceVecAlg>
Expand Down Expand Up @@ -111,7 +112,7 @@ class RBDYN_DLLAPI ForwardDynamics

// H computation
std::vector<sva::RBInertiad> I_st_;
std::vector<Eigen::Matrix<double, 6, Eigen::Dynamic>> F_;
std::vector<Eigen::Matrix<double, 6, Eigen::Dynamic>, Eigen::aligned_allocator<Eigen::Matrix<double, 6, Eigen::Dynamic>> > F_;

// C computation
std::vector<sva::MotionVecd> acc_;
Expand Down
6 changes: 3 additions & 3 deletions src/IS.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,7 +60,7 @@ InverseStatics::InverseStatics(const MultiBody& mb)
void InverseStatics::setJacobianSize(
const MultiBody& mb,
const MultiBodyConfig& mbc,
const std::vector<Eigen::MatrixXd>& jacMomentsAndForces)
const std::vector<Eigen::MatrixXd, Eigen::aligned_allocator<Eigen::MatrixXd> >& jacMomentsAndForces)
{
const std::vector<Body>& bodies = mb.bodies();

Expand Down Expand Up @@ -120,7 +120,7 @@ void InverseStatics::inverseStatics(const MultiBody& mb, MultiBodyConfig& mbc)

void InverseStatics::computeTorqueJacobianJoint(
const MultiBody& mb, MultiBodyConfig& mbc,
const std::vector<MatrixXd>& jacMomentsAndForces)
const std::vector<MatrixXd, Eigen::aligned_allocator<Eigen::MatrixXd> >& jacMomentsAndForces)
{
assert(jacMomentsAndForces.size() == static_cast<size_t>(mb.nrBodies()));

Expand Down Expand Up @@ -230,7 +230,7 @@ void InverseStatics::computeTorqueJacobianJoint(
void InverseStatics::computeTorqueJacobianJoint(const MultiBody& mb,
MultiBodyConfig& mbc)
{
std::vector<MatrixXd> jacMandF;
std::vector<MatrixXd, Eigen::aligned_allocator<Eigen::MatrixXd> > jacMandF;
for (int i = 0; i < mb.nrJoints(); ++i) jacMandF.push_back(MatrixXd(0, 0));

computeTorqueJacobianJoint(mb, mbc, jacMandF);
Expand Down
10 changes: 5 additions & 5 deletions src/IS.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,7 @@ class RBDYN_DLLAPI InverseStatics
InverseStatics(const MultiBody& mb);

void setJacobianSize(const MultiBody& mb, const MultiBodyConfig& mbc,
const std::vector<Eigen::MatrixXd>& jacMomentsAndForces);
const std::vector<Eigen::MatrixXd, Eigen::aligned_allocator<Eigen::MatrixXd> >& jacMomentsAndForces);

/**
* Compute the inverse statics.
Expand Down Expand Up @@ -69,7 +69,7 @@ class RBDYN_DLLAPI InverseStatics
*/
void computeTorqueJacobianJoint(
const MultiBody& mb, MultiBodyConfig& mbc,
const std::vector<Eigen::MatrixXd>& jacMomentsAndForces);
const std::vector<Eigen::MatrixXd, Eigen::aligned_allocator<Eigen::MatrixXd> >& jacMomentsAndForces);

/**
* Default version of computeTorqeuJacobienJoint
Expand All @@ -91,7 +91,7 @@ class RBDYN_DLLAPI InverseStatics
*/
const std::vector<sva::ForceVecd>& f() const { return f_; };

const std::vector<Eigen::MatrixXd>& jointTorqueDiff() const
const std::vector<Eigen::MatrixXd, Eigen::aligned_allocator<Eigen::MatrixXd> >& jointTorqueDiff() const
{
return jointTorqueDiff_;
};
Expand All @@ -101,8 +101,8 @@ class RBDYN_DLLAPI InverseStatics
/// f_ is the vector of forces transmitted from body λ(i) to body i across
/// joint i.
std::vector<sva::ForceVecd> f_;
std::vector<Eigen::MatrixXd> df_;
std::vector<Eigen::MatrixXd> jointTorqueDiff_;
std::vector<Eigen::MatrixXd, Eigen::aligned_allocator<Eigen::MatrixXd> > df_;
std::vector<Eigen::MatrixXd, Eigen::aligned_allocator<Eigen::MatrixXd> > jointTorqueDiff_;
std::vector<Jacobian> jacW_;
Eigen::MatrixXd fullJac_;
bool jacobianSizeHasBeenSet_;
Expand Down
2 changes: 1 addition & 1 deletion src/Momentum.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -200,7 +200,7 @@ class RBDYN_DLLAPI CentroidalMomentumMatrix
Eigen::MatrixXd jacFull_;

std::vector<Jacobian> jacVec_;
std::vector<Eigen::MatrixXd> jacWork_;
std::vector<Eigen::MatrixXd, Eigen::aligned_allocator<Eigen::MatrixXd> > jacWork_;
std::vector<double> bodiesWeight_;
std::vector<sva::MotionVecd> normalAcc_;
public:
Expand Down
6 changes: 3 additions & 3 deletions src/MultiBodyConfig.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -74,9 +74,9 @@ void MultiBodyConfig::zero(const MultiBody& mb)
}


std::vector<Eigen::MatrixXd> MultiBodyConfig::python_motionSubspace()
std::vector<Eigen::MatrixXd, Eigen::aligned_allocator<Eigen::MatrixXd> > MultiBodyConfig::python_motionSubspace()
{
std::vector<Eigen::MatrixXd> ret(motionSubspace.size());
std::vector<Eigen::MatrixXd, Eigen::aligned_allocator<Eigen::MatrixXd> > ret(motionSubspace.size());

for(std::size_t i = 0; i < ret.size(); ++i)
{
Expand All @@ -87,7 +87,7 @@ std::vector<Eigen::MatrixXd> MultiBodyConfig::python_motionSubspace()
}


void MultiBodyConfig::python_motionSubspace(const std::vector<Eigen::MatrixXd>& v)
void MultiBodyConfig::python_motionSubspace(const std::vector<Eigen::MatrixXd, Eigen::aligned_allocator<Eigen::MatrixXd> >& v)
{
motionSubspace.resize(v.size());
for(std::size_t i = 0; i < v.size(); ++i)
Expand Down
48 changes: 24 additions & 24 deletions src/MultiBodyConfig.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -68,7 +68,7 @@ struct RBDYN_DLLAPI MultiBodyConfig


/// Motion subspace (Xj.j.subspace).
std::vector<Eigen::Matrix<double, 6, Eigen::Dynamic>> motionSubspace;
std::vector<Eigen::Matrix<double, 6, Eigen::Dynamic>, Eigen::aligned_allocator<Eigen::Matrix<double, 6, Eigen::Dynamic>> > motionSubspace;



Expand All @@ -95,8 +95,8 @@ struct RBDYN_DLLAPI MultiBodyConfig

// python binding function

std::vector<Eigen::MatrixXd> python_motionSubspace();
void python_motionSubspace(const std::vector<Eigen::MatrixXd>& v);
std::vector<Eigen::MatrixXd, Eigen::aligned_allocator<Eigen::MatrixXd> > python_motionSubspace();
void python_motionSubspace(const std::vector<Eigen::MatrixXd, Eigen::aligned_allocator<Eigen::MatrixXd> >& v);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
Expand All @@ -118,11 +118,11 @@ class RBDYN_DLLAPI ConfigConverter
* Convert a vector representing joint data.
* The first joint (base) is ignored.
*/
template<typename T>
void convertJoint(const std::vector<T>& from, std::vector<T>& to) const;
template<typename T, typename U>
void convertJoint(const std::vector<T,U>& from, std::vector<T,U>& to) const;

template<typename T>
std::vector<T> convertJoint(const std::vector<T>& from) const;
template<typename T, typename U>
std::vector<T,U> convertJoint(const std::vector<T,U>& from) const;

// safe version for python binding

Expand All @@ -139,8 +139,8 @@ class RBDYN_DLLAPI ConfigConverter
/** safe version of @see convertJoint.
* @throw std::domain_error If mb don't match mbc.
*/
template<typename T>
void sConvertJoint(const std::vector<T>& from, std::vector<T>& to) const;
template<typename T, typename U>
void sConvertJoint(const std::vector<T,U>& from, std::vector<T,U>& to) const;

private:
std::vector<int> jInd_;
Expand Down Expand Up @@ -258,13 +258,13 @@ RBDYN_DLLAPI std::vector<std::vector<double>> sVectorToDof(const MultiBody& mb,


/// @throw std::domain_error If there is a mismatch between mb.nrBodies and vec.size()
template <typename T>
void checkMatchBodiesVector(const MultiBody& mb, const std::vector<T>& vec,
template <typename T, typename U>
void checkMatchBodiesVector(const MultiBody& mb, const std::vector<T,U>& vec,
const std::string& name);

/// @throw std::domain_error If there is a mismatch between mb.nrJoints and vec.size()
template <typename T>
void checkMatchJointsVector(const MultiBody& mb, const std::vector<T>& vec,
template <typename T, typename U>
void checkMatchJointsVector(const MultiBody& mb, const std::vector<T,U>& vec,
const std::string& name);

/// @throw std::domain_error If there is a mismatch between mb and mbc.bodyPosW.
Expand Down Expand Up @@ -306,8 +306,8 @@ RBDYN_DLLAPI void checkMatchForce(const MultiBody& mb, const MultiBodyConfig& mb



template <typename T>
void checkMatchBodiesVector(const MultiBody& mb, const std::vector<T>& vec,
template <typename T,typename U>
void checkMatchBodiesVector(const MultiBody& mb, const std::vector<T,U>& vec,
const std::string& name)
{
if(int(vec.size()) != mb.nrBodies())
Expand All @@ -320,8 +320,8 @@ void checkMatchBodiesVector(const MultiBody& mb, const std::vector<T>& vec,
}


template <typename T>
void checkMatchJointsVector(const MultiBody& mb, const std::vector<T>& vec,
template <typename T, typename U>
void checkMatchJointsVector(const MultiBody& mb, const std::vector<T,U>& vec,
const std::string& name)
{
if(int(vec.size()) != mb.nrJoints())
Expand All @@ -335,9 +335,9 @@ void checkMatchJointsVector(const MultiBody& mb, const std::vector<T>& vec,



template<typename T>
template<typename T, typename U>
inline void
ConfigConverter::convertJoint(const std::vector<T>& from, std::vector<T>& to) const
ConfigConverter::convertJoint(const std::vector<T,U>& from, std::vector<T,U>& to) const
{
for(std::size_t i = 0; i < jInd_.size(); ++i)
{
Expand All @@ -347,9 +347,9 @@ ConfigConverter::convertJoint(const std::vector<T>& from, std::vector<T>& to) co



template<typename T>
template<typename T, typename U>
inline void
ConfigConverter::sConvertJoint(const std::vector<T>& from, std::vector<T>& to) const
ConfigConverter::sConvertJoint(const std::vector<T,U>& from, std::vector<T,U>& to) const
{
if(from.size() != to.size())
{
Expand All @@ -361,9 +361,9 @@ ConfigConverter::sConvertJoint(const std::vector<T>& from, std::vector<T>& to) c



template<typename T>
inline std::vector<T>
ConfigConverter::convertJoint(const std::vector<T>& from) const
template<typename T, typename U>
inline std::vector<T,U>
ConfigConverter::convertJoint(const std::vector<T,U>& from) const
{
std::vector<T> to(from.size());
for(std::size_t i = 0; i < jInd_.size(); ++i)
Expand Down
5 changes: 3 additions & 2 deletions src/util.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,12 +22,13 @@
#include <map>

#include <Eigen/Core>
#include <Eigen/StdVector>

namespace rbd
{
/// \brief Display a vector.
template <typename T>
std::ostream& operator<< (std::ostream&, const std::vector<T>&);
template <typename T, typename U>
std::ostream& operator<< (std::ostream&, const std::vector<T,U>&);

/// \brief Display a pair.
template <typename T1, typename T2>
Expand Down
4 changes: 2 additions & 2 deletions src/util.hxx
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,8 +17,8 @@

#pragma once

template <typename T>
std::ostream& operator<< (std::ostream& o, const std::vector<T>& vect)
template <typename T, typename U>
std::ostream& operator<< (std::ostream& o, const std::vector<T,U>& vect)
{
typedef typename std::vector<T>::const_iterator citer_t;

Expand Down
2 changes: 1 addition & 1 deletion tests/InverseStaticsTest.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,7 @@ void test(boost::shared_ptr<boost::test_tools::output_test_stream> output,
{
Eigen::MatrixXd jacQ(3,3);
Eigen::MatrixXd jacF(3,24);
std::vector<Eigen::MatrixXd> jacMomentAndForces(4);
std::vector<Eigen::MatrixXd, Eigen::aligned_allocator<Eigen::MatrixXd> > jacMomentAndForces(4);
(*output) << "\n\n\nChange config to " << q.transpose() << std::endl;
mbc.q[1][0] = q(0);
mbc.q[2][0] = q(1);
Expand Down

0 comments on commit ac6b40a

Please sign in to comment.