Commit 7f5a93e5 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

Merge tag 'v4.11.1'

Release of version 4.11.1.
parents 283301dd b4124445
Pipeline #12170 passed with stage
in 32 minutes and 25 seconds
......@@ -37,20 +37,21 @@ CHECK_MINIMAL_CXX_STANDARD(11 ENFORCE)
# Project dependencies
ADD_PROJECT_DEPENDENCY(dynamic-graph REQUIRED PKG_CONFIG_REQUIRES dynamic-graph)
ADD_PROJECT_DEPENDENCY(pinocchio REQUIRED PKG_CONFIG_REQUIRES pinocchio)
ADD_PROJECT_DEPENDENCY(example-robot-data)
SET(BOOST_COMPONENTS filesystem system thread program_options
unit_test_framework regex)
ADD_PROJECT_DEPENDENCY(Boost REQUIRED COMPONENTS regex)
IF(BUILD_TESTING)
ADD_PROJECT_DEPENDENCY(example-robot-data)
FIND_PACKAGE(Boost REQUIRED COMPONENTS unit_test_framework program_options)
ENDIF()
IF(BUILD_PYTHON_INTERFACE)
FINDPYTHON()
STRING(REGEX REPLACE "-" "_" PYTHON_DIR ${CUSTOM_HEADER_DIR})
ADD_PROJECT_DEPENDENCY(dynamic-graph-python REQUIRED
ADD_PROJECT_DEPENDENCY(dynamic-graph-python 4.0.0 REQUIRED
PKG_CONFIG_REQUIRES dynamic-graph-python)
SET(BOOST_COMPONENTS ${BOOST_COMPONENTS} python)
ADD_PROJECT_DEPENDENCY(eigenpy REQUIRED PKG_CONFIG_REQUIRES eigenpy)
SEARCH_FOR_BOOST_PYTHON(REQUIRED)
ENDIF(BUILD_PYTHON_INTERFACE)
SEARCH_FOR_BOOST()
# Verbosity level
IF(NOT (\"${CMAKE_VERBOSITY_LEVEL}\" STREQUAL \"\"))
ADD_DEFINITIONS(-DVP_DEBUG_MODE=${CMAKE_VERBOSITY_LEVEL} -DVP_DEBUG)
......@@ -172,9 +173,7 @@ SET(${PROJECT_NAME}_SOURCES
ADD_LIBRARY(${PROJECT_NAME} SHARED
${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS})
TARGET_INCLUDE_DIRECTORIES(${PROJECT_NAME} PUBLIC $<INSTALL_INTERFACE:include>)
TARGET_INCLUDE_DIRECTORIES(${PROJECT_NAME}
SYSTEM PUBLIC ${Boost_INCLUDE_DIRS})
TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${Boost_LIBRARIES}
TARGET_LINK_LIBRARIES(${PROJECT_NAME} PUBLIC Boost::regex
dynamic-graph::dynamic-graph pinocchio::pinocchio)
IF(SUFFIX_SO_VERSION)
......
Subproject commit 82030f34fb590cfcece473b97ba910e7b6da30fe
Subproject commit 9dcde03a880cccc86531019a6845708f5c54c35d
......@@ -97,6 +97,9 @@ public:
void setTorqueBounds(const Vector &lower, const Vector &upper);
/// \}
PeriodicCall& periodicCallBefore() { return periodicCallBefore_; }
PeriodicCall& periodicCallAfter() { return periodicCallAfter_; }
public: /* --- DISPLAY --- */
virtual void display(std::ostream &os) const;
virtual void cmdDisplay();
......
......@@ -17,10 +17,10 @@ namespace dynamicgraph {
namespace sot {
class DoubleConstant : public Entity {
public:
static const std::string CLASS_NAME;
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
public:
DoubleConstant(const std::string &name);
virtual ~DoubleConstant(void) {}
......
......@@ -97,15 +97,15 @@ public:
int time);
/** Static Feature selection. */
inline static Flags selectX(void) { return FLAG_LINE_1; }
inline static Flags selectY(void) { return FLAG_LINE_2; }
inline static Flags selectZ(void) { return FLAG_LINE_3; }
inline static Flags selectRX(void) { return FLAG_LINE_4; }
inline static Flags selectRY(void) { return FLAG_LINE_5; }
inline static Flags selectRZ(void) { return FLAG_LINE_6; }
inline static Flags selectTranslation(void) { return Flags(7); }
inline static Flags selectRotation(void) { return Flags(56); }
inline static Flags selectX(void) { return Flags("100000"); }
inline static Flags selectY(void) { return Flags("010000"); }
inline static Flags selectZ(void) { return Flags("001000"); }
inline static Flags selectRX(void) { return Flags("000100"); }
inline static Flags selectRY(void) { return Flags("000010"); }
inline static Flags selectRZ(void) { return Flags("000001"); }
inline static Flags selectTranslation(void) { return Flags("111000"); }
inline static Flags selectRotation(void) { return Flags("000111"); }
virtual void display(std::ostream &os) const;
......
......@@ -139,17 +139,6 @@ public:
virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res,
int time);
/** Static Feature selection. */
inline static Flags selectX(void) { return FLAG_LINE_1; }
inline static Flags selectY(void) { return FLAG_LINE_2; }
inline static Flags selectZ(void) { return FLAG_LINE_3; }
inline static Flags selectRX(void) { return FLAG_LINE_4; }
inline static Flags selectRY(void) { return FLAG_LINE_5; }
inline static Flags selectRZ(void) { return FLAG_LINE_6; }
inline static Flags selectTranslation(void) { return Flags(7); }
inline static Flags selectRotation(void) { return Flags(56); }
virtual void display(std::ostream &os) const;
public:
......
......@@ -80,8 +80,8 @@ public:
int time);
/** Static Feature selection. */
inline static Flags selectX(void) { return FLAG_LINE_1; }
inline static Flags selectY(void) { return FLAG_LINE_2; }
inline static Flags selectX(void) { return Flags("10"); }
inline static Flags selectY(void) { return Flags("01"); }
virtual void display(std::ostream &os) const;
};
......
......@@ -19,6 +19,7 @@
#include <vector>
/* SOT */
#include <dynamic-graph/signal-caster.h>
#include "sot/core/api.hh"
/* --------------------------------------------------------------------- */
......@@ -30,18 +31,15 @@ namespace sot {
class SOT_CORE_EXPORT Flags {
protected:
std::vector<char> flags;
bool reverse;
char operator[](const unsigned int &i) const;
std::vector<bool> flags;
bool outOfRangeFlag;
public:
Flags(const bool &b = false);
Flags(const char &c);
Flags(const int &c4);
Flags(const char *flags);
Flags(const std::vector<bool> &flags);
void add(const char &c);
void add(const int &c4);
void add(const bool &b);
Flags operator!(void) const;
SOT_CORE_EXPORT friend Flags operator&(const Flags &f1, const Flags &f2);
......@@ -49,14 +47,8 @@ public:
Flags &operator&=(const Flags &f2);
Flags &operator|=(const Flags &f2);
SOT_CORE_EXPORT friend Flags operator&(const Flags &f1, const bool &b);
SOT_CORE_EXPORT friend Flags operator|(const Flags &f1, const bool &b);
Flags &operator&=(const bool &b);
Flags &operator|=(const bool &b);
SOT_CORE_EXPORT friend std::ostream &operator<<(std::ostream &os,
const Flags &fl);
SOT_CORE_EXPORT friend char operator>>(const Flags &flags, const int &i);
SOT_CORE_EXPORT friend std::istream &operator>>(std::istream &is, Flags &fl);
bool operator()(const int &i) const;
......@@ -64,23 +56,12 @@ public:
void unset(const unsigned int &i);
void set(const unsigned int &i);
public: /* Selec "matlab-style" : 1:15, 1:, :45 ... */
static void readIndexMatlab(std::istream &iss, unsigned int &indexStart,
unsigned int &indexEnd, bool &unspecifiedEnd);
static Flags readIndexMatlab(std::istream &iss);
};
SOT_CORE_EXPORT extern const Flags FLAG_LINE_1;
SOT_CORE_EXPORT extern const Flags FLAG_LINE_2;
SOT_CORE_EXPORT extern const Flags FLAG_LINE_3;
SOT_CORE_EXPORT extern const Flags FLAG_LINE_4;
SOT_CORE_EXPORT extern const Flags FLAG_LINE_5;
SOT_CORE_EXPORT extern const Flags FLAG_LINE_6;
SOT_CORE_EXPORT extern const Flags FLAG_LINE_7;
SOT_CORE_EXPORT extern const Flags FLAG_LINE_8;
} // namespace sot
template <>
struct signal_io<sot::Flags> : signal_io_unimplemented<sot::Flags> {};
} // namespace dynamicgraph
#endif /* #ifndef __SOT_FLAGS_H */
......@@ -66,7 +66,7 @@ public:
"pushDenomCoef",
makeCommandVoid1(
*this, &IntegratorAbstract::pushDenomCoef,
docCommandVoid1("Push a new denomicator coefficient", typeName)));
docCommandVoid1("Push a new denominator coefficient", typeName)));
addCommand(
"popNumCoef",
......@@ -75,7 +75,7 @@ public:
addCommand(
"popDenomCoef",
makeCommandVoid0(*this, &IntegratorAbstract::popDenomCoef,
docCommandVoid0("Pop a new denomicator coefficient")));
docCommandVoid0("Pop a new denominator coefficient")));
}
virtual ~IntegratorAbstract() {}
......@@ -90,11 +90,33 @@ public:
void popNumCoef() { numerator.pop_back(); }
void popDenomCoef() { denominator.pop_back(); }
const std::vector<coefT>& numCoeffs() const { return numerator; }
void numCoeffs(const std::vector<coefT>& coeffs) { numerator = coeffs; }
const std::vector<coefT>& denomCoeffs() const { return denominator; }
void denomCoeffs(const std::vector<coefT>& coeffs) { denominator = coeffs; }
public:
dynamicgraph::SignalPtr<sigT, int> SIN;
dynamicgraph::SignalTimeDependent<sigT, int> SOUT;
virtual void display(std::ostream &os) const
{
os << this->getClassName() << ": " << getName() << '\n'
<< " ";
if (numerator.empty() || denominator.empty()) {
os << "ill-formed.";
return;
}
os << numerator[0];
for (std::size_t i = 1; i < numerator.size(); ++i)
os << " + " << numerator[i] << " s^" << i;
os << "\n " << denominator[0];
for (std::size_t i = 1; i < denominator.size(); ++i)
os << " + " << denominator[i] << " s^" << i;
}
protected:
std::vector<coefT> numerator;
std::vector<coefT> denominator;
......
......@@ -168,6 +168,18 @@ public:
double getSamplingPeriod() const { return dt; }
void initialize() {
if (denominator.empty() || numerator.empty())
throw dynamicgraph::ExceptionSignal(
dynamicgraph::ExceptionSignal::GENERIC,
"The numerator or the denominator is empty.");
// Check that denominator.back is the identity
if (!internal::integratorEulerCoeffIsIdentity(denominator.back()))
throw dynamicgraph::ExceptionSignal(
dynamicgraph::ExceptionSignal::GENERIC,
"The coefficient of the highest order derivative of denominator "
"should be 1 (the last pushDenomCoef should be the identity).");
std::size_t numsize = numerator.size();
inputMemory.resize(numsize);
......@@ -181,13 +193,6 @@ public:
for (std::size_t i = 0; i < denomsize; ++i) {
outputMemory[i] = inputMemory[0];
}
// Check that denominator.back is the identity
if (!internal::integratorEulerCoeffIsIdentity(denominator.back()))
throw dynamicgraph::ExceptionSignal(
dynamicgraph::ExceptionSignal::GENERIC,
"The coefficient of the highest order derivative of denominator "
"should be 1 (the last pushDenomCoef should be the identity).");
}
};
......
......@@ -31,16 +31,21 @@
namespace dynamicgraph {
namespace sot {
template <class Object> class Mailbox : public dynamicgraph::Entity {
namespace dg = dynamicgraph;
template <class Object> struct MailboxTimestampedObject
{
Object obj;
struct timeval timestamp;
};
template <class Object> class Mailbox : public dg::Entity {
public:
static const std::string CLASS_NAME;
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
public:
struct sotTimestampedObject {
Object obj;
struct timeval timestamp;
};
typedef MailboxTimestampedObject<Object> sotTimestampedObject;
public:
Mailbox(const std::string &name);
......@@ -61,12 +66,17 @@ protected:
bool update;
public: /* --- SIGNALS --- */
dynamicgraph::SignalTimeDependent<struct sotTimestampedObject, int> SOUT;
dynamicgraph::SignalTimeDependent<sotTimestampedObject, int> SOUT;
dynamicgraph::SignalTimeDependent<Object, int> objSOUT;
dynamicgraph::SignalTimeDependent<struct timeval, int> timeSOUT;
};
} /* namespace sot */
template <class Object> struct signal_io<sot::MailboxTimestampedObject<Object> >
: signal_io_unimplemented<sot::MailboxTimestampedObject<Object> > {};
template <> struct signal_io<timeval> : signal_io_unimplemented<timeval > {};
} /* namespace dynamicgraph */
#endif // #ifndef __SOT_MAILBOX_HH
......@@ -19,6 +19,7 @@
#include <vector>
/* SOT */
#include <dynamic-graph/signal-caster.h>
#include "sot/core/api.hh"
#include <sot/core/exception-task.hh>
......@@ -71,6 +72,8 @@ SOT_CORE_EXPORT std::ostream &operator<<(std::ostream &os,
SOT_CORE_EXPORT std::istream &operator>>(std::istream &os, VectorMultiBound &v);
} /* namespace sot */
template <> struct signal_io<sot::MultiBound> : signal_io_unimplemented<sot::MultiBound> {};
} /* namespace dynamicgraph */
#endif // #ifndef __SOT_MultiBound_H__
......@@ -77,9 +77,6 @@ public:
void clear(void) { signalMap.clear(); }
void display(std::ostream &os) const;
void addSpecificCommands(dynamicgraph::Entity &ent,
dynamicgraph::Entity::CommandMap_t &commap,
const std::string &prefix = "");
};
} // namespace sot
......
......@@ -5,50 +5,23 @@
*
*/
#ifndef __sot_torque_control_common_H__
#define __sot_torque_control_common_H__
#ifndef __sot_core_robot_utils_H__
#define __sot_core_robot_utils_H__
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/** pinocchio is forcing the BOOST_MPL_LIMIT_VECTOR_SIZE to a specific value.
This happen to be not working when including the boost property_tree
library. For this reason if defined, the current value of
BOOST_MPL_LIMIT_VECTOR_SIZE is saved in the preprocessor stack and unset.
Once the property_tree included the pinocchio value of this variable is
restored.
*/
#ifdef BOOST_MPL_LIMIT_VECTOR_SIZE
#pragma push_macro("BOOST_MPL_LIMIT_VECTOR_SIZE")
#define UNDEF_BOOST_MPL_LIMIT_VECTOR_SIZE
#undef BOOST_MPL_LIMIT_VECTOR_SIZE
#endif
#include <map>
#ifdef BOOST_MPL_LIMIT_LIST_SIZE
#pragma push_macro("BOOST_MPL_LIMIT_LIST_SIZE")
#define UNDEF_BOOST_MPL_LIMIT_LIST_SIZE
#undef BOOST_MPL_LIMIT_LIST_SIZE
#endif
#include <pinocchio/fwd.hpp>
#include <boost/assign.hpp>
#include <boost/property_tree/ptree.hpp>
#ifdef UNDEF_BOOST_MPL_LIMIT_VECTOR_SIZE
#pragma pop_macro("BOOST_MPL_LIMIT_VECTOR_SIZE")
#undef UNDEF_BOOST_MPL_LIMIT_VECTOR_SIZE
#endif
#ifdef UNDEF_BOOST_MPL_LIMIT_LIST_SIZE
#pragma pop_macro("BOOST_MPL_LIMIT_LIST_SIZE")
#undef UNDEF_BOOST_MPL_LIMIT_LIST_SIZE
#endif
#include "boost/assign.hpp"
#include <dynamic-graph/linear-algebra.h>
#include <dynamic-graph/logger.h>
#include <dynamic-graph/signal-helper.h>
#include <map>
#include <sot/core/matrix-geometry.hh>
namespace dynamicgraph {
......
/*
* Copyright 2010,
* François Bleibel,
* Olivier Stasse,
*
* CNRS/AIST
*
*/
#ifndef __SOT_SEQPLAY_HH
#define __SOT_SEQPLAY_HH
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* -- MaaL --- */
#include <dynamic-graph/linear-algebra.h>
/* SOT */
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
#include <list>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined(WIN32)
#if defined(seq_play_EXPORTS)
#define SOTSEQPLAY_EXPORT __declspec(dllexport)
#else
#define SOTSEQPLAY_EXPORT __declspec(dllimport)
#endif
#else
#define SOTSEQPLAY_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
namespace dynamicgraph {
namespace sot {
class SOTSEQPLAY_EXPORT SeqPlay : public dynamicgraph::Entity {
public:
static const std::string CLASS_NAME;
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
protected:
typedef std::list<dynamicgraph::Vector> StateList;
StateList stateList;
StateList::iterator currPos;
unsigned int currRank;
bool init;
int time;
public:
/* --- CONSTRUCTION --- */
SeqPlay(const std::string &name);
virtual ~SeqPlay(void) {}
void loadFile(const std::string &name);
dynamicgraph::Vector &getNextPosition(dynamicgraph::Vector &pos,
const int &time);
public: /* --- DISPLAY --- */
virtual void display(std::ostream &os) const;
SOTSEQPLAY_EXPORT friend std::ostream &operator<<(std::ostream &os,
const SeqPlay &r) {
r.display(os);
return os;
}
public: /* --- SIGNALS --- */
dynamicgraph::SignalTimeDependent<int, int> refresherSINTERN;
dynamicgraph::SignalTimeDependent<dynamicgraph::Vector, int> positionSOUT;
};
} /* namespace sot */
} /* namespace dynamicgraph */
#endif /* #ifndef __SOT_SEQPLAY_HH */
/*
* Copyright 2010,
* François Bleibel,
* Olivier Stasse,
*
* CNRS/AIST
*
*/
#ifndef __SOT_SEQPLAY_HH
#define __SOT_SEQPLAY_HH
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* -- MaaL --- */
#include <dynamic-graph/linear-algebra.h>
/* SOT */
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
#include <list>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined(WIN32)
#if defined(seq_play_EXPORTS)
#define SOTSEQPLAY_EXPORT __declspec(dllexport)
#else
#define SOTSEQPLAY_EXPORT __declspec(dllimport)
#endif
#else
#define SOTSEQPLAY_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
namespace dynamicgraph {
namespace sot {
class SOTSEQPLAY_EXPORT SeqPlay : public dynamicgraph::Entity {
public:
static const std::string CLASS_NAME;
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
protected:
typedef std::list<dynamicgraph::Vector> StateList;
StateList stateList;
StateList::iterator currPos;
unsigned int currRank;
bool init;
int time;
public:
/* --- CONSTRUCTION --- */
SeqPlay(const std::string &name);
virtual ~SeqPlay(void) {}
void loadFile(const std::string &name);
dynamicgraph::Vector &getNextPosition(dynamicgraph::Vector &pos,
const int &time);
public: /* --- DISPLAY --- */
virtual void display(std::ostream &os) const;
SOTSEQPLAY_EXPORT friend std::ostream &operator<<(std::ostream &os,
const SeqPlay &r) {
r.display(os);
return os;
}
public: /* --- SIGNALS --- */
dynamicgraph::SignalTimeDependent<int, int> refresherSINTERN;
dynamicgraph::SignalTimeDependent<dynamicgraph::Vector, int> positionSOUT;
};
} /* namespace sot */
} /* namespace dynamicgraph */
#endif /* #ifndef __SOT_SEQPLAY_HH */
......@@ -22,6 +22,7 @@ template <typename Value, typename Time = int>
class SOT_CORE_DLLAPI Switch : public VariadicAbstract<Value, Value, Time> {
DYNAMIC_GRAPH_ENTITY_DECL();
public:
typedef VariadicAbstract<Value, Value, Time> Base;
Switch(const std::string &name)
......@@ -55,6 +56,9 @@ class SOT_CORE_DLLAPI Switch : public VariadicAbstract<Value, Value, Time> {
return "Dynamically select a given signal based on a input information.\n";
}
SignalPtr<int, Time> selectionSIN;
SignalPtr<bool, Time> boolSelectionSIN;
private:
Value &signal(Value &ret, const Time &time) {
int sel;
......@@ -70,9 +74,6 @@ private:
ret = this->signalsIN[sel]->access(time);
return ret;
}
SignalPtr<int, Time> selectionSIN;