diff --git a/cmake b/cmake index 47c0cbf0..84f1ae12 160000 --- a/cmake +++ b/cmake @@ -1 +1 @@ -Subproject commit 47c0cbf0b29433060682377dcb28266e8dc38995 +Subproject commit 84f1ae122856a2aaf43b265f4b0689c0296afdc6 diff --git a/include/sot/core/device.hh b/include/sot/core/device.hh index bc1bbf8a..b2d86754 100644 --- a/include/sot/core/device.hh +++ b/include/sot/core/device.hh @@ -80,7 +80,7 @@ class SOT_CORE_EXPORT Device : public Entity { /* --- DESTRUCTION --- */ virtual ~Device(); - virtual void setStateSize(const size_type &size); + virtual void setStateSize(const std::int64_t &size); // Set number of joints that are controlled by the device. void setControlSize(const size_type &size); // Get the number of joints that are controlled by the device. diff --git a/include/sot/core/feature-posture.hh b/include/sot/core/feature-posture.hh index e8728e84..b5532587 100644 --- a/include/sot/core/feature-posture.hh +++ b/include/sot/core/feature-posture.hh @@ -79,7 +79,7 @@ class SOTFEATUREPOSTURE_EXPORT FeaturePosture : public FeatureAbstract { private: std::vector activeDofs_; - std::size_t nbActiveDofs_; + dynamicgraph::size_type nbActiveDofs_; }; // class FeaturePosture } // namespace sot } // namespace dynamicgraph diff --git a/include/sot/core/filter-differentiator.hh b/include/sot/core/filter-differentiator.hh index b33f2b7a..db649eba 100644 --- a/include/sot/core/filter-differentiator.hh +++ b/include/sot/core/filter-differentiator.hh @@ -43,6 +43,8 @@ #include #include +#include + namespace dynamicgraph { namespace sot { /** \addtogroup Filters @@ -84,7 +86,7 @@ class SOTFILTERDIFFERENTIATOR_EXPORT FilterDifferentiator protected: double m_dt; /// sampling timestep of the input signal - size_type m_x_size; + std::int64_t m_x_size; /// polynomial-fitting filters CausalFilter *m_filter; @@ -106,7 +108,7 @@ class SOTFILTERDIFFERENTIATOR_EXPORT FilterDifferentiator * polynomial fitting. The larger the delay, * the smoother the estimations. */ - void init(const double ×tep, const size_type &xSize, + void init(const double ×tep, const std::int64_t &xSize, const Eigen::VectorXd &filter_numerator, const Eigen::VectorXd &filter_denominator); diff --git a/include/sot/core/fir-filter.hh b/include/sot/core/fir-filter.hh index 4882194e..e81d99b0 100644 --- a/include/sot/core/fir-filter.hh +++ b/include/sot/core/fir-filter.hh @@ -17,6 +17,7 @@ #include #include +#include #include #include @@ -147,7 +148,7 @@ class FIRFilter : public Entity { "\n" " Input:\n" " - positive int: size\n"; - addCommand("setSize", new Setter( + addCommand("setSize", new Setter( *this, &FIRFilter::resizeBuffer, docstring)); docstring = @@ -155,7 +156,7 @@ class FIRFilter : public Entity { "\n" " Return:\n" " - positive int: size\n"; - addCommand("getSize", new Getter( + addCommand("getSize", new Getter( *this, &FIRFilter::getBufferSize, docstring)); } @@ -174,14 +175,14 @@ class FIRFilter : public Entity { return res; } - void resizeBuffer(const std::size_t &size) { - size_t s = static_cast(size); + void resizeBuffer(const std::int64_t &size) { + std::int64_t s = static_cast(size); data.reset_capacity(s); coefs.resize(s); } - std::size_t getBufferSize() const { - return static_cast(coefs.size()); + std::int64_t getBufferSize() const { + return static_cast(coefs.size()); } void setElement(const std::size_t &rank, const coefT &coef) { @@ -219,7 +220,7 @@ Value SetElement::doExecute() { FIRFilter &entity = static_cast &>(owner()); std::vector values = getParameterValues(); - std::size_t rank = values[0].value(); + std::uint64_t rank = values[0].value(); coefT coef = values[1].value(); entity.setElement(rank, coef); return Value(); @@ -235,7 +236,7 @@ Value GetElement::doExecute() { FIRFilter &entity = static_cast &>(owner()); std::vector values = getParameterValues(); - std::size_t rank = values[0].value(); + std::uint64_t rank = values[0].value(); return Value(entity.getElement(rank)); } } // namespace command diff --git a/include/sot/core/integrator.hh b/include/sot/core/integrator.hh index cc78dec7..d42dd387 100644 --- a/include/sot/core/integrator.hh +++ b/include/sot/core/integrator.hh @@ -38,30 +38,14 @@ #include #include "sot/core/periodic-call.hh" +namespace dg = dynamicgraph; + namespace dynamicgraph { namespace sot { namespace internal { class Signal : public ::dynamicgraph::Signal { protected: - enum SignalType { CONSTANT, REFERENCE, REFERENCE_NON_CONST, FUNCTION }; - static const SignalType SIGNAL_TYPE_DEFAULT = CONSTANT; - - const Vector *Treference; - Vector *TreferenceNonConst; - boost::function2 Tfunction; - - bool keepReference; - const static bool KEEP_REFERENCE_DEFAULT = false; - public: -#ifdef HAVE_LIBBOOST_THREAD - typedef boost::try_mutex Mutex; - typedef boost::lock_error MutexError; -#else - typedef size_type *Mutex; - typedef size_type *MutexError; -#endif - protected: Mutex *providerMutex; using SignalBase::signalTime; @@ -81,10 +65,13 @@ class Signal : public ::dynamicgraph::Signal { /* --- Generic Set function --- */ virtual void setConstant(const Vector &t); - virtual void setReference(const Vector *t, Mutex *mutexref = NULL); - virtual void setReferenceNonConstant(Vector *t, Mutex *mutexref = NULL); - virtual void setFunction(boost::function2 t, - Mutex *mutexref = NULL); + virtual void setReference( + const Vector *t, dg::Signal::Mutex *mutexref = NULL); + virtual void setReferenceNonConstant( + Vector *t, dg::Signal::Mutex *mutexref = NULL); + virtual void setFunction( + boost::function2 t, + dg::Signal::Mutex *mutexref = NULL); /* --- Signal computation --- */ virtual const Vector &access(const sigtime_t &t); diff --git a/include/sot/core/matrix-constant.hh b/include/sot/core/matrix-constant.hh index ca17562a..980f84d6 100644 --- a/include/sot/core/matrix-constant.hh +++ b/include/sot/core/matrix-constant.hh @@ -13,6 +13,7 @@ /* Matrix */ #include +#include /* --------------------------------------------------------------------- */ /* --- MATRIX ---------------------------------------------------------- */ /* --------------------------------------------------------------------- */ @@ -32,7 +33,7 @@ class MatrixConstant : public Entity { static const std::string CLASS_NAME; virtual const std::string &getClassName(void) const { return CLASS_NAME; } - size_type rows, cols; + std::int64_t rows, cols; double color; void setValue(const dynamicgraph::Matrix &inValue); diff --git a/include/sot/core/reader.hh b/include/sot/core/reader.hh index 2d38b039..9b4cc1b1 100644 --- a/include/sot/core/reader.hh +++ b/include/sot/core/reader.hh @@ -14,11 +14,14 @@ /* --- INCLUDE --------------------------------------------------------- */ /* --------------------------------------------------------------------- */ +#include + /* Matrix */ #include /* STD */ #include +#include #include #include #include @@ -82,7 +85,7 @@ class SOTREADER_EXPORT sotReader : public Entity { const std::size_t time); dynamicgraph::Matrix &getNextMatrix(dynamicgraph::Matrix &res, const std::size_t time); - void resize(const size_type &nbRow, const size_type &nbCol); + void resize(const std::int64_t &nbRow, const std::int64_t &nbCol); public: /* --- PARAMS --- */ diff --git a/include/sot/core/segment.hh b/include/sot/core/segment.hh index 0f03e28d..6480031a 100644 --- a/include/sot/core/segment.hh +++ b/include/sot/core/segment.hh @@ -31,6 +31,8 @@ #ifndef SOT_CORE_SEGMENT_HH #define SOT_CORE_SEGMENT_HH +#include + #include #include #include @@ -43,7 +45,7 @@ class SOT_CORE_DLLAPI Segment : public Entity { static const std::string CLASS_NAME; virtual const std::string& getClassName() const { return CLASS_NAME; } // Set the range of input vector that is provided as output. - void setRange(const size_type& i0, const size_type& length) { + void setRange(const std::int64_t& i0, const std::int64_t& length) { range_ = std::make_pair(i0, length); } diff --git a/include/sot/core/smooth-reach.hh b/include/sot/core/smooth-reach.hh index e3806b55..8b772e6c 100644 --- a/include/sot/core/smooth-reach.hh +++ b/include/sot/core/smooth-reach.hh @@ -68,12 +68,12 @@ class SOTSMOOTHREACH_EXPORT SmoothReach : public dynamicgraph::Entity { dynamicgraph::Vector &goalSOUT_function(dynamicgraph::Vector &goal, const sigtime_t &time); - void set(const dynamicgraph::Vector &goal, const size_type &length); + void set(const dynamicgraph::Vector &goal, const std::int64_t &length); const dynamicgraph::Vector &getGoal(void); const size_type &getLength(void); const size_type &getStart(void); - void setSmoothing(const size_type &mode, const double ¶m); + void setSmoothing(const std::int64_t &mode, const double ¶m); public: /* --- PARAMS --- */ virtual void display(std::ostream &os) const; diff --git a/include/sot/core/sot.hh b/include/sot/core/sot.hh index 34ebbeb6..8ffb1763 100644 --- a/include/sot/core/sot.hh +++ b/include/sot/core/sot.hh @@ -10,6 +10,8 @@ #ifndef __SOT_SOT_HH #define __SOT_SOT_HH +#include + /* --------------------------------------------------------------------- */ /* --- INCLUDE --------------------------------------------------------- */ /* --------------------------------------------------------------------- */ @@ -148,8 +150,8 @@ class SOTSOT_CORE_EXPORT Sot : public Entity { /*! \brief This method defines the part of the state vector which correspond to the free flyer of the robot. */ - virtual void defineNbDof(const size_type &nbDof); - virtual const size_type &getNbDof() const { return nbJoints; } + virtual void defineNbDof(const std::int64_t &nbDof); + virtual const std::int64_t &getNbDof() const { return nbJoints; } /*! @} */ public: /* --- CONTROL --- */ diff --git a/include/sot/core/switch.hh b/include/sot/core/switch.hh index a5d87b01..2eec18bf 100644 --- a/include/sot/core/switch.hh +++ b/include/sot/core/switch.hh @@ -27,7 +27,8 @@ class SOT_CORE_DLLAPI Switch : public VariadicAbstract { Switch(const std::string &name) : Base(name, CLASS_NAME), - selectionSIN(NULL, "Switch(" + name + ")::input(size_type)::selection"), + selectionSIN(NULL, + "Switch(" + name + ")::input(std::int64_t)::selection"), boolSelectionSIN(NULL, "Switch(" + name + ")::input(bool)::boolSelection") { this->signalRegistration(selectionSIN << boolSelectionSIN); @@ -47,7 +48,7 @@ class SOT_CORE_DLLAPI Switch : public VariadicAbstract { "\n" " Get number of input signals\n"; this->addCommand("getSignalNumber", - new command::Getter( + new command::Getter( *this, &Base::getSignalNumber, docstring)); } @@ -58,7 +59,7 @@ class SOT_CORE_DLLAPI Switch : public VariadicAbstract { return "Dynamically select a given signal based on a input information.\n"; } - SignalPtr selectionSIN; + SignalPtr selectionSIN; SignalPtr boolSelectionSIN; private: @@ -73,7 +74,7 @@ class SOT_CORE_DLLAPI Switch : public VariadicAbstract { if (sel < 0 || sel >= size_type(this->signalsIN.size())) throw std::runtime_error("Signal selection is out of range."); - ret = this->signalsIN[sel]->access(time); + ret = this->signalsIN[static_cast(sel)]->access(time); return ret; } }; diff --git a/include/sot/core/variadic-op.hh b/include/sot/core/variadic-op.hh index 9557f0df..d9b95676 100644 --- a/include/sot/core/variadic-op.hh +++ b/include/sot/core/variadic-op.hh @@ -86,11 +86,12 @@ class VariadicAbstract : public Entity { signalsIN.pop_back(); } - void setSignalNumber(const size_type &n) { + void setSignalNumber(const std::int64_t &n) { assert(n >= 0); const std::size_t oldSize = signalsIN.size(); - for (std::size_t i = n; i < oldSize; ++i) _removeSignal(i); - signalsIN.resize(n, NULL); + for (std::size_t i = static_cast(n); i < oldSize; ++i) + _removeSignal(i); + signalsIN.resize(static_cast(n), NULL); // names.resize(n); for (std::size_t i = oldSize; i < (std::size_t)n; ++i) { @@ -106,12 +107,14 @@ class VariadicAbstract : public Entity { updateSignalNumber(n); } - size_type getSignalNumber() const { return (size_type)signalsIN.size(); } + std::int64_t getSignalNumber() const { + return static_cast(signalsIN.size()); + } signal_t *getSignalIn(size_type i) { if (i < 0 || i >= (size_type)signalsIN.size()) throw std::out_of_range("Wrong signal index"); - return signalsIN[i]; + return signalsIN[static_cast(i)]; } protected: diff --git a/include/sot/core/vector-constant.hh b/include/sot/core/vector-constant.hh index 019ef129..7e6386ed 100644 --- a/include/sot/core/vector-constant.hh +++ b/include/sot/core/vector-constant.hh @@ -16,6 +16,7 @@ /* Matrix */ #include +#include /* --------------------------------------------------------------------- */ /* --- VECTOR ---------------------------------------------------------- */ /* --------------------------------------------------------------------- */ @@ -30,7 +31,7 @@ class Resize; class VectorConstant : public Entity { friend class command::vectorConstant::Resize; - size_type rows; + std::int64_t rows; public: static const std::string CLASS_NAME; diff --git a/src/feature/feature-posture.cpp b/src/feature/feature-posture.cpp index 387f8061..84a9d7b2 100644 --- a/src/feature/feature-posture.cpp +++ b/src/feature/feature-posture.cpp @@ -28,7 +28,7 @@ class FeaturePosture::SelectDof : public Command { virtual Value doExecute() { FeaturePosture &feature = static_cast(owner()); std::vector values = getParameterValues(); - std::size_t dofId = values[0].value(); + std::uint64_t dofId = values[0].value(); bool control = values[1].value(); feature.selectDof(dofId, control); return Value(); @@ -67,7 +67,7 @@ FeaturePosture::FeaturePosture(const std::string &name) FeaturePosture::~FeaturePosture() {} size_type &FeaturePosture::getDimension(size_type &res, sigtime_t) { - res = static_cast(nbActiveDofs_); + res = static_cast(nbActiveDofs_); return res; } @@ -77,9 +77,10 @@ dg::Vector &FeaturePosture::computeError(dg::Vector &res, sigtime_t t) { res.resize(nbActiveDofs_); std::size_t index = 0; - for (std::size_t i = 0; i < activeDofs_.size(); ++i) { - if (activeDofs_[i]) { - res(index) = state(i) - posture(i); + for (dg::size_type i = 0; i < static_cast(activeDofs_.size()); + ++i) { + if (activeDofs_[static_cast::size_type>(i)]) { + res(static_cast(index)) = state(i) - posture(i); index++; } } @@ -98,7 +99,9 @@ dg::Vector &FeaturePosture::computeErrorDot(dg::Vector &res, sigtime_t t) { res.resize(nbActiveDofs_); std::size_t index = 0; for (std::size_t i = 0; i < activeDofs_.size(); ++i) { - if (activeDofs_[i]) res(index++) = -postureDot(i); + if (activeDofs_[i]) + res(static_cast(index++)) = + -postureDot(static_cast(i)); } return res; } @@ -106,7 +109,7 @@ dg::Vector &FeaturePosture::computeErrorDot(dg::Vector &res, sigtime_t t) { void FeaturePosture::selectDof(std::size_t dofId, bool control) { const Vector &state = state_.accessCopy(); const Vector &posture = posture_.accessCopy(); - std::size_t dim(state.size()); + std::size_t dim(static_cast(state.size())); if (dim != (std::size_t)posture.size()) { throw std::runtime_error("Posture and State should have same dimension."); @@ -137,12 +140,12 @@ void FeaturePosture::selectDof(std::size_t dofId, bool control) { } } // recompute jacobian - Matrix J(Matrix::Zero(nbActiveDofs_, dim)); + Matrix J(Matrix::Zero(nbActiveDofs_, static_cast(dim))); - std::size_t index = 0; + Eigen::Index index = 0; for (std::size_t i = 0; i < activeDofs_.size(); ++i) { if (activeDofs_[i]) { - J(index, i) = 1; + J(index, static_cast(i)) = 1; index++; } } diff --git a/src/filters/filter-differentiator.cpp b/src/filters/filter-differentiator.cpp index 687d5735..177d8e7a 100644 --- a/src/filters/filter-differentiator.cpp +++ b/src/filters/filter-differentiator.cpp @@ -89,7 +89,8 @@ FilterDifferentiator::FilterDifferentiator(const std::string &name) /* --- COMMANDS ------------------------------------------------------ */ /* --- COMMANDS ------------------------------------------------------ */ /* --- COMMANDS ------------------------------------------------------ */ -void FilterDifferentiator::init(const double ×tep, const size_type &xSize, +void FilterDifferentiator::init(const double ×tep, + const std::int64_t &xSize, const Eigen::VectorXd &filter_numerator, const Eigen::VectorXd &filter_denominator) { m_x_size = xSize; diff --git a/src/matrix/matrix-constant-command.h b/src/matrix/matrix-constant-command.h index 18dad5d8..437e32d8 100644 --- a/src/matrix/matrix-constant-command.h +++ b/src/matrix/matrix-constant-command.h @@ -35,8 +35,8 @@ class Resize : public Command { virtual Value doExecute() { MatrixConstant &mc = static_cast(owner()); std::vector values = getParameterValues(); - size_type rows = values[0].value(); - size_type cols = values[1].value(); + std::int64_t rows = values[0].value(); + std::int64_t cols = values[1].value(); Matrix m(Matrix::Zero(rows, cols)); mc.SOUT.setConstant(m); diff --git a/src/matrix/operator.hh b/src/matrix/operator.hh index 3215d093..c574c8d4 100644 --- a/src/matrix/operator.hh +++ b/src/matrix/operator.hh @@ -94,12 +94,12 @@ struct VectorSelecter : public UnaryOpHeader { using namespace dynamicgraph::command; std::string doc; - boost::function setBound = + boost::function setBound = boost::bind(&VectorSelecter::setBounds, this, _1, _2); doc = docCommandVoid2("Set the bound of the selection [m,M[.", "size_type (min)", "size_type (max)"); ADD_COMMAND("selec", makeCommandVoid2(ent, setBound, doc)); - boost::function addBound = + boost::function addBound = boost::bind(&VectorSelecter::addBounds, this, _1, _2); doc = docCommandVoid2("Add a segment to be selected [m,M[.", "size_type (min)", "size_type (max)"); @@ -151,11 +151,11 @@ struct MatrixSelector : public UnaryOpHeader { size_type imin, imax; size_type jmin, jmax; - inline void setBoundsRow(const size_type &m, const size_type &M) { + inline void setBoundsRow(const std::int64_t &m, const std::int64_t &M) { imin = m; imax = M; } - inline void setBoundsCol(const size_type &m, const size_type &M) { + inline void setBoundsCol(const std::int64_t &m, const std::int64_t &M) { jmin = m; jmax = M; } @@ -165,17 +165,17 @@ struct MatrixSelector : public UnaryOpHeader { using namespace dynamicgraph::command; std::string doc; - boost::function setBoundsRow = - boost::bind(&MatrixSelector::setBoundsRow, this, _1, _2); - boost::function setBoundsCol = - boost::bind(&MatrixSelector::setBoundsCol, this, _1, _2); + boost::function + setBoundsRow = boost::bind(&MatrixSelector::setBoundsRow, this, _1, _2); + boost::function + setBoundsCol = boost::bind(&MatrixSelector::setBoundsCol, this, _1, _2); - doc = docCommandVoid2("Set the bound on rows.", "size_type (min)", - "size_type (max)"); + doc = docCommandVoid2("Set the bound on rows.", "std::int64_t (min)", + "std::int64_t (max)"); ADD_COMMAND("selecRows", makeCommandVoid2(ent, setBoundsRow, doc)); - doc = docCommandVoid2("Set the bound on cols [m,M[.", "size_type (min)", - "size_type (max)"); + doc = docCommandVoid2("Set the bound on cols [m,M[.", "std::int64_t (min)", + "std::int64_t (max)"); ADD_COMMAND("selecCols", makeCommandVoid2(ent, setBoundsCol, doc)); } }; @@ -188,13 +188,13 @@ struct MatrixColumnSelector : public UnaryOpHeader { assert(jcol < m.cols()); res.resize(imax - imin); - for (size_type i = imin; i < imax; ++i) res(i - imin) = m(i, jcol); + for (std::int64_t i = imin; i < imax; ++i) res(i - imin) = m(i, jcol); } - size_type imin, imax; - size_type jcol; - inline void selectCol(const size_type &m) { jcol = m; } - inline void setBoundsRow(const size_type &m, const size_type &M) { + std::int64_t imin, imax; + std::int64_t jcol; + inline void selectCol(const std::int64_t &m) { jcol = m; } + inline void setBoundsRow(const std::int64_t &m, const std::int64_t &M) { imin = m; imax = M; } @@ -204,16 +204,18 @@ struct MatrixColumnSelector : public UnaryOpHeader { using namespace dynamicgraph::command; std::string doc; - boost::function setBoundsRow = - boost::bind(&MatrixColumnSelector::setBoundsRow, this, _1, _2); - boost::function selectCol = + boost::function + setBoundsRow = + boost::bind(&MatrixColumnSelector::setBoundsRow, this, _1, _2); + boost::function selectCol = boost::bind(&MatrixColumnSelector::selectCol, this, _1); - doc = docCommandVoid2("Set the bound on rows.", "size_type (min)", - "size_type (max)"); + doc = docCommandVoid2("Set the bound on rows.", "std::int64_t (min)", + "std::int64_t (max)"); ADD_COMMAND("selecRows", makeCommandVoid2(ent, setBoundsRow, doc)); - doc = docCommandVoid1("Select the col to copy.", "size_type (col index)"); + doc = + docCommandVoid1("Select the col to copy.", "std::int64_t (col index)"); ADD_COMMAND("selecCols", makeCommandVoid1(ent, selectCol, doc)); } }; @@ -232,20 +234,20 @@ struct Diagonalizer : public UnaryOpHeader { public: Diagonalizer(void) : nbr(0), nbc(0) {} std::size_t nbr, nbc; - inline void resize(const size_type &r, const size_type &c) { - nbr = r; - nbc = c; + inline void resize(const std::int64_t &r, const std::int64_t &c) { + nbr = static_cast(r); + nbc = static_cast(c); } inline void addSpecificCommands(Entity &ent, Entity::CommandMap_t &commandMap) { using namespace dynamicgraph::command; std::string doc; - boost::function resize = + boost::function resize = boost::bind(&Diagonalizer::resize, this, _1, _2); - doc = docCommandVoid2("Set output size.", "size_type (row)", - "size_type (col)"); + doc = docCommandVoid2("Set output size.", "std::int64_t (row)", + "std::int64_t (col)"); ADD_COMMAND("resize", makeCommandVoid2(ent, resize, doc)); } }; @@ -366,8 +368,8 @@ struct MatrixHomoToPoseRollPitchYaw dg::Vector t(3); t = M.translation(); res.resize(6); - for (std::size_t i = 0; i < 3; ++i) res(i) = t(i); - for (std::size_t i = 0; i < 3; ++i) res(i + 3) = r(i); + for (Eigen::Index i = 0; i < 3; ++i) res(i) = t(i); + for (Eigen::Index i = 0; i < 3; ++i) res(i + 3) = r(i); } }; @@ -375,14 +377,14 @@ struct PoseRollPitchYawToMatrixHomo : public UnaryOpHeader { inline void operator()(const dg::Vector &vect, MatrixHomogeneous &Mres) { VectorRollPitchYaw r; - for (std::size_t i = 0; i < 3; ++i) r(i) = vect(i + 3); + for (Eigen::Index i = 0; i < 3; ++i) r(i) = vect(i + 3); MatrixRotation R = (Eigen::AngleAxisd(r(2), Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(r(1), Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(r(0), Eigen::Vector3d::UnitX())) .toRotationMatrix(); dg::Vector t(3); - for (std::size_t i = 0; i < 3; ++i) t(i) = vect(i); + for (Eigen::Index i = 0; i < 3; ++i) t(i) = vect(i); // buildFrom(R,t); Mres = Eigen::Translation3d(t) * R; @@ -392,7 +394,7 @@ struct PoseRollPitchYawToMatrixHomo struct PoseRollPitchYawToPoseUTheta : public UnaryOpHeader { inline void operator()(const dg::Vector &vect, dg::Vector &vectres) { VectorRollPitchYaw r; - for (std::size_t i = 0; i < 3; ++i) r(i) = vect(i + 3); + for (Eigen::Index i = 0; i < 3; ++i) r(i) = vect(i + 3); MatrixRotation R = (Eigen::AngleAxisd(r(2), Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(r(1), Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(r(0), Eigen::Vector3d::UnitX())) @@ -401,7 +403,7 @@ struct PoseRollPitchYawToPoseUTheta : public UnaryOpHeader { VectorUTheta rrot(R); vectres.resize(6); - for (std::size_t i = 0; i < 3; ++i) { + for (Eigen::Index i = 0; i < 3; ++i) { vectres(i) = vect(i); vectres(i + 3) = rrot.angle() * rrot.axis()(i); } @@ -597,29 +599,29 @@ struct VectorStack : public BinaryOpHeader { public: - size_type v1min, v1max; - size_type v2min, v2max; + std::int64_t v1min, v1max; + std::int64_t v2min, v2max; inline void operator()(const dynamicgraph::Vector &v1, const dynamicgraph::Vector &v2, dynamicgraph::Vector &res) const { assert((v1max >= v1min) && (v1.size() >= v1max)); assert((v2max >= v2min) && (v2.size() >= v2max)); - const size_type v1size = v1max - v1min, v2size = v2max - v2min; + const std::int64_t v1size = v1max - v1min, v2size = v2max - v2min; res.resize(v1size + v2size); - for (size_type i = 0; i < v1size; ++i) { + for (std::int64_t i = 0; i < v1size; ++i) { res(i) = v1(i + v1min); } - for (size_type i = 0; i < v2size; ++i) { + for (std::int64_t i = 0; i < v2size; ++i) { res(v1size + i) = v2(i + v2min); } } - inline void selec1(const size_type &m, const size_type M) { + inline void selec1(const std::int64_t &m, const std::int64_t M) { v1min = m; v1max = M; } - inline void selec2(const size_type &m, const size_type M) { + inline void selec2(const std::int64_t &m, const std::int64_t M) { v2min = m; v2max = M; } @@ -629,21 +631,23 @@ struct VectorStack using namespace dynamicgraph::command; std::string doc; - boost::function selec1 = + boost::function selec1 = boost::bind(&VectorStack::selec1, this, _1, _2); - boost::function selec2 = + boost::function selec2 = boost::bind(&VectorStack::selec2, this, _1, _2); - ADD_COMMAND("selec1", - makeCommandVoid2( - ent, selec1, - docCommandVoid2("set the min and max of selection.", - "size_type (imin)", "size_type (imax)"))); - ADD_COMMAND("selec2", - makeCommandVoid2( - ent, selec2, - docCommandVoid2("set the min and max of selection.", - "size_type (imin)", "size_type (imax)"))); + ADD_COMMAND( + "selec1", + makeCommandVoid2( + ent, selec1, + docCommandVoid2("set the min and max of selection.", + "std::int64_t (imin)", "std::int64_t (imax)"))); + ADD_COMMAND( + "selec2", + makeCommandVoid2( + ent, selec2, + docCommandVoid2("set the min and max of selection.", + "std::int64_t (imin)", "std::int64_t (imax)"))); } }; @@ -675,13 +679,13 @@ struct ConvolutionTemporal res.resize(nsig); res.fill(0); - std::size_t j = 0; + Eigen::Index j = 0; for (MemoryType::const_iterator iter = f1.begin(); iter != f1.end(); iter++) { const dynamicgraph::Vector &s_tau = *iter; sotDEBUG(45) << "Sig" << j << ": " << s_tau; if (s_tau.size() != nsig) return; // TODO: error throw; - for (size_type i = 0; i < nsig; ++i) { + for (Eigen::Index i = 0; i < nsig; ++i) { res(i) += f2(i, j) * s_tau(i); } j++; @@ -842,7 +846,7 @@ struct VariadicOpHeader { } template inline void initialize(VariadicOp *, Entity::CommandMap_t &) {} - inline void updateSignalNumber(const size_type &) {} + inline void updateSignalNumber(const std::int64_t &) {} inline std::string getDocString() const { return std::string( "Undocumented variadic operator\n" @@ -878,9 +882,9 @@ struct VectorMix : public VariadicOpHeader { } } - inline void addSelec(const size_type &sigIdx, const size_type &i, - const size_type &s) { - idxs.push_back(segment_t(i, s, sigIdx)); + inline void addSelec(const std::int64_t &sigIdx, const std::int64_t &i, + const std::int64_t &s) { + idxs.push_back(segment_t(i, s, static_cast(sigIdx))); } inline void initialize(Base *ent, Entity::CommandMap_t &commandMap) { @@ -889,17 +893,17 @@ struct VectorMix : public VariadicOpHeader { ent->addSignal("default"); - boost::function selec = boost::bind(&VectorMix::addSelec, this, _1, _2, _3); commandMap.insert(std::make_pair( "addSelec", - makeCommandVoid3( + makeCommandVoid3( *ent, selec, docCommandVoid3("add selection from a vector.", - "size_type (signal index >= 1)", - "size_type (index)", "size_type (size)")))); + "std::int64_t (signal index >= 1)", + "std::int64_t (index)", "std::int64_t (size)")))); } }; @@ -916,7 +920,8 @@ struct AdderVariadic : public VariadicOpHeader { assert(vs.size() == (std::size_t)coeffs.size()); if (vs.size() == 0) return; res = coeffs[0] * (*vs[0]); - for (std::size_t i = 1; i < vs.size(); ++i) res += coeffs[i] * (*vs[i]); + for (std::size_t i = 1; i < vs.size(); ++i) + res += coeffs[static_cast(i)] * (*vs[i]); } inline void setCoeffs(const Vector &c) { @@ -924,7 +929,7 @@ struct AdderVariadic : public VariadicOpHeader { throw std::invalid_argument("Invalid coefficient size."); coeffs = c; } - inline void updateSignalNumber(const size_type &n) { + inline void updateSignalNumber(const std::int64_t &n) { coeffs = Vector::Ones(n); } @@ -993,7 +998,7 @@ inline void Multiplier::operator()( } /* --- BOOLEAN --------------------------------------------------------- */ -template +template struct BoolOp : public VariadicOpHeader { typedef VariadicOp Base; diff --git a/src/matrix/vector-constant-command.h b/src/matrix/vector-constant-command.h index da22f1c8..0f2f47b1 100644 --- a/src/matrix/vector-constant-command.h +++ b/src/matrix/vector-constant-command.h @@ -34,7 +34,7 @@ class Resize : public Command { virtual Value doExecute() { VectorConstant &vc = static_cast(owner()); std::vector values = getParameterValues(); - size_type size = values[0].value(); + std::int64_t size = values[0].value(); Vector m(Vector::Zero(size)); vc.SOUT.setConstant(m); diff --git a/src/python-module.cc b/src/python-module.cc index a9a0efc6..a40107bb 100644 --- a/src/python-module.cc +++ b/src/python-module.cc @@ -62,13 +62,13 @@ BOOST_PYTHON_MODULE(wrap) { bp::class_("Flags", bp::init<>()) .def(bp::init()) .def("__init__", bp::make_constructor(+[](bp::list bools) { - std::vector flags(bp::len(bools)); + std::vector flags(static_cast(bp::len(bools))); for (std::size_t i = 0; i < flags.size(); ++i) flags[i] = bp::extract(bools[i]); return new Flags(flags); })) .def("__init__", bp::make_constructor(+[](bp::tuple bools) { - std::vector flags(bp::len(bools)); + std::vector flags(static_cast(bp::len(bools))); for (std::size_t i = 0; i < flags.size(); ++i) flags[i] = bp::extract(bools[i]); return new Flags(flags); diff --git a/src/sot/sot.cpp b/src/sot/sot.cpp index 85149faf..2932af65 100644 --- a/src/sot/sot.cpp +++ b/src/sot/sot.cpp @@ -88,7 +88,7 @@ Sot::Sot(const std::string &name) " - a positive integer : number of degrees of freedom of " "the robot.\n" " \n"; - addCommand("setSize", new dynamicgraph::command::Setter( + addCommand("setSize", new dynamicgraph::command::Setter( *this, &Sot::defineNbDof, docstring)); docstring = @@ -100,7 +100,7 @@ Sot::Sot(const std::string &name) "the robot.\n" " \n"; addCommand("getSize", - new dynamicgraph::command::Getter( + new dynamicgraph::command::Getter( *this, &Sot::getNbDof, docstring)); addCommand("enablePostureTaskAcceleration", @@ -316,7 +316,7 @@ void Sot::clear(void) { controlSOUT.setReady(); } -void Sot::defineNbDof(const size_type &nbDof) { +void Sot::defineNbDof(const std::int64_t &nbDof) { nbJoints = nbDof; controlSOUT.setReady(); } @@ -457,8 +457,8 @@ MemoryTaskSOT *getMemory(TaskAbstract &t, const Matrix::Index &tDim, void Sot::taskVectorToMlVector(const VectorMultiBound &taskVector, Vector &res) { - res.resize(taskVector.size()); - std::size_t i = 0; + res.resize(static_cast(taskVector.size())); + Eigen::Index i = 0; for (VectorMultiBound::const_iterator iter = taskVector.begin(); iter != taskVector.end(); ++iter, ++i) { @@ -536,7 +536,8 @@ dynamicgraph::Vector &Sot::computeControlLaw(dynamicgraph::Vector &control, /// Computing first the jacobian may be a little faster overall. if (!fullPostureTask) taskA.jacobianSOUT.recompute(iterTime); taskA.taskSOUT.recompute(iterTime); - const Matrix::Index dim = taskA.taskSOUT.accessCopy().size(); + const Matrix::Index dim = + static_cast(taskA.taskSOUT.accessCopy().size()); sotCOUNTER(0, 1); // Direct Dynamic /* Init memory. */ diff --git a/src/tools/device.cpp b/src/tools/device.cpp index 3a50fcdc..b5ec793a 100644 --- a/src/tools/device.cpp +++ b/src/tools/device.cpp @@ -12,7 +12,7 @@ /* SOT */ #define ENABLE_RT_LOG - +#include #include "sot/core/device.hh" #include @@ -135,7 +135,7 @@ Device::Device(const std::string &n) "\n" " Set size of state vector\n" "\n"; - addCommand("resize", new command::Setter( + addCommand("resize", new command::Setter( *this, &Device::setStateSize, docstring)); docstring = "\n" @@ -227,21 +227,23 @@ void Device::getControl(map &controlOut, const double &period) { sotDEBUGIN(25); std::vector control; + typedef std::vector::size_type control_stype; lastTimeControlWasRead_ += (sigtime_t)floor(period / Integrator::dt); Vector dgControl(controlSIN(lastTimeControlWasRead_)); // Specify the joint values for the controller. - control.resize(dgControl.size()); + control.resize(static_cast(dgControl.size())); if (controlInputType_ == POSITION_CONTROL) { CHECK_BOUNDS(dgControl, lowerPosition_, upperPosition_, "position", 1e-6); } - for (size_type i = 0; i < dgControl.size(); ++i) control[i] = dgControl[i]; + for (size_type i = 0; i < dgControl.size(); ++i) + control[static_cast(i)] = dgControl[i]; controlOut["control"].setValues(control); sotDEBUGOUT(25); } -void Device::setStateSize(const size_type &size) { +void Device::setStateSize(const std::int64_t &size) { state_.resize(size); state_.fill(.0); stateSOUT.setConstant(state_); @@ -283,7 +285,7 @@ void Device::setRoot(const MatrixHomogeneous &worldMwaist) { VectorRollPitchYaw r = (worldMwaist.linear().eulerAngles(2, 1, 0)).reverse(); Vector q = state_; q = worldMwaist.translation(); // abusive ... but working. - for (std::size_t i = 0; i < 3; ++i) q(i + 3) = r(i); + for (Eigen::Index i = 0; i < 3; ++i) q(i + 3) = r(i); } void Device::setSecondOrderIntegration() {} diff --git a/src/tools/integrator.cpp b/src/tools/integrator.cpp index 1908fc5c..f29865a0 100644 --- a/src/tools/integrator.cpp +++ b/src/tools/integrator.cpp @@ -66,16 +66,18 @@ void Signal::setConstant(const Vector &) { throw std::runtime_error("Not implemented."); } -void Signal::setReference(const Vector *, Mutex *) { +void Signal::setReference(const Vector *, + dg::Signal::Mutex *) { throw std::runtime_error("Not implemented."); } -void Signal::setReferenceNonConstant(Vector *, Mutex *) { +void Signal::setReferenceNonConstant(Vector *, + dg::Signal::Mutex *) { throw std::runtime_error("Not implemented."); } void Signal::setFunction(boost::function2 t, - Mutex *mutexref) { + dg::Signal::Mutex *mutexref) { signalType = ::dynamicgraph::Signal::FUNCTION; Tfunction = t; providerMutex = mutexref; diff --git a/src/tools/parameter-server.cpp b/src/tools/parameter-server.cpp index cc0589b6..97ce8935 100644 --- a/src/tools/parameter-server.cpp +++ b/src/tools/parameter-server.cpp @@ -150,7 +150,7 @@ ParameterServer::ParameterServer(const std::string &name) "(string) ParameterName", "(bool) ParameterValue"))); addCommand("setParameterInt", makeCommandVoid2( - *this, &ParameterServer::setParameter, + *this, &ParameterServer::setParameter, docCommandVoid2("Set a parameter named ParameterName to value " "ParameterValue (string format).", "(string) ParameterName", @@ -179,13 +179,13 @@ ParameterServer::ParameterServer(const std::string &name) " named ParameterName.", "(string) ParameterName"))); - addCommand( - "getParameterInt", - makeCommandReturnType1(*this, &ParameterServer::getParameter, - docCommandReturnType1( - "Return the parameter value for parameter" - " named ParameterName.", - "(size_type) ParameterName"))); + addCommand("getParameterInt", + makeCommandReturnType1( + *this, &ParameterServer::getParameter, + docCommandReturnType1( + "Return the parameter value for parameter" + " named ParameterName.", + "(size_type) ParameterName"))); addCommand( "getParameterDbl", @@ -389,7 +389,8 @@ bool ParameterServer::convertJointNameToJointId(const std::string &name, MSG_TYPE_ERROR); std::stringstream ss; for (long unsigned int it = 0; it < m_robot_util->m_nbJoints; it++) - ss << m_robot_util->get_name_from_id(it) << ", "; + ss << m_robot_util->get_name_from_id(static_cast(it)) + << ", "; SEND_MSG("Possible joint names are: " + ss.str(), MSG_TYPE_INFO); return false; } diff --git a/src/tools/smooth-reach.cpp b/src/tools/smooth-reach.cpp index ef90bb7c..de012836 100644 --- a/src/tools/smooth-reach.cpp +++ b/src/tools/smooth-reach.cpp @@ -76,7 +76,7 @@ double SmoothReach::smoothFunction(double x) { return 0; } -void SmoothReach::setSmoothing(const size_type &mode, const double ¶m) { +void SmoothReach::setSmoothing(const std::int64_t &mode, const double ¶m) { smoothMode = mode; smoothParam = param; } @@ -104,7 +104,7 @@ dynamicgraph::Vector &SmoothReach::goalSOUT_function(dynamicgraph::Vector &res, } void SmoothReach::set(const dynamicgraph::Vector &goalDes, - const size_type &lengthDes) { + const std::int64_t &lengthDes) { goal = goalDes; lengthTime = lengthDes; isParam = true; diff --git a/src/traces/reader.cpp b/src/traces/reader.cpp index 64da5605..c2415f12 100644 --- a/src/traces/reader.cpp +++ b/src/traces/reader.cpp @@ -172,7 +172,7 @@ void sotReader::initCommands() { addCommand("resize", dc::makeCommandVoid2(*this, &sotReader::resize, " ")); } -void sotReader::resize(const size_type &row, const size_type &col) { +void sotReader::resize(const std::int64_t &row, const std::int64_t &col) { rows = row; cols = col; } diff --git a/tests/features/test_feature_generic.cpp b/tests/features/test_feature_generic.cpp index 463feaf8..cc1fefd5 100644 --- a/tests/features/test_feature_generic.cpp +++ b/tests/features/test_feature_generic.cpp @@ -113,7 +113,8 @@ class FeatureTestBase { expectedTaskOutput_.size()); Vector taskOutput(expectedTaskOutput_.size()); for (int i = 0; i < expectedTaskOutput_.size(); ++i) - taskOutput[i] = task_.taskSOUT.accessCopy()[i].getSingleBound(); + taskOutput[i] = task_.taskSOUT.accessCopy()[static_cast(i)] + .getSingleBound(); EIGEN_VECTOR_IS_APPROX(taskOutput, expectedTaskOutput_, 1e-6); } @@ -170,7 +171,7 @@ class TestFeatureGeneric : public FeatureTestBase { : FeatureTestBase(dim, name), feature_("feature" + name), featureDes_("featureDes" + name), - dim_(dim) { + dim_(static_cast(dim)) { feature_.setReference(&featureDes_); feature_.selectionSIN = Flags(true); @@ -302,7 +303,8 @@ Vector7 toVector(const pinocchio::SE3 &M) { Vector toVector(const std::vector &in) { Vector out(in.size()); - for (int i = 0; i < (int)in.size(); ++i) out[i] = in[i].getSingleBound(); + for (int i = 0; i < (int)in.size(); ++i) + out[i] = in[static_cast(i)].getSingleBound(); return out; } diff --git a/tests/tools/test_boost.cpp b/tests/tools/test_boost.cpp index 00888355..34f5272f 100644 --- a/tests/tools/test_boost.cpp +++ b/tests/tools/test_boost.cpp @@ -130,9 +130,9 @@ int main(int argc, char **argv) { // const unsigned int r=1; // const unsigned int c=30; unsigned int r = 1; - if (argc > 1) r = atoi(argv[1]); + if (argc > 1) r = static_cast(atoi(argv[1])); unsigned int c = 30; - if (argc > 2) c = atoi(argv[2]); + if (argc > 2) c = static_cast(atoi(argv[2])); static const int BENCH = 100; dynamicgraph::Matrix M(r, c);