Skip to content

Commit abcdfa4

Browse files
jmirabeljmirabel
committed
Add entity VariadicOp and update Mix_of_vector entity.
1 parent f5aab9c commit abcdfa4

File tree

2 files changed

+292
-52
lines changed

2 files changed

+292
-52
lines changed

‎include/sot/core/variadic-op.hh

Lines changed: 198 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,198 @@
1+
/*
2+
* Copyright 2018,
3+
* Mirabel Joseph
4+
*
5+
* CNRS/AIST
6+
*
7+
* This file is part of sot-core.
8+
* sot-core is free software: you can redistribute it and/or
9+
* modify it under the terms of the GNU Lesser General Public License
10+
* as published by the Free Software Foundation, either version 3 of
11+
* the License, or (at your option) any later version.
12+
* sot-core is distributed in the hope that it will be
13+
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty
14+
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15+
* GNU Lesser General Public License for more details. You should
16+
* have received a copy of the GNU Lesser General Public License along
17+
* with sot-core. If not, see <http://www.gnu.org/licenses/>.
18+
*/
19+
20+
#ifndef SOT_CORE_VARIADICOP_HH
21+
#define SOT_CORE_VARIADICOP_HH
22+
23+
/* --------------------------------------------------------------------- */
24+
/* --- INCLUDE --------------------------------------------------------- */
25+
/* --------------------------------------------------------------------- */
26+
27+
/* Matrix */
28+
#include <dynamic-graph/linear-algebra.h>
29+
30+
/* SOT */
31+
#include <sot/core/flags.hh>
32+
#include <dynamic-graph/entity.h>
33+
#include <sot/core/pool.hh>
34+
#include <dynamic-graph/all-signals.h>
35+
#include <sot/core/matrix-geometry.hh>
36+
37+
/* STD */
38+
#include <string>
39+
40+
#include <boost/function.hpp>
41+
42+
namespace dynamicgraph {
43+
namespace sot {
44+
45+
/* --------------------------------------------------------------------- */
46+
/* --- CLASS ----------------------------------------------------------- */
47+
/* --------------------------------------------------------------------- */
48+
49+
template< typename Tin, typename Tout, typename Time >
50+
class VariadicAbstract
51+
:public Entity
52+
{
53+
public: /* --- CONSTRUCTION --- */
54+
55+
static std::string getTypeInName ( void );
56+
static std::string getTypeOutName( void );
57+
58+
VariadicAbstract( const std::string& name , const std::string& className)
59+
: Entity(name)
60+
,SOUT( className+"("+name+")::output("+getTypeOutName()+")::sout")
61+
,baseSigname(className+"("+name+")::input("+getTypeInName()+")::")
62+
{
63+
signalRegistration( SOUT );
64+
}
65+
66+
virtual ~VariadicAbstract( void )
67+
{
68+
for (std::size_t i = 0; i < signalsIN.size(); ++i) {
69+
_removeSignal (i);
70+
delete signalsIN[i];
71+
}
72+
};
73+
74+
public: /* --- SIGNAL --- */
75+
76+
SignalTimeDependent<Tout,int> SOUT;
77+
78+
std::size_t addSignal ()
79+
{
80+
std::ostringstream oss; oss << "sin" << signalsIN.size();
81+
return addSignal (oss.str());
82+
}
83+
84+
std::size_t addSignal (const std::string& name)
85+
{
86+
signal_t* sig = new signal_t (NULL, baseSigname + name);
87+
try {
88+
_declareSignal(sig);
89+
signalsIN.push_back (sig);
90+
// names.push_back (name);
91+
return signalsIN.size()-1;
92+
} catch (const ExceptionAbstract&) {
93+
delete sig;
94+
throw;
95+
}
96+
}
97+
98+
void removeSignal ()
99+
{
100+
assert (signalsIN.size()>0);
101+
_removeSignal (signalsIN().size()-1);
102+
// names.pop_back();
103+
signalsIN.pop_back();
104+
}
105+
106+
void setSignalNumber (const int& n)
107+
{
108+
assert (n>=0);
109+
const std::size_t oldSize = signalsIN.size();
110+
for (std::size_t i = n; i < oldSize; ++i)
111+
_removeSignal (i);
112+
signalsIN.resize(n,NULL);
113+
// names.resize(n);
114+
115+
for (std::size_t i = oldSize; i < (std::size_t)n; ++i)
116+
{
117+
assert (signalsIN[i]==NULL);
118+
std::ostringstream oss;
119+
oss << baseSigname << "sin" << i;
120+
// names[i] = oss.str();
121+
// signal_t* s = new signal_t (NULL,names[i]);
122+
signal_t* s = new signal_t (NULL,oss.str());
123+
signalsIN[i] = s;
124+
_declareSignal (s);
125+
}
126+
}
127+
128+
int getSignalNumber () const
129+
{
130+
return (int)signalsIN.size();
131+
}
132+
133+
protected:
134+
typedef SignalPtr<Tin,int> signal_t;
135+
std::vector< signal_t* > signalsIN;
136+
// Use signal->shortName instead
137+
// std::vector< std::string > names;
138+
139+
private:
140+
void _removeSignal (const std::size_t i)
141+
{
142+
// signalDeregistration(names[i]);
143+
signalDeregistration(signalsIN[i]->shortName());
144+
SOUT.removeDependency (*signalsIN[i]);
145+
delete signalsIN[i];
146+
}
147+
void _declareSignal (signal_t* s)
148+
{
149+
signalRegistration(*s);
150+
SOUT.addDependency (*s);
151+
}
152+
const std::string baseSigname;
153+
};
154+
155+
template< typename Operator >
156+
class VariadicOp
157+
:public VariadicAbstract<typename Operator::Tin, typename Operator::Tout, int>
158+
{
159+
Operator op;
160+
typedef typename Operator::Tin Tin;
161+
typedef typename Operator::Tout Tout;
162+
typedef VariadicOp<Operator> Self;
163+
164+
public: /* --- CONSTRUCTION --- */
165+
typedef VariadicAbstract<Tin,Tout,int> Base;
166+
167+
// static std::string getTypeInName ( void ) { return Operator::nameTypeIn (); }
168+
// static std::string getTypeOutName( void ) { return Operator::nameTypeOut(); }
169+
static const std::string CLASS_NAME;
170+
virtual const std::string& getClassName () const { return CLASS_NAME; }
171+
std::string getDocString () const { return op.getDocString ();}
172+
173+
VariadicOp( const std::string& name )
174+
: Base(name, CLASS_NAME)
175+
{
176+
this->SOUT.setFunction (boost::bind(&Self::computeOperation,this,_1,_2));
177+
op.initialize(this,this->commandMap);
178+
}
179+
180+
virtual ~VariadicOp( void ) {};
181+
182+
protected:
183+
Tout& computeOperation( Tout& res,int time )
184+
{
185+
std::vector< const Tin* > in (this->signalsIN.size());
186+
for (std::size_t i = 0; i < this->signalsIN.size(); ++i) {
187+
const Tin& x = this->signalsIN[i]->access (time);
188+
in[i] = &x;
189+
}
190+
op(in,res);
191+
return res;
192+
}
193+
};
194+
} // namespace sot
195+
} // namespace dynamicgraph
196+
197+
198+
#endif // #ifndef SOT_CORE_VARIADICOP_HH

