From 5fce0e8f3eea3bdb3b25173c9cee26144d37ae9a Mon Sep 17 00:00:00 2001
From: Francois Bleibel <fbleibel@gmail.com>
Date: Wed, 30 Jun 2010 18:21:47 +0900
Subject: [PATCH] Added three tools.
---
include/sot-core/robot-simu.h | 130 ++++++++++
include/sot-core/seq-play.h | 108 +++++++++
include/sot-core/sequencer.h | 140 +++++++++++
src/CMakeLists.txt | 39 ++-
src/tools/robot-simu.cpp | 229 ++++++++++++++++++
src/tools/seq-play.cpp | 174 ++++++++++++++
src/tools/sequencer.cpp | 436 ++++++++++++++++++++++++++++++++++
7 files changed, 1233 insertions(+), 23 deletions(-)
create mode 100644 include/sot-core/robot-simu.h
create mode 100644 include/sot-core/seq-play.h
create mode 100644 include/sot-core/sequencer.h
create mode 100644 src/tools/robot-simu.cpp
create mode 100644 src/tools/seq-play.cpp
create mode 100644 src/tools/sequencer.cpp
diff --git a/include/sot-core/robot-simu.h b/include/sot-core/robot-simu.h
new file mode 100644
index 00000000..b7bd0627
--- /dev/null
+++ b/include/sot-core/robot-simu.h
@@ -0,0 +1,130 @@
+/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ * Copyright Projet JRL-Japan, 2007
+ *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ *
+ * File: RobotSimu.h
+ * Project: SOT
+ * Author: Nicolas Mansard
+ *
+ * Version control
+ * ===============
+ *
+ * $Id$
+ *
+ * Description
+ * ============
+ *
+ *
+ * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+
+
+
+#ifndef __SOT_ROBOT_SIMU_HH
+#define __SOT_ROBOT_SIMU_HH
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+/* -- MaaL --- */
+#include <MatrixAbstractLayer/boost.h>
+namespace ml= maal::boost;
+/* SOT */
+#include <dynamic-graph/entity.h>
+#include <dynamic-graph/all-signals.h>
+#include <sot-core/vector-roll-pitch-yaw.h>
+#include <sot-core/periodic-call.h>
+
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined (WIN32)
+# if defined (robot_simu_EXPORTS)
+# define SOTROBOTSIMU_EXPORT __declspec(dllexport)
+# else
+# define SOTROBOTSIMU_EXPORT __declspec(dllimport)
+# endif
+#else
+# define SOTROBOTSIMU_EXPORT
+#endif
+
+namespace sot {
+
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+class SOTROBOTSIMU_EXPORT RobotSimu
+:public dynamicgraph::Entity
+{
+ public:
+ static const std::string CLASS_NAME;
+ virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
+
+ enum ForceSignalSource
+ {
+ FORCE_SIGNAL_RLEG,
+ FORCE_SIGNAL_LLEG,
+ FORCE_SIGNAL_RARM,
+ FORCE_SIGNAL_LARM
+ };
+
+ protected:
+ ml::Vector state;
+
+ PeriodicCall periodicCallBefore;
+ PeriodicCall periodicCallAfter;
+ bool withForceSignals[4];
+
+
+ public:
+
+ /* --- CONSTRUCTION --- */
+ RobotSimu( const std::string& name );
+
+ void setStateSize( const unsigned int size );
+ void setState( const ml::Vector st );
+ void increment( const double dt = 5e-2 );
+
+
+
+ public: /* --- DISPLAY --- */
+ virtual void display( std::ostream& os ) const;
+ SOTROBOTSIMU_EXPORT friend std::ostream& operator<< ( std::ostream& os,const RobotSimu& r )
+ { r.display(os); return os;}
+
+ public: /* --- SIGNALS --- */
+
+ dynamicgraph::SignalPtr<ml::Vector,int> controlSIN;
+ //dynamicgraph::SignalPtr<MatrixRotation,int> attitudeSIN;
+ dynamicgraph::SignalPtr<ml::Vector,int> attitudeSIN;
+ dynamicgraph::SignalPtr<ml::Vector,int> zmpSIN;
+
+ dynamicgraph::Signal<ml::Vector,int> stateSOUT;
+ dynamicgraph::Signal<MatrixRotation,int> attitudeSOUT;
+ dynamicgraph::Signal<ml::Vector,int>* forcesSOUT[4];
+
+ dynamicgraph::Signal<ml::Vector,int> pseudoTorqueSOUT;
+ dynamicgraph::Signal<ml::Vector,int> previousControlSOUT;
+
+ /*! \brief The current state of the robot from the command viewpoint. */
+ dynamicgraph::Signal<ml::Vector,int> motorcontrolSOUT;
+ /*! \brief The ZMP reference send by the previous controller. */
+ dynamicgraph::Signal<ml::Vector,int> ZMPPreviousControllerSOUT;
+
+ public: /* --- COMMANDS --- */
+ virtual void commandLine( const std::string& cmdLine,std::istringstream& cmdArgs,
+ std::ostream& os );
+
+};
+
+
+} // namespace sot
+
+
+#endif /* #ifndef __SOT_ROBOT_SIMU_HH */
+
+
+
+
diff --git a/include/sot-core/seq-play.h b/include/sot-core/seq-play.h
new file mode 100644
index 00000000..08d0e277
--- /dev/null
+++ b/include/sot-core/seq-play.h
@@ -0,0 +1,108 @@
+/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ * Copyright Projet JRL-Japan, 2007
+ *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ *
+ * File: SeqPlay.h
+ * Project: SOT
+ * Author: Nicolas Mansard
+ *
+ * Version control
+ * ===============
+ *
+ * $Id$
+ *
+ * Description
+ * ============
+ *
+ *
+ * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+
+
+
+#ifndef __SOT_SEQPLAY_HH
+#define __SOT_SEQPLAY_HH
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+/* -- MaaL --- */
+#include <MatrixAbstractLayer/boost.h>
+namespace ml= maal::boost;
+/* SOT */
+#include <dynamic-graph/entity.h>
+#include <dynamic-graph/all-signals.h>
+
+#include <list>
+
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined (WIN32)
+# if defined (seq_play_EXPORTS)
+# define SOTSEQPLAY_EXPORT __declspec(dllexport)
+# else
+# define SOTSEQPLAY_EXPORT __declspec(dllimport)
+# endif
+#else
+# define SOTSEQPLAY_EXPORT
+#endif
+
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+namespace sot {
+
+class SOTSEQPLAY_EXPORT SeqPlay
+:public dynamicgraph::Entity
+{
+ public:
+ static const std::string CLASS_NAME;
+ virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
+
+ protected:
+
+ typedef std::list<ml::Vector> StateList;
+ StateList stateList;
+ StateList::iterator currPos; unsigned int currRank;
+ bool init;
+ int time;
+
+ public:
+
+ /* --- CONSTRUCTION --- */
+ SeqPlay( const std::string& name );
+ virtual ~SeqPlay( void ) { }
+
+ void loadFile( const std::string& name );
+
+ ml::Vector& getNextPosition( ml::Vector& pos, const int& time );
+
+ public: /* --- DISPLAY --- */
+ virtual void display( std::ostream& os ) const;
+ SOTSEQPLAY_EXPORT friend std::ostream& operator<< ( std::ostream& os,const SeqPlay& r )
+ { r.display(os); return os;}
+
+ public: /* --- SIGNALS --- */
+
+ //dynamicgraph::SignalPtr<ml::Vector,int> positionSIN;
+ //dynamicgraph::SignalTimeDependant<ml::Vector,int> velocitySOUT;
+ dynamicgraph::SignalTimeDependent<int,int> refresherSINTERN;
+ dynamicgraph::SignalTimeDependent<ml::Vector,int> positionSOUT;
+
+ public: /* --- COMMANDS --- */
+ virtual void commandLine( const std::string& cmdLine,std::istringstream& cmdArgs,
+ std::ostream& os );
+
+};
+
+
+} // namespace sot
+
+#endif /* #ifndef __SOT_SEQPLAY_HH */
+
+
+
+
diff --git a/include/sot-core/sequencer.h b/include/sot-core/sequencer.h
new file mode 100644
index 00000000..3bab79ca
--- /dev/null
+++ b/include/sot-core/sequencer.h
@@ -0,0 +1,140 @@
+/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ * Copyright Projet JRL-Japan, 2007
+ *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ *
+ * File: Sequencer.h
+ * Project: SOT
+ * Author: Nicolas Mansard
+ *
+ * Version control
+ * ===============
+ *
+ * $Id$
+ *
+ * Description
+ * ============
+ *
+ *
+ * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+
+
+
+#ifndef __SOT_SOTSEQUENCER_H__
+#define __SOT_SOTSEQUENCER_H__
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+/* Matrix */
+#include <MatrixAbstractLayer/boost.h>
+namespace ml = maal::boost;
+
+/* SOT */
+#include <dynamic-graph/entity.h>
+#include <dynamic-graph/all-signals.h>
+#include <sot-core/task-abstract.h>
+
+/* STD */
+#include <string>
+#include <map>
+#include <list>
+
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined (WIN32)
+# if defined (sequencer_EXPORTS)
+# define SOTSEQUENCER_EXPORT __declspec(dllexport)
+# else
+# define SOTSEQUENCER_EXPORT __declspec(dllimport)
+# endif
+#else
+# define SOTSEQUENCER_EXPORT
+#endif
+
+namespace sot {
+
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+class Sot;
+
+class SOTSEQUENCER_EXPORT Sequencer
+:public dynamicgraph::Entity
+{
+ public:
+ static const std::string CLASS_NAME;
+
+ public:
+ class sotEventAbstract
+ {
+ public:
+ enum sotEventType
+ {
+ EVENT_ADD
+ ,EVENT_RM
+ ,EVENT_CMD
+ };
+ protected:
+ std::string name;
+ void setName( const std::string& name_ ) { name = name_; }
+ int eventType;
+ public:
+ sotEventAbstract( const std::string & name ) : name(name) {};
+ virtual ~sotEventAbstract( void ) {}
+ virtual const std::string& getName() const { return name; }
+ int getEventType( ) const { return eventType; }
+ virtual void operator() ( Sot* sotPtr ) = 0;
+ virtual void display( std::ostream& os ) const { os << name; }
+ };
+
+ protected:
+ Sot* sotPtr;
+ typedef std::list< sotEventAbstract* > TaskList;
+ typedef std::map< unsigned int,TaskList > TaskMap;
+
+ TaskMap taskMap;
+ /* All the events are counting wrt to this t0. If t0 is -1, it
+ * is set to the first time of trig. */
+ int timeInit;
+ bool playMode;
+ std::ostream* outputStreamPtr;
+ bool noOutput; /*! if true, display nothing standard output on except errors*/
+
+ public: /* --- CONSTRUCTION --- */
+
+ Sequencer( const std::string& name );
+ virtual ~Sequencer( void );
+
+ public: /* --- TASK MANIP --- */
+
+ void setSotRef( Sot* sot ) { sotPtr = sot; }
+ void addTask( sotEventAbstract* task,const unsigned int time );
+ void rmTask( int eventType, const std::string& name,const unsigned int time );
+ void clearAll( );
+
+ public: /* --- SIGNAL --- */
+ dynamicgraph::SignalTimeDependent<int,int> triggerSOUT;
+
+ public: /* --- FUNCTIONS --- */
+ int& trigger( int& dummy,const int& time );
+
+ public: /* --- PARAMS --- */
+ virtual void display( std::ostream& os ) const;
+ virtual void commandLine( const std::string& cmdLine,
+ std::istringstream& cmdArgs,
+ std::ostream& os );
+
+
+};
+
+
+} // namespace dynamicgraph
+
+
+
+#endif // #ifndef __SOT_SOTSEQUENCER_H__
+
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 3f511784..b01e88b8 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -8,11 +8,9 @@
INCLUDE_DIRECTORIES(${CMAKE_SOURCE_DIR}/include)
#define VP_DEBUG if we're building in debug mode
-IF(${CMAKE_BUILD_TYPE} STREQUAL DEBUG)
- #ADD_DEFINITIONS(-DVP_DEBUG)
-ENDIF (${CMAKE_BUILD_TYPE} STREQUAL DEBUG)
-
-ADD_DEFINITIONS(-DDEBUG=2)
+IF("${CMAKE_BUILD_TYPE}" STREQUAL DEBUG)
+ ADD_DEFINITIONS(-DDEBUG=2)
+ENDIF ("${CMAKE_BUILD_TYPE}" STREQUAL DEBUG)
# Add lapack compilation flags and link to library libLapack.so
ADD_DEFINITIONS(${LAPACK_CFLAGS})
@@ -83,10 +81,11 @@ SET(plugins
traces/reader
- tools/utils-windows
- tools/periodic-call
tools/time-stamp
- tools//timer
+ tools/timer
+ tools/seq-play
+ tools/sequencer
+ tools/robot-simu
)
#Build sot-core
@@ -128,6 +127,9 @@ SET(${PROJECT_NAME}_SOURCES
factory/factory.cpp
factory/pool.cpp
factory/command/import.cpp
+
+ tools/utils-windows
+ tools/periodic-call
)
ADD_LIBRARY(${LIBRARY_NAME}
@@ -139,11 +141,11 @@ SET_TARGET_PROPERTIES(${LIBRARY_NAME}
SOVERSION ${PROJECT_VERSION}
INSTALL_RPATH ${CMAKE_INSTALL_PREFIX}/lib)
-SET_TARGET_PROPERTIES(${LIBRARY_NAME}
- PROPERTIES
- COMPILE_FLAGS "${${PROJECT_NAME}_src_CXXFLAGS}"
- LINK_FLAGS "${${PROJECT_NAME}_src_LDFLAGS}"
-)
+#SET_TARGET_PROPERTIES(${LIBRARY_NAME}
+# PROPERTIES
+ #COMPILE_FLAGS "${${PROJECT_NAME}_src_CXXFLAGS}"
+ #LINK_FLAGS "${${PROJECT_NAME}_src_LDFLAGS}"
+#)
TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${LAPACK_LIBRARIES})
@@ -160,11 +162,6 @@ IF(UNIX)
TARGET_LINK_LIBRARIES(${LIBRARY_NAME} dl)
ENDIF(UNIX)
-SET_TARGET_PROPERTIES(${LIBRARY_NAME}
- PROPERTIES
- LINK_FLAGS "${${PROJECT_NAME}_src_LDFLAGS}"
-)
-
INSTALL(TARGETS ${LIBRARY_NAME}
DESTINATION ${CMAKE_INSTALL_PREFIX}/lib)
@@ -203,11 +200,7 @@ FOREACH(plugin ${plugins})
TARGET_LINK_LIBRARIES(${LIBRARY_NAME} dl)
ENDIF(UNIX)
- SET_TARGET_PROPERTIES(${LIBRARY_NAME}
- PROPERTIES
- COMPILE_FLAGS "${${PROJECT_NAME}_src_CXXFLAGS}"
- LINK_FLAGS "${${PROJECT_NAME}_src_LDFLAGS}"
- )
+ TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${ADDITIONAL_${LIBRARY_NAME}_LIBS})
INSTALL(TARGETS ${LIBRARY_NAME}
DESTINATION ${CMAKE_INSTALL_PREFIX}/lib/plugin)
diff --git a/src/tools/robot-simu.cpp b/src/tools/robot-simu.cpp
new file mode 100644
index 00000000..4219d0b2
--- /dev/null
+++ b/src/tools/robot-simu.cpp
@@ -0,0 +1,229 @@
+/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ * Copyright Projet JRL-Japan, 2007
+ *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ *
+ * File: RobotSimu.cpp
+ * Project: SOT
+ * Author: Nicolas Mansard
+ *
+ * Version control
+ * ===============
+ *
+ * $Id$
+ *
+ * Description
+ * ============
+ *
+ *
+ * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+/* SOT */
+#include <sot-core/robot-simu.h>
+#include <sot-core/debug.h>
+using namespace std;
+
+#include <dynamic-graph/factory.h>
+using namespace sot;
+using namespace dynamicgraph;
+
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RobotSimu,"RobotSimu");
+
+
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+
+RobotSimu::
+RobotSimu( const std::string& n )
+ :Entity(n)
+ ,state(6)
+ ,periodicCallBefore( )
+ ,periodicCallAfter( )
+ ,controlSIN( NULL,"RobotSimu("+n+")::input(double)::control" )
+ //,attitudeSIN(NULL,"RobotSimu::input(matrixRot)::attitudeIN")
+ ,attitudeSIN(NULL,"RobotSimu::input(vector3)::attitudeIN")
+ ,zmpSIN(NULL,"RobotSimu::input(vector3)::zmp")
+ ,stateSOUT( "RobotSimu("+n+")::output(vector)::state" )
+ ,attitudeSOUT( "RobotSimu("+n+")::output(matrixRot)::attitude" )
+ ,pseudoTorqueSOUT( "RobotSimu::output(vector)::ptorque" )
+ ,previousControlSOUT( "RobotSimu("+n+")::output(vector)::previousControl" )
+ ,motorcontrolSOUT( "RobotSimu("+n+")::output(vector)::motorcontrol" )
+ ,ZMPPreviousControllerSOUT( "RobotSimu("+n+")::output(vector)::zmppreviouscontroller" )
+{
+ /* --- FORCES --- */
+ for( int i=0;i<4;++i ){ withForceSignals[i] = false; }
+ forcesSOUT[0] =
+ new Signal<ml::Vector, int>("OpenHRP::output(vector6)::forceRLEG");
+ forcesSOUT[1] =
+ new Signal<ml::Vector, int>("OpenHRP::output(vector6)::forceLLEG");
+ forcesSOUT[2] =
+ new Signal<ml::Vector, int>("OpenHRP::output(vector6)::forceRARM");
+ forcesSOUT[3] =
+ new Signal<ml::Vector, int>("OpenHRP::output(vector6)::forceLARM");
+
+ signalRegistration( controlSIN<<stateSOUT<<attitudeSOUT<<attitudeSIN<<zmpSIN
+ <<*forcesSOUT[0]<<*forcesSOUT[1]<<*forcesSOUT[2]<<*forcesSOUT[3]
+ <<previousControlSOUT <<pseudoTorqueSOUT
+ << motorcontrolSOUT << ZMPPreviousControllerSOUT );
+ state.fill(.0); stateSOUT.setConstant( state );
+}
+
+void RobotSimu::
+setStateSize( const unsigned int size )
+{
+ state.resize(size); state.fill( .0 );
+ stateSOUT .setConstant( state );
+ previousControlSOUT.setConstant( state );
+ pseudoTorqueSOUT.setConstant( state );
+ motorcontrolSOUT .setConstant( state );
+
+ ml::Vector zmp(3); zmp.fill( .0 );
+ ZMPPreviousControllerSOUT .setConstant( zmp );
+}
+
+void RobotSimu::
+setState( const ml::Vector st )
+{
+ state = st;
+ stateSOUT .setConstant( state );
+ motorcontrolSOUT .setConstant( state );
+}
+
+void RobotSimu::
+increment( const double dt )
+{
+ sotDEBUG(25) << "Time : " << controlSIN.getTime()+1 << std::endl;
+
+ periodicCallBefore.runSignals( controlSIN.getTime()+1 );
+ periodicCallBefore.runCmds();
+
+ stateSOUT .setConstant( state );
+ const ml::Vector control = controlSIN( controlSIN.getTime()+1 );
+
+ sotDEBUG(25) << "Cl" <<controlSIN.getTime()<<" = "
+ << control*dt << ": " << control << endl;
+
+ sotDEBUG(25) << "St"<<state.size() << controlSIN.getTime() << ": " << state << endl;
+ for( unsigned int i=6;i<state.size();++i )
+ { state(i) += (control(i-6)*dt); }
+
+ sotDEBUG(25) << "St"<<state.size() << controlSIN.getTime() << ": " << state << endl;
+
+
+ ml::Vector forceNull(6); forceNull.fill(0);
+ for( int i=0;i<4;++i ){
+ if( withForceSignals[i] ) forcesSOUT[i]->setConstant(forceNull);
+ }
+
+ motorcontrolSOUT .setConstant( state );
+
+ ml::Vector zmp(3); zmp.fill( .0 );
+ ZMPPreviousControllerSOUT .setConstant( zmp );
+
+
+ periodicCallAfter.runSignals( controlSIN.getTime() );
+ periodicCallAfter.runCmds();
+
+}
+
+
+
+/* --- DISPLAY ------------------------------------------------------------ */
+/* --- DISPLAY ------------------------------------------------------------ */
+/* --- DISPLAY ------------------------------------------------------------ */
+
+void RobotSimu::display ( std::ostream& os ) const
+{os <<name<<": "<<state<<endl; }
+
+
+/* --- PARAMS --------------------------------------------------------------- */
+/* --- PARAMS --------------------------------------------------------------- */
+/* --- PARAMS --------------------------------------------------------------- */
+#include <sot/sotPool.h>
+
+void RobotSimu::
+commandLine( const std::string& cmdLine
+ ,std::istringstream& cmdArgs
+ ,std::ostream& os )
+{
+ if( cmdLine=="help" )
+ {
+ os << "RobotSimu: "<<endl
+ << " - resize <uint size>"<<endl
+ << " - set <vector>"<<endl
+ << " - inc [<dt>]"<<endl;
+ Entity::commandLine( cmdLine,cmdArgs,os );
+ }
+ else if( cmdLine=="resize" )
+ {
+ unsigned int size; cmdArgs >> size;
+ setStateSize( size );
+ }
+ else if( cmdLine=="set" )
+ {
+ ml::Vector q; cmdArgs >> q;
+ setState( q );
+ }
+ else if( cmdLine == "pause" ) { os << "Not valid in simu" << endl; }
+ else if( cmdLine == "play" ) { os << "Not valid in simu" << endl; }
+ else if( cmdLine == "withForces" )
+ {
+ int index;
+ cmdArgs >> index;
+ if((index >= 0) && (index < 4))
+ {
+ std::string val;
+ cmdArgs >> val;
+ if ( ("1"==val)||("true"==val) )
+ {
+ withForceSignals[index] = true;
+ ml::Vector forceNull(6); forceNull.fill(0);
+ forcesSOUT[index]->setConstant(forceNull);
+ } else withForceSignals[index] = false;
+ }
+ }
+ else if( cmdLine == "whichForces" )
+ {
+ os << "Force signals: " << endl;
+ for( unsigned int i=0;i<4;++i )
+ {
+ os << "\t- Force " << i << ": ";
+ if( withForceSignals[i] )
+ { os << "Active"; } else { os << "Inactive"; }
+ os << endl;
+ }
+ }
+ else if( (cmdLine == "withPreviousControl")||(cmdLine == "withPseudoTorque") )
+ {
+ // Just for compatibility.
+ }
+ else if( cmdLine=="inc" )
+ {
+ increment();
+
+ }
+ else if( cmdLine=="time" )
+ {
+ os << "control.time = " << controlSIN.getTime() << endl;
+ }
+ else if(( cmdLine=="periodicCall" )||( cmdLine=="periodicCallAfter" ))
+ {
+ string cmd2; cmdArgs >> cmd2;
+ periodicCallAfter .commandLine( cmd2,cmdArgs,os );
+ }
+ else if( cmdLine=="periodicCallBefore" )
+ {
+ string cmd2; cmdArgs >> cmd2;
+ periodicCallBefore .commandLine( cmd2,cmdArgs,os );
+ }
+ else
+ {
+ Entity::commandLine( cmdLine,cmdArgs,os );
+ }
+}
diff --git a/src/tools/seq-play.cpp b/src/tools/seq-play.cpp
new file mode 100644
index 00000000..701e0a25
--- /dev/null
+++ b/src/tools/seq-play.cpp
@@ -0,0 +1,174 @@
+/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ * Copyright Projet JRL-Japan, 2007
+ *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ *
+ * File: SeqPlay.cpp
+ * Project: SOT
+ * Author: Nicolas Mansard
+ *
+ * Version control
+ * ===============
+ *
+ * $Id$
+ *
+ * Description
+ * ============
+ *
+ *
+ * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+/* SOT */
+#include <sot-core/seq-play.h>
+#include <sot-core/debug.h>
+using namespace std;
+
+#include <fstream>
+#include <sstream>
+
+#include <dynamic-graph/factory.h>
+using namespace dynamicgraph;
+using namespace sot;
+
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SeqPlay,"SeqPlay");
+
+
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+
+SeqPlay::
+SeqPlay( const std::string& n )
+ :Entity(n)
+ ,stateList()
+ ,currPos(stateList.begin())
+ ,currRank(0)
+ ,init(false)
+ ,time(0)
+ ,refresherSINTERN( "SeqPlay("+n+")::intern(dummy)::refresher" )
+ ,positionSOUT( boost::bind(&SeqPlay::getNextPosition,this,_1,_2),
+ refresherSINTERN,
+ "SeqPlay("+n+")::output(vector)::position" )
+{
+ signalRegistration( positionSOUT );
+ refresherSINTERN.setDependencyType( TimeDependency<int>::ALWAYS_READY );
+}
+
+/* --- COMPUTE ----------------------------------------------------------- */
+/* --- COMPUTE ----------------------------------------------------------- */
+/* --- COMPUTE ----------------------------------------------------------- */
+ml::Vector& SeqPlay::
+getNextPosition( ml::Vector& pos, const int& time )
+{
+ sotDEBUGIN(15);
+ if( !init )
+ {
+ if( stateList.empty() ) return pos;
+ currPos=stateList.begin(); init=true; currRank = 0;
+ }
+
+
+ {
+ const ml::Vector& posCur = *currPos;
+ pos=posCur;
+
+ currPos++;
+ if( currPos==stateList.end() ) currPos--;
+ else currRank++;
+ }
+
+ sotDEBUGOUT(15);
+ return pos;
+}
+
+/* --- LIST -------------------------------------------------------------- */
+/* --- LIST -------------------------------------------------------------- */
+/* --- LIST -------------------------------------------------------------- */
+void SeqPlay::
+loadFile( const std::string& filename )
+{
+ sotDEBUGIN(15);
+
+ sotDEBUG( 25 ) << " Load " << filename << endl;
+ std::ifstream file(filename.c_str());
+ const unsigned int SIZE = 1024;
+ char buffer[SIZE];
+
+ ml::Vector res(1); unsigned int ressize = 1;
+ double time;
+
+ while( file.good() )
+ {
+ file.getline( buffer,SIZE );
+ if( file.gcount()<5 ) break;
+
+ sotDEBUG(25) << buffer<<endl;
+ std::istringstream iss( buffer );
+
+ iss>>time; unsigned int i;
+
+ for( i=0;iss.good();++i )
+ {
+ if( i==ressize ) { ressize*=2; res.resize(ressize,false); }
+ iss>>res(i); sotDEBUG(35) <<i<< ": " << res(i)<<endl;
+ }
+ ressize=i-1; res.resize(ressize,false);
+ stateList.push_back( res );
+ sotDEBUG(15) << time << ": " << res << endl;
+ }
+
+ sotDEBUGOUT(15);
+}
+
+
+
+
+/* --- DISPLAY ------------------------------------------------------------ */
+/* --- DISPLAY ------------------------------------------------------------ */
+/* --- DISPLAY ------------------------------------------------------------ */
+
+void SeqPlay::display ( std::ostream& os ) const
+{os <<name<<endl; }
+
+
+/* --- PARAMS --------------------------------------------------------------- */
+/* --- PARAMS --------------------------------------------------------------- */
+/* --- PARAMS --------------------------------------------------------------- */
+#include <dynamic-graph/pool.h>
+
+void SeqPlay::
+commandLine( const std::string& cmdLine
+ ,std::istringstream& cmdArgs
+ ,std::ostream& os )
+{
+ if( cmdLine=="help" )
+ {
+ os << "SeqPlay: "<<endl
+ << " - load <file>"<<endl
+ << " - size" <<endl;
+ Entity::commandLine( cmdLine,cmdArgs,os );
+ }
+ else if( cmdLine=="load" )
+ {
+ std::string n; cmdArgs >> n;
+ loadFile(n);
+ }
+ else if( cmdLine == "empty" )
+ {
+ stateList.clear(); init=false;
+ }
+ else if( cmdLine == "size" )
+ {
+ os << "size = " << stateList.size() << endl;
+ os << "rank = " << currRank << endl;
+ }
+ else
+ {
+ Entity::commandLine( cmdLine,cmdArgs,os );
+ }
+}
diff --git a/src/tools/sequencer.cpp b/src/tools/sequencer.cpp
new file mode 100644
index 00000000..5670e002
--- /dev/null
+++ b/src/tools/sequencer.cpp
@@ -0,0 +1,436 @@
+/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ * Copyright Projet JRL-Japan, 2007
+ *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ *
+ * File: Sequencer.h
+ * Project: SOT
+ * Author: Nicolas Mansard
+ *
+ * Version control
+ * ===============
+ *
+ * $Id$
+ *
+ * Description
+ * ============
+ *
+ *
+ * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+
+#include <sot-core/sequencer.h>
+#include <sot-core/debug.h>
+#include <sot-core/exception-tools.h>
+#include <sot-core/sot.h>
+#include <dynamic-graph/pool.h>
+#include <dynamic-graph/interpreter.h>
+#include <dynamic-graph/factory.h>
+
+using namespace sot;
+using namespace dynamicgraph;
+
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Sequencer,"Sequencer");
+
+Sequencer::
+Sequencer( const std::string & name )
+ :Entity(name)
+ ,timeInit(-1)
+ ,playMode(false)
+ ,outputStreamPtr(NULL)
+ ,noOutput(false)
+ ,triggerSOUT( boost::bind(&Sequencer::trigger,this,_1,_2),
+ sotNOSIGNAL,
+ "Sequencer("+name+")::output(dummy)::trigger" )
+{
+ sotDEBUGIN(5);
+
+ signalRegistration( triggerSOUT );
+ triggerSOUT.setNeedUpdateFromAllChildren( true );
+
+ sotDEBUGOUT(5);
+}
+
+
+Sequencer::
+~Sequencer( void )
+{
+ sotDEBUGIN(5);
+
+ sotDEBUGOUT(5);
+ return;
+}
+
+/* --- SPECIFIC EVENT ------------------------------------------------------- */
+/* --- SPECIFIC EVENT ------------------------------------------------------- */
+/* --- SPECIFIC EVENT ------------------------------------------------------- */
+
+class sotEventTaskBased
+ : public Sequencer::sotEventAbstract
+{
+protected:
+ TaskAbstract * taskPtr;
+ const std::string defaultTaskName;
+public:
+ sotEventTaskBased( const std::string name = "",TaskAbstract* task = NULL )
+ :sotEventAbstract( name )
+ ,taskPtr( task )
+ ,defaultTaskName("NULL")
+ {}
+
+ void init( std::istringstream& cmdArgs )
+ {
+ cmdArgs >> std::ws;
+ if( cmdArgs.good() )
+ {
+ std::string taskname; cmdArgs >> taskname;
+ sotDEBUG(15) << "Add task " << taskname << std::endl;
+ taskPtr
+ = dynamic_cast< TaskAbstract* > (&g_pool.getEntity( taskname ));
+ }
+ }
+ virtual void display( std::ostream& os ) const
+ { if( taskPtr ) os << taskPtr->getName(); else os << "NULL"; }
+ virtual const std::string& getName() const
+ { if( taskPtr ) return taskPtr->getName(); else return defaultTaskName; }
+
+};
+
+class sotEventAddATask
+ : public sotEventTaskBased
+{
+public:
+ sotEventAddATask( const std::string name = "",TaskAbstract* task=NULL )
+ :sotEventTaskBased( name,task )
+ {
+ eventType = EVENT_ADD;
+ }
+
+ void operator()( Sot* sotptr )
+ {
+ sotDEBUGIN(15);
+ sotDEBUG(45) << "Sot = " << sotptr << ". Task = " << taskPtr << "." << std::endl;
+ if( (NULL!=sotptr)&&(NULL!=taskPtr) ) sotptr->push(*taskPtr );
+ sotDEBUGOUT(15);
+ }
+
+ virtual void display( std::ostream& os ) const
+ {os << "Add<"; sotEventTaskBased::display(os); os<<">"; }
+
+};
+
+
+class sotEventRemoveATask
+ : public sotEventTaskBased
+{
+public:
+ sotEventRemoveATask( const std::string name = "",TaskAbstract* task=NULL )
+ :sotEventTaskBased( name,task )
+ {
+ eventType = EVENT_RM;
+ }
+
+ void operator()( Sot* sotptr )
+ {
+ sotDEBUGIN(15);
+ sotDEBUG(45) << "Sot = " << sotptr << ". Task = " << taskPtr << "." << std::endl;
+ if( (NULL!=sotptr)&&(NULL!=taskPtr) ) sotptr->remove(*taskPtr );
+ sotDEBUGOUT(15);
+ }
+
+ virtual void display( std::ostream& os ) const
+ { os << "Remove<"; sotEventTaskBased::display(os); os<<">"; }
+
+};
+
+
+class sotEventCmd
+ : public Sequencer::sotEventAbstract
+{
+protected:
+ std::string cmd;
+
+public:
+ sotEventCmd( const std::string cmdLine = "" )
+ :sotEventAbstract( cmdLine+"<cmd>" )
+ ,cmd( cmdLine )
+ {
+ eventType = EVENT_CMD;
+ sotDEBUGINOUT(15);
+ }
+
+ void init( std::istringstream& args )
+ {
+ sotDEBUGIN(15);
+ std::stringbuf* pbuf=args.rdbuf();
+ const unsigned int size = pbuf->in_avail();
+ char* buffer = new char ( size+1 );
+ pbuf->sgetn( buffer,size );
+
+ buffer[size]='\0';
+ cmd = buffer;
+ sotDEBUGOUT(15);
+ delete buffer;
+ }
+ const std::string & getEventCmd() const
+ { return cmd; }
+ virtual void display( std::ostream& os ) const
+ { os << "Run: " << cmd; }
+ virtual void operator() ( Sot* sotPtr )
+ {
+ std::ostringstream onull; onull.clear( std::ios::failbit );
+ std::istringstream iss( cmd );
+ std::string cmdName; iss >> cmdName;
+ g_shell.cmd( cmdName,iss,onull );
+ } ;
+};
+
+
+/* --- TASK MANIP ----------------------------------------------------------- */
+/* --- TASK MANIP ----------------------------------------------------------- */
+/* --- TASK MANIP ----------------------------------------------------------- */
+
+void Sequencer::
+addTask( sotEventAbstract* task,const unsigned int timeSpec )
+{
+ TaskMap::iterator listKey = taskMap.find( timeSpec );
+ if( taskMap.end()==listKey )
+ {
+ sotDEBUG(15) << "New element at " << timeSpec << std::endl;
+ taskMap[timeSpec].push_back( task );
+ }
+ else
+ {
+ TaskList& tl = listKey->second;
+ tl.push_back( task );
+ }
+}
+
+//rmTask
+void Sequencer::
+rmTask( int eventType, const std::string & name,const unsigned int time )
+{
+ TaskMap::iterator listKey = taskMap.find( time );
+ if( taskMap.end()!=listKey ) //the time exist
+ {
+ TaskList& tl = listKey->second;
+ for( TaskList::iterator itL = tl.begin(); itL != tl.end(); ++itL)
+ {
+ if ((*itL)->getEventType() == eventType && (*itL)->getName() == name)
+ {
+ tl.remove(*itL);
+ break;
+ }
+ }
+
+ //remove the list if empty
+ if (tl.empty())
+ taskMap.erase(listKey);
+ }
+}
+
+//clearAll
+void Sequencer::
+clearAll( )
+{
+ TaskMap::iterator itM;
+ for(itM = taskMap.begin(); itM != taskMap.end(); ++itM )
+ {
+ TaskList::iterator itL;
+ TaskList& currentMap = itM->second;
+ for (itL=currentMap.begin(); itL!=currentMap.end(); ++itL)
+ delete (*itL);
+ itM->second.clear();
+ }
+ //remove all the lists
+ taskMap.clear();
+}
+/* --- SIGNALS -------------------------------------------------------------- */
+/* --- SIGNALS -------------------------------------------------------------- */
+/* --- SIGNALS -------------------------------------------------------------- */
+
+int& Sequencer::
+trigger( int& dummy,const int& timeSpec )
+{
+ sotDEBUGIN(15);
+
+ if(! playMode ) return dummy;
+ if( -1==timeInit ) timeInit = timeSpec;
+
+ sotDEBUG(15) << "Ref time: " << (timeSpec-timeInit) << std::endl;
+ TaskMap::iterator listKey = taskMap.find( timeSpec-timeInit );
+ if( taskMap.end()!=listKey )
+ {
+ sotDEBUG(1) << "Time: "<< (timeSpec-timeInit) << ": we've got a task to do!"
+ << std::endl;
+ TaskList & tl = listKey->second;
+ for( TaskList::iterator iter=tl.begin();iter!=tl.end();++iter )
+ {
+ if( *iter )
+ {
+ (*iter)->operator() (sotPtr);
+ if( NULL!=outputStreamPtr )
+ {
+ (*outputStreamPtr) << "At time t=" << timeSpec << ": ";
+ (*iter)->display(*outputStreamPtr);
+ (*outputStreamPtr) << std::endl;
+ }
+ }
+ }
+ }
+
+ sotDEBUGOUT(15);
+ return dummy;
+}
+
+/* --- PARAMS --------------------------------------------------------------- */
+/* --- PARAMS --------------------------------------------------------------- */
+/* --- PARAMS --------------------------------------------------------------- */
+
+void Sequencer::
+display( std::ostream& os ) const
+{
+ if (noOutput) return;
+
+ os << "Sequencer " << getName() << "(t0=" << timeInit
+ << ",mode=" << ( (playMode)?"play":"pause" ) << "): " << std::endl;
+ for( TaskMap::const_iterator iterMap = taskMap.begin();
+ iterMap!=taskMap.end();iterMap++ )
+ {
+ os << " - t=" << (iterMap->first) << ":\t";
+ const TaskList & tl = iterMap->second;
+ for( TaskList::const_iterator iterList = tl.begin();
+ iterList!=tl.end();iterList++ )
+ {
+ (*iterList)->display(os); os << " ";
+ }
+ os << std::endl;
+ }
+
+}
+
+
+void Sequencer::
+commandLine( const std::string& cmdLine,
+ std::istringstream& cmdArgs,
+ std::ostream& os )
+{
+ sotDEBUG(25) << "Cmd " << cmdLine <<std::endl;
+
+ if( cmdLine == "help" )
+ {
+ os << "Sequencer: " << std::endl
+ << " - sot [<sotname>]" << std::endl
+ << " - addEvent: <eventType> <time> <args>" << std::endl
+ << " - rmEvent: <eventType> <time> <args>" << std::endl
+ << " - clear: erase all events" << std::endl
+ << " - start/stop" << std::endl
+ << " - reset: reset the t0 counter. " << std::endl
+ << " - verbose/normal/mute: change output model : detailed/default (normal messages)/only errors" << std::endl;
+ }
+ else if( cmdLine == "sot" )
+ {
+ cmdArgs>>std::ws; if( cmdArgs.good() )
+ {
+ std::string sotname; cmdArgs >> sotname;
+ Sot * sotptr = dynamic_cast< Sot* > (& (g_pool.getEntity( sotname )));
+ if(! sotptr ) os << "! Entity <" << sotname << "> does not exist "
+ << "(see you later, next patient please!"
+ << std::endl;
+ else setSotRef( sotptr );
+ }
+ else
+ {
+ if( sotPtr ) os << "sot = " << sotPtr->getName() << std::endl;
+ else os << "No sot specified yet. " << std::endl;
+ }
+ }
+ else if( cmdLine == "addEvent" )
+ {
+ std::string eventType;
+ unsigned int timeref=0;
+
+
+ cmdArgs>>std::ws; if(! cmdArgs.good() )
+ { if (!noOutput) {os <<"! addEvent: <eventType> <time> <args>" <<std::endl;} return; }
+ cmdArgs >> eventType >> std::ws;
+
+ if(! cmdArgs.good() )
+ { if (!noOutput) {os <<"! addEvent: <eventType> <time> <args>" <<std::endl;} return; }
+ cmdArgs >> timeref ;
+
+ sotEventTaskBased* event;
+ if( eventType=="add" ) event = new sotEventAddATask();
+ else if( eventType=="rm" ) event = new sotEventRemoveATask();
+ else
+ {
+ os<<"! Event type <" <<eventType <<"> is not recognized." <<std::endl;
+ return;
+ }
+ event->init( cmdArgs );
+ addTask( event,timeref );
+ }
+ else if( cmdLine == "rmEvent" )
+ {
+ std::string eventType;
+ unsigned int timeref=0;
+
+
+ cmdArgs>>std::ws; if(! cmdArgs.good() )
+ { if (!noOutput) {os <<"! rmEvent: <eventType> <time> <args>" <<std::endl;} return; }
+ cmdArgs >> eventType >> std::ws;
+
+ if(! cmdArgs.good() )
+ { if (!noOutput) {os <<"! rmEvent: <eventType> <time> <args>" <<std::endl;} return; }
+ cmdArgs >> timeref ;
+
+ //the type of event
+ int event;
+ if( eventType=="add" ) event = sotEventAbstract::EVENT_ADD;
+ else if( eventType=="rm" ) event = sotEventAbstract::EVENT_RM;
+ else
+ {
+ os<<"! Event type <" <<eventType <<"> is not recognized." <<std::endl;
+ return;
+ }
+
+ //the name of the event
+ std::string name;
+ cmdArgs >> name;
+ rmTask( event, name, timeref );
+ }
+ else if( cmdLine == "addCmd" )
+ {
+ unsigned int timeref=0; cmdArgs >> std::ws;
+ sotDEBUG(15)<<std::endl;
+ if(! cmdArgs.good() )
+ { if (!noOutput) {os <<"! addEvent: <eventType> <time> <args>" <<std::endl;} return; }
+ sotDEBUG(15)<<std::endl;
+ cmdArgs >> timeref ;
+ sotDEBUG(15)<<std::endl;
+ sotEventCmd * eventcmd = new sotEventCmd();
+ sotDEBUG(15)<<std::endl;
+ eventcmd->init( cmdArgs );
+ addTask( eventcmd,timeref );
+ }
+ else if( cmdLine == "rmCmd" )
+ {
+ unsigned int timeref=0; cmdArgs >> std::ws;
+ if(! cmdArgs.good() )
+ { if (!noOutput) {os <<"! addEvent: <eventType> <time> <args>" <<std::endl;} return; }
+ cmdArgs >> timeref ;
+ int index = sotEventAbstract::EVENT_CMD;
+
+ //get the name of the command
+ std::string name;
+ cmdArgs >> name;
+ rmTask( index, name,timeref );
+ }
+ else if( "clear"==cmdLine ) { clearAll(); }
+ else if( "reset"==cmdLine ) { timeInit = -1; }
+ else if( "start"==cmdLine ) { playMode = true; }
+ else if( "stop"==cmdLine ) { playMode = false; }
+ else if( "verbose"==cmdLine ) { outputStreamPtr = &os; }
+ else if( "mute"==cmdLine ) { outputStreamPtr = NULL; noOutput=true;}
+ else if( "normal"==cmdLine ) { noOutput=false; }
+ else { Entity::commandLine( cmdLine,cmdArgs,os); }
+}
+
--
GitLab