diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 32ea5981cd45b62d146e553c20e1ab40d896eb6e..90153707f5164507be39504cb45bf51cd45a1769 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -146,7 +146,6 @@ IF(UNIX)
TARGET_LINK_LIBRARIES(${LIBRARY_NAME} dl)
ENDIF(UNIX)
-MESSAGE(STATUS "ldflags ${${PROJECT_NAME}_src_LDFLAGS}" )
SET_TARGET_PROPERTIES(${LIBRARY_NAME}
PROPERTIES
LINK_FLAGS "${${PROJECT_NAME}_src_LDFLAGS}"
diff --git a/tools/CMakeLists.txt b/tools/CMakeLists.txt
index 58473bc6be2bef967f905543a332ccb4df39dd20..c6e574eb227e031438c42d4aaa9925f701fbbfb4 100644
--- a/tools/CMakeLists.txt
+++ b/tools/CMakeLists.txt
@@ -3,28 +3,59 @@
#
### tools
+SET(TEST_LINK_LIBRARIES
+ task
+ feature-visual-point
+ gain-adaptative
+)
+
SET(tools
- ""
+ sot/tsot
)
FOREACH(tool_name ${tools})
- SET(EXECUTABLE_NAME ${tool_name})
-
+ GET_FILENAME_COMPONENT(EXECUTABLE_NAME ${tool_name} NAME)
+
+ INCLUDE_DIRECTORIES(${CMAKE_SOURCE_DIR}/include)
+
+ LINK_DIRECTORIES(${${PROJECT_NAME}_BINARY_DIR}/src)
+
+ # provide path to library libMatrixAbstractLayer.so
+ LINK_DIRECTORIES(${MATRIXABSTRACTLAYER_LIBRARY_DIRS})
+
+ # provide path to library libdynamic-graph.so
+ LINK_DIRECTORIES(${DYNAMIC_GRAPH_LIBRARY_DIRS})
+
+
ADD_DEFINITIONS(-DDEBUG=2)
ADD_EXECUTABLE(${EXECUTABLE_NAME}
${tool_name}.cpp)
- INCLUDE_DIRECTORIES(
- ${CMAKE_CURRENT_SOURCE_DIR}/../include
- )
-
- LINK_DIRECTORIES(${${PROJECT_NAME}_BINARY_DIR}/lib)
+
+ TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME}
+ ${PROJECT_NAME})
+
+ # Add MatrixAbstractLayer compilation flags and link to library libMatrixAbstractLayer.so
+ ADD_DEFINITIONS(${MATRIXABSTRACTLAYER_CFLAGS})
+ TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} ${MATRIXABSTRACTLAYER_LIBRARIES})
+
+ # Add lapack compilation flags and link to library libLapack.so
+ ADD_DEFINITIONS(${LAPACK_CFLAGS})
+ TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} ${LAPACK_LIBRARIES})
+ # Add dynamic-graph compilation flags and link to library libdynamic-graph.so
+ ADD_DEFINITIONS(${DYNAMIC_GRAPH_CFLAGS})
+ TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} ${DYNAMIC_GRAPH_LIBRARIES})
+
+ IF (UNIX)
TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME}
- ${PROJECT_NAME}
- dl)
+ dl)
+ ENDIF(UNIX)
+
+ # Add tests libraries
+ TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} ${TEST_LINK_LIBRARIES})
- INSTALL(TARGETS ${tool_name}
- DESTINATION ${CMAKE_INSTALL_PREFIX}/bin/${PROJECT_NAME})
+ INSTALL(TARGETS ${EXECUTABLE_NAME}
+ DESTINATION ${CMAKE_INSTALL_PREFIX}/bin/${PROJECT_NAME}/)
ENDFOREACH(tool_name)
\ No newline at end of file
diff --git a/tools/sot/Makefile.in b/tools/sot/Makefile.in
new file mode 100644
index 0000000000000000000000000000000000000000..bb2f1fb8fcd38ec3eaf4d5f21a2e6f57b72cbe7c
--- /dev/null
+++ b/tools/sot/Makefile.in
@@ -0,0 +1,36 @@
+
+@build_mk@
+
+CXXFLAGS += $(INCLUDE)
+LDFLAGS += $(LIBPATH)
+LIBS += $(LIB) $(LIBS_FLAGS)
+
+MAINS = tsot.cpp
+LOCAL_DIR_BIN = $(PROJECT_TESTS_PATH)/bin
+
+tsot_OBJ =
+tsot_EXT_CLASS = sotTask sotFeatureVisualPoint sotGainAdaptative
+tsot_EXT_OBJ = $(tsot_EXT_CLASS:%=$(PROJECT_PLUGIN_PATH)/%.so)
+
+.DEFAULT: all
+all: install
+
+@directory_mk@
+
+@bin_mk@
+
+install: directory $(LOCAL_DIR_BIN)_MKDIR $(PROGS_EXE)
+
+$(VISP_LIBNAME):
+ cd $(VISP_DIR_SRC); $(MAKE)
+
+
+uninstall:
+ rm -f $(PROGS)
+ rm -f $(DEPFILES)
+ rm -rf $(LOCAL_DEP_PATH) $(LOCAL_OBJ_PATH)
+
+@clean_mk@
+
+@depend_mk@
+
diff --git a/tools/sot/test_solverSoth.cpp b/tools/sot/test_solverSoth.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..95403d9fe784591cf2405ddb89057df4ee730b2e
--- /dev/null
+++ b/tools/sot/test_solverSoth.cpp
@@ -0,0 +1,510 @@
+//#define VP_DEBUG
+#define VP_DEBUG_MODE 45
+#include <sot/sotDebug.h>
+
+#include <sot/sotSolverHierarchicalInequalities.h>
+#include <fstream>
+
+
+#ifndef WIN32
+# include <sys/time.h>
+#else /*WIN32*/
+// When including Winsock2.h, the MAL must be included first
+//#include <MatrixAbstractLayer/boost.h>
+# include <sot/sotUtilsWindows.h>
+# include <Winsock2.h>
+#endif /*WIN32*/
+//#define WITH_CHRONO
+
+/* ---------------------------------------------------------- */
+/* ---------------------------------------------------------- */
+/* ---------------------------------------------------------- */
+
+void parseTest( const std::string filename )
+{
+ using namespace std;
+ std::ifstream off( filename.c_str() );
+ std::string bs;
+
+ bubMatrix Rh;
+ sotSolverHierarchicalInequalities::ConstraintList constraintH;
+ std::vector<bubMatrix> Jes;
+ std::vector<bubVector> ees;
+ std::vector<bubMatrix> Jis;
+ std::vector<bubVector> eiInfs;
+ std::vector<bubVector> eiSups;
+ std::vector<sotConstraintMem::BoundSideVector> bounds;
+
+ int nJ;
+ int me,mi;
+ bubMatrix Je,Ji;
+ bubVector ee,eiInf,eiSup;
+ sotConstraintMem::BoundSideVector eiBoundSide;
+
+ off >> bs; if(bs!="variable") { cerr << "!! '" << bs << "'" << endl; return; }
+ off >> bs; if(bs!="size") { cerr << "!! '" << bs << "'" << endl; return; }
+ off >> nJ;
+
+ sotRotationComposedInExtenso Qh(nJ);
+ std::deque<sotSolverHierarchicalInequalities*> solvers;
+
+ for( unsigned int level=0;;++level )
+ {
+ /* --- Parse egalities --- */
+ off >> bs;
+ if(bs=="end") { break; }
+ else if(bs!="level") { cerr << "!! '" << bs << "'" << endl; return; }
+ off >> bs; if(bs!="equalities") { cerr << "!! '" << bs << "'" << endl; return; }
+ off >> me;
+ Je.resize(me,nJ); ee.resize(me);
+ if(me>0)
+ for( int i=0;i<me;++i )
+ {
+ for( int j=0;j<nJ;++j )
+ off >> Je(i,j);
+ off >> ee(i);
+ }
+
+ /* --- Parse inequalities --- */
+ off >> bs; if(bs!="inequalities") { cerr << "!! '" << bs << "'" << endl; return; }
+ off >> mi;
+ Ji.resize(mi,nJ); eiInf.resize(mi); eiSup.resize(mi); eiBoundSide.resize(mi);
+ if(mi>0)
+ for( int i=0;i<mi;++i )
+ {
+ for( int j=0;j<nJ;++j )
+ off >> Ji(i,j);
+ std::string number;
+ eiBoundSide[i] = sotConstraintMem::BOUND_VOID;
+ off>>number;
+ if( number!="X" )
+ {
+ eiBoundSide[i] = (sotConstraintMem::BoundSideType)
+ (eiBoundSide[i]|sotConstraintMem::BOUND_INF);
+ eiInf(i) = atof(number.c_str());
+ } else { eiInf(i) = 1e-66; }
+ off>>number;
+ if( number!="X" )
+ {
+ eiBoundSide[i] = (sotConstraintMem::BoundSideType)
+ (eiBoundSide[i]|sotConstraintMem::BOUND_SUP);
+ eiSup(i) = atof(number.c_str());
+ } else { eiSup(i) = 1e-66; }
+ }
+
+ struct timeval t0,t1;
+ double dtsolver;
+ sotDEBUG(15) << "/* ----------------------------------------------------- */" << std::endl;
+ sotDEBUG(15) << "/* ----------------------------------------------------- */" << std::endl;
+ sotDEBUG(15) << "/* ----------------------------------------------------- */" << std::endl;
+ sotDEBUG(5) << "ee" << level << " = " << (MATLAB) ee << endl;
+ sotDEBUG(5) << "eiInf" << level << " = " << (MATLAB) eiInf << endl;
+ sotDEBUG(5) << "eiSup" << level << " = " << (MATLAB) eiSup << endl;
+ sotDEBUG(5) << "Je" << level << " = " << (MATLAB) Je << endl;
+ sotDEBUG(5) << "Ji" << level << " = " << (MATLAB) Ji << endl;
+ gettimeofday(&t0,NULL);
+
+
+ Jes.push_back(Je);
+ Jis.push_back(Ji);
+ ees.push_back(ee);
+ eiInfs.push_back(eiInf);
+ eiSups.push_back(eiSup);
+ bounds.push_back(eiBoundSide);
+
+ sotDEBUG(1) << "--- Level " << level << std::endl;
+ sotSolverHierarchicalInequalities * solver
+ = new sotSolverHierarchicalInequalities(nJ,Qh,Rh,constraintH);
+ solver->initConstraintSize(Je.size1()+Ji.size1());
+ if( solvers.size()==0 ) solver->setInitialConditionVoid();
+ else
+ {
+ sotSolverHierarchicalInequalities * solverPrec
+ = solvers.back();
+ solver->setInitialCondition( solverPrec->u0,solverPrec->rankh );
+ }
+ solver->recordInitialConditions();
+ solvers.push_back(solver);
+
+
+ solver->solve(Je,ee,Ji,eiInf,eiSup,eiBoundSide);
+
+ /*!*/gettimeofday(&t1,NULL);
+ /*!*/dtsolver = (t1.tv_sec-t0.tv_sec) * 1000. + (t1.tv_usec-t0.tv_usec+0.)/1000. ;
+ /*!*/sotDEBUG(1) << "u" << level << " = " << (MATLAB)solver->u0 << endl;
+ /*!*/cout << "dtSOLV_" << level << " = " << dtsolver << "%ms" << endl;
+
+ solver->computeDifferentialCondition();
+#ifdef VP_DEBUG
+ solver->printDifferentialCondition(sotDEBUGFLOW.outputbuffer);
+#endif
+
+ }
+
+ for( unsigned int repet=0;repet<1;++repet )
+ {
+ cout << "Repet = " << repet << std::endl;
+ struct timeval t0,t1; double dtsolver;
+ constraintH.resize(0);
+ Rh*=0;
+ Qh.clear();
+ for( unsigned int level=0;level<solvers.size();++level )
+ {
+ gettimeofday(&t0,NULL);
+ sotDEBUG(1) << "--- Level " << level << std::endl;
+ sotSolverHierarchicalInequalities * solver = solvers[level];
+ if( level==0 ) solver->setInitialConditionVoid();
+ else
+ {
+ sotSolverHierarchicalInequalities * solverPrec = solvers[level-1];
+ solver->setInitialCondition( solverPrec->u0,solverPrec->rankh );
+ }
+
+ solver->recordInitialConditions();
+#ifdef WITH_WARM_START
+ solver->warmStart();
+#endif
+ /*!*/gettimeofday(&t1,NULL);
+ /*!*/dtsolver = (t1.tv_sec-t0.tv_sec) * 1000. + (t1.tv_usec-t0.tv_usec+0.)/1000. ;
+ /*!*/sotDEBUG(1) << "dtWS_" << level << " = " << dtsolver << "%ms" << endl;
+ /*!*/cout << " " << dtsolver ;
+ /*!*/gettimeofday(&t0,NULL);
+
+ solver->solve( Jes[level],ees[level],Jis[level],
+ eiInfs[level],eiSups[level],bounds[level],
+ solver->getSlackActiveSet() );
+ sotDEBUG(1) << "u" << level << " = " << (MATLAB)solver->u0 << endl;
+ /*!*/gettimeofday(&t1,NULL);
+ /*!*/dtsolver = (t1.tv_sec-t0.tv_sec) * 1000. + (t1.tv_usec-t0.tv_usec+0.)/1000. ;
+ /*!*/cout << " " << dtsolver ;
+ /*!*/sotDEBUG(1) << "dtSOLV_" << level << " = " << dtsolver << "%ms" << endl;
+ /*!*/gettimeofday(&t0,NULL);
+
+ solver->computeDifferentialCondition();
+#ifdef VP_DEBUG
+ solver->printDifferentialCondition(sotDEBUGFLOW.outputbuffer);
+#endif
+
+ /*!*/gettimeofday(&t1,NULL);
+ /*!*/dtsolver = (t1.tv_sec-t0.tv_sec) * 1000. + (t1.tv_usec-t0.tv_usec+0.)/1000. ;
+ /*!*/cout << " " << dtsolver ;
+ /*!*/sotDEBUG(1) << "dtREC_" << level << " = " << dtsolver << "%ms" << endl;
+ }
+ std::cout << std::endl;
+ }
+
+
+ for( unsigned int level=0;level<solvers.size();++level )
+ { delete solvers[level]; solvers[level]=NULL; }
+ solvers.clear();
+}
+
+/* ---------------------------------------------------------- */
+void deparse( std::vector<bubMatrix> Jes,std::vector<bubVector> ees,
+ std::vector<bubMatrix> Jis,std::vector<bubVector> eiInfs,
+ std::vector<bubVector> eiSups,std::vector<sotConstraintMem::BoundSideVector> bounds )
+{
+ using namespace std;
+ cout << "variable size " << Jes[0].size2() << endl;
+
+ for( unsigned int i=0;i<Jes.size();++i )
+ {
+ bubMatrix & Je = Jes[i];
+ bubMatrix & Ji = Jis[i];
+ bubVector & ee = ees[i];
+ bubVector & eiInf = eiInfs[i];
+ bubVector & eiSup = eiSups[i];
+ sotConstraintMem::BoundSideVector & boundSide = bounds[i];
+
+ cout << endl << endl << "level" << endl << endl << "equalities " << ee.size() << endl;
+ if(ee.size()>0)
+ for( unsigned int i=0;i<ee.size();++i )
+ {
+ for( unsigned int j=0;j<Je.size2();++j )
+ cout << Je(i,j) << " ";
+ cout << "\t" << ee(i) << endl;
+ }
+
+ unsigned int nbIneq = 0;
+ for( unsigned int i=0;i<boundSide.size();++i )
+ {
+ if( boundSide[i]&sotConstraintMem::BOUND_INF ) nbIneq++;
+ if( boundSide[i]&sotConstraintMem::BOUND_SUP ) nbIneq++;
+ }
+
+ cout << endl << "inequalities " << nbIneq << endl;
+ if(eiInf.size()>0)
+ for( unsigned int i=0;i<eiInf.size();++i )
+ {
+ if( boundSide[i]&sotConstraintMem::BOUND_INF )
+ {
+ for( unsigned int j=0;j<Ji.size2();++j )
+ cout << -Ji(i,j) << " ";
+ cout << "\t" << " X " << -eiInf(i) << endl;
+ }
+ if( boundSide[i]&sotConstraintMem::BOUND_SUP )
+ {
+ for( unsigned int j=0;j<Ji.size2();++j )
+ cout << Ji(i,j) << " ";
+ cout << "\t" << " X " << eiSup(i) << endl;
+ }
+ }
+ }
+ sotDEBUG(15) << endl << endl << "end" << endl;
+}
+
+
+void convertDoubleToSingle( const std::string filename )
+{
+ using namespace std;
+ std::ifstream off( filename.c_str() );
+ std::string bs;
+
+ int nJ;
+ int me,mi;
+ bubMatrix Je,Ji;
+ bubVector ee,eiInf,eiSup;
+ sotConstraintMem::BoundSideVector eiBoundSide;
+
+ off >> bs; if(bs!="variable") { cerr << "!! '" << bs << "'" << endl; return; }
+ off >> bs; if(bs!="size") { cerr << "!! '" << bs << "'" << endl; return; }
+ off >> nJ;
+
+ /* --- Set config file --- */
+ std::vector<bubMatrix> Jes;
+ std::vector<bubVector> ees;
+ std::vector<bubMatrix> Jis;
+ std::vector<bubVector> eiInfs;
+ std::vector<bubVector> eiSups;
+ std::vector<sotConstraintMem::BoundSideVector> bounds;
+
+ for( unsigned int level=0;;++level )
+ {
+ off >> bs;
+ if(bs=="end") { break; }
+ else if(bs!="level") { cerr << "!! '" << bs << "'" << endl; return; }
+ off >> bs; if(bs!="equalities") { cerr << "!! '" << bs << "'" << endl; return; }
+ off >> me;
+ Je.resize(me,nJ); ee.resize(me);
+ if(me>0)
+ for( int i=0;i<me;++i )
+ {
+ for( int j=0;j<nJ;++j )
+ off >> Je(i,j);
+ off >> ee(i);
+ }
+
+ off >> bs; if(bs!="inequalities") { cerr << "!! '" << bs << "'" << endl; return; }
+ off >> mi;
+ Ji.resize(mi,nJ); eiInf.resize(mi); eiSup.resize(mi); eiBoundSide.resize(mi);
+ if(mi>0)
+ for( int i=0;i<mi;++i )
+ {
+ for( int j=0;j<nJ;++j )
+ off >> Ji(i,j);
+ std::string number;
+ eiBoundSide[i] = sotConstraintMem::BOUND_VOID;
+ off>>number; // std::cout << "toto '" << number << "'" << std::endl;
+ if( number!="X" )
+ {
+ eiBoundSide[i] = (sotConstraintMem::BoundSideType)(eiBoundSide[i]|sotConstraintMem::BOUND_INF);
+ eiInf(i) = atof(number.c_str());
+ } else { eiInf(i) = 1e-66; }
+ off>>number;
+ if( number!="X" )
+ {
+ eiBoundSide[i] = (sotConstraintMem::BoundSideType)(eiBoundSide[i]|sotConstraintMem::BOUND_SUP);
+ eiSup(i) = atof(number.c_str());
+ } else { eiSup(i) = 1e-66; }
+ }
+ //Ji*=-1; eiInf*=-1;
+
+ Jes.push_back(Je);
+ ees.push_back(ee);
+ Jis.push_back(Ji);
+ eiInfs.push_back(eiInf);
+ eiSups.push_back(eiSup);
+ bounds.push_back(eiBoundSide);
+ }
+
+ deparse(Jes,ees,Jis,eiInfs,eiSups,bounds);
+}
+
+
+/* ---------------------------------------------------------- */
+/* ---------------------------------------------------------- */
+/* ---------------------------------------------------------- */
+void randBound( sotConstraintMem::BoundSideVector& M,const unsigned int row)
+{
+ M.resize(row);
+ for( unsigned int i=0;i<row;++i )
+ {
+ double c = ((rand()+0.0)/RAND_MAX*2)-1.;
+ if( c<0 ) M[i] = sotConstraintMem::BOUND_INF;
+ else M[i] = sotConstraintMem::BOUND_SUP;
+ }
+}
+
+void randTest( const unsigned int nJ,const bool enableSolve[] )
+{
+ bubVector eiInf0(nJ),ee0(1),eiSup0(nJ);
+ bubMatrix Ji0(nJ,nJ),Je0(1,nJ);
+ sotConstraintMem::BoundSideVector bound0(nJ,sotConstraintMem::BOUND_INF);
+ Ji0.assign( bub::identity_matrix<double>(nJ));
+ eiInf0.assign( bub::zero_vector<double>(nJ));
+ Je0.assign( bub::zero_matrix<double>(1,nJ));
+ //std::fill(ee0.data().begin(),ee0.data().end(),1);
+ ee0(0)=0; Je0(0,0)=1;
+
+ bubVector ee1,eiInf1,eiSup1;
+ bubMatrix Je1,Ji1;
+ sotConstraintMem::BoundSideVector bound1;
+ randVector(ee1,3); sotDEBUG(15) << "ee1 = " << (MATLAB)ee1 << std::endl;
+ randVector(eiInf1,3); sotDEBUG(15) << "eiInf1 = " << (MATLAB)eiInf1 << std::endl;
+ randVector(eiSup1,3); sotDEBUG(15) << "eiSup1 = " << (MATLAB)eiSup1 << std::endl;
+ randBound(bound1,3);
+ randMatrix(Je1,3,nJ); //sotDEBUG(15) << "Je1 = " << (MATLAB)Je1 << std::endl;
+ randMatrix(Ji1,3,nJ); sotDEBUG(15) << "Ji1 = " << (MATLAB)Ji1 << std::endl;
+ bubMatrix xhi; randMatrix(xhi,1,3); xhi(0,2)=0;
+ bub::project(Je1,bub::range(2,3),bub::range(0,nJ)) = bub::prod(xhi,Je1);
+ sotDEBUG(15) << "Je1 = " << (MATLAB)Je1 << std::endl;
+ //randMatrix(xhi,3,3);
+ // bub::noalias(Ji1) = bub::prod(xhi,Je1);
+ // sotDEBUG(15) << "Ji1 = " << (MATLAB)Ji1 << std::endl;
+
+
+ bubVector ee2,eiInf2,eiSup2;
+ bubMatrix Je2,Ji2;
+ sotConstraintMem::BoundSideVector bound2;
+ randVector(ee2,3); sotDEBUG(15) << "ee2 = " << (MATLAB)ee2 << std::endl;
+ randVector(eiInf2,3); sotDEBUG(15) << "eiInf2 = " << (MATLAB)eiInf2 << std::endl;
+ randVector(eiSup2,3); sotDEBUG(15) << "eiSup2 = " << (MATLAB)eiSup2 << std::endl;
+ randBound(bound1,3);
+ randMatrix(Je2,3,nJ); sotDEBUG(15) << "Je2 = " << (MATLAB)Je2 << std::endl;
+ randMatrix(Ji2,3,nJ); sotDEBUG(15) << "Ji2 = " << (MATLAB)Ji2 << std::endl;
+
+ bubVector ee3,eiInf3,eiSup3;
+ bubMatrix Je3,Ji3;
+ sotConstraintMem::BoundSideVector bound3;
+ randVector(ee3,3); sotDEBUG(15) << "ee3 = " << (MATLAB)ee3 << std::endl;
+ randVector(eiInf3,3); sotDEBUG(15) << "eiInf3 = " << (MATLAB)eiInf3 << std::endl;
+ randVector(eiSup3,3); sotDEBUG(15) << "eiSup3 = " << (MATLAB)eiSup3 << std::endl;
+ randBound(bound1,3);
+ randMatrix(Je3,3,nJ); sotDEBUG(15) << "Je3 = " << (MATLAB)Je3 << std::endl;
+ randMatrix(Ji3,3,nJ); sotDEBUG(15) << "Ji3 = " << (MATLAB)Ji3 << std::endl;
+
+ bubVector ee4(nJ),eiInf4(1),eiSup4(1);
+ bubMatrix Je4(nJ,nJ),Ji4(1,nJ);
+ sotConstraintMem::BoundSideVector bound4(1,sotConstraintMem::BOUND_INF);
+ Je4.assign( bub::identity_matrix<double>(nJ));
+ ee4.assign( bub::zero_vector<double>(nJ));
+ Ji4.assign( bub::zero_matrix<double>(1,nJ));
+ eiInf4.assign( bub::zero_vector<double>(1));
+ eiSup4.assign( bub::zero_vector<double>(1));
+
+ /* --- Set config file --- */
+ std::vector<bubMatrix> Jes;
+ std::vector<bubVector> ees;
+ std::vector<bubMatrix> Jis;
+ std::vector<bubVector> eiInfs;
+ std::vector<bubVector> eiSups;
+ std::vector<sotConstraintMem::BoundSideVector> bounds;
+
+ if(enableSolve[0])
+ {
+ Jes.push_back(Je0);
+ ees.push_back(ee0);
+ Jis.push_back(Ji0);
+ eiInfs.push_back(eiInf0);
+ eiSups.push_back(eiSup0);
+ bounds.push_back(bound0);
+ }
+
+ if(enableSolve[1])
+ {
+ Jes.push_back(Je1);
+ ees.push_back(ee1);
+ Jis.push_back(Ji1);
+ eiInfs.push_back(eiInf1);
+ eiSups.push_back(eiSup1);
+ bounds.push_back(bound1);
+ }
+ if(enableSolve[2])
+ {
+ Jes.push_back(Je2);
+ ees.push_back(ee2);
+ Jis.push_back(Ji2);
+ eiInfs.push_back(eiInf2);
+ eiSups.push_back(eiSup2);
+ bounds.push_back(bound2);
+ }
+ if(enableSolve[3])
+ {
+ Jes.push_back(Je3);
+ ees.push_back(ee3);
+ Jis.push_back(Ji3);
+ eiInfs.push_back(eiInf3);
+ eiSups.push_back(eiSup3);
+ bounds.push_back(bound3);
+ }
+ if(enableSolve[4])
+ {
+ Jes.push_back(Je4);
+ ees.push_back(ee4);
+ Jis.push_back(Ji4);
+ eiInfs.push_back(eiInf4);
+ eiSups.push_back(eiSup4);
+ bounds.push_back(bound4);
+ }
+ deparse(Jes,ees,Jis,eiInfs,eiSups,bounds);
+
+ sotRotationComposedInExtenso Qh(nJ);
+ bubMatrix Rh;
+ sotSolverHierarchicalInequalities::ConstraintList constraintH;
+ sotSolverHierarchicalInequalities solver(nJ,Qh,Rh,constraintH);
+ solver.initConstraintSize((40+6+6+6+40)+2);
+
+
+ /* ---------------------------------------------------------- */
+
+ if(enableSolve[0]) solver.solve(Je0,ee0,Ji0,eiInf0,eiSup0,bound0);
+ sotDEBUG(15) << "/* ----------------------------------------------------- */" << std::endl;
+ sotDEBUG(15) << "/* ----------------------------------------------------- */" << std::endl;
+ sotDEBUG(15) << "/* ----------------------------------------------------- */" << std::endl;
+ if(enableSolve[1]) solver.solve(Je1,ee1,Ji1,eiInf1,eiSup1,bound1);
+ sotDEBUG(15) << "/* ----------------------------------------------------- */" << std::endl;
+ sotDEBUG(15) << "/* ----------------------------------------------------- */" << std::endl;
+ sotDEBUG(15) << "/* ----------------------------------------------------- */" << std::endl;
+ if(enableSolve[2]) solver.solve(Je2,ee2,Ji2,eiInf2,eiSup2,bound2);
+ sotDEBUG(15) << "/* ----------------------------------------------------- */" << std::endl;
+ sotDEBUG(15) << "/* ----------------------------------------------------- */" << std::endl;
+ sotDEBUG(15) << "/* ----------------------------------------------------- */" << std::endl;
+ if(enableSolve[3]) solver.solve(Je3,ee3,Ji3,eiInf3,eiSup3,bound3);
+ sotDEBUG(15) << "/* ----------------------------------------------------- */" << std::endl;
+ sotDEBUG(15) << "/* ----------------------------------------------------- */" << std::endl;
+ sotDEBUG(15) << "/* ----------------------------------------------------- */" << std::endl;
+ if(enableSolve[4]) solver.solve(Je4,ee4,Ji4,eiInf4,eiSup4,bound4);
+
+ /* ---------------------------------------------------------- */
+
+
+ return;
+}
+
+/* ---------------------------------------------------------- */
+/* ---------------------------------------------------------- */
+/* ---------------------------------------------------------- */
+int main( void )
+{
+
+ // convertDoubleToSingle("/home/nmansard/src/StackOfTasks/tests/tools/testFR.txt"); exit(0);
+
+#ifndef VP_DEBUG
+#ifndef WITH_CHRONO
+ for(int i=0;i<10;++i)
+#endif
+#endif
+ parseTest("/home/nmansard/src/StackOfTasks/tests/sot/t.txt");
+// bool enable [5] ={ 1,1,0,0,0};
+// randTest(9,enable );
+}
diff --git a/tools/sot/tsot.cpp b/tools/sot/tsot.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..901e79145f20c473ef0ef0eda76b3b8b55cecdcd
--- /dev/null
+++ b/tools/sot/tsot.cpp
@@ -0,0 +1,92 @@
+/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ * 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.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 )
+{
+ sotDEBUGF( "# In {" );
+
+ 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("p1des");
+
+
+ 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("task");
+ task->addFeature(*p1);
+ task->addFeature(*p1);
+
+ GainAdaptative * lambda = new GainAdaptative("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.displayDependancies(cout)<<endl;
+
+ sotDEBUG(0) << "J"<< task->jacobianSOUT(2);
+ sotDEBUG(0) <<"H"<< task->featureActivationSOUT(2)<<endl;
+ sotDEBUG(0) <<"e"<< task->errorSOUT(2) <<endl;
+
+
+ sotDEBUGF( "# Out }" );
+
+ return 0;
+}
diff --git a/unitTesting/CMakeLists.txt b/unitTesting/CMakeLists.txt
index 2fd7e6141671da5df6b640d79339126fdfe37590..e8317b8f1213153e18ca90223c4fd54aad615e40 100644
--- a/unitTesting/CMakeLists.txt
+++ b/unitTesting/CMakeLists.txt
@@ -28,7 +28,7 @@ FOREACH(test ${tests})
LINK_DIRECTORIES(${${PROJECT_NAME}_BINARY_DIR}/src)
TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME}
- sot-core)
+ ${PROJECT_NAME})
# Add MatrixAbstractLayer compilation flags and link to library libMatrixAbstractLayer.so
ADD_DEFINITIONS(${MATRIXABSTRACTLAYER_CFLAGS})
@@ -42,7 +42,6 @@ FOREACH(test ${tests})
ADD_DEFINITIONS(${DYNAMIC_GRAPH_CFLAGS})
TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} ${DYNAMIC_GRAPH_LIBRARIES})
- # FIXME
IF (UNIX)
TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME}
dl)