Merge pull request #53 from jmirabel/master

Add entity Event, Latch, Switch and some operators.

include/CMakeLists.txt

@@ -22,6 +22,7 @@ SET(NEWHEADERS
   sot/core/matrix-svd.hh
   sot/core/contiifstream.hh
   sot/core/debug.hh
+  sot/core/event.hh
   sot/core/exception-abstract.hh
   sot/core/exception-dynamic.hh
   sot/core/exception-factory.hh
@@ -31,7 +32,7 @@ SET(NEWHEADERS
   sot/core/exception-tools.hh
   sot/core/binary-op.hh
   sot/core/derivator.hh
-  sot/core/switch.hh
+  sot/core/latch.hh
   sot/core/fir-filter.hh
   sot/core/integrator-abstract.hh
   sot/core/integrator-euler.hh
@@ -86,6 +87,7 @@ SET(NEWHEADERS
   sot/core/periodic-call.hh
   sot/core/periodic-call-entity.hh
   sot/core/trajectory.hh
+  sot/core/switch.hh
   )
 INSTALL(FILES ${NEWHEADERS}
   DESTINATION include/sot/core

include/sot/core/event.hh

@@ -0,0 +1,116 @@
+// Copyright (c) 2018, Joseph Mirabel
+// Authors: Joseph Mirabel ([email protected])
+//
+// This file is part of sot-core.
+// sot-core 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.
+//
+// sot-core 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
+// sot-core. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef __SOT_EVENT_H__
+# define __SOT_EVENT_H__
+
+#include <dynamic-graph/entity.h>
+#include <dynamic-graph/signal.h>
+#include <dynamic-graph/signal-ptr.h>
+#include <dynamic-graph/signal-time-dependent.h>
+#include <dynamic-graph/pool.h>
+#include <dynamic-graph/command-bind.h>
+#include <dynamic-graph/command-getter.h>
+
+#include <sot/core/config.hh>
+
+namespace dynamicgraph {
+  namespace sot {
+    /// Event
+    class SOT_CORE_DLLAPI Event : public dynamicgraph::Entity
+    {
+      DYNAMIC_GRAPH_ENTITY_DECL();
+
+      Event (const std::string& name) :
+        Entity (name),
+        checkSOUT ("Event("+name+")::output(bool)::check"),
+        conditionSIN(NULL,"Event("+name+")::input(bool)::condition"),
+        lastVal_ (2) // lastVal_ should be different true and false.
+      {
+        checkSOUT.setFunction
+          (boost::bind (&Event::check, this, _1, _2));
+        signalRegistration (conditionSIN);
+        signalRegistration (checkSOUT);
+
+        using command::makeCommandVoid1;
+        std::string docstring =
+          "\n"
+          "    Add a signal\n";
+        addCommand ("addSignal", makeCommandVoid1
+            (*this, &Event::addSignal, docstring));
+
+        docstring =
+          "\n"
+          "    Get list of signals\n";
+        addCommand ("list", new command::Getter<Event, std::string>
+            (*this, &Event::getSignalsByName, docstring));
+      }
+
+      ~Event () {}
+
+      /// Header documentation of the python class
+      virtual std::string getDocString () const
+      {
+        return
+          "Send an event when the input changes\n\n"
+          "  The signal triggered is called whenever the condition is satisfied.\n";
+      }
+
+      void addSignal (const std::string& signal)
+      {
+        std::istringstream iss (signal);
+        triggers.push_back(&PoolStorage::getInstance()->getSignal (iss));
+      }
+
+      // Returns the Python string representation of the list of signal names.
+      std::string getSignalsByName () const
+      {
+        std::ostringstream oss;
+        oss << "(";
+        for (Triggers_t::const_iterator _sig = triggers.begin();
+            _sig != triggers.end(); ++_sig)
+          oss << '\'' << (*_sig)->getName() << "\', ";
+        oss << ")";
+        return oss.str();
+      }
+
+      private:
+      typedef SignalBase<int>* Trigger_t;
+      typedef std::vector<Trigger_t> Triggers_t;
+
+      bool& check (bool& ret, const int& time)
+      {
+        const bool& val = conditionSIN (time);
+        ret = (val != lastVal_);
+        if (ret) {
+          lastVal_ = val;
+          for (Triggers_t::const_iterator _s = triggers.begin();
+              _s != triggers.end(); ++_s)
+            (*_s)->recompute (time);
+        }
+        return ret;
+      }
+
+      Signal <bool, int> checkSOUT;
+
+      Triggers_t triggers;
+      SignalPtr <bool, int> conditionSIN;
+
+      bool lastVal_;
+    };
+  } // namespace sot
+} // namespace dynamicgraph
+#endif // __SOT_EVENT_H__

include/sot/core/integrator-abstract.hh

@@ -33,6 +33,7 @@
 #include <dynamic-graph/entity.h>
 #include <dynamic-graph/pool.h>
 #include <dynamic-graph/all-signals.h>
+#include <dynamic-graph/command-bind.h>
 #include <sot/core/debug.hh>
 
 /* STD */
@@ -55,11 +56,6 @@ template<class sigT, class coefT>
 class IntegratorAbstract
 :public dg::Entity
 {
- public:
-  virtual const std::string& getClassName() const { return dg::Entity::getClassName(); }
-  static std::string getTypeName( void ) { return "Unknown"; }
-  static const std::string CLASS_NAME;
-
  public:
   IntegratorAbstract ( const std::string& name )
     :dg::Entity(name)
@@ -69,6 +65,29 @@ class IntegratorAbstract
 		 "sotIntegratorAbstract("+name+")::output(vector)::sout")
   {
     signalRegistration( SIN<<SOUT );
+
+    using namespace dg::command;
+
+    const std::string typeName =
+      Value::typeName(dg::command::ValueHelper<coefT>::TypeID);
+
+    addCommand ("pushNumCoef",
+        makeCommandVoid1 (*this, &IntegratorAbstract::pushNumCoef,
+          docCommandVoid1 ("Push a new numerator coefficient", typeName)
+          ));
+    addCommand ("pushDenomCoef",
+        makeCommandVoid1 (*this, &IntegratorAbstract::pushDenomCoef,
+          docCommandVoid1 ("Push a new denomicator coefficient", typeName)
+          ));
+
+    addCommand ("popNumCoef",
+        makeCommandVoid0 (*this, &IntegratorAbstract::popNumCoef,
+          docCommandVoid0 ("Pop a new numerator coefficient")
+          ));
+    addCommand ("popDenomCoef",
+        makeCommandVoid0 (*this, &IntegratorAbstract::popDenomCoef,
+          docCommandVoid0 ("Pop a new denomicator coefficient")
+          ));
   }
 
   virtual ~IntegratorAbstract() {}

include/sot/core/integrator-euler-impl.hh

@@ -57,7 +57,9 @@
 
 namespace dynamicgraph { 
   namespace sot {
-	DECLARE_SPECIFICATION(IntegratorEulerVectorMatrix,dg::Vector,dg::Matrix)
+	DECLARE_SPECIFICATION(IntegratorEulerDoubleDouble,double,double)
+	DECLARE_SPECIFICATION(IntegratorEulerVectorDouble,Vector,double)
+	DECLARE_SPECIFICATION(IntegratorEulerVectorMatrix,Vector,Matrix)
   }
 }
 

include/sot/core/integrator-euler.hh

@@ -27,6 +27,8 @@
 
 /* SOT */
 #include <sot/core/integrator-abstract.hh>
+#include <dynamic-graph/command-setter.h>
+#include <dynamic-graph/command-getter.h>
 
 /* --------------------------------------------------------------------- */
 /* --- CLASS ----------------------------------------------------------- */
@@ -50,7 +52,7 @@ class IntegratorEuler
 {
 
  public: 
-  virtual const std::string& getClassName( void ) const { return dg::Entity::getClassName(); }
+  virtual const std::string& getClassName( void ) const;
   static std::string getTypeName( void ) { return "Unknown"; }
   static const std::string CLASS_NAME;
 
@@ -63,8 +65,29 @@ class IntegratorEuler
  public:
   IntegratorEuler( const std::string& name )
     : IntegratorAbstract<sigT,coefT>( name )
+    , derivativeSOUT(boost::bind(&IntegratorEuler<sigT,coefT>::derivative,this,_1,_2),
+		     SOUT,
+		     "sotIntegratorEuler("+name+")::output(vector)::derivativesout")
   {
-    SOUT.addDependency(SIN);
+    this->signalRegistration( derivativeSOUT );
+
+    using namespace dg::command;
+
+    setSamplingPeriod (0.005);
+
+    this->addCommand ("setSamplingPeriod",
+        new Setter<IntegratorEuler,double> (*this,
+          &IntegratorEuler::setSamplingPeriod,
+          "Set the time during two sampling."));
+    this->addCommand ("getSamplingPeriod",
+        new Getter<IntegratorEuler,double> (*this,
+          &IntegratorEuler::getSamplingPeriod,
+          "Get the time during two sampling."));
+
+    this->addCommand ("initialize",
+        makeCommandVoid0 (*this, &IntegratorEuler::initialize,
+          docCommandVoid0 ("Initialize internal memory from current value of input")
+          ));
   }
 
   virtual ~IntegratorEuler( void ) {}
@@ -73,14 +96,16 @@ protected:
   std::vector<sigT> inputMemory;
   std::vector<sigT> outputMemory;
 
+  dg::SignalTimeDependent<sigT, int> derivativeSOUT;
+
+  double dt;
+  double invdt;
+
 public:
   sigT& integrate( sigT& res, int time )
   {
     sotDEBUG(15)<<"# In {"<<std::endl;
 
-    const double dt = 0.005;
-    const double invdt = 200;
-
     sigT sum;
     sigT tmp1, tmp2;
     const std::vector<coefT>& num = numerator;
@@ -89,33 +114,32 @@ public:
     // Step 1
     tmp1 = inputMemory[0];
     inputMemory[0] = SIN.access(time);
-    sum.resize(tmp1.size());
-    sum = denom[0] * inputMemory[0];
+    sum = num[0] * inputMemory[0];
     // End of step 1. Here, sum is b_0 X
 
     // Step 2
-    int denomsize = denom.size();
-    for(int i = 1; i < denomsize; ++i)
+    int numsize = (int)num.size();
+    for(int i = 1; i < numsize; ++i)
     {
       tmp2 = inputMemory[i-1] - tmp1;
       tmp2 *= invdt;
       tmp1 = inputMemory[i];
       inputMemory[i] = tmp2;
-      sum += (denom[i] * inputMemory[i]);
+      sum += (num[i] * inputMemory[i]);
     }
     // End of step 2. Here, sum is b_m * d(m)X / dt^m + ... - b_0 X
 
     // Step 3
-    int numsize = num.size() - 1;
-    for(int i = 0; i < numsize; ++i)
+    int denomsize = (int)denom.size() - 1;
+    for(int i = 0; i < denomsize; ++i)
     {
-      sum -= (num[i] * outputMemory[i]);
+      sum -= (denom[i] * outputMemory[i]);
     }
     // End of step 3. Here, sum is b_m * d(m)X / dt^m + ... - b_0 X - a_0 Y - ... a_n-1 d(n-1)Y / dt^(n-1)
 
     // Step 4
-    outputMemory[numsize] = sum;
-    for(int i = numsize - 1; i >= 0; --i)
+    outputMemory[denomsize] = sum;
+    for(int i = denomsize-1; i >= 0; --i)
     {
       outputMemory[i] += (outputMemory[i+1] * dt);
     }
@@ -127,6 +151,47 @@ public:
     sotDEBUG(15)<<"# Out }"<<std::endl;
     return res;
   }
+
+  sigT& derivative ( sigT& res, int time )
+  {
+    if (outputMemory.size() < 2)
+      throw dg::ExceptionSignal (dg::ExceptionSignal::GENERIC,
+          "Integrator does not compute the derivative.");
+
+    SOUT.recompute(time);
+    res = outputMemory[1];
+    return res;
+  }
+
+  void setSamplingPeriod (const double& period)
+  {
+    dt = period;
+    invdt = 1/period;
+  }
+
+  double getSamplingPeriod () const
+  {
+    return dt;
+  }
+
+  void initialize ()
+  {
+    std::size_t numsize = numerator.size();
+    inputMemory.resize(numsize);
+
+    inputMemory[0] = SIN.accessCopy();
+    for(std::size_t i = 1; i < numsize; ++i)
+    {
+      inputMemory[i] = inputMemory[0];
+    }
+
+    std::size_t denomsize = denominator.size();
+    outputMemory.resize(denomsize);
+    for(std::size_t i = 0; i < denomsize; ++i)
+    {
+      outputMemory[i] = inputMemory[0];
+    }
+  }
 };
 
 } /* namespace sot */} /* namespace dynamicgraph */