From 83e04ee635eafea00b89badacaf73623d784cd07 Mon Sep 17 00:00:00 2001 From: Joseph Mirabel Date: Mon, 11 Jun 2018 13:40:00 +0200 Subject: [PATCH 01/17] Make integration time in IntegratorEuler parameterizable. --- include/sot/core/integrator-euler.hh | 29 +++++++++++++++++++++++++--- 1 file changed, 26 insertions(+), 3 deletions(-) diff --git a/include/sot/core/integrator-euler.hh b/include/sot/core/integrator-euler.hh index 128fce8cf..64fd9e3fe 100644 --- a/include/sot/core/integrator-euler.hh +++ b/include/sot/core/integrator-euler.hh @@ -27,6 +27,8 @@ /* SOT */ #include +#include +#include /* --------------------------------------------------------------------- */ /* --- CLASS ----------------------------------------------------------- */ @@ -64,7 +66,17 @@ class IntegratorEuler IntegratorEuler( const std::string& name ) : IntegratorAbstract( name ) { + setSamplingPeriod (0.005); + SOUT.addDependency(SIN); + this->addCommand ("setSamplingPeriod", + new dg::command::Setter (*this, + &IntegratorEuler::setSamplingPeriod, + "Set the time during two sampling.")); + this->addCommand ("getSamplingPeriod", + new dg::command::Getter (*this, + &IntegratorEuler::getSamplingPeriod, + "Get the time during two sampling.")); } virtual ~IntegratorEuler( void ) {} @@ -73,14 +85,14 @@ protected: std::vector inputMemory; std::vector outputMemory; + double dt; + double invdt; + public: sigT& integrate( sigT& res, int time ) { sotDEBUG(15)<<"# In {"<& num = numerator; @@ -127,6 +139,17 @@ public: sotDEBUG(15)<<"# Out }"< Date: Mon, 11 Jun 2018 14:58:47 +0200 Subject: [PATCH 02/17] Add command to control IntegratorAbstract --- include/sot/core/integrator-abstract.hh | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/include/sot/core/integrator-abstract.hh b/include/sot/core/integrator-abstract.hh index 6ae1a86be..81a9bca5b 100644 --- a/include/sot/core/integrator-abstract.hh +++ b/include/sot/core/integrator-abstract.hh @@ -33,6 +33,7 @@ #include #include #include +#include #include /* STD */ @@ -69,6 +70,29 @@ class IntegratorAbstract "sotIntegratorAbstract("+name+")::output(vector)::sout") { signalRegistration( SIN<::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() {} From 83df0f2a9ea78ab2f5ec06f601bd8c6b8fdb1346 Mon Sep 17 00:00:00 2001 From: Joseph Mirabel Date: Mon, 11 Jun 2018 17:44:32 +0200 Subject: [PATCH 03/17] Fix IntegratorEuler (mix between numerator and denominator) --- include/sot/core/integrator-euler.hh | 46 ++++++++++++++++++++++------ src/matrix/integrator-euler.t.cpp | 13 ++++---- 2 files changed, 42 insertions(+), 17 deletions(-) diff --git a/include/sot/core/integrator-euler.hh b/include/sot/core/integrator-euler.hh index 64fd9e3fe..d60f56009 100644 --- a/include/sot/core/integrator-euler.hh +++ b/include/sot/core/integrator-euler.hh @@ -66,17 +66,24 @@ class IntegratorEuler IntegratorEuler( const std::string& name ) : IntegratorAbstract( name ) { + using namespace dg::command; + setSamplingPeriod (0.005); SOUT.addDependency(SIN); this->addCommand ("setSamplingPeriod", - new dg::command::Setter (*this, + new Setter (*this, &IntegratorEuler::setSamplingPeriod, "Set the time during two sampling.")); this->addCommand ("getSamplingPeriod", - new dg::command::Getter (*this, + new Getter (*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 ) {} @@ -106,28 +113,28 @@ public: // 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 = 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 = 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); } @@ -150,6 +157,25 @@ public: { 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 */ diff --git a/src/matrix/integrator-euler.t.cpp b/src/matrix/integrator-euler.t.cpp index 12ca9e0e6..a2dd8fde9 100644 --- a/src/matrix/integrator-euler.t.cpp +++ b/src/matrix/integrator-euler.t.cpp @@ -29,18 +29,17 @@ using namespace dynamicgraph; std::string sotClassType:: \ getTypeName( void ) { return #sotSigType; } \ template<> \ - const std::string sotClassType::CLASS_NAME \ - = std::string(className)+"<"+#sotSigType+","+#sotCoefType+">"; \ + const std::string sotClassType::CLASS_NAME = className; \ template<> \ const std::string& sotClassType:: \ - getClassName( void ) const { return CLASS_NAME; } \ + getClassName( void ) const { return CLASS_NAME; } \ extern "C" { \ - Entity *regFunction##_##sotSigType( const std::string& objname ) \ + Entity *regFunction##_##sotSigType( const std::string& objname ) \ { \ return new sotClassType( objname ); \ } \ - EntityRegisterer \ - regObj##_##sotSigType(std::string(className)+"<"+#sotSigType+","+#sotCoefType+">", \ + EntityRegisterer \ + regObj##_##sotSigType(sotClassType::CLASS_NAME, \ ®Function##_##sotSigType ); \ } @@ -48,6 +47,6 @@ using namespace std; namespace dynamicgraph { namespace sot { SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN_EULER(IntegratorEuler,Vector,Matrix, - "integratorEuler") + "IntegratorEulerVectorMatrix") } // namespace sot } // namespace dynamicgraph From 0827cca24712c897ebc05f74a919ed42ac41f0a1 Mon Sep 17 00:00:00 2001 From: Joseph Mirabel Date: Mon, 11 Jun 2018 17:46:16 +0200 Subject: [PATCH 04/17] Add access to derivative of output in IntegratorEuler --- include/sot/core/integrator-euler.hh | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/include/sot/core/integrator-euler.hh b/include/sot/core/integrator-euler.hh index d60f56009..725b6358f 100644 --- a/include/sot/core/integrator-euler.hh +++ b/include/sot/core/integrator-euler.hh @@ -65,12 +65,16 @@ class IntegratorEuler public: IntegratorEuler( const std::string& name ) : IntegratorAbstract( name ) + , derivativeSOUT(boost::bind(&IntegratorEuler::derivative,this,_1,_2), + SOUT, + "sotIntegratorAbstract("+name+")::output(vector)::derivativesout") { + this->signalRegistration( derivativeSOUT ); + using namespace dg::command; setSamplingPeriod (0.005); - SOUT.addDependency(SIN); this->addCommand ("setSamplingPeriod", new Setter (*this, &IntegratorEuler::setSamplingPeriod, @@ -92,6 +96,8 @@ protected: std::vector inputMemory; std::vector outputMemory; + dg::SignalTimeDependent derivativeSOUT; + double dt; double invdt; @@ -147,6 +153,16 @@ public: 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]; + } + void setSamplingPeriod (const double& period) { dt = period; From 0aba1a27db6c5011570f444e989af72df49837d8 Mon Sep 17 00:00:00 2001 From: Joseph Mirabel Date: Mon, 11 Jun 2018 19:05:51 +0200 Subject: [PATCH 05/17] Add binary operator entity Mix_of_vector --- src/matrix/operator.cpp | 52 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 52 insertions(+) diff --git a/src/matrix/operator.cpp b/src/matrix/operator.cpp index b75c42eb4..b3386c271 100644 --- a/src/matrix/operator.cpp +++ b/src/matrix/operator.cpp @@ -764,6 +764,58 @@ namespace dynamicgraph { }; REGISTER_BINARY_OP(VectorStack,Stack_of_vector); + /* --- STACK ------------------------------------------------------------ */ + struct VectorMix + : public BinaryOpHeader + { + public: + std::vector useV1; + std::vector idx1, idx2; + void operator()( const dynamicgraph::Vector& v1,const dynamicgraph::Vector& v2,dynamicgraph::Vector& res ) const + { + res.resize(useV1.size()); + std::size_t k1=0, k2=0; + for (std::size_t i = 0; i < useV1.size(); ++i) + { + if (useV1[i]) { + assert (k1 < idx1.size()); + res[i] = v1[idx1[k1]]; + ++k1; + } else { + assert (k2 < idx2.size()); + res[i] = v2[idx2[k2]]; + ++k2; + } + } + assert (k1 == idx1.size()); + assert (k2 == idx2.size()); + } + + void addSelec1( const int & i) { useV1.push_back(true ); idx1.push_back(i); } + void addSelec2( const int & i) { useV1.push_back(false); idx2.push_back(i); } + + void addSpecificCommands(Entity& ent, + Entity::CommandMap_t& commandMap ) + { + using namespace dynamicgraph::command; + + boost::function< void( const int& ) > selec1 + = boost::bind( &VectorMix::addSelec1,this,_1 ); + boost::function< void( const int& ) > selec2 + = boost::bind( &VectorMix::addSelec2,this,_1 ); + + ADD_COMMAND( "addSelec1", + makeCommandVoid1(ent, selec1, + docCommandVoid1("append value from vector 1.", + "int (index in vector 1)"))); + ADD_COMMAND( "addSelec2", + makeCommandVoid1(ent, selec2, + docCommandVoid1("append value from vector 2.", + "int (index in vector 2)"))); + } + }; + REGISTER_BINARY_OP(VectorMix,Mix_of_vector); + /* ---------------------------------------------------------------------- */ struct Composer From 45d91b869fa4b35e1593c582b315200bb065cd2f Mon Sep 17 00:00:00 2001 From: Joseph Mirabel Date: Tue, 12 Jun 2018 17:41:28 +0200 Subject: [PATCH 06/17] Fix IntegratorEuler::integrate --- include/sot/core/integrator-euler.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/sot/core/integrator-euler.hh b/include/sot/core/integrator-euler.hh index 725b6358f..732def74d 100644 --- a/include/sot/core/integrator-euler.hh +++ b/include/sot/core/integrator-euler.hh @@ -115,7 +115,7 @@ public: 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 From e832cff849c9c55c268b632e7bf17be431858152 Mon Sep 17 00:00:00 2001 From: Joseph Mirabel Date: Thu, 14 Jun 2018 10:37:47 +0200 Subject: [PATCH 07/17] Add signal to Switch entity --- include/sot/core/switch.hh | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/include/sot/core/switch.hh b/include/sot/core/switch.hh index 57518942a..18b04464c 100644 --- a/include/sot/core/switch.hh +++ b/include/sot/core/switch.hh @@ -45,13 +45,17 @@ namespace dynamicgraph { public: /* --- SIGNAL --- */ DYNAMIC_GRAPH_ENTITY_DECL(); - dynamicgraph::Signal outSOUT; + Signal outSOUT; + Signal turnOnSOUT; + Signal turnOffSOUT; protected: bool signalOutput; void turnOn(){ signalOutput = true; } + bool& turnOnSwitch(bool& res, int){ res = signalOutput = true; return res; } void turnOff(){ signalOutput = false; } + bool& turnOffSwitch(bool& res, int){ res = signalOutput = false; return res; } bool& switchOutput(bool& res, int){ res = signalOutput; return res; } @@ -59,10 +63,14 @@ namespace dynamicgraph { Switch( const std::string& name ) : Entity(name) ,outSOUT("Switch("+name+")::output(bool)::out") + ,turnOnSOUT ("Switch("+name+")::output(bool)::turnOnSout") + ,turnOffSOUT("Switch("+name+")::output(bool)::turnOffSout") { outSOUT.setFunction(boost::bind(&Switch::switchOutput,this,_1,_2)); + turnOnSOUT .setFunction(boost::bind(&Switch::turnOnSwitch ,this,_1,_2)); + turnOffSOUT.setFunction(boost::bind(&Switch::turnOffSwitch,this,_1,_2)); signalOutput = false; - signalRegistration (outSOUT ); + signalRegistration (outSOUT << turnOnSOUT << turnOffSOUT); addCommand ("turnOn", makeCommandVoid0 (*this, &Switch::turnOn, docCommandVoid0 ("Turn on the switch"))); From 0f0bb642b32f271a8b74c3de8dacb075fed7b8e2 Mon Sep 17 00:00:00 2001 From: Joseph Mirabel Date: Thu, 14 Jun 2018 10:39:33 +0200 Subject: [PATCH 08/17] Add known type bool to operators --- src/matrix/operator.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/matrix/operator.cpp b/src/matrix/operator.cpp index b3386c271..b9749245e 100644 --- a/src/matrix/operator.cpp +++ b/src/matrix/operator.cpp @@ -54,6 +54,7 @@ namespace dynamicgraph { #define ADD_KNOWN_TYPE( typeid ) \ template<>const std::string TypeNameHelper::typeName = #typeid + ADD_KNOWN_TYPE(bool); ADD_KNOWN_TYPE(double); ADD_KNOWN_TYPE(Vector); ADD_KNOWN_TYPE(Matrix); From f1b6da881028e772afa4a452d891e9d3e8752063 Mon Sep 17 00:00:00 2001 From: Joseph Mirabel Date: Thu, 14 Jun 2018 10:46:15 +0200 Subject: [PATCH 09/17] Rename entity Switch into Latch --- include/CMakeLists.txt | 2 +- include/sot/core/{switch.hh => latch.hh} | 40 ++++++++++++------------ src/CMakeLists.txt | 2 +- src/tools/{switch.cpp => latch.cpp} | 6 ++-- 4 files changed, 25 insertions(+), 25 deletions(-) rename include/sot/core/{switch.hh => latch.hh} (68%) rename src/tools/{switch.cpp => latch.cpp} (84%) diff --git a/include/CMakeLists.txt b/include/CMakeLists.txt index 0d9783716..a8791c9ca 100644 --- a/include/CMakeLists.txt +++ b/include/CMakeLists.txt @@ -31,7 +31,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 diff --git a/include/sot/core/switch.hh b/include/sot/core/latch.hh similarity index 68% rename from include/sot/core/switch.hh rename to include/sot/core/latch.hh index 18b04464c..b5823bd13 100644 --- a/include/sot/core/switch.hh +++ b/include/sot/core/latch.hh @@ -1,5 +1,5 @@ /* - * Copyright 2017-, Rohan Budhiraja, CNRS + * 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 @@ -14,8 +14,8 @@ * with sot-core. If not, see . */ -#ifndef __SOT_SWITCH_H__ -#define __SOT_SWITCH_H__ +#ifndef __SOT_LATCH_H__ +#define __SOT_LATCH_H__ /* --------------------------------------------------------------------- */ /* --- INCLUDE --------------------------------------------------------- */ @@ -40,7 +40,7 @@ namespace dynamicgraph { using dynamicgraph::command::makeCommandVoid0; using dynamicgraph::command::docCommandVoid0; - class Switch : public Entity + class Latch : public Entity { public: /* --- SIGNAL --- */ @@ -52,34 +52,34 @@ namespace dynamicgraph { protected: bool signalOutput; void turnOn(){ signalOutput = true; } - bool& turnOnSwitch(bool& res, int){ res = signalOutput = true; return res; } + bool& turnOnLatch(bool& res, int){ res = signalOutput = true; return res; } void turnOff(){ signalOutput = false; } - bool& turnOffSwitch(bool& res, int){ res = signalOutput = false; return res; } + bool& turnOffLatch(bool& res, int){ res = signalOutput = false; return res; } - bool& switchOutput(bool& res, int){ res = signalOutput; return res; } + bool& latchOutput(bool& res, int){ res = signalOutput; return res; } public: - Switch( const std::string& name ) + Latch( const std::string& name ) : Entity(name) - ,outSOUT("Switch("+name+")::output(bool)::out") - ,turnOnSOUT ("Switch("+name+")::output(bool)::turnOnSout") - ,turnOffSOUT("Switch("+name+")::output(bool)::turnOffSout") + ,outSOUT("Latch("+name+")::output(bool)::out") + ,turnOnSOUT ("Latch("+name+")::output(bool)::turnOnSout") + ,turnOffSOUT("Latch("+name+")::output(bool)::turnOffSout") { - outSOUT.setFunction(boost::bind(&Switch::switchOutput,this,_1,_2)); - turnOnSOUT .setFunction(boost::bind(&Switch::turnOnSwitch ,this,_1,_2)); - turnOffSOUT.setFunction(boost::bind(&Switch::turnOffSwitch,this,_1,_2)); + 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, &Switch::turnOn, - docCommandVoid0 ("Turn on the switch"))); + makeCommandVoid0 (*this, &Latch::turnOn, + docCommandVoid0 ("Turn on the latch"))); addCommand ("turnOff", - makeCommandVoid0 (*this, &Switch::turnOff, - docCommandVoid0 ("Turn off the switch"))); + makeCommandVoid0 (*this, &Latch::turnOff, + docCommandVoid0 ("Turn off the latch"))); } - virtual ~Switch( void ) {}; + virtual ~Latch( void ) {}; }; } /* namespace sot */ @@ -87,4 +87,4 @@ namespace dynamicgraph { -#endif // #ifndef __SOT_SWITCH_H__ +#endif // #ifndef __SOT_LATCH_H__ diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 043b50801..a00183aaf 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -91,7 +91,7 @@ SET(plugins tools/binary-int-to-uint tools/periodic-call-entity tools/joint-trajectory-entity - tools/switch + tools/latch control/control-gr control/control-pd diff --git a/src/tools/switch.cpp b/src/tools/latch.cpp similarity index 84% rename from src/tools/switch.cpp rename to src/tools/latch.cpp index f99777f1a..de715806d 100644 --- a/src/tools/switch.cpp +++ b/src/tools/latch.cpp @@ -1,5 +1,5 @@ /* - * Copyright 2017-, Rohan Budhiraja, CNRS + * Copyright 2017-2018, Rohan Budhiraja, Joseph Mirabel, CNRS * * CNRS/AIST * @@ -16,11 +16,11 @@ * with sot-core. If not, see . */ -#include +#include #include namespace dynamicgraph { namespace sot { - DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN (Switch, "Switch"); + DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN (Latch, "Latch"); } // namespace sot } // namespace dynamicgraph From 171862bc3803b3bdecc2d23dfbe106fa609e8a3b Mon Sep 17 00:00:00 2001 From: Joseph Mirabel Date: Thu, 14 Jun 2018 11:20:45 +0200 Subject: [PATCH 10/17] Move TypeNameHelper to independant file. --- src/matrix/operator.cpp | 26 +++--------------- src/tools/type-name-helper.hh | 50 +++++++++++++++++++++++++++++++++++ 2 files changed, 53 insertions(+), 23 deletions(-) create mode 100644 src/tools/type-name-helper.hh diff --git a/src/matrix/operator.cpp b/src/matrix/operator.cpp index b9749245e..cd28dedcf 100644 --- a/src/matrix/operator.cpp +++ b/src/matrix/operator.cpp @@ -3,6 +3,7 @@ * François Bleibel, * Olivier Stasse, * Nicolas Mansard + * Joseph Mirabel * * CNRS/AIST * @@ -35,6 +36,8 @@ #include #include +#include "../tools/type-name-helper.hh" + namespace dg = ::dynamicgraph; /* ---------------------------------------------------------------------------*/ @@ -43,27 +46,6 @@ namespace dg = ::dynamicgraph; namespace dynamicgraph { namespace sot { - template< typename TypeRef > - struct TypeNameHelper - { - static const std::string typeName; - }; - template< typename TypeRef > - const std::string TypeNameHelper::typeName = "unspecified"; - -#define ADD_KNOWN_TYPE( typeid ) \ - template<>const std::string TypeNameHelper::typeName = #typeid - - ADD_KNOWN_TYPE(bool); - ADD_KNOWN_TYPE(double); - ADD_KNOWN_TYPE(Vector); - ADD_KNOWN_TYPE(Matrix); - ADD_KNOWN_TYPE(MatrixRotation); - ADD_KNOWN_TYPE(MatrixTwist); - ADD_KNOWN_TYPE(MatrixHomogeneous); - ADD_KNOWN_TYPE(VectorQuaternion); - ADD_KNOWN_TYPE(VectorRollPitchYaw); - template< typename TypeIn, typename TypeOut > struct UnaryOpHeader { @@ -81,8 +63,6 @@ namespace dynamicgraph { std::string ("\n"); } }; - - } /* namespace sot */ } /* namespace dynamicgraph */ diff --git a/src/tools/type-name-helper.hh b/src/tools/type-name-helper.hh new file mode 100644 index 000000000..f52a03472 --- /dev/null +++ b/src/tools/type-name-helper.hh @@ -0,0 +1,50 @@ +/* + * Copyright 2010, + * François Bleibel, + * Olivier Stasse, + * Nicolas Mansard + * Joseph Mirabel + * + * CNRS/AIST + * + * 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 . + */ + +#include + +namespace dynamicgraph { + namespace sot { + template< typename TypeRef > + struct TypeNameHelper + { + static const std::string typeName; + }; + template< typename TypeRef > + const std::string TypeNameHelper::typeName = "unspecified"; + +#define ADD_KNOWN_TYPE( typeid ) \ + template<>const std::string TypeNameHelper::typeName = #typeid + + ADD_KNOWN_TYPE(bool); + ADD_KNOWN_TYPE(double); + ADD_KNOWN_TYPE(Vector); + ADD_KNOWN_TYPE(Matrix); + ADD_KNOWN_TYPE(MatrixRotation); + ADD_KNOWN_TYPE(MatrixTwist); + ADD_KNOWN_TYPE(MatrixHomogeneous); + ADD_KNOWN_TYPE(VectorQuaternion); + ADD_KNOWN_TYPE(VectorRollPitchYaw); + +#undef ADD_KNOWN_TYPE + } /* namespace sot */ +} /* namespace dynamicgraph */ From 94d0af53f595c92a4c4caf9e5838b45c9c910a18 Mon Sep 17 00:00:00 2001 From: Joseph Mirabel Date: Thu, 14 Jun 2018 11:21:46 +0200 Subject: [PATCH 11/17] Add entity Switch --- include/CMakeLists.txt | 1 + include/sot/core/switch.hh | 118 +++++++++++++++++++++++++++++++++++++ src/CMakeLists.txt | 1 + src/tools/switch.cpp | 35 +++++++++++ 4 files changed, 155 insertions(+) create mode 100644 include/sot/core/switch.hh create mode 100644 src/tools/switch.cpp diff --git a/include/CMakeLists.txt b/include/CMakeLists.txt index a8791c9ca..f3d903d51 100644 --- a/include/CMakeLists.txt +++ b/include/CMakeLists.txt @@ -86,6 +86,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 diff --git a/include/sot/core/switch.hh b/include/sot/core/switch.hh new file mode 100644 index 000000000..5a9235f3e --- /dev/null +++ b/include/sot/core/switch.hh @@ -0,0 +1,118 @@ +// 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 . + +#ifndef __SOT_SWITCH_H__ +# define __SOT_SWITCH_H__ + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace dynamicgraph { + namespace sot { + /// Switch + template + class SOT_CORE_DLLAPI Switch : public dynamicgraph::Entity + { + DYNAMIC_GRAPH_ENTITY_DECL(); + + 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)); + } + + ~Switch () {} + + /// Header documentation of the python class + virtual std::string getDocString () const + { + return + "Dynamically select a given signal based on a input information.\n"; + } + + 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]); + } + } + + private: + typedef SignalPtr Signal_t; + typedef std::vector Signals_t; + + static const std::string& typeName (); + + Value& signal (Value& ret, const Time& time) + { + int sel; + if (selectionSIN.isPlugged()) { + sel = selectionSIN (time); + } else { + const bool& b = boolSelectionSIN(time); + sel = b ? 1 : 0; + } + if (sel < 0 || sel >= int(signals.size())) + throw std::runtime_error ("Signal selection is out of range."); + + ret = (*signals[sel]) (time); + return ret; + } + + Signals_t signals; + SignalPtr selectionSIN; + SignalPtr boolSelectionSIN; + + Signal signalSOUT; + }; + } // namespace sot +} // namespace dynamicgraph +#endif // __SOT_SWITCH_H__ diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index a00183aaf..dfb2f2a66 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -92,6 +92,7 @@ SET(plugins tools/periodic-call-entity tools/joint-trajectory-entity tools/latch + tools/switch control/control-gr control/control-pd diff --git a/src/tools/switch.cpp b/src/tools/switch.cpp new file mode 100644 index 000000000..27c89fc3b --- /dev/null +++ b/src/tools/switch.cpp @@ -0,0 +1,35 @@ +// Copyright (c) 2017, Joseph Mirabel +// Authors: Joseph Mirabel (joseph.mirabel@laas.fr) +// +// This file is part of sot_hpp. +// sot_hpp 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_hpp 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_hpp. If not, see . + +#include + +#include + +#include "type-name-helper.hh" + +namespace dynamicgraph { + namespace sot { + template + const std::string& Switch::typeName () + { + return TypeNameHelper::typeName; + } + + typedef Switch SwitchVector; + template<> + DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN (SwitchVector, "SwitchVector"); + } // namespace sot +} // namespace dynamicgraph From ea9ecf6242bf669bbacbaf27e20249bdefca5b2b Mon Sep 17 00:00:00 2001 From: Joseph Mirabel Date: Thu, 14 Jun 2018 11:57:21 +0200 Subject: [PATCH 12/17] Add boolean reduction --- src/matrix/operator.cpp | 68 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 68 insertions(+) diff --git a/src/matrix/operator.cpp b/src/matrix/operator.cpp index cd28dedcf..85ff878a9 100644 --- a/src/matrix/operator.cpp +++ b/src/matrix/operator.cpp @@ -853,6 +853,74 @@ namespace dynamicgraph { }; REGISTER_BINARY_OP( ConvolutionTemporal,ConvolutionTemporal ); + /* --- BOOLEAN REDUCTION ------------------------------------------------ */ + + template < typename T > struct Comparison + : public BinaryOpHeader + { + void operator()( const T& a,const T& b, bool& res ) const + { + res = ( a < b); + } + virtual std::string getDocString () const + { + typedef BinaryOpHeader Base; + return std::string + ("Comparison of inputs:\n" + " - input ") + Base::nameTypeIn1 () + + std::string ("\n" + " - ") + Base::nameTypeIn2 () + + std::string ("\n" + " - output ") + Base::nameTypeOut () + + std::string ("\n"" sout = ( sin1 < sin2 )\n"); + } + }; + + template < typename T1, typename T2 = T1 > struct MatrixComparison + : public BinaryOpHeader + { + // TODO T1 or T2 could be a scalar type. + typedef Eigen::Array Array; + void operator()( const T1& a,const T2& b, bool& res ) const + { + Array r; + if (equal) r = (a.array() <= b.array()); + else r = (a.array() < b.array()); + if (any) res = r.any(); + else res = r.all(); + } + virtual std::string getDocString () const + { + typedef BinaryOpHeader Base; + return std::string + ("Comparison of inputs:\n" + " - input ") + Base::nameTypeIn1 () + + std::string ("\n" + " - ") + Base::nameTypeIn2 () + + std::string ("\n" + " - output ") + Base::nameTypeOut () + + std::string ("\n"" sout = ( sin1 < sin2 ).op()\n") + + std::string ("\n"" where op is either any (default) or all. The comparison can be made <=.\n"); + } + MatrixComparison () : any (true), equal (false) {} + void addSpecificCommands(Entity& ent, + Entity::CommandMap_t& commandMap) + { + using namespace dynamicgraph::command; + ADD_COMMAND( "setTrueIfAny", + makeDirectSetter(ent,&any,docDirectSetter("trueIfAny","bool"))); + ADD_COMMAND( "getTrueIfAny", + makeDirectGetter(ent,&any,docDirectGetter("trueIfAny","bool"))); + ADD_COMMAND( "setEqual", + makeDirectSetter(ent,&equal,docDirectSetter("equal","bool"))); + ADD_COMMAND( "getEqual", + makeDirectGetter(ent,&equal,docDirectGetter("equal","bool"))); + } + bool any, equal; + }; + + REGISTER_BINARY_OP (Comparison, CompareDouble); + REGISTER_BINARY_OP (MatrixComparison, CompareVector); } /* namespace sot */} /* namespace dynamicgraph */ From f3bdd370af59336cf0e9c8869cca2144f45b8c28 Mon Sep 17 00:00:00 2001 From: Joseph Mirabel Date: Thu, 14 Jun 2018 14:02:53 +0200 Subject: [PATCH 13/17] Add entity Event --- include/CMakeLists.txt | 1 + include/sot/core/event.hh | 116 ++++++++++++++++++++++++++++++++++++++ src/CMakeLists.txt | 1 + src/tools/event.cpp | 25 ++++++++ 4 files changed, 143 insertions(+) create mode 100644 include/sot/core/event.hh create mode 100644 src/tools/event.cpp diff --git a/include/CMakeLists.txt b/include/CMakeLists.txt index f3d903d51..47322bb8f 100644 --- a/include/CMakeLists.txt +++ b/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 diff --git a/include/sot/core/event.hh b/include/sot/core/event.hh new file mode 100644 index 000000000..4445f1ca9 --- /dev/null +++ b/include/sot/core/event.hh @@ -0,0 +1,116 @@ +// 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 . + +#ifndef __SOT_EVENT_H__ +# define __SOT_EVENT_H__ + +#include +#include +#include +#include +#include +#include +#include + +#include + +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 + (*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* Trigger_t; + typedef std::vector 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 checkSOUT; + + Triggers_t triggers; + SignalPtr conditionSIN; + + bool lastVal_; + }; + } // namespace sot +} // namespace dynamicgraph +#endif // __SOT_EVENT_H__ diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index dfb2f2a66..fe9c5fb25 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -75,6 +75,7 @@ SET(plugins traces/reader + tools/event tools/time-stamp tools/timer tools/seq-play diff --git a/src/tools/event.cpp b/src/tools/event.cpp new file mode 100644 index 000000000..5f69f0155 --- /dev/null +++ b/src/tools/event.cpp @@ -0,0 +1,25 @@ +// Copyright (c) 2017, 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 . + +#include + +#include + +namespace dynamicgraph { + namespace sot { + DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN (Event, "Event"); + } // namespace sot +} // namespace dynamicgraph From 2682f7bd8633c71ed74f4c020897c844415b8906 Mon Sep 17 00:00:00 2001 From: Joseph Mirabel Date: Thu, 14 Jun 2018 14:54:52 +0200 Subject: [PATCH 14/17] Add entity IntegratorEulerVectorDouble --- include/sot/core/integrator-euler-impl.hh | 1 + include/sot/core/integrator-euler.hh | 5 +++-- src/matrix/integrator-euler.cpp | 6 +++++- src/matrix/integrator-euler.t.cpp | 8 +++++--- 4 files changed, 14 insertions(+), 6 deletions(-) diff --git a/include/sot/core/integrator-euler-impl.hh b/include/sot/core/integrator-euler-impl.hh index 1dca77069..ae5809732 100644 --- a/include/sot/core/integrator-euler-impl.hh +++ b/include/sot/core/integrator-euler-impl.hh @@ -58,6 +58,7 @@ namespace dynamicgraph { namespace sot { DECLARE_SPECIFICATION(IntegratorEulerVectorMatrix,dg::Vector,dg::Matrix) + DECLARE_SPECIFICATION(IntegratorEulerVectorDouble,dg::Vector,double) } } diff --git a/include/sot/core/integrator-euler.hh b/include/sot/core/integrator-euler.hh index 732def74d..2dbb93cfa 100644 --- a/include/sot/core/integrator-euler.hh +++ b/include/sot/core/integrator-euler.hh @@ -119,7 +119,7 @@ public: // End of step 1. Here, sum is b_0 X // Step 2 - int numsize = num.size(); + int numsize = (int)num.size(); for(int i = 1; i < numsize; ++i) { tmp2 = inputMemory[i-1] - tmp1; @@ -131,7 +131,7 @@ public: // End of step 2. Here, sum is b_m * d(m)X / dt^m + ... - b_0 X // Step 3 - int denomsize = denom.size() - 1; + int denomsize = (int)denom.size() - 1; for(int i = 0; i < denomsize; ++i) { sum -= (denom[i] * outputMemory[i]); @@ -161,6 +161,7 @@ public: SOUT.recompute(time); res = outputMemory[1]; + return res; } void setSamplingPeriod (const double& period) diff --git a/src/matrix/integrator-euler.cpp b/src/matrix/integrator-euler.cpp index d66f57b89..47a7d3c66 100644 --- a/src/matrix/integrator-euler.cpp +++ b/src/matrix/integrator-euler.cpp @@ -28,6 +28,10 @@ #ifdef WIN32 IntegratorEulerVectorMatrix::IntegratorEulerVectorMatrix( const std::string& name ) : IntegratorEuler(name) {} - std::string IntegratorEulerVectorMatrix::getTypeName( void ) { return "double"; } + std::string IntegratorEulerVectorMatrix::getTypeName( void ) { return "IntegratorEulerVectorMatrix"; } + + IntegratorEulerVectorDouble::IntegratorEulerVectorDouble( const std::string& name ) : + IntegratorEuler(name) {} + std::string IntegratorEulerVectorDouble::getTypeName( void ) { return "IntegratorEulerVectorDouble"; } #endif // WIN32 diff --git a/src/matrix/integrator-euler.t.cpp b/src/matrix/integrator-euler.t.cpp index a2dd8fde9..5e5bf7190 100644 --- a/src/matrix/integrator-euler.t.cpp +++ b/src/matrix/integrator-euler.t.cpp @@ -34,13 +34,13 @@ using namespace dynamicgraph; const std::string& sotClassType:: \ getClassName( void ) const { return CLASS_NAME; } \ extern "C" { \ - Entity *regFunction##_##sotSigType( const std::string& objname ) \ + Entity *regFunction##_##sotSigType##_##sotCoefType( const std::string& objname ) \ { \ return new sotClassType( objname ); \ } \ EntityRegisterer \ - regObj##_##sotSigType(sotClassType::CLASS_NAME, \ - ®Function##_##sotSigType ); \ + regObj##_##sotSigType##_##sotCoefType(sotClassType::CLASS_NAME, \ + ®Function##_##sotSigType##_##sotCoefType ); \ } using namespace std; @@ -48,5 +48,7 @@ namespace dynamicgraph { namespace sot { SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN_EULER(IntegratorEuler,Vector,Matrix, "IntegratorEulerVectorMatrix") + SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN_EULER(IntegratorEuler,Vector,double, + "IntegratorEulerVectorDouble") } // namespace sot } // namespace dynamicgraph From 24339bc67fe2449585b3af8b9d953df3181a5bd0 Mon Sep 17 00:00:00 2001 From: Joseph Mirabel Date: Thu, 14 Jun 2018 16:05:14 +0200 Subject: [PATCH 15/17] Add IntegratorEuler for double --- include/sot/core/integrator-euler-impl.hh | 5 +++-- include/sot/core/integrator-euler.hh | 5 ++--- src/matrix/integrator-euler.t.cpp | 6 ++++++ 3 files changed, 11 insertions(+), 5 deletions(-) diff --git a/include/sot/core/integrator-euler-impl.hh b/include/sot/core/integrator-euler-impl.hh index ae5809732..3bf7cab79 100644 --- a/include/sot/core/integrator-euler-impl.hh +++ b/include/sot/core/integrator-euler-impl.hh @@ -57,8 +57,9 @@ namespace dynamicgraph { namespace sot { - DECLARE_SPECIFICATION(IntegratorEulerVectorMatrix,dg::Vector,dg::Matrix) - DECLARE_SPECIFICATION(IntegratorEulerVectorDouble,dg::Vector,double) + DECLARE_SPECIFICATION(IntegratorEulerDoubleDouble,double,double) + DECLARE_SPECIFICATION(IntegratorEulerVectorDouble,Vector,double) + DECLARE_SPECIFICATION(IntegratorEulerVectorMatrix,Vector,Matrix) } } diff --git a/include/sot/core/integrator-euler.hh b/include/sot/core/integrator-euler.hh index 2dbb93cfa..5caf13ff8 100644 --- a/include/sot/core/integrator-euler.hh +++ b/include/sot/core/integrator-euler.hh @@ -52,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; @@ -67,7 +67,7 @@ class IntegratorEuler : IntegratorAbstract( name ) , derivativeSOUT(boost::bind(&IntegratorEuler::derivative,this,_1,_2), SOUT, - "sotIntegratorAbstract("+name+")::output(vector)::derivativesout") + "sotIntegratorEuler("+name+")::output(vector)::derivativesout") { this->signalRegistration( derivativeSOUT ); @@ -114,7 +114,6 @@ public: // Step 1 tmp1 = inputMemory[0]; inputMemory[0] = SIN.access(time); - sum.resize(tmp1.size()); sum = num[0] * inputMemory[0]; // End of step 1. Here, sum is b_0 X diff --git a/src/matrix/integrator-euler.t.cpp b/src/matrix/integrator-euler.t.cpp index 5e5bf7190..2f0324fdd 100644 --- a/src/matrix/integrator-euler.t.cpp +++ b/src/matrix/integrator-euler.t.cpp @@ -46,9 +46,15 @@ using namespace dynamicgraph; using namespace std; namespace dynamicgraph { namespace sot { + SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN_EULER(IntegratorEuler,double,double, + "IntegratorEulerDoubleDouble") SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN_EULER(IntegratorEuler,Vector,Matrix, "IntegratorEulerVectorMatrix") SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN_EULER(IntegratorEuler,Vector,double, "IntegratorEulerVectorDouble") + + template class IntegratorEuler; + template class IntegratorEuler; + template class IntegratorEuler; } // namespace sot } // namespace dynamicgraph From bff3eece18fd73e334f80958cf21b80f1f4f12f6 Mon Sep 17 00:00:00 2001 From: Joseph Mirabel Date: Thu, 14 Jun 2018 16:07:18 +0200 Subject: [PATCH 16/17] Make IntegratorAbstract pure virtual class. --- include/sot/core/integrator-abstract.hh | 5 --- src/matrix/integrator-abstract.cpp | 6 ++-- src/matrix/integrator-abstract.t.cpp | 46 ------------------------- src/matrix/integrator-euler.cpp | 1 - 4 files changed, 3 insertions(+), 55 deletions(-) delete mode 100644 src/matrix/integrator-abstract.t.cpp diff --git a/include/sot/core/integrator-abstract.hh b/include/sot/core/integrator-abstract.hh index 81a9bca5b..e663d4116 100644 --- a/include/sot/core/integrator-abstract.hh +++ b/include/sot/core/integrator-abstract.hh @@ -56,11 +56,6 @@ template 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) diff --git a/src/matrix/integrator-abstract.cpp b/src/matrix/integrator-abstract.cpp index 9fa22ee1e..6dbb9382e 100644 --- a/src/matrix/integrator-abstract.cpp +++ b/src/matrix/integrator-abstract.cpp @@ -20,9 +20,6 @@ #include -// The specilization -#include "integrator-abstract.t.cpp" - // This ends the specialization part. // Note that on WIN32, the specialization has to be realized // before the declaration of the general model. @@ -34,5 +31,8 @@ IntegratorAbstractVector::IntegratorAbstractVector( const std::string& name ) : IntegratorAbstract (name) {} + + IntegratorAbstractVector::IntegratorAbstractVectorDouble( const std::string& name ) : + IntegratorAbstract (name) {} #endif diff --git a/src/matrix/integrator-abstract.t.cpp b/src/matrix/integrator-abstract.t.cpp deleted file mode 100644 index 4e475ecee..000000000 --- a/src/matrix/integrator-abstract.t.cpp +++ /dev/null @@ -1,46 +0,0 @@ -/* - * Copyright 2010, - * François Bleibel, - * Olivier Stasse, - * - * CNRS/AIST - * - * 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 . - */ - -#include -#include - -using namespace dynamicgraph::sot; -using namespace dynamicgraph; - -#define SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN(sotClassType,sotSigType,sotCoefType,className) \ - template<> \ - std::string sotClassType:: \ - getTypeName( void ) { return #sotSigType; } \ - template<> \ - const std::string sotClassType::CLASS_NAME \ - = std::string(className)+"<"+#sotSigType+","+#sotCoefType+">"; \ - template<> \ - const std::string& sotClassType:: \ - getClassName( void ) const { return CLASS_NAME; } - - -namespace dynamicgraph { - namespace sot { - SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN(IntegratorAbstract,double,double, - "integratorAbstract") - SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN(IntegratorAbstract,Vector,Matrix, - "integratorAbstract") - } // namespace sot -} // namespace dynamicgraph diff --git a/src/matrix/integrator-euler.cpp b/src/matrix/integrator-euler.cpp index 47a7d3c66..a69860898 100644 --- a/src/matrix/integrator-euler.cpp +++ b/src/matrix/integrator-euler.cpp @@ -20,7 +20,6 @@ #include -#include "integrator-abstract.t.cpp" #include "integrator-euler.t.cpp" #include From a111a80d7e4a1411af478998b35e3ae341baac35 Mon Sep 17 00:00:00 2001 From: Joseph Mirabel Date: Fri, 15 Jun 2018 11:02:23 +0200 Subject: [PATCH 17/17] Add Switch::getSignalNumber --- include/sot/core/switch.hh | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/include/sot/core/switch.hh b/include/sot/core/switch.hh index 5a9235f3e..d1ad53da0 100644 --- a/include/sot/core/switch.hh +++ b/include/sot/core/switch.hh @@ -52,6 +52,12 @@ namespace dynamicgraph { " 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 (*this, &Switch::getSignalNumber, docstring)); } ~Switch () {} @@ -85,6 +91,11 @@ namespace dynamicgraph { } } + int getSignalNumber () const + { + return (int)signals.size(); + } + private: typedef SignalPtr Signal_t; typedef std::vector Signals_t;