Unverified Commit 5733488a authored by olivier stasse's avatar olivier stasse Committed by GitHub
Browse files

Merge pull request #53 from jmirabel/master

Add entity Event, Latch, Switch and some operators.
parents a1d1a216 a111a80d
......@@ -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
......
// Copyright (c) 2018, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// 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__
......@@ -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() {}
......
......@@ -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)
}
}
......
......@@ -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 */
......
/*
* Copyright 2017-, Rohan Budhiraja, Joseph Mirabel, CNRS
*
* 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 Lesser General 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_LATCH_H__
#define __SOT_LATCH_H__
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* SOT */
#include <dynamic-graph/command-bind.h>
#include <sot/core/pool.hh>
#include <dynamic-graph/entity.h>
#include <dynamic-graph/all-signals.h>
/* STD */
#include <string>
namespace dynamicgraph {
namespace sot {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
using dynamicgraph::Entity;
using dynamicgraph::command::makeCommandVoid0;
using dynamicgraph::command::docCommandVoid0;
class Latch : public Entity
{
public: /* --- SIGNAL --- */
DYNAMIC_GRAPH_ENTITY_DECL();
Signal<bool,int> outSOUT;
Signal<bool,int> turnOnSOUT;
Signal<bool,int> turnOffSOUT;
protected:
bool signalOutput;
void turnOn(){ signalOutput = true; }
bool& turnOnLatch(bool& res, int){ res = signalOutput = true; return res; }
void turnOff(){ signalOutput = false; }
bool& turnOffLatch(bool& res, int){ res = signalOutput = false; return res; }
bool& latchOutput(bool& res, int){ res = signalOutput; return res; }
public:
Latch( const std::string& name )
: Entity(name)
,outSOUT("Latch("+name+")::output(bool)::out")
,turnOnSOUT ("Latch("+name+")::output(bool)::turnOnSout")
,turnOffSOUT("Latch("+name+")::output(bool)::turnOffSout")
{
outSOUT.setFunction(boost::bind(&Latch::latchOutput,this,_1,_2));
turnOnSOUT .setFunction(boost::bind(&Latch::turnOnLatch ,this,_1,_2));
turnOffSOUT.setFunction(boost::bind(&Latch::turnOffLatch,this,_1,_2));
signalOutput = false;
signalRegistration (outSOUT << turnOnSOUT << turnOffSOUT);
addCommand ("turnOn",
makeCommandVoid0 (*this, &Latch::turnOn,
docCommandVoid0 ("Turn on the latch")));
addCommand ("turnOff",
makeCommandVoid0 (*this, &Latch::turnOff,
docCommandVoid0 ("Turn off the latch")));
}
virtual ~Latch( void ) {};
};
} /* namespace sot */
} /* namespace dynamicgraph */
#endif // #ifndef __SOT_LATCH_H__
/*
* Copyright 2017-, Rohan Budhiraja, CNRS
*
* 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 Lesser General 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/>.
*/
// Copyright (c) 2018, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// 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_SWITCH_H__
#define __SOT_SWITCH_H__
# define __SOT_SWITCH_H__
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <iostream>
/* SOT */
#include <dynamic-graph/command-bind.h>
#include <sot/core/pool.hh>
#include <dynamic-graph/entity.h>
#include <dynamic-graph/all-signals.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>
/* STD */
#include <string>
#include <sot/core/config.hh>
namespace dynamicgraph {
namespace dynamicgraph {
namespace sot {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
using dynamicgraph::Entity;
using dynamicgraph::command::makeCommandVoid0;
using dynamicgraph::command::docCommandVoid0;
class Switch : public Entity
/// Switch
template <typename Value, typename Time = int>
class SOT_CORE_DLLAPI Switch : public dynamicgraph::Entity
{
public: /* --- SIGNAL --- */
DYNAMIC_GRAPH_ENTITY_DECL();
dynamicgraph::Signal<bool,int> outSOUT;
protected:
bool signalOutput;
void turnOn(){ signalOutput = true; }
void turnOff(){ signalOutput = false; }
Switch (const std::string& name) :
Entity (name),
selectionSIN(NULL,"Switch("+name+")::input(int)::selection"),
boolSelectionSIN(NULL,"Switch("+name+")::input(bool)::boolSelection"),
signalSOUT ("Switch("+name+")::output(" + typeName() + ")::sout")
{
signalSOUT.setFunction (boost::bind (&Switch::signal, this, _1, _2));
signalRegistration (selectionSIN << boolSelectionSIN << signalSOUT);
using command::makeCommandVoid1;
std::string docstring =
"\n"
" Set number of input signals\n";
addCommand ("setSignalNumber", makeCommandVoid1
(*this, &Switch::setSignalNumber, docstring));
docstring =
"\n"
" Get number of input signals\n";
addCommand ("getSignalNumber",
new command::Getter<Switch, int> (*this, &Switch::getSignalNumber, docstring));
}
~Switch () {}
/// Header documentation of the python class
virtual std::string getDocString () const
{
return
"Dynamically select a given signal based on a input information.\n";
}
bool& switchOutput(bool& res, int){ res = signalOutput; return res; }
void setSignalNumber (const int& n)
{
assert (n>=0);
const std::size_t oldSize = signals.size();
for (std::size_t i = n; i < oldSize; ++i)
{
std::ostringstream oss; oss << "sin" << i;
signalDeregistration(oss.str());
delete signals[i];
}
signals.resize(n,NULL);
for (std::size_t i = oldSize; i < (std::size_t)n; ++i)
{
assert (signals[i]==NULL);
std::ostringstream oss;
oss << "Switch("<< getName()<< ")::input(" << typeName() << ")::sin" << i;
signals[i] = new Signal_t (NULL,oss.str());
signalRegistration(*signals[i]);
}
}
public:
Switch( const std::string& name )
: Entity(name)
,outSOUT("Switch("+name+")::output(bool)::out")
int getSignalNumber () const
{
outSOUT.setFunction(boost::bind(&Switch::switchOutput,this,_1,_2));
signalOutput = false;
signalRegistration (outSOUT );
addCommand ("turnOn",
makeCommandVoid0 (*this, &Switch::turnOn,
docCommandVoid0 ("Turn on the switch")));
addCommand ("turnOff",
makeCommandVoid0 (*this, &Switch::turnOff,
docCommandVoid0 ("Turn off the switch")));
return (int)signals.size();
}
virtual ~Switch( void ) {};
};
} /* namespace sot */