‎src/matrix/operator.cpp

Lines changed: 94 additions & 52 deletions
Original file line numberDiff line numberDiff line change
@@ -24,6 +24,7 @@
2424

2525
#include <sot/core/unary-op.hh>
2626
#include <sot/core/binary-op.hh>
27+
#include <sot/core/variadic-op.hh>
2728

2829
#include <sot/core/matrix-geometry.hh>
2930

@@ -745,58 +746,6 @@ namespace dynamicgraph {
745746
};
746747
REGISTER_BINARY_OP(VectorStack,Stack_of_vector);
747748

748-
/* --- STACK ------------------------------------------------------------ */
749-
struct VectorMix
750-
: public BinaryOpHeader<dynamicgraph::Vector,dynamicgraph::Vector,dynamicgraph::Vector>
751-
{
752-
public:
753-
std::vector<bool> useV1;
754-
std::vector<int> idx1, idx2;
755-
void operator()( const dynamicgraph::Vector& v1,const dynamicgraph::Vector& v2,dynamicgraph::Vector& res ) const
756-
{
757-
res.resize(useV1.size());
758-
std::size_t k1=0, k2=0;
759-
for (std::size_t i = 0; i < useV1.size(); ++i)
760-
{
761-
if (useV1[i]) {
762-
assert (k1 < idx1.size());
763-
res[i] = v1[idx1[k1]];
764-
++k1;
765-
} else {
766-
assert (k2 < idx2.size());
767-
res[i] = v2[idx2[k2]];
768-
++k2;
769-
}
770-
}
771-
assert (k1 == idx1.size());
772-
assert (k2 == idx2.size());
773-
}
774-
775-
void addSelec1( const int & i) { useV1.push_back(true ); idx1.push_back(i); }
776-
void addSelec2( const int & i) { useV1.push_back(false); idx2.push_back(i); }
777-
778-
void addSpecificCommands(Entity& ent,
779-
Entity::CommandMap_t& commandMap )
780-
{
781-
using namespace dynamicgraph::command;
782-
783-
boost::function< void( const int& ) > selec1
784-
= boost::bind( &VectorMix::addSelec1,this,_1 );
785-
boost::function< void( const int& ) > selec2
786-
= boost::bind( &VectorMix::addSelec2,this,_1 );
787-
788-
ADD_COMMAND( "addSelec1",
789-
makeCommandVoid1(ent, selec1,
790-
docCommandVoid1("append value from vector 1.",
791-
"int (index in vector 1)")));
792-
ADD_COMMAND( "addSelec2",
793-
makeCommandVoid1(ent, selec2,
794-
docCommandVoid1("append value from vector 2.",
795-
"int (index in vector 2)")));
796-
}
797-
};
798-
REGISTER_BINARY_OP(VectorMix,Mix_of_vector);
799-
800749
/* ---------------------------------------------------------------------- */
801750

