diff --git a/include/sot-core/multi-bound.h b/include/sot-core/multi-bound.h
index 98a35148c92f59040c34513d1544daa5423d80b9..c7ded0b44a8f824b5279543d524fa76c6e7f298c 100644
--- a/include/sot-core/multi-bound.h
+++ b/include/sot-core/multi-bound.h
@@ -76,9 +76,9 @@ class SOT_CORE_EXPORT MultiBound
};
/* --------------------------------------------------------------------- */
-typedef std::vector< MultiBound > sotVectorMultiBound;
-SOT_CORE_EXPORT std::ostream& operator<< (std::ostream& os, const sotVectorMultiBound& v );
-SOT_CORE_EXPORT std::istream& operator>> (std::istream& os, sotVectorMultiBound& v );
+typedef std::vector< MultiBound > VectorMultiBound;
+SOT_CORE_EXPORT std::ostream& operator<< (std::ostream& os, const VectorMultiBound& v );
+SOT_CORE_EXPORT std::istream& operator>> (std::istream& os, VectorMultiBound& v );
} // namespace sot
diff --git a/include/sot-core/signal-cast.h b/include/sot-core/signal-cast.h
index afebf30c742e6306864c6fa3323f9b7057fe6945..fc368dfc848803e5be4ce0091f556235be209636 100644
--- a/include/sot-core/signal-cast.h
+++ b/include/sot-core/signal-cast.h
@@ -162,7 +162,7 @@ public: \
/* --- OTHER --- */
SOT_SIGNAL_CAST_DEFINITION(Flags);
-SOT_SIGNAL_CAST_DEFINITION_TRACE(sotVectorMultiBound);
+SOT_SIGNAL_CAST_DEFINITION_TRACE(VectorMultiBound);
typedef FeatureAbstract* SignalCast_sotFeatureAbstractPtr ;
diff --git a/include/sot-core/sot.h b/include/sot-core/sot.h
index 72db9ccb993a6bdaca8be572a02f36d852c84c1f..5abee9f2f687ca06b6eae75492e4100d7a0d2d46 100644
--- a/include/sot-core/sot.h
+++ b/include/sot-core/sot.h
@@ -136,7 +136,7 @@ class SOTSOT_CORE_EXPORT Sot
ml::Matrix& Jact );
static ml::Matrix & computeJacobianConstrained( const TaskAbstract& task,
const ml::Matrix& K );
- static ml::Vector taskVectorToMlVector( const sotVectorMultiBound& taskVector );
+ static ml::Vector taskVectorToMlVector( const VectorMultiBound& taskVector );
public:
diff --git a/include/sot-core/task-abstract.h b/include/sot-core/task-abstract.h
index eb6c6b8e79bd18f5f12cf7e47b174a2848650d8b..e99fb664c1322001245b1054e226401ac43f8367 100644
--- a/include/sot-core/task-abstract.h
+++ b/include/sot-core/task-abstract.h
@@ -82,7 +82,7 @@ class SOT_CORE_EXPORT TaskAbstract
public: /* --- SIGNALS --- */
- dg::SignalTimeDependent< sotVectorMultiBound,int > taskSOUT;
+ dg::SignalTimeDependent< VectorMultiBound,int > taskSOUT;
dg::SignalTimeDependent< ml::Matrix,int > jacobianSOUT;
dg::SignalTimeDependent< ml::Vector,int > featureActivationSOUT;
diff --git a/include/sot-core/task-conti.h b/include/sot-core/task-conti.h
index c9b10b9b2b3e7a3ceaebf194507f5a8e07f21fe6..b3e351bbb79cf164ff17c57e010011b29c6665f6 100644
--- a/include/sot-core/task-conti.h
+++ b/include/sot-core/task-conti.h
@@ -87,7 +87,7 @@ class SOTTASKCONTI_EXPORT TaskConti
const int & referenceTime( void ) { return timeRef; }
/* --- COMPUTATION --- */
- sotVectorMultiBound& computeContiDesiredVelocity( sotVectorMultiBound &task,
+ VectorMultiBound& computeContiDesiredVelocity( VectorMultiBound &task,
const int & time );
diff --git a/include/sot-core/task-pd.h b/include/sot-core/task-pd.h
index ea220ce4b69ceb07affbc098f74e12493f07e7c8..0eb2301d30345c6bf717cf8c4e2eb1db635bd4be 100644
--- a/include/sot-core/task-pd.h
+++ b/include/sot-core/task-pd.h
@@ -67,7 +67,7 @@ class SOTTASKPD_EXPORT TaskPD
/* --- COMPUTATION --- */
ml::Vector& computeErrorDot( ml::Vector& error,int time );
- sotVectorMultiBound& computeTaskModif( sotVectorMultiBound& error,int time );
+ VectorMultiBound& computeTaskModif( VectorMultiBound& error,int time );
/* --- SIGNALS ------------------------------------------------------------ */
diff --git a/include/sot-core/task-unilateral.h b/include/sot-core/task-unilateral.h
index a227d7eee1246051d514413e284207d421ca08bf..0d09ae82713c14c99072dc02cdc1509f94a6be2c 100644
--- a/include/sot-core/task-unilateral.h
+++ b/include/sot-core/task-unilateral.h
@@ -77,7 +77,7 @@ class SOTTASKUNILATERAL_EXPORT TaskUnilateral
TaskUnilateral( const std::string& n );
/* --- COMPUTATION --- */
- sotVectorMultiBound& computeTaskUnilateral( sotVectorMultiBound& res,int time );
+ VectorMultiBound& computeTaskUnilateral( VectorMultiBound& res,int time );
/* --- SIGNALS ------------------------------------------------------------ */
diff --git a/include/sot-core/task.h b/include/sot-core/task.h
index 4ccdf23c6ee7b278d9749e2626731d782cece5be..72f123f89b4b5a6ede2d169419978b87323fff7e 100644
--- a/include/sot-core/task.h
+++ b/include/sot-core/task.h
@@ -109,8 +109,8 @@ class SOTTASK_EXPORT Task
/* --- COMPUTATION --- */
ml::Vector& computeError( ml::Vector& error,int time );
- sotVectorMultiBound&
- computeTaskExponentialDecrease( sotVectorMultiBound& errorRef,int time );
+ VectorMultiBound&
+ computeTaskExponentialDecrease( VectorMultiBound& errorRef,int time );
ml::Matrix& computeJacobian( ml::Matrix& J,int time );
ml::Vector& computeFeatureActivation( ml::Vector& h,int time );
diff --git a/src/signal/signal-cast.cpp b/src/signal/signal-cast.cpp
index 95b114b592182f9390d4d2a5f97122b3c8973621..2df964f8fc82c4e9a3433e0438418a5f2e6eceb0 100644
--- a/src/signal/signal-cast.cpp
+++ b/src/signal/signal-cast.cpp
@@ -65,10 +65,10 @@ disp( const struct timeval& t,std::ostream& os )
os << t.tv_sec << "s "<< t.tv_usec << "ms";
}
-void SignalCast<sotVectorMultiBound>::
-trace( const sotVectorMultiBound& t,std::ostream& os )
+void SignalCast<VectorMultiBound>::
+trace( const VectorMultiBound& t,std::ostream& os )
{
- for( sotVectorMultiBound::const_iterator iter=t.begin();t.end()!=iter;++iter )
+ for( VectorMultiBound::const_iterator iter=t.begin();t.end()!=iter;++iter )
{
switch( iter->mode )
{
@@ -191,7 +191,7 @@ trace( const ml::Matrix& t,std::ostream& os )
namespace {
SOT_SIGNAL_CAST_DECLARATION(SignalCast_sotFeatureAbstractPtr);
SOT_SIGNAL_CAST_DECLARATION(Flags);
- SOT_SIGNAL_CAST_DECLARATION(sotVectorMultiBound);
+ SOT_SIGNAL_CAST_DECLARATION(VectorMultiBound);
SOT_SIGNAL_CAST_DECLARATION(timeval );
SOT_SIGNAL_CAST_DECLARATION_NAMED(maal::boost::Vector, maal_boost_Vector);
SOT_SIGNAL_CAST_DECLARATION_NAMED(maal::boost::Matrix, maal_boost_Matrix);
diff --git a/src/sot/flags.cpp b/src/sot/flags.cpp
index 23907b1f4cb8f9e673c3aa215037d2e5f47da9d7..466142207128e9606aa0312ecaa8df0a49a16869 100644
--- a/src/sot/flags.cpp
+++ b/src/sot/flags.cpp
@@ -121,6 +121,59 @@ add( const int& c4 )
for(unsigned int i=0;i<sizeof(int);++i) add(c4p[i]);
}
+/* --------------------------------------------------------------------- */
+void Flags::
+set( const unsigned int & idx )
+{
+ unsigned int d= (idx/8), m=(idx%8);
+
+ char brik = (reverse)?(255-(1<<m)):(1<<m);
+
+ if( flags.size()>d )
+ {
+ sotDEBUG(45) << "List long enough. Modify." << std::endl;
+ char & el = flags[d];
+ if( reverse ) el &= brik; else el |= brik;
+ }
+ else
+ {
+ sotDEBUG(45) << "List not long enough. Add "
+ << flags.size() <<" "<<d << std::endl;
+ if(! reverse )
+ {
+ for( unsigned i=flags.size();i<d;++i ) add((char)0);
+ add(brik);
+ }
+ }
+ sotDEBUG(45) << "New flag: "<< *this << endl;
+}
+
+void Flags::
+unset( const unsigned int & idx )
+{
+ unsigned int d= (idx/8), m=(idx%8);
+
+ char brik = (reverse)?(1<<m):(255-(1<<m));
+ if( flags.size()>d )
+ {
+ sotDEBUG(45) << "List long enough. Modify." << std::endl;
+ char & el = flags[d];
+ if( reverse ) el |= brik; else el &= brik;
+ }
+ else
+ {
+ sotDEBUG(45) << "List not long enough. Add." << std::endl;
+ if( reverse )
+ {
+ for( unsigned i=flags.size();i<d;++i ) add((char)255);
+ add(brik);
+ }
+ }
+ sotDEBUG(45) << "New flag: "<< *this << endl;
+}
+
+
+namespace sot {
/* --------------------------------------------------------------------- */
Flags Flags::
@@ -185,61 +238,8 @@ operator&= ( const bool& b ){ if(!b) { flags.clear(); reverse=false; } return *t
Flags& Flags::
operator|= ( const bool& b ){ if(b) { flags.clear(); reverse=true; } return *this;}
-/* --------------------------------------------------------------------- */
-void Flags::
-set( const unsigned int & idx )
-{
- unsigned int d= (idx/8), m=(idx%8);
-
- char brik = (reverse)?(255-(1<<m)):(1<<m);
-
- if( flags.size()>d )
- {
- sotDEBUG(45) << "List long enough. Modify." << std::endl;
- char & el = flags[d];
- if( reverse ) el &= brik; else el |= brik;
- }
- else
- {
- sotDEBUG(45) << "List not long enough. Add "
- << flags.size() <<" "<<d << std::endl;
- if(! reverse )
- {
- for( unsigned i=flags.size();i<d;++i ) add((char)0);
- add(brik);
- }
- }
- sotDEBUG(45) << "New flag: "<< *this << endl;
-}
-
-void Flags::
-unset( const unsigned int & idx )
-{
- unsigned int d= (idx/8), m=(idx%8);
-
- char brik = (reverse)?(1<<m):(255-(1<<m));
- if( flags.size()>d )
- {
- sotDEBUG(45) << "List long enough. Modify." << std::endl;
- char & el = flags[d];
- if( reverse ) el |= brik; else el &= brik;
- }
- else
- {
- sotDEBUG(45) << "List not long enough. Add." << std::endl;
- if( reverse )
- {
- for( unsigned i=flags.size();i<d;++i ) add((char)255);
- add(brik);
- }
- }
- sotDEBUG(45) << "New flag: "<< *this << endl;
-}
-
-
/* --------------------------------------------------------------------- */
-namespace sot {
std::ostream& operator<< (std::ostream& os, const Flags& fl )
{
if( fl.reverse ) os << "...11111 ";
@@ -371,7 +371,6 @@ std::istream& operator>> (std::istream& is, Flags& fl )
return is;
}
-} // namespace sot
/* --------------------------------------------------------------------- */
const Flags FLAG_LINE_1( (char)0x1 );
@@ -383,6 +382,8 @@ const Flags FLAG_LINE_6( (char)0x20 );
const Flags FLAG_LINE_7( (char)0x40 );
const Flags FLAG_LINE_8( (char)0x80 );
+} // namespace sot
+
/* --------------------------------------------------------------------- */
void Flags::
diff --git a/src/sot/sot-h.cpp b/src/sot/sot-h.cpp
index d83042e7a03a1126da30c489baef195b1c7f0aaa..3cc34564ff71d209ea2ff2a9810e9152ef3234c4 100644
--- a/src/sot/sot-h.cpp
+++ b/src/sot/sot-h.cpp
@@ -118,7 +118,7 @@ commandLine( const std::string& cmdLine,std::istringstream& cmdArgs,
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-void buildTaskVectors( const sotVectorMultiBound& err,
+void buildTaskVectors( const VectorMultiBound& err,
const ml::Matrix & JK,
bubVector & ee,bubVector & eiinf,bubVector & eisup,
ConstraintMem::BoundSideVector& bounds,
@@ -128,7 +128,7 @@ void buildTaskVectors( const sotVectorMultiBound& err,
sotDEBUG(25) << "/* Compute the task sizes. */"<< std::endl;
unsigned int sizei=0,sizee=0;
- for( sotVectorMultiBound::const_iterator iter=err.begin();
+ for( VectorMultiBound::const_iterator iter=err.begin();
err.end()!=iter;++iter )
{ if( iter->getMode()==MultiBound::MODE_SINGLE )
sizee++; else sizei++; }
@@ -267,7 +267,7 @@ computeControlLaw( ml::Vector& control,const int& iterTime )
TaskAbstract & task = **iter;
sotDEBUG(15) << "Task: e_" << task.getName() << std::endl;
const ml::Matrix &Jac = task.jacobianSOUT(iterTime);
- const sotVectorMultiBound &err = task.taskSOUT(iterTime);
+ const VectorMultiBound &err = task.taskSOUT(iterTime);
unsigned int ntask=err.size();
sotDEBUG(25) << "/* Init memory. */" << std::endl;
@@ -376,7 +376,7 @@ computeControlLaw( ml::Vector& control,const int& iterTime )
TaskAbstract & task = **iter;
MemoryTaskSOTH * mem = dynamic_cast<MemoryTaskSOTH *>( task.memoryInternal );
if( mem == NULL ) continue;
- sotVectorMultiBound taskVector = task.taskSOUT(iterTime);
+ VectorMultiBound taskVector = task.taskSOUT(iterTime);
const ml::Matrix JK = mem->jacobianConstrainedSINOUT.accessCopy();
ml::Vector JKu(taskVector.size()); JKu = JK*control;
ml::Vector diff(taskVector.size());
diff --git a/src/sot/sot.cpp b/src/sot/sot.cpp
index fe186699f74fcdf7958054e8d2b793351c5c4585..c0849f7061956b19985e6ec849e31675bc85cdfd 100644
--- a/src/sot/sot.cpp
+++ b/src/sot/sot.cpp
@@ -367,10 +367,10 @@ static void computeJacobianActivated( Task* taskSpec,
#endif // #ifdef WITH_CHRONO
ml::Vector Sot::
-taskVectorToMlVector( const sotVectorMultiBound& taskVector )
+taskVectorToMlVector( const VectorMultiBound& taskVector )
{
ml::Vector res(taskVector.size()); unsigned int i=0;
- for( sotVectorMultiBound::const_iterator iter=taskVector.begin();
+ for( VectorMultiBound::const_iterator iter=taskVector.begin();
iter!=taskVector.end();++iter,++i )
{
res(i)=iter->getSingleBound();
diff --git a/src/task/multi-bound.cpp b/src/task/multi-bound.cpp
index 3aaf5f55f057d201781bc48b5a6ed9c27f9d344b..bd922a673f90bdfe541001be1a97529c24de63dc 100644
--- a/src/task/multi-bound.cpp
+++ b/src/task/multi-bound.cpp
@@ -146,7 +146,7 @@ setSingleBound( double boundValue )
inline static void SOT_MULTI_BOUND_CHECK_C(std::istream& is,
char check,
- sotVectorMultiBound& v)
+ VectorMultiBound& v)
{
char c;
is.get(c);
@@ -238,16 +238,16 @@ std::istream& operator>> ( std::istream& is, MultiBound & m )
return is;
}
-std::ostream& operator<< (std::ostream& os, const sotVectorMultiBound& v )
+std::ostream& operator<< (std::ostream& os, const VectorMultiBound& v )
{
os << "[" << v.size() << "](";
- for( sotVectorMultiBound::const_iterator iter=v.begin();
+ for( VectorMultiBound::const_iterator iter=v.begin();
iter!=v.end();++iter )
{ if(iter!=v.begin()) os<<","; os << (*iter); }
return os<<")";
}
-std::istream& operator>> (std::istream& is, sotVectorMultiBound& v )
+std::istream& operator>> (std::istream& is, VectorMultiBound& v )
{
unsigned int vali;
diff --git a/src/task/task-conti.cpp b/src/task/task-conti.cpp
index 172b25666f07747fec71ed0b296375f19d88b5b7..bb77038dad3faa41579bd50e032b3f0b291da759 100644
--- a/src/task/task-conti.cpp
+++ b/src/task/task-conti.cpp
@@ -52,8 +52,8 @@ TaskConti( const std::string& n )
}
-sotVectorMultiBound& TaskConti::
-computeContiDesiredVelocity( sotVectorMultiBound& desvel2b,const int & timecurr )
+VectorMultiBound& TaskConti::
+computeContiDesiredVelocity( VectorMultiBound& desvel2b,const int & timecurr )
{
sotDEBUG(15) << "# In {" << endl;
diff --git a/src/task/task-pd.cpp b/src/task/task-pd.cpp
index e8fe236f6eb8530cda92cb93af568951bc60809b..dfd3fa8901ed72f2054bd95897ad7ec9aebd96af 100644
--- a/src/task/task-pd.cpp
+++ b/src/task/task-pd.cpp
@@ -87,8 +87,8 @@ computeErrorDot( ml::Vector& errorDot,int time )
return errorDot;
}
-sotVectorMultiBound& TaskPD::
-computeTaskModif( sotVectorMultiBound& task,int time )
+VectorMultiBound& TaskPD::
+computeTaskModif( VectorMultiBound& task,int time )
{
sotDEBUG(15) << "# In {" << endl;
diff --git a/src/task/task-unilateral.cpp b/src/task/task-unilateral.cpp
index e67e2b5d31031eb4dc19d220b9b7a4f55624dd90..6fcebaec299963e6b46557fa7207aa552019ecf9 100644
--- a/src/task/task-unilateral.cpp
+++ b/src/task/task-unilateral.cpp
@@ -67,8 +67,8 @@ TaskUnilateral( const std::string& n )
/* --- COMPUTATION ---------------------------------------------------------- */
/* --- COMPUTATION ---------------------------------------------------------- */
-sotVectorMultiBound& TaskUnilateral::
-computeTaskUnilateral( sotVectorMultiBound& res,int time )
+VectorMultiBound& TaskUnilateral::
+computeTaskUnilateral( VectorMultiBound& res,int time )
{
sotDEBUG(45) << "# In " << getName() << " {" << endl;
const ml::Vector & position = positionSIN(time);
diff --git a/src/task/task.cpp b/src/task/task.cpp
index 368a375f65b71076d0ef439fb2bb0aef2b5808f9..68cfad9cbfa137939f8a182054d4f007d648e5d8 100644
--- a/src/task/task.cpp
+++ b/src/task/task.cpp
@@ -171,8 +171,8 @@ computeError( ml::Vector& error,int time )
return error;
}
-sotVectorMultiBound& Task::
-computeTaskExponentialDecrease( sotVectorMultiBound& errorRef,int time )
+VectorMultiBound& Task::
+computeTaskExponentialDecrease( VectorMultiBound& errorRef,int time )
{
sotDEBUG(15) << "# In {" << endl;
const ml::Vector & errSingleBound = errorSOUT(time);
diff --git a/unitTesting/CMakeLists.txt b/unitTesting/CMakeLists.txt
index 40ec196f516206fe892ca997629a8f6775540754..fdc0797642b8c984e1e4323147f955644645237b 100644
--- a/unitTesting/CMakeLists.txt
+++ b/unitTesting/CMakeLists.txt
@@ -21,6 +21,14 @@ SET(TEST_traces_LIBS
${CMAKE_INSTALL_PREFIX}/lib/plugin/tracer.so
)
+SET(TEST_test_gain_LIBS
+ gain-adaptive feature-visual-point
+)
+
+SET(TEST_test_task_LIBS
+ gain-adaptive feature-visual-point task
+)
+
#test paths and names (without .cpp extension)
SET (tests
link
@@ -32,7 +40,11 @@ SET (tests
sot/test_solverSoth
factory/test_factory
traces/files
- traces/traces)
+ traces/traces
+ task/test_flags
+ task/test_gain
+ task/test_multi_bound
+ task/test_task)
FOREACH(test ${tests})
GET_FILENAME_COMPONENT(EXECUTABLE_NAME ${test} NAME)
diff --git a/unitTesting/task/test_flags.cpp b/unitTesting/task/test_flags.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..55353c764b98b42c9ea6cdbace471fde05c366e1
--- /dev/null
+++ b/unitTesting/task/test_flags.cpp
@@ -0,0 +1,75 @@
+/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ * Copyright Projet JRL 2007
+ *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ *
+ * File: test_flags.cc
+ * Project: sot
+ * Author: Nicolas Mansard
+ *
+ * Version control
+ * ===============
+ *
+ * $Id$
+ *
+ * Description
+ * ============
+ *
+ *
+ * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+
+/* -------------------------------------------------------------------------- */
+/* --- INCLUDES ------------------------------------------------------------- */
+/* -------------------------------------------------------------------------- */
+#include <sot-core/flags.h>
+#include <iostream>
+#include <sstream>
+
+using namespace std;
+using namespace sot;
+
+int main( void )
+{
+ cout <<"Entering test" <<endl;
+ Flags f1(128*112+84);
+ Flags f2(198);
+ cout<<"f1 "<<"\t"<<f1<<endl;
+ cout<<"f2 "<<"\t"<<f2<<endl;
+
+ cout<<endl;
+ cout<<"1|2 "<<"\t"<<(f1|f2)<<endl;
+ cout<<"1&2 "<<"\t"<<(f1&f2)<<endl;
+ cout<<"TRUE "<<"\t"<<(Flags(true))<<endl;
+ cout<<"1&TRUE"<<"\t"<<(f1&Flags(true))<<endl;
+ cout<<"1&!2 "<<"\t"<<(f1&!f2)<<endl;
+ cout<<"1XOR2 "<<"\t"<<((f1&!f2)|(!f1&f2))<<endl;
+
+ cout<<endl;
+ cout<<"f1 "<<"\t"<<f1<<endl;
+ cout<<"!2 "<<"\t"<<!f2<<endl;
+ cout<<"1|!2 "<<"\t"<<(f1|!f2)<<endl;
+
+ cout<<endl;
+ if( f1&f2 ) cout<<"TRUE"; else cout<<"FALSE"; cout<<endl;
+ if( f1&Flags() ) cout<<"TRUE"; else cout<<"FALSE"; cout<<endl;
+
+ cout<<endl;
+ cout<<"f1>>3 "<<"\t"<<Flags(f1>>3)<<endl;
+ cout<<"f1>>5 "<<"\t"<<Flags(f1>>5)<<endl;
+
+ cout<<"f1 byte per byte:";
+ for(int i=0;i<16;++i) {if(! (i%8)) cout<<" "; cout << f1(i); } cout<<endl;
+
+ cout<<endl;
+ cout<<"L1 \t"<<FLAG_LINE_1<<endl;
+ cout<<"L4 \t"<<FLAG_LINE_4<<endl;
+ cout<<"L8 \t"<<FLAG_LINE_8<<endl;
+ cout<<endl;
+
+ istringstream iss("00101");
+ Flags flread;
+ iss >> flread;
+ cout<<flread<<endl<<endl;
+
+
+ return 0;
+}
diff --git a/unitTesting/task/test_gain.cpp b/unitTesting/task/test_gain.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0fb290af6f6dcb2d879ba9a2f2f6c8de0c17cc6b
--- /dev/null
+++ b/unitTesting/task/test_gain.cpp
@@ -0,0 +1,84 @@
+/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ * Copyright Projet JRL-Japan, Tsukuba, 2007
+ *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ *
+ * File: test_task.cpp
+ * Project: SOT
+ * Author: Nicolas Mansard
+ *
+ * Version control
+ * ===============
+ *
+ * $Id$
+ *
+ * Description
+ * ============
+ *
+ *
+ * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+
+/* -------------------------------------------------------------------------- */
+/* --- INCLUDES ------------------------------------------------------------- */
+/* -------------------------------------------------------------------------- */
+
+#include <dynamic-graph/signal.h>
+#include <sot-core/gain-adaptive.h>
+#include <iostream>
+using namespace std;
+using namespace sot;
+using namespace dynamicgraph;
+
+namespace ml = maal::boost;
+
+class DummyClass
+{
+public:
+ ml::Vector err;
+
+ ml::Vector& getError( ml::Vector& res,int t )
+ {
+ cout << "Dummy::getError ["<< t<< "] "<<endl;
+ return res=err;
+ }
+
+};
+
+DummyClass dummy;
+
+
+
+/* -------------------------------------------------------------------------- */
+/* --- INCLUDES ------------------------------------------------------------- */
+/* -------------------------------------------------------------------------- */
+
+int main( void )
+{
+ dummy.err.resize(3);
+ dummy.err.fill(3);
+
+ GainAdaptive * gain = new GainAdaptive("gain",4,1,5);
+
+ Signal<ml::Vector,int> errSig("test");
+ errSig.setFunction( boost::bind(&DummyClass::getError,dummy,_1,_2) );
+
+ gain->errorSIN.plug( &errSig );
+ cout <<"Appel of errSig and display of the result." << endl;
+ cout << errSig(0) <<endl;
+
+// double res;
+// cout <<"Compute gain from Gain object."<< endl;
+// gain->computeGain( res,0 );
+
+ Signal<double,int> &gainSig = gain->gainSOUT;
+
+ cout <<"Compute gain from Gain Signal and display."<< endl;
+ cout << gainSig(0) << endl;
+
+// sotSignalPtr<ml::Vector,int> sigPtr( &errSig );
+// cout << errSig(0) <<endl;
+// cout << (*sigPtr).access(0) <<endl;
+
+ delete gain;
+
+ return 0;
+}
diff --git a/unitTesting/task/test_multi_bound.cpp b/unitTesting/task/test_multi_bound.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6d5ae15fa013b94557cd6dd742c824451c1f3c45
--- /dev/null
+++ b/unitTesting/task/test_multi_bound.cpp
@@ -0,0 +1,76 @@
+/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ * Copyright Projet JRL 2007
+ *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ *
+ * File: test_flags.cc
+ * Project: sot
+ * Author: Nicolas Mansard
+ *
+ * Version control
+ * ===============
+ *
+ * $Id$
+ *
+ * Description
+ * ============
+ *
+ *
+ * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+
+/* -------------------------------------------------------------------------- */
+/* --- INCLUDES ------------------------------------------------------------- */
+/* -------------------------------------------------------------------------- */
+#include <sot-core/multi-bound.h>
+#include <iostream>
+#include <sstream>
+#include <sot-core/debug.h>
+
+using namespace std;
+using namespace sot;
+
+int main( void )
+{
+ DebugTrace::openFile();
+
+ MultiBound mbs(1.2), mbdi(3.4,MultiBound::BOUND_INF)
+ ,mbds(5.6, MultiBound::BOUND_SUP),mbdb(-7.8,9.10);
+ cout << "mbs =" << mbs << std::endl;
+ cout << "mbdi=" << mbdi << std::endl;
+ cout << "mbds=" << mbds << std::endl;
+ cout << "mbdb=" << mbdb << std::endl;
+
+ {
+ ostringstream oss; istringstream iss;
+ oss.str(""); oss << mbs; iss.str(oss.str()); iss.seekg(0);
+ //{char strbuf[256]; iss.getline(strbuf,256); cout << "#"<<strbuf<<"#"<<std::endl;}
+ iss >> mbs;
+ cout << oss.str() << "=> mbs =" << mbs << std::endl;
+ }
+
+ {
+ ostringstream oss; istringstream iss;
+ oss.str(""); oss << mbdi; iss.str(oss.str()); iss.seekg(0);
+ iss >> mbdi;
+ cout << oss.str() << "=> mbdi =" << mbdi << std::endl;
+ }
+ {
+ ostringstream oss; istringstream iss;
+ oss.str(""); oss << mbds; iss.str(oss.str()); iss.seekg(0);
+ iss >> mbds;
+ cout << oss.str() << "=> mbds =" << mbds << std::endl;
+ }
+ {
+ ostringstream oss; istringstream iss;
+ oss.str(""); oss << mbdb; iss.seekg(0); iss.str(oss.str());
+ iss >> mbdb;
+ cout << oss.str() << "=> mbdb =" << mbdb << std::endl;
+ }
+
+ ostringstream oss; istringstream iss;
+ oss << "[4](" << mbs << "," << mbdi << "," << mbds << "," << mbdb << ")" << std::endl;
+ iss.str(oss.str());
+ VectorMultiBound vmb; iss >> vmb;
+ cout << "vmb4 = " << vmb << std::endl;
+
+ return 0;
+}
diff --git a/unitTesting/task/test_task.cpp b/unitTesting/task/test_task.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..611029e6c0ddcf777f9828e3e9b8c18d023f05ce
--- /dev/null
+++ b/unitTesting/task/test_task.cpp
@@ -0,0 +1,88 @@
+/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ * Copyright Projet VISTA / IRISA, 2003
+ *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ *
+ * File: test_categorie.cc
+ * Project: Traces
+ * Author: Nicolas Mansard
+ *
+ * Version control
+ * ===============
+ *
+ * $Id: test_boost.cpp,v 1.1.1.1 2006-07-03 05:17:37 nmansard Exp $
+ *
+ * Description
+ * ============
+ *
+ * Test la classe CategorieTrace.
+ *
+ * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+
+/* -------------------------------------------------------------------------- */
+/* --- INCLUDES ------------------------------------------------------------- */
+/* -------------------------------------------------------------------------- */
+#include <sot-core/sot.h>
+#include <sot-core/feature-visual-point.h>
+#include <sot-core/feature-abstract.h>
+#include <sot-core/debug.h>
+#include <sot-core/task.h>
+#include <sot-core/gain-adaptive.h>
+
+using namespace std;
+using namespace sot;
+
+namespace ml = maal::boost;
+
+double drand( void ) { return 2*((double)rand())/RAND_MAX-1; }
+ml::Matrix& mrand( ml::Matrix& J )
+{
+ for( unsigned int i=0;i<J.nbRows();++i)
+ for( unsigned int j=0;j<J.nbCols();++j)
+ J(i,j) = drand();
+ return J;
+}
+
+int main( void )
+{
+ srand(12);
+ ml::Matrix Jq(6,6); Jq.setIdentity();
+
+ ml::Vector p1xy(2); p1xy(0)=1.; p1xy(1)=-2;
+
+ sotDEBUGF("Create feature");
+ FeatureVisualPoint * p1 = new FeatureVisualPoint("p1");
+ FeatureVisualPoint * p1des = new FeatureVisualPoint("p1d");
+
+ p1->articularJacobianSIN.setReference(&Jq);
+ p1->selectionSIN = Flags(true);
+ p1->desiredValueSIN = p1des;
+ p1->xySIN = p1xy;
+
+ p1des->xySIN = ml::Vector(2);
+
+
+ sotDEBUGF("Create Task");
+ sotDEBUG(0) << ml::MATLAB;
+
+ Task * task = new Task("t");
+ task->addFeature(*p1);
+ task->addFeature(*p1);
+
+ GainAdaptive * lambda = new GainAdaptive("g");
+ lambda->errorSIN.plug( &task->errorSOUT );
+
+ task->controlGainSIN.plug( &lambda->gainSOUT );
+ task->dampingGainSINOUT = .1;
+ task->controlSelectionSIN = Flags(true);
+
+ task->jacobianSOUT.display(cout)<<endl;
+ task->jacobianSOUT.displayDependencies(cout)<<endl;
+
+ sotDEBUG(0) << ml::MATLAB << "J"<< task->jacobianSOUT(2);
+ sotDEBUG(0) <<"H"<< task->featureActivationSOUT(2)<<endl;
+ sotDEBUG(0) <<"e"<< task->errorSOUT(2) <<endl;
+
+
+
+ return 0;
+}