Merge tag 'v4.11.1'

Release of version 4.11.1.

CMakeLists.txt

@@ -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)

cmake @ 9dcde03a

-Subproject commit 82030f34fb590cfcece473b97ba910e7b6da30fe
+Subproject commit 9dcde03a880cccc86531019a6845708f5c54c35d

include/sot/core/device.hh

@@ -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();

include/sot/core/double-constant.hh

@@ -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) {}

include/sot/core/feature-point6d.hh

@@ -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;
 

include/sot/core/feature-pose.hh

@@ -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:

include/sot/core/feature-visual-point.hh

@@ -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;
 };

include/sot/core/flags.hh

@@ -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 */

include/sot/core/integrator-abstract.hh

@@ -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;

include/sot/core/integrator-euler.hh

@@ -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).");
   }
 };
 

include/sot/core/mailbox.hh

@@ -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

include/sot/core/multi-bound.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__

include/sot/core/periodic-call.hh

@@ -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

include/sot/core/robot-utils.hh

@@ -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 {

include/sot/core/seq-play.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 */

include/sot/core/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 */

include/sot/core/switch.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;
   }
-