802751
struct Composer
@@ -968,6 +917,99 @@ namespace dynamicgraph {
968917
}
969918
}
970919

920+
#define REGISTER_VARIADIC_OP( OpType,name ) \
921+
template<> \
922+
const std::string VariadicOp< OpType >::CLASS_NAME = std::string(#name); \
923+
Entity *regFunction##_##name( const std::string& objname ) \
924+
{ \
925+
return new VariadicOp< OpType >( objname ); \
926+
} \
927+
EntityRegisterer regObj##_##name( std::string(#name),&regFunction##_##name)
928+
929+
namespace dynamicgraph {
930+
namespace sot {
931+
template< typename Tin, typename Tout, typename Time >
932+
std::string VariadicAbstract<Tin,Tout,Time>::getTypeInName (void) { return TypeNameHelper<Tin>::typeName; }
933+
template< typename Tin, typename Tout, typename Time >
934+
std::string VariadicAbstract<Tin,Tout,Time>::getTypeOutName(void) { return TypeNameHelper<Tout>::typeName; }
935+
936+
template< typename TypeIn, typename TypeOut >
937+
struct VariadicOpHeader
938+
{
939+
typedef TypeIn Tin ;
940+
typedef TypeOut Tout;
941+
static const std::string & nameTypeIn (void) { return TypeNameHelper<Tin >::typeName; }
942+
static const std::string & nameTypeOut(void) { return TypeNameHelper<Tout>::typeName; }
943+
template <typename Op>
944+
void initialize(VariadicOp<Op>*, Entity::CommandMap_t& ) {}
945+
virtual std::string getDocString () const
946+
{
947+
return std::string (
948+
"Undocumented variadic operator\n"
949+
" - input " + nameTypeIn () +
950+
"\n"
951+
" - output " + nameTypeOut () +
952+
"\n");
953+
}
954+
};
955+
956+
/* --- VectorMix ------------------------------------------------------------ */
957+
struct VectorMix
958+
: public VariadicOpHeader<Vector,Vector>
959+
{
960+
public:
961+
typedef VariadicOp<VectorMix> Base;
962+
struct segment_t {
963+
Vector::Index index, size, input;
964+
std::size_t sigIdx;
965+
segment_t (Vector::Index i, Vector::Index s, std::size_t sig) : index (i), size(s), sigIdx (sig) {}
966+
};
967+
typedef std::vector <segment_t> segments_t;
968+
Base* entity;
969+
segments_t idxs;
970+
void operator()( const std::vector<const Vector*>& vs, Vector& res ) const
971+
{
972+
res = *vs[0];
973+
for (std::size_t i = 0; i < idxs.size(); ++i)
974+
{
975+
const segment_t& s = idxs[i];
976+
if (s.sigIdx >= vs.size())
977+
throw std::invalid_argument ("Index out of range in VectorMix");
978+
res.segment (s.index, s.size) = *vs[s.sigIdx];
979+
}
980+
}
981+
982+
void addSelec( const int & sigIdx, const int & i, const int& s) { idxs.push_back(segment_t(i,s,sigIdx)); }
983+
984+
void initialize(Base* ent,
985+
Entity::CommandMap_t& commandMap )
986+
{
987+
using namespace dynamicgraph::command;
988+
entity = ent;
989+
990+
ent->addSignal ("default");
991+
992+
boost::function< void( const int&, const int&, const int& ) > selec
993+
= boost::bind( &VectorMix::addSelec,this,_1,_2,_3 );
994+
995+
ADD_COMMAND( "setSignalNumber", makeCommandVoid1(*(typename Base::Base*)ent, &Base::setSignalNumber,
996+
docCommandVoid1("set the number of input vector.", "int (size)")));
997+
998+
commandMap.insert(std::make_pair( "getSignalNumber",
999+
new Getter<Base, int> (*ent, &Base::getSignalNumber,
1000+
"Get the number of input vector.")));
1001+
1002+
commandMap.insert(std::make_pair("addSelec",
1003+
makeCommandVoid3<Base,int,int,int>(*ent, selec,
1004+
docCommandVoid3("add selection from a vector.",
1005+
"int (signal index >= 1)", "int (index)","int (size)"))));
1006+
}
1007+
};
1008+
REGISTER_VARIADIC_OP(VectorMix,Mix_of_vector);
1009+
}
1010+
}
1011+
1012+
9711013
/* --- TODO ------------------------------------------------------------------*/
9721014
// The following commented lines are sot-v1 entities that are still waiting
9731015
// for conversion. Help yourself!

0 commit comments

Comments
 (0)