diff --git a/CMakeLists.txt b/CMakeLists.txt
index c2246122a4d7fa463f000d2195abb6b05c616d98..135a89a88a3d5513809d35bbf8cd5ec20c414b0f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -56,8 +56,24 @@ SET(libs
LIST(APPEND LOGGING_WATCHED_TARGETS ${libs})
+SET(headers
+ commands-helper.h
+ entity-helper.h
+ signal-helper.h
+
+ mal-to-eigen.h
+ stack-template.h
+ stack-template.t.cpp
+
+ controller-pd.h
+ task-dyn-pd.h
+ dynamic-integrator.h
+ solver-op-space.h
+ zmp-estimator.h
+ pseudo-robot-dynamic.h
+)
+
# Add subdirectories.
-ADD_SUBDIRECTORY(include)
ADD_SUBDIRECTORY(src)
ADD_SUBDIRECTORY(unitTesting)
diff --git a/include/sot-dyninv/pseudo-robot-dynamic.h b/include/sot-dyninv/pseudo-robot-dynamic.h
deleted file mode 100644
index a4ede95c58acca53937dec95336afb96eedbdad6..0000000000000000000000000000000000000000
--- a/include/sot-dyninv/pseudo-robot-dynamic.h
+++ /dev/null
@@ -1,114 +0,0 @@
-/*
- * Copyright 2011, Nicolas Mansard, LAAS-CNRS
- *
- * This file is part of sot-dyninv.
- * sot-dyninv is free software: you can redistribute it and/or
- * modify it under the terms of the GNU Lesser General Public License
- * as published by the Free Software Foundation, either version 3 of
- * the License, or (at your option) any later version.
- * sot-dyninv is distributed in the hope that it will be
- * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
- * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU Lesser General Public License for more details. You should
- * have received a copy of the GNU Lesser General Public License along
- * with sot-dyninv. If not, see <http://www.gnu.org/licenses/>.
- */
-
-#ifndef __sot_dyninv_PseudoRobotDynamic_H__
-#define __sot_dyninv_PseudoRobotDynamic_H__
-/* --------------------------------------------------------------------- */
-/* --- API ------------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-#if defined (WIN32)
-# if defined (pseudo_robot_dynamic_EXPORTS)
-# define SOTPSEUDOROBOTDYNAMIC_EXPORT __declspec(dllexport)
-# else
-# define SOTPSEUDOROBOTDYNAMIC_EXPORT __declspec(dllimport)
-# endif
-#else
-# define SOTPSEUDOROBOTDYNAMIC_EXPORT
-#endif
-
-/* --------------------------------------------------------------------- */
-/* --- INCLUDE --------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-
-/* SOT */
-#include <sot-dyninv/signal-helper.h>
-#include <sot-dyninv/entity-helper.h>
-#include <sot-dyninv/dynamic-integrator.h>
-
-namespace sot {
- namespace dyninv {
-
- /* --------------------------------------------------------------------- */
- /* --- CLASS ----------------------------------------------------------- */
- /* --------------------------------------------------------------------- */
-
- /* Inside this entity is an integrator, taking position, velocity and
- * acceleration at time t, and computing pos and vel at time t+1. Around
- * this integrator, a wrapper is made to make the stuff take the place of
- * the OpenHRP kinematic entity. it thus take a "control" signal, to triger
- * the computations, and possesses all the functionnalities of the OpenHRP
- * entity. Plus, it has the signals to feed the OpenHRP entity that
- * wrappes the simulator. All the computations are triggered by computing
- * the "qdot" signal.
- */
-
- class SOTPSEUDOROBOTDYNAMIC_EXPORT PseudoRobotDynamic
- :public DynamicIntegrator
- ,public ::dynamicgraph::EntityHelper<PseudoRobotDynamic>
- {
-
- public: /* --- CONSTRUCTOR ---- */
-
- PseudoRobotDynamic( const std::string & name );
-
- public: /* --- ENTITY INHERITANCE --- */
-
- static const std::string CLASS_NAME;
- virtual void display( std::ostream& os ) const;
- virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
-
- virtual void commandLine( const std::string& cmdLine,
- std::istringstream& cmdArgs,
- std::ostream& os );
-
- typedef ::dynamicgraph::EntityHelper<PseudoRobotDynamic>::EntityClassName
- EntityClassName;
-
- public: /* --- SIGNALS --- */
-
- DECLARE_SIGNAL_IN( control,ml::Vector );
- DECLARE_SIGNAL_OUT( qdot,ml::Vector );
-
- DECLARE_SIGNAL(rotation,OUT,ml::Vector);
- DECLARE_SIGNAL(translation,OUT,ml::Vector);
- //sotSignal< ml::Vector,int > rotationSOUT;
- //sotSignal< ml::Vector,int > translationSOUT;
- ::dynamicgraph::SignalPtr< ml::Vector,int > stateSOUT;
-
- public: /* --- SIGNALS --- */
-
- void replaceSimulatorEntity( const std::string& formerName,
- const bool& plug = false );
- void setRoot( const ml::Matrix & M );
-
- public: /* --- COMMAND --- */
- template< typename T1 >
- void forwardVoidCommandToSimu( const std::string& cmdName,
- const T1& arg1 );
- void addForward( const std::string& cmdName );
-
- private:
- ::dynamicgraph::Entity* formerOpenHRP;
-
- }; // class PseudoRobotDynamic
- } // namespace dyninv
-} // namespace sot
-
-
-
-#endif // #ifndef __sot_dyninv_PseudoRobotDynamic_H__
diff --git a/include/sot-dyninv/solver-op-space.h b/include/sot-dyninv/solver-op-space.h
deleted file mode 100644
index 8a5c9b0e00fd7dc3e0a85b6285b500b1f5dd7bdf..0000000000000000000000000000000000000000
--- a/include/sot-dyninv/solver-op-space.h
+++ /dev/null
@@ -1,158 +0,0 @@
-/*
- * Copyright 2011, Nicolas Mansard, LAAS-CNRS
- *
- * This file is part of sot-dyninv.
- * sot-dyninv is free software: you can redistribute it and/or
- * modify it under the terms of the GNU Lesser General Public License
- * as published by the Free Software Foundation, either version 3 of
- * the License, or (at your option) any later version.
- * sot-dyninv is distributed in the hope that it will be
- * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
- * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU Lesser General Public License for more details. You should
- * have received a copy of the GNU Lesser General Public License along
- * with sot-dyninv. If not, see <http://www.gnu.org/licenses/>.
- */
-
-#ifndef __sot_dyninv_SolverOpSpace_H__
-#define __sot_dyninv_SolverOpSpace_H__
-/* --------------------------------------------------------------------- */
-/* --- API ------------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-#if defined (WIN32)
-# if defined (solver_op_space_EXPORTS)
-# define SOTSOLVEROPSPACE_EXPORT __declspec(dllexport)
-# else
-# define SOTSOLVEROPSPACE_EXPORT __declspec(dllimport)
-# endif
-#else
-# define SOTSOLVEROPSPACE_EXPORT
-#endif
-
-/* --------------------------------------------------------------------- */
-/* --- INCLUDE --------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-
-/* SOT */
-#include <sot-dyninv/signal-helper.h>
-#include <sot-dyninv/entity-helper.h>
-#include <sot-dyninv/stack-template.h>
-#include <sot-dyninv/task-dyn-pd.h>
-#include <soth/HCOD.hpp>
-
-namespace sot {
- namespace dyninv {
-
- /* --------------------------------------------------------------------- */
- /* --- CLASS ----------------------------------------------------------- */
- /* --------------------------------------------------------------------- */
-
- class SOTSOLVEROPSPACE_EXPORT SolverOpSpace
- :public ::dynamicgraph::Entity
- ,public ::dynamicgraph::EntityHelper<SolverOpSpace>
- ,public sot::Stack< TaskDynPD >
- {
-
- public: /* --- CONSTRUCTOR ---- */
-
- SolverOpSpace( const std::string & name );
-
- public: /* --- STACK INHERITANCE --- */
-
- typedef sot::Stack<TaskDynPD> stack_t;
- using stack_t::TaskDependancyList_t;
- using stack_t::StackIterator_t;
- using stack_t::StackConstIterator_t;
- using stack_t::stack;
-
- virtual TaskDependancyList_t getTaskDependancyList( const TaskDynPD& task );
- virtual void addDependancy( const TaskDependancyList_t& depList );
- virtual void removeDependancy( const TaskDependancyList_t& depList );
- virtual void resetReady( void );
-
- public: /* --- ENTITY INHERITANCE --- */
-
- static const std::string CLASS_NAME;
- virtual void display( std::ostream& os ) const;
- virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
-
- virtual void commandLine( const std::string& cmdLine,
- std::istringstream& cmdArgs,
- std::ostream& os );
-
- public: /* --- SIGNALS --- */
-
- DECLARE_SIGNAL_IN(matrixInertia,ml::Matrix);
- DECLARE_SIGNAL_IN(velocity,ml::Vector);
- DECLARE_SIGNAL_IN(dyndrift,ml::Vector);
- DECLARE_SIGNAL_IN(damping,double);
- DECLARE_SIGNAL_IN(breakFactor,double);
-
- DECLARE_SIGNAL_OUT(control,ml::Vector);
-
- DECLARE_SIGNAL(zmp,OUT,ml::Vector);
- DECLARE_SIGNAL(acceleration,OUT,ml::Vector);
-
- private: /* --- CONTACT POINTS --- */
-
- typedef boost::shared_ptr<dynamicgraph::SignalPtr<ml::Matrix,int> > matrixSINPtr;
- typedef boost::shared_ptr<dynamicgraph::SignalPtr<ml::Vector,int> > vectorSINPtr;
- typedef boost::shared_ptr<dynamicgraph::Signal<ml::Vector,int> > vectorSOUTPtr;
- struct Contact
- {
- matrixSINPtr jacobianSIN;
- matrixSINPtr JdotSIN;
- matrixSINPtr supportSIN;
- vectorSINPtr correctorSIN;
- vectorSOUTPtr forceSOUT,fnSOUT;
- std::pair<int,int> range;
- };
- typedef std::map< std::string,Contact > contacts_t;
- contacts_t contactMap;
-
- public:
- void addContact( const std::string & name,
- dynamicgraph::Signal<ml::Matrix,int> * jacobianSignal,
- dynamicgraph::Signal<ml::Matrix,int> * JdotSignal,
- dynamicgraph::Signal<ml::Vector,int> * corrSignal,
- dynamicgraph::Signal<ml::Matrix,int> * contactPointsSignal );
- void addContactFromTask( const std::string & taskName, const std::string & contactName );
- void removeContact( const std::string & name );
- void dispContacts( std::ostream& os ) const;
-
-
- public: /* --- COMMANDS --- */
- void debugOnce( void );
-
-
- private: /* --- INTERNAL COMPUTATIONS --- */
- void refreshTaskTime( int time );
- void initialResizeSolver( void );
- void resizeSolver( void );
-
- private:
- int nbParam, nq,ntau;
- int nbContactBodies,nbContactPoints,nfs;
-
- typedef boost::shared_ptr<soth::HCOD> hcod_ptr_t;
- hcod_ptr_t hsolver;
-
- Eigen::MatrixXd Cdyn,Ccontact,Czmp,Czero;
- soth::VectorBound bdyn,bcontact,bzmp,bzero;
- std::vector< Eigen::MatrixXd > Ctasks;
- std::vector< soth::VectorBound > btasks;
-
- Eigen::VectorXd solution;
-
- static const int nbPoint = 4;
-
- }; // class SolverOpSpace
-
- } // namespace dyninv
-} // namespace sot
-
-
-
-#endif // #ifndef __sot_dyninv_SolverOpSpace_H__
diff --git a/include/sot-dyninv/stack-template.h b/include/sot-dyninv/stack-template.h
deleted file mode 100644
index 4edb9254ef9adba25b54102e8bec08c1f1af51c4..0000000000000000000000000000000000000000
--- a/include/sot-dyninv/stack-template.h
+++ /dev/null
@@ -1,192 +0,0 @@
-/*
- * Copyright 2011, Nicolas Mansard, LAAS-CNRS
- *
- * This file is part of sot-dyninv.
- * sot-dyninv is free software: you can redistribute it and/or
- * modify it under the terms of the GNU Lesser General Public License
- * as published by the Free Software Foundation, either version 3 of
- * the License, or (at your option) any later version.
- * sot-dyninv is distributed in the hope that it will be
- * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
- * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU Lesser General Public License for more details. You should
- * have received a copy of the GNU Lesser General Public License along
- * with sot-dyninv. If not, see <http://www.gnu.org/licenses/>.
- */
-
-#ifndef __sot_dyninv_StackTemplate_H__
-#define __sot_dyninv_StackTemplate_H__
-/* --------------------------------------------------------------------- */
-/* --- API ------------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-#if defined (WIN32)
-# if defined (dynamic_interpretor_EXPORTS)
-# define SOTSTACKTEMPLATE_EXPORT __declspec(dllexport)
-# else
-# define SOTSTACKTEMPLATE_EXPORT __declspec(dllimport)
-# endif
-#else
-# define SOTSTACKTEMPLATE_EXPORT
-#endif
-
-/* --------------------------------------------------------------------- */
-/* --- INCLUDE --------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-#include <sot-dyninv/signal-helper.h>
-
-namespace sot {
-
- /* --------------------------------------------------------------------- */
- /* --- CLASS ----------------------------------------------------------- */
- /* --------------------------------------------------------------------- */
-
- namespace dg = dynamicgraph;
-
- template< typename TaskGeneric >
- class SOTSTACKTEMPLATE_EXPORT Stack
- {
-
- protected:
-
- typedef std::vector<TaskGeneric*> Stack_t;
- typedef typename Stack_t::iterator StackIterator_t;
- typedef typename Stack_t::const_iterator StackConstIterator_t;
-
- /*! \brief List of task (controllers) managed by the stack of tasks. */
- Stack_t stack;
-
- /*! \brief Number of joints to be used to compute the control law. */
- int nbDofs;
-
- public:
-
- /*! \brief Number of joints by default. */
- static const unsigned int NB_JOINTS_DEFAULT; // = 48;
-
- public: /* --- Constructors --- */
-
- Stack( void );
- ~Stack( void ) { /* TODO!! */ }
-
- public: /* --- Modifiors --- */
-
- /*! \name Methods to handle the stack.
- @{
- */
-
- /*! \brief Push the task in the stack.
- It has a lowest priority than the previous ones.
- If this is the first task, then it has the highest
- priority. */
- virtual void push( TaskGeneric& task );
- /*! \brief Pop the task from the stack.
- This method removes the task with the smallest
- priority in the task. The other are projected
- in the null-space of their predecessors. */
- virtual TaskGeneric& pop( void );
-
- /*! \brief This method allows to know if a task exists or not */
- virtual bool exist( const TaskGeneric& task );
-
- /*! \brief Remove a task regardless to its position in the stack.
- It removes also the signals connected to the output signal of this stack.*/
- virtual void remove( const TaskGeneric& task );
-
- /*! \brief This method makes the task to swap with the task having the
- immediate superior priority. */
- virtual void up( const TaskGeneric& task );
-
- /*! \brief This method makes the task to swap with the task having the
- immediate inferior priority. */
- virtual void down( const TaskGeneric& task );
-
- /*! \brief Remove all the tasks from the stack. */
- virtual void clear( void );
-
- /*! Change the number of DOF, ie the field nbDofs. */
- virtual void defineNbDof( const int& nbDof );
-
- /*! @} */
-
- dg::SignalBase<int>* tatata;
-
- int titit;
- typedef std::list< const dg::SignalBase<int>* > TaskDependancyList_t;
-
- int totol;
-
- /*! \brief Return the signal to be added/removed from the dependancy
- * list of the control signal. */
- virtual TaskDependancyList_t getTaskDependancyList( const TaskGeneric& task ) = 0;
- virtual void addDependancy( const TaskDependancyList_t& depList ) = 0;
- virtual void removeDependancy( const TaskDependancyList_t& depList ) = 0;
- virtual void resetReady( void ) = 0;
-
- public: /* --- DISPLAY --- */
-
- /*! \name Methods to display the stack of tasks.
- @{
- */
-
- /*! Display the stack of tasks in text mode as a tree. */
- virtual void display( std::ostream& os ) const;
- /*! \brief Write the priority between tasks in the outstream os. */
- virtual std::ostream &
- writeGraph(const std::string & name, std::ostream & os) const;
-
- /*! @} */
-
- public: /* --- COMMANDS --- */
-
- /*! @{ */
-
- /* To use the macro, the typedef sot::Stack<TaskSpec> stack_t has to be set,
- * as well as the EntityClassName. */
- //#define ADD_COMMANDS_FOR_THE_STACK /// Code in the .t.cpp
-
-
- /*! \brief This method deals with the command line.
- The command given in argument is send to the stack of tasks by the shell.
- The command understood by sot are:
- <ul>
- <li> Tasks
- <ul>
- <li> push <task> : Push a task in the stack (FILO).
- <li> pop : Remove the task push in the stack.
- <li> down <task> : Make the task have a higher priority, i.e.
- swap with the task immediatly superior in priority.
- <li> up <task> : Make the task have a lowest priority, i.e.
- swap with the task immediatly inferior in priority.
- <li> rm <task> : Remove the task from the stack.
- </ul>
- */
- virtual bool stackCommandLine( const std::string& cmdLine,
- std::istringstream& cmdArgs,
- std::ostream& os );
-
- void pushByTaskName( const std::string& taskName );
- void removeByTaskName( const std::string& taskName );
- void upByTaskName( const std::string& taskName );
- void downByTaskName( const std::string& taskName );
-
- /*! @} */
-
- }; // class StackTemplate
-
-
- template< typename TaskGeneric >
- std::ostream&
- operator<< ( std::ostream& os,const Stack<TaskGeneric>& sot )
- {
- sot.display(os);
- return os;
- }
-
-
-} // namespace sot
-
-#include <sot-dyninv/stack-template.t.cpp>
-
-#endif // #ifndef __sot_dyninv_StackTemplate_H__
diff --git a/include/sot-dyninv/stack-template.t.cpp b/include/sot-dyninv/stack-template.t.cpp
deleted file mode 100644
index 6aca8c6d6d9cc207387c172b9bac47d21571b181..0000000000000000000000000000000000000000
--- a/include/sot-dyninv/stack-template.t.cpp
+++ /dev/null
@@ -1,368 +0,0 @@
-/*
- * Copyright 2011, Nicolas Mansard, LAAS-CNRS
- *
- * This file is part of sot-dyninv.
- * sot-dyninv is free software: you can redistribute it and/or
- * modify it under the terms of the GNU Lesser General Public License
- * as published by the Free Software Foundation, either version 3 of
- * the License, or (at your option) any later version.
- * sot-dyninv is distributed in the hope that it will be
- * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
- * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU Lesser General Public License for more details. You should
- * have received a copy of the GNU Lesser General Public License along
- * with sot-dyninv. If not, see <http://www.gnu.org/licenses/>.
- */
-
-#ifndef __sot_dyninv_StackTemplate_TCC__
-#define __sot_dyninv_StackTemplate_TCC__
-
-#include <dynamic-graph/pool.h>
-#include <sot-core/debug.h>
-#include <sot-core/task-abstract.h>
-
-namespace sot
-{
-
- /* --------------------------------------------------------------------- */
- /* --- CLASS ----------------------------------------------------------- */
- /* --------------------------------------------------------------------- */
-
- template< typename TaskGeneric >
- const unsigned int Stack<TaskGeneric>::NB_JOINTS_DEFAULT = 46;
-
- /* --------------------------------------------------------------------- */
- /* --- CONSTRUCTION ---------------------------------------------------- */
- /* --------------------------------------------------------------------- */
-
- template< typename TaskGeneric >
- Stack<TaskGeneric>::
- Stack( void )
- :stack()
- ,nbDofs( NB_JOINTS_DEFAULT )
- {
- }
-
- /* --------------------------------------------------------------------- */
- /* --- STACK MANIPULATION --- */
- /* --------------------------------------------------------------------- */
-
- template< typename TaskGeneric >
- void Stack<TaskGeneric>::
- push( TaskGeneric& task )
- {
- stack.push_back( &task );
- addDependancy( getTaskDependancyList( task ) );
- resetReady();
- }
- template< typename TaskGeneric >
- TaskGeneric& Stack<TaskGeneric>::
- pop( void )
- {
- TaskGeneric* res = stack.back();
- stack.pop_back();
- removeDependancy( getTaskDependancyList( *res ) );
- resetReady();
- return *res;
- }
- template< typename TaskGeneric >
- bool Stack<TaskGeneric>::
- exist( const TaskGeneric& key )
- {
- StackIterator_t it;
- for ( it=stack.begin();stack.end()!=it;++it )
- {
- if( *it == &key ) { return true; }
- }
- return false;
- }
- template< typename TaskGeneric >
- void Stack<TaskGeneric>::
- remove( const TaskGeneric& key )
- {
- bool find =false;
- StackIterator_t it;
- for ( it=stack.begin();stack.end()!=it;++it )
- {
- if( *it == &key ) { find=true; break; }
- }
- if(! find ){ return; }
-
- stack.erase( it );
- removeDependancy( getTaskDependancyList( key ) );
- resetReady();
- }
-
- template< typename TaskGeneric >
- void Stack<TaskGeneric>::
- up( const TaskGeneric& key )
- {
- bool find =false;
- StackIterator_t it;
- for ( it=stack.begin();stack.end()!=it;++it )
- {
- if( *it == &key ) { find=true; break; }
- }
- if( stack.begin()==it ) { return; }
- if(! find ){ return; }
-
- StackIterator_t pos=it; pos--;
- TaskGeneric * task = *it;
- stack.erase( it );
- stack.insert( pos,task );
- resetReady();
- }
- template< typename TaskGeneric >
- void Stack<TaskGeneric>::
- down( const TaskGeneric& key )
- {
- bool find =false;
- StackIterator_t it;
- for ( it=stack.begin();stack.end()!=it;++it )
- {
- if( *it == &key ) { find=true; break; }
- }
- if( stack.end()==it ) { return; }
- if(! find ){ return; }
-
- StackIterator_t pos=it; pos++;
- TaskGeneric* task=*it;
- stack.erase( it );
- if( stack.end()==pos ){ stack.push_back(task); }
- else
- {
- pos++;
- stack.insert( pos,task );
- }
- resetReady();
- }
-
- template< typename TaskGeneric >
- void Stack<TaskGeneric>::
- clear( void )
- {
- for ( StackIterator_t it=stack.begin();stack.end()!=it;++it )
- {
- removeDependancy( getTaskDependancyList( **it ) );
- }
- stack.clear();
- resetReady();
- }
-
- template< typename TaskGeneric >
- void Stack<TaskGeneric>::
- defineNbDof( const int& nbDof )
- {
- assert(nbDof >= 0);
- nbDofs = nbDof;
- resetReady();
- }
-
- /* --------------------------------------------------------------------- */
- /* --- DISPLAY --------------------------------------------------------- */
- /* --------------------------------------------------------------------- */
-
- template< typename TaskGeneric >
- void Stack<TaskGeneric>::
- display( std::ostream& os ) const
- {
-
- os << "+-----------------"<<std::endl<<"+ SOT "
- << std::endl<< "+-----------------"<<std::endl;
- for ( StackConstIterator_t it=this->stack.begin();
- this->stack.end()!=it;++it )
- //BOOST_FOREACH( TaskGeneric* task, stack )
- {
- os << "| " << (*it)->getName() <<std::endl;
- }
- }
-
-
- /* --------------------------------------------------------------------- */
- /* --- COMMAND --------------------------------------------------------- */
- /* --------------------------------------------------------------------- */
-
-#define ACT_BY_TASK_NAME(ACT) \
- template< typename TaskGeneric > \
- void Stack<TaskGeneric>:: \
- ACT##ByTaskName( const std::string& tname ) \
- { \
- dg::Entity & taska = dg::g_pool.getEntity( tname ); \
- TaskGeneric & task = dynamic_cast<TaskGeneric&>( taska ); \
- ACT(task); \
- }
-
- ACT_BY_TASK_NAME(push)
- ACT_BY_TASK_NAME(remove)
- ACT_BY_TASK_NAME(up)
- ACT_BY_TASK_NAME(down)
-
- template< typename TaskGeneric >
- bool Stack<TaskGeneric>::
- stackCommandLine( const std::string& cmdLine,
- std::istringstream& cmdArgs,
- std::ostream& os )
- {
- sotDEBUGIN(15);
-
- if( cmdLine == "help")
- {
- os << "Stack of Tasks: "<<std::endl
- << " - push <task>"<< std::endl
- << " - pop"<< std::endl
- << " - down <task>"<< std::endl
- << " - up <task>"<< std::endl
- << " - rm <task>"<< std::endl
- << " - clear"<< std::endl
- << " - display"<< std::endl
-
- << " - nbDofs <nb> "<<std::endl;
- }
- else if( cmdLine == "clear")
- {
- clear();
- }
- else if( cmdLine == "push")
- {
- std::string tname; cmdArgs >> tname;
- pushByTaskName(tname);
- }
- else if( cmdLine == "up")
- {
- std::string tname; cmdArgs >> tname;
- upByTaskName(tname);
- }
- else if( cmdLine == "down")
- {
- std::string tname; cmdArgs >> tname;
- downByTaskName(tname);
- }
- else if( cmdLine == "rm")
- {
- std::string tname; cmdArgs >> tname;
- removeByTaskName(tname);
- }
- else if( cmdLine == "pop")
- {
- TaskGeneric& task = pop();
- os << "Remove : "<< task << std::endl;
- }
-
- else if( cmdLine == "nbDofs")
- {
- cmdArgs>>std::ws;
- if( cmdArgs.good() )
- {
- unsigned int nbDof;
- cmdArgs >> nbDof;
- defineNbDof(nbDof);
- } else { os << "nbDofs = "<< nbDofs <<std::endl; }
- }
- else if( cmdLine == "display")
- {
- display(os);
- }
- else return false;
-
- return true;
-
- sotDEBUGOUT(15);
- }
-
- template< typename TaskGeneric >
- std::ostream& Stack<TaskGeneric>::
- writeGraph( const std::string & name, std::ostream& os ) const
- {
- StackConstIterator_t iter;
- for( iter = stack.begin(); iter!=stack.end();++iter )
- {
- const TaskGeneric & task = **iter;
- StackConstIterator_t nextiter =iter;
- nextiter++;
-
- if (nextiter!=stack.end())
- {
- TaskGeneric & nexttask = **nextiter;
- os << "\t\t\t" << task.getName() << " -> " << nexttask.getName()
- << " [color=red]" << std::endl;
- }
- }
-
- os << "\t\tsubgraph cluster_Tasks {" <<std::endl
- << "\t\t\tsubgraph cluster_" << name << " {" << std::endl
- << "\t\t\t\tcolor=lightsteelblue1; label=\"" << name
- << "\"; style=filled;" << std::endl;
-
- for( iter = stack.begin(); iter!=stack.end();++iter )
- {
- const TaskGeneric & task = **iter;
- os << "\t\t\t\t" << task.getName()
- << " [ label = \"" << task.getName() << "\" ," << std::endl
- << "\t\t\t\t fontcolor = black, color = black, "
- << "fillcolor = magenta, style=filled, shape=box ]" << std::endl;
-
- }
- os << "\t\t\t}" << std::endl;
- os << "\t\t\t}" <<std::endl;
- return os;
- }
-
-
-
-
- /* To use the macro, the typedef sot::Stack<TaskSpec> stack_t has to be set,
- * as well as the EntityClassName. */
-#define ADD_COMMANDS_FOR_THE_STACK \
- addCommand("dispStack", \
- makeCommandVerbose(*this,&stack_t::display, \
- docCommandVerbose("Guess what?"))); \
- addCommand("up", \
- makeCommandVoid1(*this, \
- (void (EntityClassName::*)(const std::string&)) \
- &stack_t::upByTaskName, \
- docCommandVoid1("Up the named task.", \
- "string (task name)"))); \
- addCommand("down", \
- makeCommandVoid1(*this, \
- (void (EntityClassName::*)(const std::string&)) \
- &stack_t::downByTaskName, \
- docCommandVoid1("Down the named task.", \
- "string (task name)"))); \
- addCommand("push", \
- makeCommandVoid1(*this, \
- (void (EntityClassName::*)(const std::string&)) \
- &stack_t::pushByTaskName, \
- docCommandVoid1("Push the named task.", \
- "string (task name)"))); \
- addCommand("rm", \
- makeCommandVoid1(*this, \
- (void (EntityClassName::*)(const std::string&)) \
- &stack_t::removeByTaskName, \
- docCommandVoid1("Remove the named task.", \
- "string (task name)"))); \
- addCommand("clear", \
- makeCommandVoid0(*this, \
- (void (EntityClassName::*)(void)) \
- &stack_t::clear, \
- docCommandVoid0("Clear the stack from all task."))); \
- addCommand("pop", \
- makeCommandVoid0(*this, \
- (void (EntityClassName::*)(void)) \
- &stack_t::pop, \
- docCommandVoid0("Remove the last (lowest) task of the stack."))); \
- \
- addCommand("setSize", \
- makeCommandVoid1(*this, \
- (void (EntityClassName::*)(const int&)) \
- &stack_t::defineNbDof, \
- docCommandVoid1("??? TODO", \
- "int"))); \
- addCommand("getSize", \
- makeDirectGetter(*this,&nbDofs, \
- docDirectGetter("size","int")))
-
-
-
-} // namespace sot
-
-#endif // #ifndef __sot_dyninv_StackTemplate_TCC__
diff --git a/python/dyndebug.py b/python/dyndebug.py
index 200ce40e3316db6490fee7e9bae13d9c2dd77f73..343fdbfaeefa0f920f9ffa7ea9815da8f1a8ddb0 100644
--- a/python/dyndebug.py
+++ b/python/dyndebug.py
@@ -3,8 +3,8 @@ from dynamic_graph.sot.core import *
from dynamic_graph.sot.core.math_small_entities import Derivator_of_Matrix
from dynamic_graph.sot.dynamics import *
from dynamic_graph.sot.dyninv import *
-import script_shortcuts
-from matlab import matlab
+import dynamic_graph.script_shortcuts
+from dynamic_graph.matlab import matlab
# --- Dynamic parameters ---
hrp2_14_pkgdatarootdir = "/home/nmansard/compil/devgiri/hpp2/share/hrp2_14"
@@ -26,9 +26,9 @@ robot.resize(robotDim)
robot.set( ( 0.0274106623863, 0.143843868989, 0.646921914726, 0.00221064938462, 0.101393756965, 1.36729741242e-05, -0.00911630330837, -0.0914099637938, -0.471978743283, 0.840380192617, -0.470232799053, 0.0896624078591, 0.00950781802257, 0.0911102868041, -0.469450351848, 0.835307995386, -0.467686190904, -0.0938029466367, -8.75646582964e-05, 0.00326431877374, -7.83380820086e-06, 0.000194742578013, 0.258370257314, -0.175099102142, -6.1173425555e-05, -0.524953548768, 3.1825099713e-06, -0.000257600047423, -3.41210481921e-06, 0.258367271628, 0.174322097546, -8.89023954997e-05, -0.524983690846, -3.46102941488e-07, -0.000265401439772, 1.00498481453e-06 ) )
-robot_old=robot
+robot_wrapper=robot
robot = PseudoRobotDynamic("dynint")
-robot.replace("robot",True)
+robot.replace(robot_wrapper.name,True)
dt=1e-3
robot.dt.value = dt
@@ -304,16 +304,16 @@ tr.add('sot.control','')
# --- RUN ------------------------------------------------
def inc():
- robot_old.increment(dt)
- tr.triger.recompute( robot_old.control.time )
+ robot_wrapper.increment(dt)
+ tr.triger.recompute( robot_wrapper.control.time )
featureComDes.errorIN.value = (0.06,0,0.8)
sot.push('taskCom')
-for i in range(2100):
- inc()
+#for i in range(2100):
+# inc()
-tr.dump()
+#tr.dump()
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 330bf7d5fd34f28d9b4e409e50d7ffc342b31da7..51ed9509b9b767a053936d0bb313956251b5869b 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -20,7 +20,20 @@ IF(CMAKE_BUILD_TYPE STREQUAL "DEBUG")
ADD_DEFINITIONS(-DDEBUG=2)
ENDIF(CMAKE_BUILD_TYPE STREQUAL "DEBUG")
+# --- HEADERS
+FOREACH(header ${headers})
+ get_filename_component(headerName ${header} NAME)
+ set(src_path ${${PROJECT_NAME}_SOURCE_DIR}/src/${header})
+ set(bin_path ${${PROJECT_NAME}_BINARY_DIR}/include/${PROJECT_NAME}/${headerName})
+ set(install_path ${CMAKE_INSTALL_PREFIX}/include/${PROJECT_NAME})
+ execute_process(COMMAND ${CMAKE_COMMAND} -E remove ${bin_path})
+ execute_process(COMMAND ${CMAKE_COMMAND} -E create_symlink ${src_path} ${bin_path} )
+ install(FILES ${src_path} DESTINATION ${install_path}
+ PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE)
+ENDFOREACH(header)
+INCLUDE_DIRECTORIES(${CMAKE_BINARY_DIR}/include)
+# --- COMPILATION OF PLUGINS
FOREACH(lib ${libs})
ADD_LIBRARY(${lib} SHARED ${lib}.cpp)
diff --git a/include/sot-dyninv/commands-helper.h b/src/commands-helper.h
similarity index 60%
rename from include/sot-dyninv/commands-helper.h
rename to src/commands-helper.h
index 1ee3adc15ed75b2b65268a2e2e78989e6d6eb2da..b595fdf90636565c2bbeff92841537f1ac46a4f8 100644
--- a/include/sot-dyninv/commands-helper.h
+++ b/src/commands-helper.h
@@ -26,22 +26,24 @@
#include <boost/function.hpp>
/* --- HELPER --------------------------------------------------------------- */
-namespace sot {
- namespace dyninv {
- using ::dynamicgraph::command::makeDirectGetter;
- using ::dynamicgraph::command::docDirectGetter;
- using ::dynamicgraph::command::makeDirectSetter;
- using ::dynamicgraph::command::docDirectSetter;
- using ::dynamicgraph::command::makeCommandVoid0;
- using ::dynamicgraph::command::docCommandVoid0;
- using ::dynamicgraph::command::makeCommandVoid1;
- using ::dynamicgraph::command::docCommandVoid1;
- using ::dynamicgraph::command::makeCommandVoid2;
- using ::dynamicgraph::command::docCommandVoid2;
- using ::dynamicgraph::command::makeCommandVerbose;
- using ::dynamicgraph::command::docCommandVerbose;
- } // namespace dyninv
-} // namespace sot
+namespace dynamicgraph {
+ namespace sot {
+ namespace dyninv {
+ using ::dynamicgraph::command::makeDirectGetter;
+ using ::dynamicgraph::command::docDirectGetter;
+ using ::dynamicgraph::command::makeDirectSetter;
+ using ::dynamicgraph::command::docDirectSetter;
+ using ::dynamicgraph::command::makeCommandVoid0;
+ using ::dynamicgraph::command::docCommandVoid0;
+ using ::dynamicgraph::command::makeCommandVoid1;
+ using ::dynamicgraph::command::docCommandVoid1;
+ using ::dynamicgraph::command::makeCommandVoid2;
+ using ::dynamicgraph::command::docCommandVoid2;
+ using ::dynamicgraph::command::makeCommandVerbose;
+ using ::dynamicgraph::command::docCommandVerbose;
+ } // namespace dyninv
+ } // namespace sot
+} // namespace dynamicgraph
diff --git a/src/controller-pd.cpp b/src/controller-pd.cpp
index cd4157cdc2b4730b6f77921dbd94f79152fc7376..6116ee07476984b9a004e7a4e7b296227f2784a0 100644
--- a/src/controller-pd.cpp
+++ b/src/controller-pd.cpp
@@ -15,279 +15,282 @@
*/
#include <sot-dyninv/controller-pd.h>
-#include <sot-core/debug.h>
+#include <sot/core/debug.hh>
#include <dynamic-graph/factory.h>
#include <sot-dyninv/commands-helper.h>
-namespace sot
+namespace dynamicgraph
{
- namespace dyninv
+ namespace sot
{
-
-
- namespace dg = ::dynamicgraph;
- using namespace dg;
-
- /* --- DG FACTORY ------------------------------------------------------- */
- DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ControllerPD,"ControllerPD");
-
- /* --- CONSTRUCTION ----------------------------------------------------- */
- /* --- CONSTRUCTION ----------------------------------------------------- */
- /* --- CONSTRUCTION ----------------------------------------------------- */
- ControllerPD::
- ControllerPD( const std::string & name )
- : Entity(name)
-
- ,CONSTRUCT_SIGNAL_IN(Kp,ml::Vector)
- ,CONSTRUCT_SIGNAL_IN(Kd,ml::Vector)
- ,CONSTRUCT_SIGNAL_IN(position,ml::Vector)
- ,CONSTRUCT_SIGNAL_IN(positionRef,ml::Vector)
- ,CONSTRUCT_SIGNAL_IN(velocity,ml::Vector)
- ,CONSTRUCT_SIGNAL_IN(velocityRef,ml::Vector)
-
- ,CONSTRUCT_SIGNAL_OUT(control,ml::Vector,
- KpSIN << KdSIN << positionSIN << positionRefSIN
- << velocitySIN << velocityRefSIN )
- {
- Entity::signalRegistration( KpSIN << KdSIN << positionSIN << positionRefSIN
- << velocitySIN << velocityRefSIN
- << controlSOUT );
-
- /* Commands. */
- addCommand("getSize",
- makeDirectGetter(*this,&_dimension,
- docDirectGetter("dimension","int")));
-
- addCommand("setSize",
- makeDirectSetter(*this, &_dimension,
- docDirectSetter("dimension","int")));
-
- addCommand("velocityOnly",
- makeCommandVoid0(*this,&ControllerPD::setGainVelocityOnly,
- docCommandVoid0("Set a friction-style controller.")));
-
- addCommand("stdGain",
- makeCommandVoid1(*this,&ControllerPD::setStandardGains,
- docCommandVoid1("Set spefic gains.",
- "string (in low|medium|high)")));
-
- }
-
-
- /* --- SIGNALS ---------------------------------------------------------- */
- /* --- SIGNALS ---------------------------------------------------------- */
- /* --- SIGNALS ---------------------------------------------------------- */
-
- ml::Vector& ControllerPD::
- controlSOUT_function( ml::Vector &tau, int iter )
- {
- sotDEBUGIN(15);
- const ml::Vector& Kp = KpSIN(iter);
- const ml::Vector& Kd = KdSIN(iter);
- const ml::Vector& position = positionSIN(iter);
- const ml::Vector& desiredposition = positionRefSIN(iter);
- const unsigned int size = Kp.size();
-
- assert( _dimension == (int)size );
- assert( size==Kp.size() ); assert( size==Kd.size() );
- assert( size==position.size() ); assert( size==desiredposition.size() );
-
- bool useVelocity = velocitySIN;
- ml::Vector velocity;
- bool useVelocityDesired = false;
- ml::Vector desiredvelocity;
- if( useVelocity ) // TODO: there is a useless copy here. Use a pointer?
- {
- velocity = velocitySIN(iter);
- assert( size==velocity.size() );
- useVelocityDesired = velocityRefSIN;
- if( useVelocityDesired )
- {
- desiredvelocity = velocityRefSIN(iter);
- assert( size==desiredvelocity.size() );
- }
- }
-
- tau.resize(size); double dv; // TODO: use dv from start to avoid the copy.
- for(unsigned i = 0u; i < size; ++i)
- {
- tau(i) = Kp(i)*(desiredposition(i)-position(i));
- if( useVelocity )
- {
- dv= velocity(i);
- if( useVelocityDesired )
- dv-=desiredvelocity(i);
- tau(i) -= Kd(i)*dv;
- }
- }
-
- sotDEBUG(15) << "p = " << position << std::endl;
- sotDEBUG(15) << "pd = " << desiredposition << std::endl;
- if( useVelocity ) {sotDEBUG(15) << "v= " << velocity << std::endl;}
- if( useVelocityDesired ) { sotDEBUG(15) << "vd= " << velocity << std::endl; }
- sotDEBUG(15) << "kp = " << Kp << std::endl;
- sotDEBUG(15) << "kd = " << Kd << std::endl;
- sotDEBUG(15) << "PD torque= " << tau << std::endl;
-
- sotDEBUGOUT(15);
- return tau;
- }
-
-
- /* --- COMMANDS ---------------------------------------------------------- */
- /* --- MEMBERS ---------------------------------------------------------- */
- void ControllerPD::
- size(const int & dimension)
+ namespace dyninv
{
- _dimension = dimension;
- }
- int ControllerPD::
- size(void) const
- {
- return _dimension;
- }
- void ControllerPD::
- setGainVelocityOnly( void )
- {
- ml::Vector zero(_dimension); zero.fill(0);
- positionSIN = zero;
- positionRefSIN = zero;
- KpSIN = zero;
- }
-
- /** Give some specific values for the Kp and Kd gains. Possible
- * configs are "low", "middle" and "high".
- * Warning: middle and high only works for dim 30.
- * TODO: properly throw errors when needed.
- */
- void ControllerPD::
- setStandardGains( const std::string & config )
- {
-
- if( config =="low" )
- {
- // Low gains
- ml::Vector Kp(_dimension); Kp.fill(100);
- ml::Vector Kd(_dimension); Kd.fill(20);
- KpSIN = Kp;
- KdSIN = Kd;
- }
- else if( config =="medium" )
- {
- // Medium gains
- if( _dimension != 30 )
- { std::cerr << "Only working for dim=30!" << std::endl; return; }
-
- ml::Vector Kp(_dimension),Kd(_dimension);
- unsigned int i=0;
- Kp(i++)=400; Kp(i++)=1000; Kp(i++)=2000; Kp(i++)=1800; Kp(i++)=2000;
- Kp(i++)=1000; Kp(i++)=400; Kp(i++)=1000; Kp(i++)=2000; Kp(i++)=1800;
- Kp(i++)=2000; Kp(i++)=1000; Kp(i++)=500; Kp(i++)=250; Kp(i++)=200;
- Kp(i++)=200; Kp(i++)=300; Kp(i++)=300; Kp(i++)=200; Kp(i++)=400;
- Kp(i++)=400; Kp(i++)=200; Kp(i++)=200; Kp(i++)=300; Kp(i++)= 300;
- Kp(i++)=200; Kp(i++)=400; Kp(i++)=400; Kp(i++)=200; Kp(i++)=200;
-
- i=0;
- Kd(i++)=40; Kd(i++)=100; Kd(i++)=200; Kd(i++)=180; Kd(i++)=200;
- Kd(i++)=100; Kd(i++)=40; Kd(i++)=100; Kd(i++)=200; Kd(i++)=180;
- Kd(i++)=200; Kd(i++)=100; Kd(i++)=50; Kd(i++)=25; Kd(i++)=20;
- Kd(i++)=20; Kd(i++)=30; Kd(i++)=30; Kd(i++)=20; Kd(i++)=40;
- Kd(i++)=40; Kd(i++)=20; Kd(i++)=20; Kd(i++)=30; Kd(i++)= 30;
- Kd(i++)=20; Kd(i++)=40; Kd(i++)=40; Kd(i++)=20; Kd(i++)=20;
-
- KpSIN = Kp;
- KdSIN = Kd;
- }
- else // high
- {
- // High gains
- if( _dimension != 30 )
- { std::cerr << "Only working for dim=30!" << std::endl; return; }
-
- ml::Vector Kp(_dimension),Kd(_dimension);
- unsigned int i=0;
- Kp(i++)=4000; Kp(i++)=10000; Kp(i++)=20000; Kp(i++)=18000; Kp(i++)=20000;
- Kp(i++)=10000; Kp(i++)=4000; Kp(i++)=10000; Kp(i++)=20000; Kp(i++)=18000;
- Kp(i++)=20000; Kp(i++)=10000; Kp(i++)=5000; Kp(i++)=2500; Kp(i++)=2000;
- Kp(i++)=2000; Kp(i++)=3000; Kp(i++)=3000; Kp(i++)=2000; Kp(i++)=4000;
- Kp(i++)=4000; Kp(i++)=2000; Kp(i++)=2000; Kp(i++)=3000; Kp(i++)= 3000;
- Kp(i++)=2000; Kp(i++)=4000; Kp(i++)=4000; Kp(i++)=2000; Kp(i++)=2000;
-
- i=0;
- Kd(i++)=120; Kd(i++)=300; Kd(i++)=600; Kd(i++)=500; Kd(i++)=600;
- Kd(i++)=300; Kd(i++)=120; Kd(i++)=300; Kd(i++)=600; Kd(i++)=500;
- Kd(i++)=600; Kd(i++)=300; Kd(i++)=150; Kd(i++)=75; Kd(i++)=60;
- Kd(i++)=60; Kd(i++)=90; Kd(i++)=90; Kd(i++)=60; Kd(i++)=120;
- Kd(i++)=120; Kd(i++)=60; Kd(i++)=60; Kd(i++)=90; Kd(i++)= 90;
- Kd(i++)=60; Kd(i++)=120; Kd(i++)=120; Kd(i++)=60; Kd(i++)=60;
-
- KpSIN = Kp;
- KdSIN = Kd;
- }
- }
- /* --- ENTITY ----------------------------------------------------------- */
- /* --- ENTITY ----------------------------------------------------------- */
- /* --- ENTITY ----------------------------------------------------------- */
-
- void ControllerPD::
- display( std::ostream& os ) const
- {
- os << "ControllerPD "<<getName();
- try
- {
- os <<"control = "<<controlSOUT;
- }
- catch (ExceptionSignal e) {}
- }
-
- void ControllerPD::
- commandLine( const std::string& cmdLine,
- std::istringstream& cmdArgs,
- std::ostream& os )
- {
- if( cmdLine == "help" )
- {
- os << "sotControlPD:\n"
- << " - size <arg>\t\tset the size of the vector.\n"
- << " - stdGain \t\tset the input vector gains according to the size for HRP2.\n"
- << " - velocityonly <arg>\t\tset Kp = 0.\n"
- << std::endl;
- }
- else if( cmdLine == "size" )
- {
- cmdArgs >> std::ws;
- if( cmdArgs.good() )
- {
- unsigned int i; cmdArgs >> i ;
- size(i);
- }
- else
- {
- os << "size = " << size() << std::endl;
- }
- }
- else if( cmdLine == "velocityonly" )
- { setGainVelocityOnly(); }
- else if( cmdLine == "stdGain" )
- {
- std::string config = "high";
- cmdArgs >> std::ws; if( cmdArgs.good() ) cmdArgs >> config;
- setStandardGains( config );
- }
- else
- {
- Entity::commandLine(cmdLine,cmdArgs,os);
- }
- }
-
-
-
-
-
-
- } // namespace dyninv
-} // namespace sot
+ namespace dg = ::dynamicgraph;
+ using namespace dg;
+
+ /* --- DG FACTORY ------------------------------------------------------- */
+ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ControllerPD,"ControllerPD");
+
+ /* --- CONSTRUCTION ----------------------------------------------------- */
+ /* --- CONSTRUCTION ----------------------------------------------------- */
+ /* --- CONSTRUCTION ----------------------------------------------------- */
+ ControllerPD::
+ ControllerPD( const std::string & name )
+ : Entity(name)
+
+ ,CONSTRUCT_SIGNAL_IN(Kp,ml::Vector)
+ ,CONSTRUCT_SIGNAL_IN(Kd,ml::Vector)
+ ,CONSTRUCT_SIGNAL_IN(position,ml::Vector)
+ ,CONSTRUCT_SIGNAL_IN(positionRef,ml::Vector)
+ ,CONSTRUCT_SIGNAL_IN(velocity,ml::Vector)
+ ,CONSTRUCT_SIGNAL_IN(velocityRef,ml::Vector)
+
+ ,CONSTRUCT_SIGNAL_OUT(control,ml::Vector,
+ KpSIN << KdSIN << positionSIN << positionRefSIN
+ << velocitySIN << velocityRefSIN )
+ {
+ Entity::signalRegistration( KpSIN << KdSIN << positionSIN << positionRefSIN
+ << velocitySIN << velocityRefSIN
+ << controlSOUT );
+
+ /* Commands. */
+ addCommand("getSize",
+ makeDirectGetter(*this,&_dimension,
+ docDirectGetter("dimension","int")));
+
+ addCommand("setSize",
+ makeDirectSetter(*this, &_dimension,
+ docDirectSetter("dimension","int")));
+
+ addCommand("velocityOnly",
+ makeCommandVoid0(*this,&ControllerPD::setGainVelocityOnly,
+ docCommandVoid0("Set a friction-style controller.")));
+
+ addCommand("stdGain",
+ makeCommandVoid1(*this,&ControllerPD::setStandardGains,
+ docCommandVoid1("Set spefic gains.",
+ "string (in low|medium|high)")));
+
+ }
+
+
+ /* --- SIGNALS ---------------------------------------------------------- */
+ /* --- SIGNALS ---------------------------------------------------------- */
+ /* --- SIGNALS ---------------------------------------------------------- */
+
+ ml::Vector& ControllerPD::
+ controlSOUT_function( ml::Vector &tau, int iter )
+ {
+ sotDEBUGIN(15);
+ const ml::Vector& Kp = KpSIN(iter);
+ const ml::Vector& Kd = KdSIN(iter);
+ const ml::Vector& position = positionSIN(iter);
+ const ml::Vector& desiredposition = positionRefSIN(iter);
+ const unsigned int size = Kp.size();
+
+ assert( _dimension == (int)size );
+ assert( size==Kp.size() ); assert( size==Kd.size() );
+ assert( size==position.size() ); assert( size==desiredposition.size() );
+
+ bool useVelocity = velocitySIN;
+ ml::Vector velocity;
+ bool useVelocityDesired = false;
+ ml::Vector desiredvelocity;
+ if( useVelocity ) // TODO: there is a useless copy here. Use a pointer?
+ {
+ velocity = velocitySIN(iter);
+ assert( size==velocity.size() );
+ useVelocityDesired = velocityRefSIN;
+ if( useVelocityDesired )
+ {
+ desiredvelocity = velocityRefSIN(iter);
+ assert( size==desiredvelocity.size() );
+ }
+ }
+
+ tau.resize(size); double dv; // TODO: use dv from start to avoid the copy.
+ for(unsigned i = 0u; i < size; ++i)
+ {
+ tau(i) = Kp(i)*(desiredposition(i)-position(i));
+ if( useVelocity )
+ {
+ dv= velocity(i);
+ if( useVelocityDesired )
+ dv-=desiredvelocity(i);
+ tau(i) -= Kd(i)*dv;
+ }
+ }
+
+ sotDEBUG(15) << "p = " << position << std::endl;
+ sotDEBUG(15) << "pd = " << desiredposition << std::endl;
+ if( useVelocity ) {sotDEBUG(15) << "v= " << velocity << std::endl;}
+ if( useVelocityDesired ) { sotDEBUG(15) << "vd= " << velocity << std::endl; }
+ sotDEBUG(15) << "kp = " << Kp << std::endl;
+ sotDEBUG(15) << "kd = " << Kd << std::endl;
+ sotDEBUG(15) << "PD torque= " << tau << std::endl;
+
+ sotDEBUGOUT(15);
+ return tau;
+ }
+
+
+ /* --- COMMANDS ---------------------------------------------------------- */
+ /* --- MEMBERS ---------------------------------------------------------- */
+ void ControllerPD::
+ size(const int & dimension)
+ {
+ _dimension = dimension;
+ }
+
+ int ControllerPD::
+ size(void) const
+ {
+ return _dimension;
+ }
+
+ void ControllerPD::
+ setGainVelocityOnly( void )
+ {
+ ml::Vector zero(_dimension); zero.fill(0);
+ positionSIN = zero;
+ positionRefSIN = zero;
+ KpSIN = zero;
+ }
+
+ /** Give some specific values for the Kp and Kd gains. Possible
+ * configs are "low", "middle" and "high".
+ * Warning: middle and high only works for dim 30.
+ * TODO: properly throw errors when needed.
+ */
+ void ControllerPD::
+ setStandardGains( const std::string & config )
+ {
+
+ if( config =="low" )
+ {
+ // Low gains
+ ml::Vector Kp(_dimension); Kp.fill(100);
+ ml::Vector Kd(_dimension); Kd.fill(20);
+ KpSIN = Kp;
+ KdSIN = Kd;
+ }
+ else if( config =="medium" )
+ {
+ // Medium gains
+ if( _dimension != 30 )
+ { std::cerr << "Only working for dim=30!" << std::endl; return; }
+
+ ml::Vector Kp(_dimension),Kd(_dimension);
+ unsigned int i=0;
+ Kp(i++)=400; Kp(i++)=1000; Kp(i++)=2000; Kp(i++)=1800; Kp(i++)=2000;
+ Kp(i++)=1000; Kp(i++)=400; Kp(i++)=1000; Kp(i++)=2000; Kp(i++)=1800;
+ Kp(i++)=2000; Kp(i++)=1000; Kp(i++)=500; Kp(i++)=250; Kp(i++)=200;
+ Kp(i++)=200; Kp(i++)=300; Kp(i++)=300; Kp(i++)=200; Kp(i++)=400;
+ Kp(i++)=400; Kp(i++)=200; Kp(i++)=200; Kp(i++)=300; Kp(i++)= 300;
+ Kp(i++)=200; Kp(i++)=400; Kp(i++)=400; Kp(i++)=200; Kp(i++)=200;
+
+ i=0;
+ Kd(i++)=40; Kd(i++)=100; Kd(i++)=200; Kd(i++)=180; Kd(i++)=200;
+ Kd(i++)=100; Kd(i++)=40; Kd(i++)=100; Kd(i++)=200; Kd(i++)=180;
+ Kd(i++)=200; Kd(i++)=100; Kd(i++)=50; Kd(i++)=25; Kd(i++)=20;
+ Kd(i++)=20; Kd(i++)=30; Kd(i++)=30; Kd(i++)=20; Kd(i++)=40;
+ Kd(i++)=40; Kd(i++)=20; Kd(i++)=20; Kd(i++)=30; Kd(i++)= 30;
+ Kd(i++)=20; Kd(i++)=40; Kd(i++)=40; Kd(i++)=20; Kd(i++)=20;
+
+ KpSIN = Kp;
+ KdSIN = Kd;
+ }
+ else // high
+ {
+ // High gains
+ if( _dimension != 30 )
+ { std::cerr << "Only working for dim=30!" << std::endl; return; }
+
+ ml::Vector Kp(_dimension),Kd(_dimension);
+ unsigned int i=0;
+ Kp(i++)=4000; Kp(i++)=10000; Kp(i++)=20000; Kp(i++)=18000; Kp(i++)=20000;
+ Kp(i++)=10000; Kp(i++)=4000; Kp(i++)=10000; Kp(i++)=20000; Kp(i++)=18000;
+ Kp(i++)=20000; Kp(i++)=10000; Kp(i++)=5000; Kp(i++)=2500; Kp(i++)=2000;
+ Kp(i++)=2000; Kp(i++)=3000; Kp(i++)=3000; Kp(i++)=2000; Kp(i++)=4000;
+ Kp(i++)=4000; Kp(i++)=2000; Kp(i++)=2000; Kp(i++)=3000; Kp(i++)= 3000;
+ Kp(i++)=2000; Kp(i++)=4000; Kp(i++)=4000; Kp(i++)=2000; Kp(i++)=2000;
+
+ i=0;
+ Kd(i++)=120; Kd(i++)=300; Kd(i++)=600; Kd(i++)=500; Kd(i++)=600;
+ Kd(i++)=300; Kd(i++)=120; Kd(i++)=300; Kd(i++)=600; Kd(i++)=500;
+ Kd(i++)=600; Kd(i++)=300; Kd(i++)=150; Kd(i++)=75; Kd(i++)=60;
+ Kd(i++)=60; Kd(i++)=90; Kd(i++)=90; Kd(i++)=60; Kd(i++)=120;
+ Kd(i++)=120; Kd(i++)=60; Kd(i++)=60; Kd(i++)=90; Kd(i++)= 90;
+ Kd(i++)=60; Kd(i++)=120; Kd(i++)=120; Kd(i++)=60; Kd(i++)=60;
+
+ KpSIN = Kp;
+ KdSIN = Kd;
+ }
+ }
+ /* --- ENTITY ----------------------------------------------------------- */
+ /* --- ENTITY ----------------------------------------------------------- */
+ /* --- ENTITY ----------------------------------------------------------- */
+
+ void ControllerPD::
+ display( std::ostream& os ) const
+ {
+ os << "ControllerPD "<<getName();
+ try
+ {
+ os <<"control = "<<controlSOUT;
+ }
+ catch (ExceptionSignal e) {}
+ }
+
+ void ControllerPD::
+ commandLine( const std::string& cmdLine,
+ std::istringstream& cmdArgs,
+ std::ostream& os )
+ {
+ if( cmdLine == "help" )
+ {
+ os << "sotControlPD:\n"
+ << " - size <arg>\t\tset the size of the vector.\n"
+ << " - stdGain \t\tset the input vector gains according to the size for HRP2.\n"
+ << " - velocityonly <arg>\t\tset Kp = 0.\n"
+ << std::endl;
+ }
+ else if( cmdLine == "size" )
+ {
+ cmdArgs >> std::ws;
+ if( cmdArgs.good() )
+ {
+ unsigned int i; cmdArgs >> i ;
+ size(i);
+ }
+ else
+ {
+ os << "size = " << size() << std::endl;
+ }
+ }
+ else if( cmdLine == "velocityonly" )
+ { setGainVelocityOnly(); }
+ else if( cmdLine == "stdGain" )
+ {
+ std::string config = "high";
+ cmdArgs >> std::ws; if( cmdArgs.good() ) cmdArgs >> config;
+ setStandardGains( config );
+ }
+ else
+ {
+ Entity::commandLine(cmdLine,cmdArgs,os);
+ }
+ }
+
+
+
+
+
+
+ } // namespace dyninv
+ } // namespace sot
+} // namespace dynamicgraph
diff --git a/include/sot-dyninv/controller-pd.h b/src/controller-pd.h
similarity index 51%
rename from include/sot-dyninv/controller-pd.h
rename to src/controller-pd.h
index bf97f66ec9016d939b98b80086a5f9595350ee44..69c2a3f4fc9a99d0ec64e44d4f087afa2049ebf1 100644
--- a/include/sot-dyninv/controller-pd.h
+++ b/src/controller-pd.h
@@ -41,59 +41,61 @@
-namespace sot {
- namespace dyninv {
+namespace dynamicgraph {
+ namespace sot {
+ namespace dyninv {
- /* --------------------------------------------------------------------- */
- /* --- CLASS ----------------------------------------------------------- */
- /* --------------------------------------------------------------------- */
+ /* --------------------------------------------------------------------- */
+ /* --- CLASS ----------------------------------------------------------- */
+ /* --------------------------------------------------------------------- */
- class SOTCONTROLLERPD_EXPORT ControllerPD
- :public ::dynamicgraph::Entity
- ,public ::dynamicgraph::EntityHelper<ControllerPD>
- {
+ class SOTCONTROLLERPD_EXPORT ControllerPD
+ :public ::dynamicgraph::Entity
+ ,public ::dynamicgraph::EntityHelper<ControllerPD>
+ {
- public: /* --- CONSTRUCTOR ---- */
+ public: /* --- CONSTRUCTOR ---- */
- ControllerPD( const std::string & name );
+ ControllerPD( const std::string & name );
- protected:
- /* Parameters of the torque-control function:
- * tau = kp * (qd-q) + kd* (dqd-dq) */
- int _dimension;
+ protected:
+ /* Parameters of the torque-control function:
+ * tau = kp * (qd-q) + kd* (dqd-dq) */
+ int _dimension;
- public:
- void size(const int & dimension);
- int size(void) const;
- void setStandardGains( const std::string & config );
- void setGainVelocityOnly( void );
+ public:
+ void size(const int & dimension);
+ int size(void) const;
+ void setStandardGains( const std::string & config );
+ void setGainVelocityOnly( void );
- public: /* --- ENTITY INHERITANCE --- */
+ public: /* --- ENTITY INHERITANCE --- */
- static const std::string CLASS_NAME;
- virtual void display( std::ostream& os ) const;
- virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
+ static const std::string CLASS_NAME;
+ virtual void display( std::ostream& os ) const;
+ virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
- virtual void commandLine( const std::string& cmdLine,
- std::istringstream& cmdArgs,
- std::ostream& os );
+ virtual void commandLine( const std::string& cmdLine,
+ std::istringstream& cmdArgs,
+ std::ostream& os );
- public: /* --- SIGNALS --- */
+ public: /* --- SIGNALS --- */
- DECLARE_SIGNAL_IN(Kp,ml::Vector);
- DECLARE_SIGNAL_IN(Kd,ml::Vector);
- DECLARE_SIGNAL_IN(position,ml::Vector);
- DECLARE_SIGNAL_IN(positionRef,ml::Vector);
- DECLARE_SIGNAL_IN(velocity,ml::Vector);
- DECLARE_SIGNAL_IN(velocityRef,ml::Vector);
+ DECLARE_SIGNAL_IN(Kp,ml::Vector);
+ DECLARE_SIGNAL_IN(Kd,ml::Vector);
+ DECLARE_SIGNAL_IN(position,ml::Vector);
+ DECLARE_SIGNAL_IN(positionRef,ml::Vector);
+ DECLARE_SIGNAL_IN(velocity,ml::Vector);
+ DECLARE_SIGNAL_IN(velocityRef,ml::Vector);
- DECLARE_SIGNAL_OUT(control,ml::Vector);
+ DECLARE_SIGNAL_OUT(control,ml::Vector);
- }; // class ControllerPD
+ }; // class ControllerPD
- } // namespace dyninv
-} // namespace sot
+ } // namespace dyninv
+ } // namespace sot
+} // namespace dynamicgraph
diff --git a/src/dynamic-integrator.cpp b/src/dynamic-integrator.cpp
index cc3f725fe8d47fb092c8862c2ad231ad7505f022..78eb76a41774f65985d1962aec3af11cc2414e1f 100644
--- a/src/dynamic-integrator.cpp
+++ b/src/dynamic-integrator.cpp
@@ -15,7 +15,7 @@
*/
#include <sot-dyninv/dynamic-integrator.h>
-#include <sot-core/debug.h>
+#include <sot/core/debug.hh>
#include <dynamic-graph/factory.h>
#include <sot-dyninv/commands-helper.h>
@@ -29,445 +29,448 @@
* has to be explicitely triggered by calling the command 'inc' (increments).
*/
-namespace sot
+namespace dynamicgraph
{
- namespace dyninv
+ namespace sot
{
+ namespace dyninv
+ {
- namespace dg = ::dynamicgraph;
- using namespace dg;
+ namespace dg = ::dynamicgraph;
+ using namespace dg;
- /* --- DG FACTORY ------------------------------------------------------- */
- DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(DynamicIntegrator,"DynamicIntegrator");
+ /* --- DG FACTORY ------------------------------------------------------- */
+ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(DynamicIntegrator,"DynamicIntegrator");
- /* --- CONSTRUCTION ----------------------------------------------------- */
- /* --- CONSTRUCTION ----------------------------------------------------- */
- /* --- CONSTRUCTION ----------------------------------------------------- */
- DynamicIntegrator::
- DynamicIntegrator( const std::string & name )
- : Entity(name)
+ /* --- CONSTRUCTION ----------------------------------------------------- */
+ /* --- CONSTRUCTION ----------------------------------------------------- */
+ /* --- CONSTRUCTION ----------------------------------------------------- */
+ DynamicIntegrator::
+ DynamicIntegrator( const std::string & name )
+ : Entity(name)
- ,CONSTRUCT_SIGNAL_IN(acceleration,ml::Vector)
- ,CONSTRUCT_SIGNAL_IN(dt,double)
+ ,CONSTRUCT_SIGNAL_IN(acceleration,ml::Vector)
+ ,CONSTRUCT_SIGNAL_IN(dt,double)
- ,CONSTRUCT_SIGNAL_OUT(velocity,ml::Vector,sotNOSIGNAL)
- ,CONSTRUCT_SIGNAL_OUT(position,ml::Vector,sotNOSIGNAL)
- {
- Entity::signalRegistration( accelerationSIN << dtSIN
- << velocitySOUT << positionSOUT );
+ ,CONSTRUCT_SIGNAL_OUT(velocity,ml::Vector,sotNOSIGNAL)
+ ,CONSTRUCT_SIGNAL_OUT(position,ml::Vector,sotNOSIGNAL)
+ {
+ Entity::signalRegistration( accelerationSIN << dtSIN
+ << velocitySOUT << positionSOUT );
- addCommand("inc",
- makeCommandVoid0(*this,&DynamicIntegrator::integrateFromSignals,
- docCommandVoid0("Integrate acc, update v and p.")));
+ addCommand("inc",
+ makeCommandVoid0(*this,&DynamicIntegrator::integrateFromSignals,
+ docCommandVoid0("Integrate acc, update v and p.")));
- addCommand("setPosition",
- makeDirectSetter(*this,&position,
- docDirectSetter("position","vector")));
- addCommand("setVelocity",
- makeDirectSetter(*this,&velocity,
- docDirectSetter("velocity","vector")));
+ addCommand("setPosition",
+ makeDirectSetter(*this,&position,
+ docDirectSetter("position","vector")));
+ addCommand("setVelocity",
+ makeDirectSetter(*this,&velocity,
+ docDirectSetter("velocity","vector")));
- }
+ }
- /* --- SIGNALS ---------------------------------------------------------- */
- /* --- SIGNALS ---------------------------------------------------------- */
- /* --- SIGNALS ---------------------------------------------------------- */
+ /* --- SIGNALS ---------------------------------------------------------- */
+ /* --- SIGNALS ---------------------------------------------------------- */
+ /* --- SIGNALS ---------------------------------------------------------- */
- ml::Vector& DynamicIntegrator::
- velocitySOUT_function( ml::Vector& mlv,int )
- {
- mlv = velocity;
- return mlv;
- }
+ ml::Vector& DynamicIntegrator::
+ velocitySOUT_function( ml::Vector& mlv,int )
+ {
+ mlv = velocity;
+ return mlv;
+ }
- ml::Vector& DynamicIntegrator::
- positionSOUT_function( ml::Vector& mlp,int )
- {
- mlp = position;
- return mlp;
- }
+ ml::Vector& DynamicIntegrator::
+ positionSOUT_function( ml::Vector& mlp,int )
+ {
+ mlp = position;
+ return mlp;
+ }
- /* --- PROXYS ----------------------------------------------------------- */
- void DynamicIntegrator::
- integrateFromSignals( const int & time )
- {
- const ml::Vector & acc = accelerationSIN(time);
- const double & dt = dtSIN(time);
+ /* --- PROXYS ----------------------------------------------------------- */
+ void DynamicIntegrator::
+ integrateFromSignals( const int & time )
+ {
+ const ml::Vector & acc = accelerationSIN(time);
+ const double & dt = dtSIN(time);
- integrate( acc,dt, velocity,position );
- velocitySOUT.setReady();
- positionSOUT.setReady();
- }
+ integrate( acc,dt, velocity,position );
+ velocitySOUT.setReady();
+ positionSOUT.setReady();
+ }
- void DynamicIntegrator::
- integrateFromSignals( void )
- {
- integrateFromSignals( accelerationSIN.getTime() + 1 );
- }
+ void DynamicIntegrator::
+ integrateFromSignals( void )
+ {
+ integrateFromSignals( accelerationSIN.getTime() + 1 );
+ }
- void DynamicIntegrator::
- setPosition( const ml::Vector& p )
- {
- position = p;
- positionSOUT.setReady();
- }
+ void DynamicIntegrator::
+ setPosition( const ml::Vector& p )
+ {
+ position = p;
+ positionSOUT.setReady();
+ }
- void DynamicIntegrator::
- setVelocity( const ml::Vector& v )
- {
- velocity = v;
- velocitySOUT.setReady();
- }
+ void DynamicIntegrator::
+ setVelocity( const ml::Vector& v )
+ {
+ velocity = v;
+ velocitySOUT.setReady();
+ }
- void DynamicIntegrator::
- setState( const ml::Vector& p,const ml::Vector& v )
- {
- sotDEBUG(5) << "State: " << p << v << std::endl;
- position = p;
- velocity = v;
- velocitySOUT.setReady();
- positionSOUT.setReady();
- }
+ void DynamicIntegrator::
+ setState( const ml::Vector& p,const ml::Vector& v )
+ {
+ sotDEBUG(5) << "State: " << p << v << std::endl;
+ position = p;
+ velocity = v;
+ velocitySOUT.setReady();
+ positionSOUT.setReady();
+ }
- /* --- COMMANDS ---------------------------------------------------------- */
- /* --- COMMANDS ---------------------------------------------------------- */
- /* --- COMMANDS ---------------------------------------------------------- */
+ /* --- COMMANDS ---------------------------------------------------------- */
+ /* --- COMMANDS ---------------------------------------------------------- */
+ /* --- COMMANDS ---------------------------------------------------------- */
- using namespace Eigen;
+ using namespace Eigen;
- namespace DynamicIntegratorStatic
- {
- template< typename D1 >
- static Matrix3d
- computeRotationMatrixFromEuler(const MatrixBase<D1> & euler)
+ namespace DynamicIntegratorStatic
{
- EIGEN_STATIC_ASSERT_VECTOR_ONLY(Eigen::MatrixBase<D1>);
-
- double Rpsi = euler[0];
- double Rtheta = euler[1];
- double Rphy = euler[2];
-
- double cosPhy = cos(Rphy);
- double sinPhy = sin(Rphy);
+ template< typename D1 >
+ static Matrix3d
+ computeRotationMatrixFromEuler(const MatrixBase<D1> & euler)
+ {
+ EIGEN_STATIC_ASSERT_VECTOR_ONLY(Eigen::MatrixBase<D1>);
- double cosTheta = cos(Rtheta);
- double sinTheta = sin(Rtheta);
+ double Rpsi = euler[0];
+ double Rtheta = euler[1];
+ double Rphy = euler[2];
- double cosPsi = cos(Rpsi);
- double sinPsi = sin(Rpsi);
+ double cosPhy = cos(Rphy);
+ double sinPhy = sin(Rphy);
- Matrix3d rotation;
+ double cosTheta = cos(Rtheta);
+ double sinTheta = sin(Rtheta);
- rotation(0, 0) = cosPhy * cosTheta;
- rotation(1, 0) = sinPhy * cosTheta;
- rotation(2, 0) = -sinTheta;
+ double cosPsi = cos(Rpsi);
+ double sinPsi = sin(Rpsi);
- double sinTheta__sinPsi = sinTheta * sinPsi;
- double sinTheta__cosPsi = sinTheta * cosPsi;
+ Matrix3d rotation;
- rotation(0, 1) = cosPhy * sinTheta__sinPsi - sinPhy * cosPsi;
- rotation(1, 1) = sinPhy * sinTheta__sinPsi + cosPhy * cosPsi;
- rotation(2, 1) = cosTheta * sinPsi;
+ rotation(0, 0) = cosPhy * cosTheta;
+ rotation(1, 0) = sinPhy * cosTheta;
+ rotation(2, 0) = -sinTheta;
- rotation(0, 2) = cosPhy * sinTheta__cosPsi + sinPhy * sinPsi;
- rotation(1, 2) = sinPhy * sinTheta__cosPsi - cosPhy * sinPsi;
- rotation(2, 2) = cosTheta * cosPsi;
+ double sinTheta__sinPsi = sinTheta * sinPsi;
+ double sinTheta__cosPsi = sinTheta * cosPsi;
- return rotation;
- }
+ rotation(0, 1) = cosPhy * sinTheta__sinPsi - sinPhy * cosPsi;
+ rotation(1, 1) = sinPhy * sinTheta__sinPsi + cosPhy * cosPsi;
+ rotation(2, 1) = cosTheta * sinPsi;
- template< typename D1 >
- Vector3d computeEulerFromRotationMatrix ( const MatrixBase<D1> & rotation )
- {
- Vector3d euler;
-
- double rotationMatrix00 = rotation(0,0);
- double rotationMatrix10 = rotation(1,0);
- double rotationMatrix20 = rotation(2,0);
- double rotationMatrix01 = rotation(0,1);
- double rotationMatrix11 = rotation(1,1);
- double rotationMatrix21 = rotation(2,1);
- double rotationMatrix02 = rotation(0,2);
- double rotationMatrix12 = rotation(1,2);
- double rotationMatrix22 = rotation(2,2);
-
- double cosTheta = sqrt(0.5 * ( rotationMatrix00*rotationMatrix00
- + rotationMatrix10*rotationMatrix10
- + rotationMatrix21*rotationMatrix21
- + rotationMatrix22*rotationMatrix22 ));
- double sinTheta = -rotationMatrix20;
- euler[1] = atan2 (sinTheta, cosTheta);
-
- double cosTheta_cosPhi = 0.5 * (rotationMatrix22 * rotationMatrix11
- - rotationMatrix21 * rotationMatrix12);
- double cosTheta_sinPhi = 0.5 * (rotationMatrix21 * rotationMatrix02
- - rotationMatrix22 * rotationMatrix01);
- double cosTheta_cosPsi = 0.5 * (rotationMatrix00 * rotationMatrix11
- - rotationMatrix01 * rotationMatrix10);
- double cosTheta_sinPsi = 0.5 * (rotationMatrix10 * rotationMatrix02
- - rotationMatrix00 * rotationMatrix12);
-
- //if cosTheta == 0
- if (fabs(cosTheta) < 1e-9 )
- {
- if (sinTheta > 0.5) // sinTheta ~= 1
- {
- //phi_psi = phi - psi
- double phi_psi = atan2(- rotationMatrix10, rotationMatrix11);
- double psi = euler[2];
+ rotation(0, 2) = cosPhy * sinTheta__cosPsi + sinPhy * sinPsi;
+ rotation(1, 2) = sinPhy * sinTheta__cosPsi - cosPhy * sinPsi;
+ rotation(2, 2) = cosTheta * cosPsi;
- double phi = phi_psi + psi;
- euler[0] = phi;
- }
- else //sinTheta ~= -1
- {
- //phi_psi = phi + psi
- double phi_psi = atan2(- rotationMatrix10, rotationMatrix11);
+ return rotation;
+ }
- double psi = euler[2];
+ template< typename D1 >
+ Vector3d computeEulerFromRotationMatrix ( const MatrixBase<D1> & rotation )
+ {
+ Vector3d euler;
+
+ double rotationMatrix00 = rotation(0,0);
+ double rotationMatrix10 = rotation(1,0);
+ double rotationMatrix20 = rotation(2,0);
+ double rotationMatrix01 = rotation(0,1);
+ double rotationMatrix11 = rotation(1,1);
+ double rotationMatrix21 = rotation(2,1);
+ double rotationMatrix02 = rotation(0,2);
+ double rotationMatrix12 = rotation(1,2);
+ double rotationMatrix22 = rotation(2,2);
+
+ double cosTheta = sqrt(0.5 * ( rotationMatrix00*rotationMatrix00
+ + rotationMatrix10*rotationMatrix10
+ + rotationMatrix21*rotationMatrix21
+ + rotationMatrix22*rotationMatrix22 ));
+ double sinTheta = -rotationMatrix20;
+ euler[1] = atan2 (sinTheta, cosTheta);
+
+ double cosTheta_cosPhi = 0.5 * (rotationMatrix22 * rotationMatrix11
+ - rotationMatrix21 * rotationMatrix12);
+ double cosTheta_sinPhi = 0.5 * (rotationMatrix21 * rotationMatrix02
+ - rotationMatrix22 * rotationMatrix01);
+ double cosTheta_cosPsi = 0.5 * (rotationMatrix00 * rotationMatrix11
+ - rotationMatrix01 * rotationMatrix10);
+ double cosTheta_sinPsi = 0.5 * (rotationMatrix10 * rotationMatrix02
+ - rotationMatrix00 * rotationMatrix12);
+
+ //if cosTheta == 0
+ if (fabs(cosTheta) < 1e-9 )
+ {
+ if (sinTheta > 0.5) // sinTheta ~= 1
+ {
+ //phi_psi = phi - psi
+ double phi_psi = atan2(- rotationMatrix10, rotationMatrix11);
+ double psi = euler[2];
+
+ double phi = phi_psi + psi;
+ euler[0] = phi;
+ }
+ else //sinTheta ~= -1
+ {
+ //phi_psi = phi + psi
+ double phi_psi = atan2(- rotationMatrix10, rotationMatrix11);
+
+ double psi = euler[2];
+
+ double phi = phi_psi;
+ euler[0] = phi - psi;
+ }
+ }
+ else
+ {
+ double cosPsi = cosTheta_cosPsi / cosTheta;
+ double sinPsi = cosTheta_sinPsi / cosTheta;
+ euler[0] = atan2 (sinPsi, cosPsi);
- double phi = phi_psi;
- euler[0] = phi - psi;
- }
- }
- else
- {
- double cosPsi = cosTheta_cosPsi / cosTheta;
- double sinPsi = cosTheta_sinPsi / cosTheta;
- euler[0] = atan2 (sinPsi, cosPsi);
+ double cosPhi = cosTheta_cosPhi / cosTheta;
+ double sinPhi = cosTheta_sinPhi / cosTheta;
+ euler[2] = atan2 (sinPhi, cosPhi);
+ }
- double cosPhi = cosTheta_cosPhi / cosTheta;
- double sinPhi = cosTheta_sinPhi / cosTheta;
- euler[2] = atan2 (sinPhi, cosPhi);
- }
+ return euler;
+ }
- return euler;
- }
+ template< typename D1 >
+ Matrix3d skew( const MatrixBase<D1> & v )
+ {
+ EIGEN_STATIC_ASSERT_VECTOR_ONLY( MatrixBase<D1> );
+ Matrix3d mat;
+ mat(0,0) = 0 ; mat(0,1)= -v[2]; mat(0,2)= v[1];
+ mat(1,0) = v[2]; mat(1,1)= 0 ; mat(1,2)= -v[0];
+ mat(2,0) = -v[1]; mat(2,1)= v[0]; mat(2,2)= 0 ;
+ return mat;
+ }
- template< typename D1 >
- Matrix3d skew( const MatrixBase<D1> & v )
- {
- EIGEN_STATIC_ASSERT_VECTOR_ONLY( MatrixBase<D1> );
- Matrix3d mat;
- mat(0,0) = 0 ; mat(0,1)= -v[2]; mat(0,2)= v[1];
- mat(1,0) = v[2]; mat(1,1)= 0 ; mat(1,2)= -v[0];
- mat(2,0) = -v[1]; mat(2,1)= v[0]; mat(2,2)= 0 ;
- return mat;
- }
+ /* Convert data expressed at the origin of the joint - typicaly acc and vel
+ * in Djj - to data expressed at the center of the world - typicaly acc
+ * and vel in Amelif - For the waist dq/ddq(1:6) is equivalent to [ v/a
+ * angular | v/a linear ] expressed at the origin of the joint.
+ */
+ template< typename D1,typename D2, typename D3 >
+ void
+ djj2amelif ( Vector3d & angAmelif, Vector3d & linAmelif,
+ const MatrixBase<D1> & angDjj, const MatrixBase<D2> & linDjj,
+ const MatrixBase<D3> & pos, const Matrix3d & /*orient*/ )
+ {
+ EIGEN_STATIC_ASSERT_VECTOR_ONLY(Eigen::MatrixBase<D1>);
+ EIGEN_STATIC_ASSERT_VECTOR_ONLY(Eigen::MatrixBase<D2>);
+ EIGEN_STATIC_ASSERT_VECTOR_ONLY(Eigen::MatrixBase<D3>);
+ assert( angDjj.size() == 3 && linDjj.size() == 3 && pos.size() == 3 );
+
+ angAmelif = angDjj;
+ linAmelif = linDjj + skew(pos)* angDjj;
+ sotDEBUG(20) << "cross = " << skew(pos)* angDjj << std::endl;
+ sotDEBUG(20) << "cross = " << skew(pos) << std::endl;
+ sotDEBUG(20) << "cross = " << angDjj << std::endl;
+ }
- /* Convert data expressed at the origin of the joint - typicaly acc and vel
- * in Djj - to data expressed at the center of the world - typicaly acc
- * and vel in Amelif - For the waist dq/ddq(1:6) is equivalent to [ v/a
- * angular | v/a linear ] expressed at the origin of the joint.
- */
- template< typename D1,typename D2, typename D3 >
- void
- djj2amelif ( Vector3d & angAmelif, Vector3d & linAmelif,
- const MatrixBase<D1> & angDjj, const MatrixBase<D2> & linDjj,
- const MatrixBase<D3> & pos, const Matrix3d & /*orient*/ )
- {
- EIGEN_STATIC_ASSERT_VECTOR_ONLY(Eigen::MatrixBase<D1>);
- EIGEN_STATIC_ASSERT_VECTOR_ONLY(Eigen::MatrixBase<D2>);
- EIGEN_STATIC_ASSERT_VECTOR_ONLY(Eigen::MatrixBase<D3>);
- assert( angDjj.size() == 3 && linDjj.size() == 3 && pos.size() == 3 );
-
- angAmelif = angDjj;
- linAmelif = linDjj + skew(pos)* angDjj;
- sotDEBUG(20) << "cross = " << skew(pos)* angDjj << std::endl;
- sotDEBUG(20) << "cross = " << skew(pos) << std::endl;
- sotDEBUG(20) << "cross = " << angDjj << std::endl;
- }
+ /* Convert data expressed at the center of the world - typicaly acc and vel
+ * in Amelif - to data expressed at the origin of the joint - typicaly acc
+ * and vel in Djj - For the waist dq/ddq(1:6) is equivalent to [ v/a
+ * angular | v/a linear ] expressed at the origin of the joint
+ */
+ template< typename D1,typename D2, typename D3 >
+ void
+ amelif2djj ( MatrixBase<D1> & angDjj, MatrixBase<D2> & linDjj,
+ const Vector3d & angAmelif, const Vector3d & linAmelif,
+ const MatrixBase<D3> & pos, const Matrix3d & /*orient*/ )
+ {
+ EIGEN_STATIC_ASSERT_VECTOR_ONLY(Eigen::MatrixBase<D1>);
+ EIGEN_STATIC_ASSERT_VECTOR_ONLY(Eigen::MatrixBase<D2>);
+ EIGEN_STATIC_ASSERT_VECTOR_ONLY(Eigen::MatrixBase<D3>);
+ assert( angDjj.size() == 3 && linDjj.size() == 3 && pos.size() == 3 );
- /* Convert data expressed at the center of the world - typicaly acc and vel
- * in Amelif - to data expressed at the origin of the joint - typicaly acc
- * and vel in Djj - For the waist dq/ddq(1:6) is equivalent to [ v/a
- * angular | v/a linear ] expressed at the origin of the joint
- */
- template< typename D1,typename D2, typename D3 >
- void
- amelif2djj ( MatrixBase<D1> & angDjj, MatrixBase<D2> & linDjj,
- const Vector3d & angAmelif, const Vector3d & linAmelif,
- const MatrixBase<D3> & pos, const Matrix3d & /*orient*/ )
- {
- EIGEN_STATIC_ASSERT_VECTOR_ONLY(Eigen::MatrixBase<D1>);
- EIGEN_STATIC_ASSERT_VECTOR_ONLY(Eigen::MatrixBase<D2>);
- EIGEN_STATIC_ASSERT_VECTOR_ONLY(Eigen::MatrixBase<D3>);
- assert( angDjj.size() == 3 && linDjj.size() == 3 && pos.size() == 3 );
+ angDjj = angAmelif;
+ linDjj = linAmelif - skew(pos)* angAmelif;
+ }
- angDjj = angAmelif;
- linDjj = linAmelif - skew(pos)* angAmelif;
}
- }
-
- /* -------------------------------------------------------------------------- */
- void DynamicIntegrator::
- integrate( const ml::Vector& mlacceleration,
- const double & dt,
- ml::Vector & mlvelocity,
- ml::Vector & mlposition )
- {
- using namespace DynamicIntegratorStatic;
- using soth::MATLAB;
- sotDEBUGIN(15);
-
- /* --- Convert acceleration, velocity and position to amelif style ------- */
- EIGEN_VECTOR_FROM_SIGNAL( acceleration,mlacceleration );
- EIGEN_VECTOR_FROM_SIGNAL( velocity,mlvelocity );
- EIGEN_VECTOR_FROM_SIGNAL( position,mlposition );
-
-
- sotDEBUG(1) << "acceleration = " << (MATLAB)acceleration << std::endl;
- sotDEBUG(1) << "velocity = " << (MATLAB)velocity << std::endl;
- sotDEBUG(1) << "position = " << (MATLAB)position << std::endl;
-
- VectorBlock< Map<VectorXd> > fftrans = position.head(3);
- VectorBlock< Map<VectorXd> > ffeuler = position.segment(3,3);
- Matrix3d ffrot = computeRotationMatrixFromEuler(ffeuler);
- sotDEBUG(15) << "Rff_start = " << (MATLAB)ffrot << std::endl;
- sotDEBUG(15) << "tff_start = " << (MATLAB)fftrans << std::endl;
-
- const VectorBlock< Map<VectorXd> > ffvtrans = velocity.head(3);
- const VectorBlock< Map<VectorXd> > ffvrot = velocity.segment(3,3);
- Vector3d v_lin,v_ang;
- djj2amelif( v_ang,v_lin,ffvrot,ffvtrans,fftrans,ffrot );
- sotDEBUG(15) << "vff_start = " << (MATLAB)v_lin << std::endl;
- sotDEBUG(15) << "wff_start = " << (MATLAB)v_ang << std::endl;
-
- const VectorBlock< Map<VectorXd> > ffatrans = acceleration.head(3);
- const VectorBlock< Map<VectorXd> > ffarot = acceleration.segment(3,3);
- Vector3d a_lin,a_ang;
- djj2amelif( a_ang,a_lin,ffarot,ffatrans,fftrans,ffrot );
- sotDEBUG(15) << "alff_start = " << (MATLAB)a_lin << std::endl;
- sotDEBUG(15) << "aaff_start = " << (MATLAB)a_ang << std::endl;
-
- /* --- Integrate velocity ------------------------------------------------- */
+ /* -------------------------------------------------------------------------- */
+ void DynamicIntegrator::
+ integrate( const ml::Vector& mlacceleration,
+ const double & dt,
+ ml::Vector & mlvelocity,
+ ml::Vector & mlposition )
{
- /* Acceleration, velocity and position of the FF. */
- Matrix3d finalBodyOrientation;
- Vector3d finalBodyPosition;
+ using namespace DynamicIntegratorStatic;
+ using soth::MATLAB;
+ sotDEBUGIN(15);
+
+ /* --- Convert acceleration, velocity and position to amelif style ------- */
+ EIGEN_VECTOR_FROM_SIGNAL( acceleration,mlacceleration );
+ EIGEN_VECTOR_FROM_SIGNAL( velocity,mlvelocity );
+ EIGEN_VECTOR_FROM_SIGNAL( position,mlposition );
+
+
+ sotDEBUG(1) << "acceleration = " << (MATLAB)acceleration << std::endl;
+ sotDEBUG(1) << "velocity = " << (MATLAB)velocity << std::endl;
+ sotDEBUG(1) << "position = " << (MATLAB)position << std::endl;
+
+ VectorBlock< Map<VectorXd> > fftrans = position.head(3);
+ VectorBlock< Map<VectorXd> > ffeuler = position.segment(3,3);
+ Matrix3d ffrot = computeRotationMatrixFromEuler(ffeuler);
+ sotDEBUG(15) << "Rff_start = " << (MATLAB)ffrot << std::endl;
+ sotDEBUG(15) << "tff_start = " << (MATLAB)fftrans << std::endl;
+
+ const VectorBlock< Map<VectorXd> > ffvtrans = velocity.head(3);
+ const VectorBlock< Map<VectorXd> > ffvrot = velocity.segment(3,3);
+ Vector3d v_lin,v_ang;
+ djj2amelif( v_ang,v_lin,ffvrot,ffvtrans,fftrans,ffrot );
+ sotDEBUG(15) << "vff_start = " << (MATLAB)v_lin << std::endl;
+ sotDEBUG(15) << "wff_start = " << (MATLAB)v_ang << std::endl;
+
+ const VectorBlock< Map<VectorXd> > ffatrans = acceleration.head(3);
+ const VectorBlock< Map<VectorXd> > ffarot = acceleration.segment(3,3);
+ Vector3d a_lin,a_ang;
+ djj2amelif( a_ang,a_lin,ffarot,ffatrans,fftrans,ffrot );
+ sotDEBUG(15) << "alff_start = " << (MATLAB)a_lin << std::endl;
+ sotDEBUG(15) << "aaff_start = " << (MATLAB)a_ang << std::endl;
+
+ /* --- Integrate velocity ------------------------------------------------- */
+ {
+ /* Acceleration, velocity and position of the FF. */
+ Matrix3d finalBodyOrientation;
+ Vector3d finalBodyPosition;
- double norm_v_ang = v_ang.norm();
+ double norm_v_ang = v_ang.norm();
- /* If there's no angular velocity : no rotation. */
- if (norm_v_ang < 1e-8 )
- {
- finalBodyPosition = v_lin*dt + fftrans;
- finalBodyOrientation = ffrot;
- }
- else
- {
- const double th = norm_v_ang * dt;
- double sth = sin(th), cth = 1.0 - cos(th);
- Vector3d wn = v_ang / norm_v_ang;
- Vector3d vol = v_lin / norm_v_ang;
-
- /* drot = wnX * sin(th) + wnX * wnX * (1 - cos (th)). */
- const Matrix3d w_wedge = skew(wn);
-
- Matrix3d drot = w_wedge * cth;
- drot += Matrix3d::Identity()*sth;
- drot = w_wedge * drot;
-
- //rot = drot + id
- Matrix3d rot(drot);
- rot += Matrix3d::Identity();
- sotDEBUG(1) << "Rv = " << (MATLAB)rot << std::endl;
-
- /* Update the body rotation for the body. */
- finalBodyOrientation = rot * ffrot;
-
- /* Update the body position for the body
- * pos = rot * pos - drot * (wn ^ vol) + th* wn *T(wn) * vol */
- finalBodyPosition = rot * fftrans;
-
- // Calculation of "- drot * crossProduct(wn, vol)"
- VectorXd tmp1 = (w_wedge*vol);
- VectorXd tmp2 = w_wedge*tmp1;
- VectorXd tmp3 = w_wedge*tmp2;
- tmp2 *= sth;
- tmp3 *= cth;
- finalBodyPosition -= tmp2;
- finalBodyPosition -= tmp3;
-
- // Calculation of "th * wn * T(wn) * vol"
- double w0v0 = wn[0u] * vol[0u];
- double w1v1 = wn[1u] * vol[1u];
- double w2v2 = wn[2u] * vol[2u];
- w0v0 += w1v1;
- w0v0 += w2v2;
- w0v0 *= th;
-
- // pos += wn * wovo
- finalBodyPosition[0u] += wn[0u] * w0v0;
- finalBodyPosition[1u] += wn[1u] * w0v0;
- finalBodyPosition[2u] += wn[2u] * w0v0;
- }
+ /* If there's no angular velocity : no rotation. */
+ if (norm_v_ang < 1e-8 )
+ {
+ finalBodyPosition = v_lin*dt + fftrans;
+ finalBodyOrientation = ffrot;
+ }
+ else
+ {
+ const double th = norm_v_ang * dt;
+ double sth = sin(th), cth = 1.0 - cos(th);
+ Vector3d wn = v_ang / norm_v_ang;
+ Vector3d vol = v_lin / norm_v_ang;
+
+ /* drot = wnX * sin(th) + wnX * wnX * (1 - cos (th)). */
+ const Matrix3d w_wedge = skew(wn);
+
+ Matrix3d drot = w_wedge * cth;
+ drot += Matrix3d::Identity()*sth;
+ drot = w_wedge * drot;
+
+ //rot = drot + id
+ Matrix3d rot(drot);
+ rot += Matrix3d::Identity();
+ sotDEBUG(1) << "Rv = " << (MATLAB)rot << std::endl;
+
+ /* Update the body rotation for the body. */
+ finalBodyOrientation = rot * ffrot;
+
+ /* Update the body position for the body
+ * pos = rot * pos - drot * (wn ^ vol) + th* wn *T(wn) * vol */
+ finalBodyPosition = rot * fftrans;
+
+ // Calculation of "- drot * crossProduct(wn, vol)"
+ VectorXd tmp1 = (w_wedge*vol);
+ VectorXd tmp2 = w_wedge*tmp1;
+ VectorXd tmp3 = w_wedge*tmp2;
+ tmp2 *= sth;
+ tmp3 *= cth;
+ finalBodyPosition -= tmp2;
+ finalBodyPosition -= tmp3;
+
+ // Calculation of "th * wn * T(wn) * vol"
+ double w0v0 = wn[0u] * vol[0u];
+ double w1v1 = wn[1u] * vol[1u];
+ double w2v2 = wn[2u] * vol[2u];
+ w0v0 += w1v1;
+ w0v0 += w2v2;
+ w0v0 *= th;
+
+ // pos += wn * wovo
+ finalBodyPosition[0u] += wn[0u] * w0v0;
+ finalBodyPosition[1u] += wn[1u] * w0v0;
+ finalBodyPosition[2u] += wn[2u] * w0v0;
+ }
- sotDEBUG(1) << "Rff_end = " << (MATLAB)finalBodyOrientation << std::endl;
+ sotDEBUG(1) << "Rff_end = " << (MATLAB)finalBodyOrientation << std::endl;
- /* --- Convert position --------------------------------------------------- */
- Vector3d ffeuleur_fin = computeEulerFromRotationMatrix( finalBodyOrientation );
+ /* --- Convert position --------------------------------------------------- */
+ Vector3d ffeuleur_fin = computeEulerFromRotationMatrix( finalBodyOrientation );
- position.head(3) = finalBodyPosition;
- position.segment(3,3) = ffeuleur_fin;
- position.tail( position.size()-6 ) += velocity.tail( position.size()-6 ) * dt;
- }
+ position.head(3) = finalBodyPosition;
+ position.segment(3,3) = ffeuleur_fin;
+ position.tail( position.size()-6 ) += velocity.tail( position.size()-6 ) * dt;
+ }
- /* --- Integrate acceleration --------------------------------------------- */
+ /* --- Integrate acceleration --------------------------------------------- */
- v_lin += a_lin*dt;
- v_ang += a_ang*dt;
+ v_lin += a_lin*dt;
+ v_ang += a_ang*dt;
- Vector3d vdjj_ang,vdjj_lin;
- amelif2djj( vdjj_ang,vdjj_lin,v_ang,v_lin,fftrans,ffrot);
- velocity.head(3) = vdjj_lin; velocity.segment(3,3) = vdjj_ang;
- //amelif2djj( ffvrot,ffvtrans,v_ang,v_lin,fftrans,ffrot);
- velocity.tail( velocity.size()-6 ) += acceleration.tail( acceleration.size()-6 )*dt;
+ Vector3d vdjj_ang,vdjj_lin;
+ amelif2djj( vdjj_ang,vdjj_lin,v_ang,v_lin,fftrans,ffrot);
+ velocity.head(3) = vdjj_lin; velocity.segment(3,3) = vdjj_ang;
+ //amelif2djj( ffvrot,ffvtrans,v_ang,v_lin,fftrans,ffrot);
+ velocity.tail( velocity.size()-6 ) += acceleration.tail( acceleration.size()-6 )*dt;
- sotDEBUG(15) << "vff_end = " << (MATLAB)v_lin << std::endl;
- sotDEBUG(15) << "wff_end = " << (MATLAB)v_ang << std::endl;
+ sotDEBUG(15) << "vff_end = " << (MATLAB)v_lin << std::endl;
+ sotDEBUG(15) << "wff_end = " << (MATLAB)v_ang << std::endl;
- sotDEBUG(1) << "velocity = " << (MATLAB)velocity << std::endl;
- sotDEBUG(1) << "position = " << (MATLAB)position << std::endl;
- sotDEBUGOUT(15);
- }
+ sotDEBUG(1) << "velocity = " << (MATLAB)velocity << std::endl;
+ sotDEBUG(1) << "position = " << (MATLAB)position << std::endl;
+ sotDEBUGOUT(15);
+ }
- /* --- ENTITY ----------------------------------------------------------- */
- /* --- ENTITY ----------------------------------------------------------- */
- /* --- ENTITY ----------------------------------------------------------- */
+ /* --- ENTITY ----------------------------------------------------------- */
+ /* --- ENTITY ----------------------------------------------------------- */
+ /* --- ENTITY ----------------------------------------------------------- */
- void DynamicIntegrator::
- display( std::ostream& os ) const
- {
- os << "DynamicIntegrator "<<getName() << ". " << std::endl;
- }
+ void DynamicIntegrator::
+ display( std::ostream& os ) const
+ {
+ os << "DynamicIntegrator "<<getName() << ". " << std::endl;
+ }
- void DynamicIntegrator::
- commandLine( const std::string& cmdLine,
- std::istringstream& cmdArgs,
- std::ostream& os )
- {
- if( cmdLine == "help" )
- {
- os << "DynamicIntegrator:" << std::endl
- << " - inc [dt]" << std::endl;
- }
- else if( cmdLine == "inc" )
- {
- if( cmdArgs >> std::ws, cmdArgs.good() )
- {
- double dt; cmdArgs >> dt; dtSIN = dt;
- }
- integrateFromSignals();
- }
- else
- {
- Entity::commandLine( cmdLine,cmdArgs,os );
- }
- }
+ void DynamicIntegrator::
+ commandLine( const std::string& cmdLine,
+ std::istringstream& cmdArgs,
+ std::ostream& os )
+ {
+ if( cmdLine == "help" )
+ {
+ os << "DynamicIntegrator:" << std::endl
+ << " - inc [dt]" << std::endl;
+ }
+ else if( cmdLine == "inc" )
+ {
+ if( cmdArgs >> std::ws, cmdArgs.good() )
+ {
+ double dt; cmdArgs >> dt; dtSIN = dt;
+ }
+ integrateFromSignals();
+ }
+ else
+ {
+ Entity::commandLine( cmdLine,cmdArgs,os );
+ }
+ }
- } // namespace dyninv
-} // namespace sot
+ } // namespace dyninv
+ } // namespace sot
+} // namespace dynamicgraph
diff --git a/include/sot-dyninv/dynamic-integrator.h b/src/dynamic-integrator.h
similarity index 51%
rename from include/sot-dyninv/dynamic-integrator.h
rename to src/dynamic-integrator.h
index e59fcad02cf3f255d7c38bb7fce5a9cc02562a61..bf0c969c4a5ec0ded73ffec4d2df88f3874a004b 100644
--- a/include/sot-dyninv/dynamic-integrator.h
+++ b/src/dynamic-integrator.h
@@ -40,58 +40,59 @@
#include <sot-dyninv/entity-helper.h>
-namespace sot {
- namespace dyninv {
+namespace dynamicgraph {
+ namespace sot {
+ namespace dyninv {
- /* --------------------------------------------------------------------- */
- /* --- CLASS ----------------------------------------------------------- */
- /* --------------------------------------------------------------------- */
+ /* --------------------------------------------------------------------- */
+ /* --- CLASS ----------------------------------------------------------- */
+ /* --------------------------------------------------------------------- */
- class SOTDYNAMICINTEGRATOR_EXPORT DynamicIntegrator
- :public ::dynamicgraph::Entity
- ,public ::dynamicgraph::EntityHelper<DynamicIntegrator>
- {
+ class SOTDYNAMICINTEGRATOR_EXPORT DynamicIntegrator
+ :public ::dynamicgraph::Entity
+ ,public ::dynamicgraph::EntityHelper<DynamicIntegrator>
+ {
- public: /* --- CONSTRUCTOR ---- */
+ public: /* --- CONSTRUCTOR ---- */
- DynamicIntegrator( const std::string & name );
+ DynamicIntegrator( const std::string & name );
- public: /* --- ENTITY INHERITANCE --- */
+ public: /* --- ENTITY INHERITANCE --- */
- static const std::string CLASS_NAME;
- virtual void display( std::ostream& os ) const;
- virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
+ static const std::string CLASS_NAME;
+ virtual void display( std::ostream& os ) const;
+ virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
- virtual void commandLine( const std::string& cmdLine,
- std::istringstream& cmdArgs,
- std::ostream& os );
+ virtual void commandLine( const std::string& cmdLine,
+ std::istringstream& cmdArgs,
+ std::ostream& os );
- public: /* --- SIGNALS --- */
+ public: /* --- SIGNALS --- */
- DECLARE_SIGNAL_IN( acceleration,ml::Vector );
- DECLARE_SIGNAL_IN( dt,double );
+ DECLARE_SIGNAL_IN( acceleration,ml::Vector );
+ DECLARE_SIGNAL_IN( dt,double );
- DECLARE_SIGNAL_OUT( velocity,ml::Vector );
- DECLARE_SIGNAL_OUT( position,ml::Vector );
+ DECLARE_SIGNAL_OUT( velocity,ml::Vector );
+ DECLARE_SIGNAL_OUT( position,ml::Vector );
- public: /* --- MODIFIORS --- */
- void integrate( const ml::Vector& acceleration, const double& dt,
- ml::Vector & velocity, ml::Vector & position );
- void integrateFromSignals( const int & time );
- void integrateFromSignals( void );
+ public: /* --- MODIFIORS --- */
+ void integrate( const ml::Vector& acceleration, const double& dt,
+ ml::Vector & velocity, ml::Vector & position );
+ void integrateFromSignals( const int & time );
+ void integrateFromSignals( void );
- void setPosition( const ml::Vector& p );
- void setVelocity( const ml::Vector& v );
- void setState( const ml::Vector& p,const ml::Vector& v );
+ void setPosition( const ml::Vector& p );
+ void setVelocity( const ml::Vector& v );
+ void setState( const ml::Vector& p,const ml::Vector& v );
- protected:
- ml::Vector position,velocity;
+ protected:
+ ml::Vector position,velocity;
- }; // class DynamicIntegrator
-
- } // namespace dyninv
-} // namespace sot
+ }; // class DynamicIntegrator
+ } // namespace dyninv
+ } // namespace sot
+} // namespace dynamicgraph
#endif // #ifndef __sot_dyninv_DynamicIntegrator_H__
diff --git a/include/sot-dyninv/entity-helper.h b/src/entity-helper.h
similarity index 100%
rename from include/sot-dyninv/entity-helper.h
rename to src/entity-helper.h
diff --git a/include/sot-dyninv/mal-to-eigen.h b/src/mal-to-eigen.h
similarity index 100%
rename from include/sot-dyninv/mal-to-eigen.h
rename to src/mal-to-eigen.h
diff --git a/src/pseudo-robot-dynamic.cpp b/src/pseudo-robot-dynamic.cpp
index 93e8ca0468810ce5bf421504f60a78913bb9d6cd..a982074dc66f4caf0b3272f31bacb4e82d7d7e17 100644
--- a/src/pseudo-robot-dynamic.cpp
+++ b/src/pseudo-robot-dynamic.cpp
@@ -15,7 +15,7 @@
*/
#include <sot-dyninv/pseudo-robot-dynamic.h>
-#include <sot-core/debug.h>
+#include <sot/core/debug.hh>
#include <dynamic-graph/factory.h>
#include <dynamic-graph/pool.h>
@@ -24,335 +24,338 @@
#include <sot-dyninv/mal-to-eigen.h>
-namespace sot
+namespace dynamicgraph
{
- namespace dyninv
+ namespace sot
{
+ namespace dyninv
+ {
- namespace dg = ::dynamicgraph;
- using namespace dg;
-
- /* --- DG FACTORY ------------------------------------------------------- */
- DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(PseudoRobotDynamic,"PseudoRobotDynamic");
+ namespace dg = ::dynamicgraph;
+ using namespace dg;
- /* --- CONSTRUCTION ----------------------------------------------------- */
- /* --- CONSTRUCTION ----------------------------------------------------- */
- /* --- CONSTRUCTION ----------------------------------------------------- */
- PseudoRobotDynamic::
- PseudoRobotDynamic( const std::string & name )
- : DynamicIntegrator(name)
+ /* --- DG FACTORY ------------------------------------------------------- */
+ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(PseudoRobotDynamic,"PseudoRobotDynamic");
- ,CONSTRUCT_SIGNAL_IN(control,ml::Vector)
- ,CONSTRUCT_SIGNAL_OUT(qdot,ml::Vector,controlSIN)
+ /* --- CONSTRUCTION ----------------------------------------------------- */
+ /* --- CONSTRUCTION ----------------------------------------------------- */
+ /* --- CONSTRUCTION ----------------------------------------------------- */
+ PseudoRobotDynamic::
+ PseudoRobotDynamic( const std::string & name )
+ : DynamicIntegrator(name)
- ,CONSTRUCT_SIGNAL(rotation,OUT,ml::Vector)
- ,CONSTRUCT_SIGNAL(translation,OUT,ml::Vector)
- ,stateSOUT( &positionSOUT,getClassName()+"("+getName()+")::output(vector)::state" )
+ ,CONSTRUCT_SIGNAL_IN(control,ml::Vector)
+ ,CONSTRUCT_SIGNAL_OUT(qdot,ml::Vector,controlSIN)
- ,formerOpenHRP( NULL )
- {
- Entity::signalRegistration( controlSIN << qdotSOUT
- << rotationSOUT << translationSOUT
- << stateSOUT );
-
- /* --- COMMANDS --- */
- std::string doc
- = docCommandVoid2("Replace in the pool a robot entity by this,"
- "and modify the plugs (if 2de arg is true).",
- "string (entity name)","bool (plug)");
- addCommand("replace",
- makeCommandVoid2(*this,&PseudoRobotDynamic::replaceSimulatorEntity,
- doc));
- doc = docCommandVoid1("Set the root position, and forward to the real simulator.",
- "matrix homogeneous");
- addCommand("setRoot",
- makeCommandVoid1(*this,&PseudoRobotDynamic::setRoot,
- doc));
-
- doc = docCommandVoid1("Add to the current entity the command of the previous simu.",
- "string (command name)");
- addCommand("addForward",
- makeCommandVoid1(*this,&PseudoRobotDynamic::addForward,
- doc));
-
- }
-
- /* --- SIGNALS ---------------------------------------------------------- */
- /* --- SIGNALS ---------------------------------------------------------- */
- /* --- SIGNALS ---------------------------------------------------------- */
-
- /* Force the recomputation of the DynInt::controlSIN, and one step
- * of the integrator. Then, export the resulting joint velocity,
- * along with the 6D position of the free floating through signals
- * rotationSOUT and translationSOUT.
- */
- ml::Vector& PseudoRobotDynamic::
- qdotSOUT_function( ml::Vector& mlqdot, int time )
- {
- sotDEBUGIN(5);
+ ,CONSTRUCT_SIGNAL(rotation,OUT,ml::Vector)
+ ,CONSTRUCT_SIGNAL(translation,OUT,ml::Vector)
+ ,stateSOUT( &positionSOUT,getClassName()+"("+getName()+")::output(vector)::state" )
- controlSIN(time);
- integrateFromSignals(time);
+ ,formerOpenHRP( NULL )
+ {
+ Entity::signalRegistration( controlSIN << qdotSOUT
+ << rotationSOUT << translationSOUT
+ << stateSOUT );
+
+ /* --- COMMANDS --- */
+ std::string doc
+ = docCommandVoid2("Replace in the pool a robot entity by this,"
+ "and modify the plugs (if 2de arg is true).",
+ "string (entity name)","bool (plug)");
+ addCommand("replace",
+ makeCommandVoid2(*this,&PseudoRobotDynamic::replaceSimulatorEntity,
+ doc));
+ doc = docCommandVoid1("Set the root position, and forward to the real simulator.",
+ "matrix homogeneous");
+ addCommand("setRoot",
+ makeCommandVoid1(*this,&PseudoRobotDynamic::setRoot,
+ doc));
+
+ doc = docCommandVoid1("Add to the current entity the command of the previous simu.",
+ "string (command name)");
+ addCommand("addForward",
+ makeCommandVoid1(*this,&PseudoRobotDynamic::addForward,
+ doc));
- EIGEN_VECTOR_FROM_SIGNAL(v,velocity );
- EIGEN_VECTOR_FROM_VECTOR( qdot,mlqdot,v.size()-6 );
- qdot = v.tail(v.size()-6);
+ }
- EIGEN_VECTOR_FROM_SIGNAL(p,position );
+ /* --- SIGNALS ---------------------------------------------------------- */
+ /* --- SIGNALS ---------------------------------------------------------- */
+ /* --- SIGNALS ---------------------------------------------------------- */
+
+ /* Force the recomputation of the DynInt::controlSIN, and one step
+ * of the integrator. Then, export the resulting joint velocity,
+ * along with the 6D position of the free floating through signals
+ * rotationSOUT and translationSOUT.
+ */
+ ml::Vector& PseudoRobotDynamic::
+ qdotSOUT_function( ml::Vector& mlqdot, int time )
{
- ml::Vector mlv3;
- EIGEN_VECTOR_FROM_VECTOR( r,mlv3,3 );
- r = p.segment(3,3);
- rotationSOUT = mlv3;
- r = p.head(3);
- translationSOUT = mlv3;
- }
+ sotDEBUGIN(5);
- sotDEBUGOUT(5);
- return mlqdot;
- }
+ controlSIN(time);
+ integrateFromSignals(time);
- /* --- TOOLS ------------------------------------------------------------- */
- /* --- TOOLS ------------------------------------------------------------- */
- /* --- TOOLS ------------------------------------------------------------- */
+ EIGEN_VECTOR_FROM_SIGNAL(v,velocity );
+ EIGEN_VECTOR_FROM_VECTOR( qdot,mlqdot,v.size()-6 );
+ qdot = v.tail(v.size()-6);
- namespace PseudoRobotDynamic_Static
- {
- using namespace Eigen;
+ EIGEN_VECTOR_FROM_SIGNAL(p,position );
+ {
+ ml::Vector mlv3;
+ EIGEN_VECTOR_FROM_VECTOR( r,mlv3,3 );
+ r = p.segment(3,3);
+ rotationSOUT = mlv3;
+ r = p.head(3);
+ translationSOUT = mlv3;
+ }
+
+ sotDEBUGOUT(5);
+ return mlqdot;
+ }
+
+ /* --- TOOLS ------------------------------------------------------------- */
+ /* --- TOOLS ------------------------------------------------------------- */
+ /* --- TOOLS ------------------------------------------------------------- */
- template< typename D1 >
- Vector3d computeEulerFromRotationMatrix ( const MatrixBase<D1> & rotation )
+ namespace PseudoRobotDynamic_Static
{
- Vector3d euler;
-
- double rotationMatrix00 = rotation(0,0);
- double rotationMatrix10 = rotation(1,0);
- double rotationMatrix20 = rotation(2,0);
- double rotationMatrix01 = rotation(0,1);
- double rotationMatrix11 = rotation(1,1);
- double rotationMatrix21 = rotation(2,1);
- double rotationMatrix02 = rotation(0,2);
- double rotationMatrix12 = rotation(1,2);
- double rotationMatrix22 = rotation(2,2);
-
- double cosTheta = sqrt(0.5 * ( rotationMatrix00*rotationMatrix00
- + rotationMatrix10*rotationMatrix10
- + rotationMatrix21*rotationMatrix21
- + rotationMatrix22*rotationMatrix22 ));
- double sinTheta = -rotationMatrix20;
- euler[1] = atan2 (sinTheta, cosTheta);
-
- double cosTheta_cosPhi = 0.5 * (rotationMatrix22 * rotationMatrix11
- - rotationMatrix21 * rotationMatrix12);
- double cosTheta_sinPhi = 0.5 * (rotationMatrix21 * rotationMatrix02
- - rotationMatrix22 * rotationMatrix01);
- double cosTheta_cosPsi = 0.5 * (rotationMatrix00 * rotationMatrix11
- - rotationMatrix01 * rotationMatrix10);
- double cosTheta_sinPsi = 0.5 * (rotationMatrix10 * rotationMatrix02
- - rotationMatrix00 * rotationMatrix12);
-
- //if cosTheta == 0
- if (fabs(cosTheta) < 1e-9 )
- {
- if (sinTheta > 0.5) // sinTheta ~= 1
- {
- //phi_psi = phi - psi
- double phi_psi = atan2(- rotationMatrix10, rotationMatrix11);
- double psi = euler[2];
+ using namespace Eigen;
- double phi = phi_psi + psi;
- euler[0] = phi;
- }
- else //sinTheta ~= -1
- {
- //phi_psi = phi + psi
- double phi_psi = atan2(- rotationMatrix10, rotationMatrix11);
+ template< typename D1 >
+ Vector3d computeEulerFromRotationMatrix ( const MatrixBase<D1> & rotation )
+ {
+ Vector3d euler;
+
+ double rotationMatrix00 = rotation(0,0);
+ double rotationMatrix10 = rotation(1,0);
+ double rotationMatrix20 = rotation(2,0);
+ double rotationMatrix01 = rotation(0,1);
+ double rotationMatrix11 = rotation(1,1);
+ double rotationMatrix21 = rotation(2,1);
+ double rotationMatrix02 = rotation(0,2);
+ double rotationMatrix12 = rotation(1,2);
+ double rotationMatrix22 = rotation(2,2);
+
+ double cosTheta = sqrt(0.5 * ( rotationMatrix00*rotationMatrix00
+ + rotationMatrix10*rotationMatrix10
+ + rotationMatrix21*rotationMatrix21
+ + rotationMatrix22*rotationMatrix22 ));
+ double sinTheta = -rotationMatrix20;
+ euler[1] = atan2 (sinTheta, cosTheta);
+
+ double cosTheta_cosPhi = 0.5 * (rotationMatrix22 * rotationMatrix11
+ - rotationMatrix21 * rotationMatrix12);
+ double cosTheta_sinPhi = 0.5 * (rotationMatrix21 * rotationMatrix02
+ - rotationMatrix22 * rotationMatrix01);
+ double cosTheta_cosPsi = 0.5 * (rotationMatrix00 * rotationMatrix11
+ - rotationMatrix01 * rotationMatrix10);
+ double cosTheta_sinPsi = 0.5 * (rotationMatrix10 * rotationMatrix02
+ - rotationMatrix00 * rotationMatrix12);
+
+ //if cosTheta == 0
+ if (fabs(cosTheta) < 1e-9 )
+ {
+ if (sinTheta > 0.5) // sinTheta ~= 1
+ {
+ //phi_psi = phi - psi
+ double phi_psi = atan2(- rotationMatrix10, rotationMatrix11);
+ double psi = euler[2];
+
+ double phi = phi_psi + psi;
+ euler[0] = phi;
+ }
+ else //sinTheta ~= -1
+ {
+ //phi_psi = phi + psi
+ double phi_psi = atan2(- rotationMatrix10, rotationMatrix11);
+
+ double psi = euler[2];
+
+ double phi = phi_psi;
+ euler[0] = phi - psi;
+ }
+ }
+ else
+ {
+ double cosPsi = cosTheta_cosPsi / cosTheta;
+ double sinPsi = cosTheta_sinPsi / cosTheta;
+ euler[0] = atan2 (sinPsi, cosPsi);
- double psi = euler[2];
+ double cosPhi = cosTheta_cosPhi / cosTheta;
+ double sinPhi = cosTheta_sinPhi / cosTheta;
+ euler[2] = atan2 (sinPhi, cosPhi);
+ }
- double phi = phi_psi;
- euler[0] = phi - psi;
- }
- }
- else
- {
- double cosPsi = cosTheta_cosPsi / cosTheta;
- double sinPsi = cosTheta_sinPsi / cosTheta;
- euler[0] = atan2 (sinPsi, cosPsi);
+ return euler;
+ }
- double cosPhi = cosTheta_cosPhi / cosTheta;
- double sinPhi = cosTheta_sinPhi / cosTheta;
- euler[2] = atan2 (sinPhi, cosPhi);
- }
+ } // namespace PseudoRobotDynamic_Static
- return euler;
- }
+ /* --- COMMANDS ---------------------------------------------------------- */
+ /* --- COMMANDS ---------------------------------------------------------- */
+ /* --- COMMANDS ---------------------------------------------------------- */
- } // namespace PseudoRobotDynamic_Static
- /* --- COMMANDS ---------------------------------------------------------- */
- /* --- COMMANDS ---------------------------------------------------------- */
- /* --- COMMANDS ---------------------------------------------------------- */
+ void PseudoRobotDynamic::
+ replaceSimulatorEntity( const std::string& formerName, const bool & plug )
+ {
+ assert( formerOpenHRP == NULL );
+ formerOpenHRP = &dg::g_pool.getEntity( formerName );
+ dg::g_pool.deregisterEntity( formerName );
+ // former.name = formerName+".old";
+ dg::g_pool.registerEntity( formerName+"-old",formerOpenHRP );
+ entityDeregistration(); name = formerName; entityRegistration();
- void PseudoRobotDynamic::
- replaceSimulatorEntity( const std::string& formerName, const bool & plug )
- {
- assert( formerOpenHRP == NULL );
- formerOpenHRP = &dg::g_pool.getEntity( formerName );
- dg::g_pool.deregisterEntity( formerName );
- // former.name = formerName+".old";
- dg::g_pool.registerEntity( formerName+"-old",formerOpenHRP );
+ if( plug )
+ {
+ formerOpenHRP->getSignal("control").plug( & qdotSOUT );
- entityDeregistration(); name = formerName; entityRegistration();
+ try
+ {
+ formerOpenHRP->getSignal("attitudeIN").plug( & rotationSOUT );
+ formerOpenHRP->getSignal("translation").plug( & translationSOUT );
+ }
+ catch (...) {}
- if( plug )
- {
- formerOpenHRP->getSignal("control").plug( & qdotSOUT );
+ const ml::Vector& pos
+ = dynamic_cast< dg::Signal<ml::Vector,int>& >
+ ( formerOpenHRP->getSignal("state") ).accessCopy();
+ try
+ {
+ const ml::Vector& vel
+ = dynamic_cast< dg::Signal<ml::Vector,int>& >
+ ( formerOpenHRP->getSignal("velocity") ).accessCopy();
+ setState(pos,vel);
+ }
+ catch (... )
+ {
+ ml::Vector velzero( pos.size() ); velzero.setZero();
+ setState(pos,velzero);
+ }
+ }
+ }
+ void PseudoRobotDynamic::
+ setRoot( const ml::Matrix & mlM )
+ {
+ sotDEBUG(15) << "Set root with " << mlM << std::endl;
+ using namespace Eigen;
+ using PseudoRobotDynamic_Static::computeEulerFromRotationMatrix;
+
+ EIGEN_MATRIX_FROM_SIGNAL(M,mlM);
+ Vector3d r = computeEulerFromRotationMatrix( M.topLeftCorner(3,3) );
+ EIGEN_VECTOR_FROM_SIGNAL( q,position );
+ if( q.size()<6 )
+ {
+ throw; // TODO
+ }
+ q.head(3) = M.col(3).head(3);
+ q.segment(3,3) = r;
+ if( formerOpenHRP )
try
{
- formerOpenHRP->getSignal("attitudeIN").plug( & rotationSOUT );
- formerOpenHRP->getSignal("translation").plug( & translationSOUT );
+ forwardVoidCommandToSimu( "setRoot",mlM );
}
- catch (...) {}
+ catch(...) {}
+ }
- const ml::Vector& pos
- = dynamic_cast< dg::Signal<ml::Vector,int>& >
- ( formerOpenHRP->getSignal("state") ).accessCopy();
- try
- {
- const ml::Vector& vel
- = dynamic_cast< dg::Signal<ml::Vector,int>& >
- ( formerOpenHRP->getSignal("velocity") ).accessCopy();
- setState(pos,vel);
- }
- catch (... )
- {
- ml::Vector velzero( pos.size() ); velzero.setZero();
- setState(pos,velzero);
- }
- }
- }
- void PseudoRobotDynamic::
- setRoot( const ml::Matrix & mlM )
- {
- sotDEBUG(15) << "Set root with " << mlM << std::endl;
- using namespace Eigen;
- using PseudoRobotDynamic_Static::computeEulerFromRotationMatrix;
-
- EIGEN_MATRIX_FROM_SIGNAL(M,mlM);
- Vector3d r = computeEulerFromRotationMatrix( M.topLeftCorner(3,3) );
- EIGEN_VECTOR_FROM_SIGNAL( q,position );
- if( q.size()<6 )
- {
- throw; // TODO
- }
- q.head(3) = M.col(3).head(3);
- q.segment(3,3) = r;
- if( formerOpenHRP )
- try
- {
- forwardVoidCommandToSimu( "setRoot",mlM );
- }
- catch(...) {}
- }
+ /* --- FORWARD ---------------------------------------------------------- */
+ /* This function is a forward on the command on the former simu entity
+ * <formerOpenHRP>: when calling it, the parameters are send to the named
+ * command, that is executed.
+ */
+ template< typename T1 >
+ void PseudoRobotDynamic::
+ forwardVoidCommandToSimu( const std::string& cmdName,
+ const T1& arg1 )
+ {
+ /* Forward to previous entity. */
+ assert( formerOpenHRP );
- /* --- FORWARD ---------------------------------------------------------- */
+ using dg::command::Command;
+ using dg::command::Value;
- /* This function is a forward on the command on the former simu entity
- * <formerOpenHRP>: when calling it, the parameters are send to the named
- * command, that is executed.
- */
- template< typename T1 >
- void PseudoRobotDynamic::
- forwardVoidCommandToSimu( const std::string& cmdName,
- const T1& arg1 )
- {
- /* Forward to previous entity. */
- assert( formerOpenHRP );
-
- using dg::command::Command;
- using dg::command::Value;
-
- Command* command = formerOpenHRP->getNewStyleCommand(cmdName);
- std::vector<Value> valuesArg;
- valuesArg.push_back( Value( arg1 ) );
- command->setParameterValues(valuesArg);
- command->execute();
- }
-
- /* Add the named command of the former simu entity <formerOpenHRP> to the
- * commands of the entity.
- */
- void PseudoRobotDynamic::
- addForward( const std::string& cmdName )
- {
- assert( formerOpenHRP );
- addCommand(cmdName,formerOpenHRP->getNewStyleCommand(cmdName));
- }
+ Command* command = formerOpenHRP->getNewStyleCommand(cmdName);
+ std::vector<Value> valuesArg;
+ valuesArg.push_back( Value( arg1 ) );
+ command->setParameterValues(valuesArg);
+ command->execute();
+ }
+ /* Add the named command of the former simu entity <formerOpenHRP> to the
+ * commands of the entity.
+ */
+ void PseudoRobotDynamic::
+ addForward( const std::string& cmdName )
+ {
+ assert( formerOpenHRP );
+ addCommand(cmdName,formerOpenHRP->getNewStyleCommand(cmdName));
+ }
- /* --- ENTITY ----------------------------------------------------------- */
- /* --- ENTITY ----------------------------------------------------------- */
- /* --- ENTITY ----------------------------------------------------------- */
- void PseudoRobotDynamic::
- display( std::ostream& os ) const
- {
- os << "PseudoRobotDynamic "<<getName() << ". " << std::endl;
- }
+ /* --- ENTITY ----------------------------------------------------------- */
+ /* --- ENTITY ----------------------------------------------------------- */
+ /* --- ENTITY ----------------------------------------------------------- */
- void PseudoRobotDynamic::
- commandLine( const std::string& cmdLine,
- std::istringstream& cmdArgs,
- std::ostream& os )
- {
- sotDEBUGIN(15);
+ void PseudoRobotDynamic::
+ display( std::ostream& os ) const
+ {
+ os << "PseudoRobotDynamic "<<getName() << ". " << std::endl;
+ }
- if( cmdLine == "help" )
- {
- os << "PseudoRobotDynamic:" << std::endl
- << " - replace <OpenHRP> [plug]" << std::endl;
- }
- else if( cmdLine == "replace" )
- {
- if( cmdArgs >> std::ws, cmdArgs.good() )
- {
- std::string repName; cmdArgs >> repName >> std::ws;
- bool plug = cmdArgs.good();
- replaceSimulatorEntity( repName,plug );
- }
- }
- else if( cmdLine=="sbs" || cmdLine=="play" || cmdLine =="withForces"
- || cmdLine == "periodicCall" || cmdLine == "periodicCallBefore"
- || cmdLine == "periodicCallAfter" )
- {
- formerOpenHRP ->commandLine( cmdLine,cmdArgs,os );
- }
- else if( cmdLine=="root" )
- {
- if( cmdArgs >> std::ws, cmdArgs.good() )
- {
- ml::Matrix M; cmdArgs >> M; setRoot(M);
- std::ostringstream osback;
- osback << M.accessToMotherLib(); cmdArgs.str(osback.str());
- formerOpenHRP ->commandLine( cmdLine,cmdArgs,os );
- }
- else
- {
- os << "TODO" << std::endl;
- }
- }
- else
- {
- Entity::commandLine( cmdLine,cmdArgs,os );
- }
- sotDEBUGOUT(15);
- }
+ void PseudoRobotDynamic::
+ commandLine( const std::string& cmdLine,
+ std::istringstream& cmdArgs,
+ std::ostream& os )
+ {
+ sotDEBUGIN(15);
+
+ if( cmdLine == "help" )
+ {
+ os << "PseudoRobotDynamic:" << std::endl
+ << " - replace <OpenHRP> [plug]" << std::endl;
+ }
+ else if( cmdLine == "replace" )
+ {
+ if( cmdArgs >> std::ws, cmdArgs.good() )
+ {
+ std::string repName; cmdArgs >> repName >> std::ws;
+ bool plug = cmdArgs.good();
+ replaceSimulatorEntity( repName,plug );
+ }
+ }
+ else if( cmdLine=="sbs" || cmdLine=="play" || cmdLine =="withForces"
+ || cmdLine == "periodicCall" || cmdLine == "periodicCallBefore"
+ || cmdLine == "periodicCallAfter" )
+ {
+ formerOpenHRP ->commandLine( cmdLine,cmdArgs,os );
+ }
+ else if( cmdLine=="root" )
+ {
+ if( cmdArgs >> std::ws, cmdArgs.good() )
+ {
+ ml::Matrix M; cmdArgs >> M; setRoot(M);
+ std::ostringstream osback;
+ osback << M.accessToMotherLib(); cmdArgs.str(osback.str());
+ formerOpenHRP ->commandLine( cmdLine,cmdArgs,os );
+ }
+ else
+ {
+ os << "TODO" << std::endl;
+ }
+ }
+ else
+ {
+ Entity::commandLine( cmdLine,cmdArgs,os );
+ }
+ sotDEBUGOUT(15);
+ }
- } // namespace dyninv
-} // namespace sot
+ } // namespace dyninv
+ } // namespace sot
+} // namespace dynamicgraph
diff --git a/src/pseudo-robot-dynamic.h b/src/pseudo-robot-dynamic.h
new file mode 100644
index 0000000000000000000000000000000000000000..3e1533ca460ff35eb01d0b85c5337fe7aeb74aab
--- /dev/null
+++ b/src/pseudo-robot-dynamic.h
@@ -0,0 +1,116 @@
+/*
+ * Copyright 2011, Nicolas Mansard, LAAS-CNRS
+ *
+ * This file is part of sot-dyninv.
+ * sot-dyninv is free software: you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public License
+ * as published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ * sot-dyninv is distributed in the hope that it will be
+ * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details. You should
+ * have received a copy of the GNU Lesser General Public License along
+ * with sot-dyninv. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __sot_dyninv_PseudoRobotDynamic_H__
+#define __sot_dyninv_PseudoRobotDynamic_H__
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined (WIN32)
+# if defined (pseudo_robot_dynamic_EXPORTS)
+# define SOTPSEUDOROBOTDYNAMIC_EXPORT __declspec(dllexport)
+# else
+# define SOTPSEUDOROBOTDYNAMIC_EXPORT __declspec(dllimport)
+# endif
+#else
+# define SOTPSEUDOROBOTDYNAMIC_EXPORT
+#endif
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+
+/* SOT */
+#include <sot-dyninv/signal-helper.h>
+#include <sot-dyninv/entity-helper.h>
+#include <sot-dyninv/dynamic-integrator.h>
+
+namespace dynamicgraph {
+ namespace sot {
+ namespace dyninv {
+
+ /* --------------------------------------------------------------------- */
+ /* --- CLASS ----------------------------------------------------------- */
+ /* --------------------------------------------------------------------- */
+
+ /* Inside this entity is an integrator, taking position, velocity and
+ * acceleration at time t, and computing pos and vel at time t+1. Around
+ * this integrator, a wrapper is made to make the stuff take the place of
+ * the OpenHRP kinematic entity. it thus take a "control" signal, to triger
+ * the computations, and possesses all the functionnalities of the OpenHRP
+ * entity. Plus, it has the signals to feed the OpenHRP entity that
+ * wrappes the simulator. All the computations are triggered by computing
+ * the "qdot" signal.
+ */
+
+ class SOTPSEUDOROBOTDYNAMIC_EXPORT PseudoRobotDynamic
+ :public DynamicIntegrator
+ ,public ::dynamicgraph::EntityHelper<PseudoRobotDynamic>
+ {
+
+ public: /* --- CONSTRUCTOR ---- */
+
+ PseudoRobotDynamic( const std::string & name );
+
+ public: /* --- ENTITY INHERITANCE --- */
+
+ static const std::string CLASS_NAME;
+ virtual void display( std::ostream& os ) const;
+ virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
+
+ virtual void commandLine( const std::string& cmdLine,
+ std::istringstream& cmdArgs,
+ std::ostream& os );
+
+ typedef ::dynamicgraph::EntityHelper<PseudoRobotDynamic>::EntityClassName
+ EntityClassName;
+
+ public: /* --- SIGNALS --- */
+
+ DECLARE_SIGNAL_IN( control,ml::Vector );
+ DECLARE_SIGNAL_OUT( qdot,ml::Vector );
+
+ DECLARE_SIGNAL(rotation,OUT,ml::Vector);
+ DECLARE_SIGNAL(translation,OUT,ml::Vector);
+ //sotSignal< ml::Vector,int > rotationSOUT;
+ //sotSignal< ml::Vector,int > translationSOUT;
+ ::dynamicgraph::SignalPtr< ml::Vector,int > stateSOUT;
+
+ public: /* --- SIGNALS --- */
+
+ void replaceSimulatorEntity( const std::string& formerName,
+ const bool& plug = false );
+ void setRoot( const ml::Matrix & M );
+
+ public: /* --- COMMAND --- */
+ template< typename T1 >
+ void forwardVoidCommandToSimu( const std::string& cmdName,
+ const T1& arg1 );
+ void addForward( const std::string& cmdName );
+
+ private:
+ ::dynamicgraph::Entity* formerOpenHRP;
+
+ }; // class PseudoRobotDynamic
+ } // namespace dyninv
+ } // namespace sot
+} // namespace dynamicgraph
+
+
+
+#endif // #ifndef __sot_dyninv_PseudoRobotDynamic_H__
diff --git a/include/sot-dyninv/signal-helper.h b/src/signal-helper.h
similarity index 100%
rename from include/sot-dyninv/signal-helper.h
rename to src/signal-helper.h
diff --git a/src/solver-op-space.cpp b/src/solver-op-space.cpp
index 185d610e2ab2cf53f6e82e0321cc1601475134ce..22d8bdcee7cbb7596741d5aa5fcad474babebe36 100644
--- a/src/solver-op-space.cpp
+++ b/src/solver-op-space.cpp
@@ -14,10 +14,20 @@
* with sot-dyninv. If not, see <http://www.gnu.org/licenses/>.
*/
+//#define VP_DEBUG
+#define VP_DEBUG_MODE 50
+#include <sot/core/debug.hh>
+#ifdef VP_DEBUG
+class solver_op_space__INIT
+{
+public:solver_op_space__INIT( void ) { dynamicgraph::sot::DebugTrace::openFile(); }
+};
+solver_op_space__INIT solver_op_space_initiator;
+#endif //#ifdef VP_DEBUG
+
#include <sot-dyninv/solver-op-space.h>
#include <sot-dyninv/commands-helper.h>
-#include <sot-core/debug.h>
#include <dynamic-graph/factory.h>
#include <boost/foreach.hpp>
#include <sot-dyninv/commands-helper.h>
@@ -26,444 +36,446 @@
#include <soth/HCOD.hpp>
#include <sot-dyninv/mal-to-eigen.h>
#include <sot-dyninv/task-dyn-pd.h>
-#include <sot-core/feature-point6d.h>
+#include <sot/core/feature-point6d.hh>
#include <sstream>
#include <soth/Algebra.hpp>
#include <Eigen/QR>
-namespace sot
+namespace dynamicgraph
{
- namespace dyninv
+ namespace sot
{
-
- namespace dg = ::dynamicgraph;
- using namespace dg;
- using dg::SignalBase;
-
- /* --- DG FACTORY ------------------------------------------------------- */
- DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SolverOpSpace,"SolverOpSpace");
-
- /* --- CONSTRUCTION ----------------------------------------------------- */
- /* --- CONSTRUCTION ----------------------------------------------------- */
- /* --- CONSTRUCTION ----------------------------------------------------- */
- SolverOpSpace::
- SolverOpSpace( const std::string & name )
- : Entity(name)
- ,stack_t()
-
- ,CONSTRUCT_SIGNAL_IN(matrixInertia,ml::Matrix)
- ,CONSTRUCT_SIGNAL_IN(velocity,ml::Vector)
- ,CONSTRUCT_SIGNAL_IN(dyndrift,ml::Vector)
- ,CONSTRUCT_SIGNAL_IN(damping,double)
- ,CONSTRUCT_SIGNAL_IN(breakFactor,double)
- ,CONSTRUCT_SIGNAL_OUT(control,ml::Vector,
- matrixInertiaSIN << dyndriftSIN
- << velocitySIN << controlSOUT)
- ,CONSTRUCT_SIGNAL(zmp,OUT,ml::Vector)
- ,CONSTRUCT_SIGNAL(acceleration,OUT,ml::Vector)
-
- ,nbParam(0), nq(0),ntau(0),nfs(0)
- ,hsolver()
-
- ,Cdyn(),Ccontact(),Czmp(),Czero()
- ,bdyn(),bcontact(),bzmp(),bzero()
- ,Ctasks(),btasks()
- ,solution()
- {
- signalRegistration( matrixInertiaSIN << dyndriftSIN
- << velocitySIN << controlSOUT
- << zmpSOUT << accelerationSOUT
- << dampingSIN << breakFactorSIN );
-
- /* Command registration. */
- boost::function<void(SolverOpSpace*,const std::string&)> f_addContact
- =
- boost::bind( &SolverOpSpace::addContact,_1,_2,
- (dynamicgraph::Signal<ml::Matrix, int>*)NULL,
- (dynamicgraph::Signal<ml::Matrix, int>*)NULL,
- (dynamicgraph::Signal<ml::Vector, int>*)NULL,
- (dynamicgraph::Signal<ml::Matrix, int>*)NULL);
- addCommand("addContact",
- makeCommandVoid1(*this,f_addContact,
- docCommandVoid1("create the contact signals, unpluged.",
- "string")));
- addCommand("addContactFromTask",
- makeCommandVoid2(*this,&SolverOpSpace::addContactFromTask,
- docCommandVoid2("Add a contact from the named task. Remmeber to plug __p.",
- "string(task name)","string (contact name)")));
- addCommand("rmContact",
- makeCommandVoid1(*this,&SolverOpSpace::removeContact,
- docCommandVoid1("remove the contact named in arguments.",
- "string")));
- addCommand("dispContacts",
- makeCommandVerbose(*this,&SolverOpSpace::dispContacts,
- docCommandVerbose("Guess what?")));
- addCommand("debugOnce",
- makeCommandVoid0(*this,&SolverOpSpace::debugOnce,
- docCommandVoid0("open trace-file for next iteration of the solver.")));
-
- ADD_COMMANDS_FOR_THE_STACK;
- }
-
- /* --- STACK ----------------------------------------------------------- */
- /* --- STACK ----------------------------------------------------------- */
- /* --- STACK ----------------------------------------------------------- */
-
- SolverOpSpace::TaskDependancyList_t SolverOpSpace::
- getTaskDependancyList( const TaskDynPD& task )
- {
- TaskDependancyList_t res;
- res.push_back( &task.taskSOUT );
- res.push_back( &task.jacobianSOUT );
- res.push_back( &task.JdotSOUT );
- return res;
- }
- void SolverOpSpace::
- addDependancy( const TaskDependancyList_t& depList )
- {
- BOOST_FOREACH( const SignalBase<int>* sig, depList )
- { controlSOUT.addDependency( *sig ); }
- }
- void SolverOpSpace::
- removeDependancy( const TaskDependancyList_t& depList )
+ namespace dyninv
{
- BOOST_FOREACH( const SignalBase<int>* sig, depList )
- { controlSOUT.removeDependency( *sig ); }
- }
- void SolverOpSpace::
- resetReady( void )
- {
- controlSOUT.setReady();
- }
-
- /* --- CONTACT LIST ---------------------------------------------------- */
- /* --- CONTACT LIST ---------------------------------------------------- */
- /* --- CONTACT LIST ---------------------------------------------------- */
- /* TODO: push this method directly in signal. */
- static std::string signalShortName( const std::string & longName )
- {
- std::istringstream iss( longName );
- const int SIZE = 128;
- char buffer[SIZE];
- while( iss.good() )
- { iss.getline(buffer,SIZE,':'); }
- return std::string( buffer );
- }
-
- void SolverOpSpace::
- addContactFromTask( const std::string & taskName,const std::string & contactName )
- {
- using namespace dynamicgraph;
+ namespace dg = ::dynamicgraph;
+ using namespace dg;
+ using dg::SignalBase;
+
+ /* --- DG FACTORY ------------------------------------------------------- */
+ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SolverOpSpace,"SolverOpSpace");
+
+ /* --- CONSTRUCTION ----------------------------------------------------- */
+ /* --- CONSTRUCTION ----------------------------------------------------- */
+ /* --- CONSTRUCTION ----------------------------------------------------- */
+ SolverOpSpace::
+ SolverOpSpace( const std::string & name )
+ : Entity(name)
+ ,stack_t()
+
+ ,CONSTRUCT_SIGNAL_IN(matrixInertia,ml::Matrix)
+ ,CONSTRUCT_SIGNAL_IN(velocity,ml::Vector)
+ ,CONSTRUCT_SIGNAL_IN(dyndrift,ml::Vector)
+ ,CONSTRUCT_SIGNAL_IN(damping,double)
+ ,CONSTRUCT_SIGNAL_IN(breakFactor,double)
+ ,CONSTRUCT_SIGNAL_OUT(control,ml::Vector,
+ matrixInertiaSIN << dyndriftSIN
+ << velocitySIN << controlSOUT)
+ ,CONSTRUCT_SIGNAL(zmp,OUT,ml::Vector)
+ ,CONSTRUCT_SIGNAL(acceleration,OUT,ml::Vector)
+
+ ,nbParam(0), nq(0),ntau(0),nfs(0)
+ ,hsolver()
+
+ ,Cdyn(),Ccontact(),Czmp(),Czero()
+ ,bdyn(),bcontact(),bzmp(),bzero()
+ ,Ctasks(),btasks()
+ ,solution()
+ {
+ signalRegistration( matrixInertiaSIN << dyndriftSIN
+ << velocitySIN << controlSOUT
+ << zmpSOUT << accelerationSOUT
+ << dampingSIN << breakFactorSIN );
+
+ /* Command registration. */
+ boost::function<void(SolverOpSpace*,const std::string&)> f_addContact
+ =
+ boost::bind( &SolverOpSpace::addContact,_1,_2,
+ (dynamicgraph::Signal<ml::Matrix, int>*)NULL,
+ (dynamicgraph::Signal<ml::Matrix, int>*)NULL,
+ (dynamicgraph::Signal<ml::Vector, int>*)NULL,
+ (dynamicgraph::Signal<ml::Matrix, int>*)NULL);
+ addCommand("add.Contact",
+ makeCommandVoid1(*this,f_addContact,
+ docCommandVoid1("create the contact signals, unpluged.",
+ "string")));
+ addCommand("addContactFromTask",
+ makeCommandVoid2(*this,&SolverOpSpace::addContactFromTask,
+ docCommandVoid2("Add a contact from the named task. Remmeber to plug __p.",
+ "string(task name)","string (contact name)")));
+ addCommand("rmContact",
+ makeCommandVoid1(*this,&SolverOpSpace::removeContact,
+ docCommandVoid1("remove the contact named in arguments.",
+ "string")));
+ addCommand("dispContacts",
+ makeCommandVerbose(*this,&SolverOpSpace::dispContacts,
+ docCommandVerbose("Guess what?")));
+ addCommand("debugOnce",
+ makeCommandVoid0(*this,&SolverOpSpace::debugOnce,
+ docCommandVoid0("open trace-file for next iteration of the solver.")));
+
+ ADD_COMMANDS_FOR_THE_STACK;
+ }
- TaskDynPD & task = dynamic_cast<TaskDynPD&> ( g_pool.getEntity( taskName ) );
- assert( task.getFeatureList().size() == 1 );
- BOOST_FOREACH( FeatureAbstract* fptr, task.getFeatureList() )
- {
- FeaturePoint6d* f6 = dynamic_cast< FeaturePoint6d* >( fptr );
- assert( NULL!=f6 );
- f6->positionSIN.recompute(controlSOUT.getTime());
- f6->servoCurrentPosition();
- f6->FeatureAbstract::selectionSIN = true;
- }
- addContact( contactName, &task.jacobianSOUT, &task.JdotSOUT,&task.taskVectorSOUT, NULL );
- }
-
- void SolverOpSpace::
- addContact( const std::string & name,
- Signal<ml::Matrix,int> * jacobianSignal,
- Signal<ml::Matrix,int> * JdotSignal,
- Signal<ml::Vector,int> * corrSignal,
- Signal<ml::Matrix,int> * contactPointsSignal )
- {
- if( contactMap.find(name) != contactMap.end())
- {
- std::cerr << "!! Contact " << name << " already exists." << std::endl;
- return;
- }
+ /* --- STACK ----------------------------------------------------------- */
+ /* --- STACK ----------------------------------------------------------- */
+ /* --- STACK ----------------------------------------------------------- */
- contactMap[name].jacobianSIN
- = matrixSINPtr( new SignalPtr<ml::Matrix,int>
- ( jacobianSignal,
- "sotDynInvWB("+getName()+")::input(matrix)::_"+name+"_J" ) );
- signalRegistration( *contactMap[name].jacobianSIN );
-
- contactMap[name].JdotSIN
- = matrixSINPtr( new SignalPtr<ml::Matrix,int>
- ( JdotSignal,
- "sotDynInvWB("+getName()+")::input(matrix)::_"+name+"_Jdot" ) );
- signalRegistration( *contactMap[name].JdotSIN );
-
- contactMap[name].supportSIN
- = matrixSINPtr( new SignalPtr<ml::Matrix,int>
- ( contactPointsSignal,
- "sotDynInvWB("+getName()+")::input(matrix)::_"+name+"_p" ) );
- signalRegistration( *contactMap[name].supportSIN );
-
- contactMap[name].correctorSIN
- = vectorSINPtr( new SignalPtr<ml::Vector,int>
- ( corrSignal,
- "sotDynInvWB("+getName()+")::input(vector)::_"+name+"_x" ) );
- signalRegistration( *contactMap[name].correctorSIN );
-
- contactMap[name].forceSOUT
- = vectorSOUTPtr( new Signal<ml::Vector,int>
- ( "sotDynInvWB("+getName()+")::output(vector)::_"+name+"_f" ) );
- signalRegistration( *contactMap[name].forceSOUT );
-
- contactMap[name].fnSOUT
- = vectorSOUTPtr( new Signal<ml::Vector,int>
- ( "sotDynInvWB("+getName()+")::output(vector)::_"+name+"_fn" ) );
- signalRegistration( *contactMap[name].fnSOUT );
-
- }
-
- void SolverOpSpace::
- removeContact( const std::string & name )
- {
- if( contactMap.find(name) == contactMap.end() )
- {
- std::cerr << "!! Contact " << name << " does not exist." << std::endl;
- return;
- }
+ SolverOpSpace::TaskDependancyList_t SolverOpSpace::
+ getTaskDependancyList( const TaskDynPD& task )
+ {
+ TaskDependancyList_t res;
+ res.push_back( &task.taskSOUT );
+ res.push_back( &task.jacobianSOUT );
+ res.push_back( &task.JdotSOUT );
+ return res;
+ }
+ void SolverOpSpace::
+ addDependancy( const TaskDependancyList_t& depList )
+ {
+ BOOST_FOREACH( const SignalBase<int>* sig, depList )
+ { controlSOUT.addDependency( *sig ); }
+ }
+ void SolverOpSpace::
+ removeDependancy( const TaskDependancyList_t& depList )
+ {
+ BOOST_FOREACH( const SignalBase<int>* sig, depList )
+ { controlSOUT.removeDependency( *sig ); }
+ }
+ void SolverOpSpace::
+ resetReady( void )
+ {
+ controlSOUT.setReady();
+ }
- signalDeregistration( signalShortName(contactMap[name].jacobianSIN->getName()) );
- signalDeregistration( signalShortName(contactMap[name].supportSIN->getName()) );
- signalDeregistration( signalShortName(contactMap[name].forceSOUT->getName()) );
- signalDeregistration( signalShortName(contactMap[name].fnSOUT->getName()) );
- signalDeregistration( signalShortName(contactMap[name].JdotSIN->getName()) );
- signalDeregistration( signalShortName(contactMap[name].correctorSIN->getName()) );
- contactMap.erase(name);
- }
-
- void SolverOpSpace::
- dispContacts( std::ostream& os ) const
- {
- os << "+-----------------\n+ Contacts\n+-----------------" << std::endl;
- // TODO BOOST FOREACH
- for( contacts_t::const_iterator iter=contactMap.begin();
- iter!=contactMap.end(); ++iter )
- {
- os << "| " << iter->first <<std::endl;
- }
- }
+ /* --- CONTACT LIST ---------------------------------------------------- */
+ /* --- CONTACT LIST ---------------------------------------------------- */
+ /* --- CONTACT LIST ---------------------------------------------------- */
- /* --------------------------------------------------------------------- */
- /* --- STATIC INTERNAL ------------------------------------------------- */
- /* --------------------------------------------------------------------- */
- namespace sotOPH
- {
- template< typename D1,typename D2 >
- void preCross( const Eigen::MatrixBase<D1> & M,Eigen::MatrixBase<D2> & Tx )
+ /* TODO: push this method directly in signal. */
+ static std::string signalShortName( const std::string & longName )
{
- EIGEN_STATIC_ASSERT_VECTOR_ONLY(Eigen::MatrixBase<D1>);
- assert( Tx.cols()==3 && Tx.rows()==3 && M.size()==3 );
- Tx( 0,0 ) = 0 ; Tx( 0,1 )=-M[2]; Tx( 0,2 ) = M[1];
- Tx( 1,0 ) = M[2]; Tx( 1,1 )= 0 ; Tx( 1,2 ) =-M[0];
- Tx( 2,0 ) =-M[1]; Tx( 2,1 )= M[0]; Tx( 2,2 ) = 0 ;
+ std::istringstream iss( longName );
+ const int SIZE = 128;
+ char buffer[SIZE];
+ while( iss.good() )
+ { iss.getline(buffer,SIZE,':'); }
+ return std::string( buffer );
}
- template< typename D1, typename D2 >
- void computeForceNormalConversion( Eigen::MatrixBase<D1> & Ci,
- Eigen::MatrixBase<D2> & positions )
+ void SolverOpSpace::
+ addContactFromTask( const std::string & taskName,const std::string & contactName )
{
- /* General Constraint is: phi^0 = Psi.fi, with Psi = [ I; skew(OP1);
- skew(OP2); skew(OP3); skew(OP4) ]. But phi^0 = X_c^0*phi^c (both feet
- are 0.105cm above the ground so X_c^0 is the transformation matri from
- actual ankle force to the actual contact force). * X_c^0*phi^c - Psi*fi
- = 0 Since phi_{x,y,Rz}^0 ~ fi_{x,y} and phi_{z,Rx,Ry} ~ fi_{z,Rx,Ry}: *
- Reduced Constraint Ci is applied as:[ X_c^0*phi^c - Psi*fi ]_{z,Rx,Ry} =
- 0 [ C_phi -C_fi ]_{z,Rx,Ry}*[phi^c; fi_{z,Rx,Ry}] = 0, so Ci = [ C_phi
- -C_fi ]_{z,Rx,Ry} with C_phi = X_c^0 and C_fi = Psi.
- */
-
- using namespace Eigen;
-
- const int nbPoint = positions.cols();
- assert( positions.rows()==3 );
- assert( Ci.rows()==3 && Ci.cols()==6+nbPoint );
-
- //Ci.leftCols(6) = -MatrixXd::Identity(6,6);
- /* Ci = [ X_c^0_{z,Rx,Ry} -Psi_{z,Rx,Ry}];
- * with X_c^0_{z,Rx,Ry} = [ 0 0 1 0 0 0; 0 -z 0 1 0 0; z 0 0 0 1 0 ]
- * and Psi_{z,Rx,Ry} = [ 1 1 1 1; y_1 y_2 y_3 y_4; -x_1 -x_2 -x_3 -x_4 ]. */
- const double z = positions(2,0);
- Ci.setZero();
- Ci(0,2)=1;
- Ci(1,1)=-z; Ci(1,3)=1;
- Ci(2,0)=+z; Ci(2,4)=1;
-
- typename D1::ColsBlockXpr Phi = Ci.rightCols( nbPoint );
- for( int i=0;i<nbPoint;++i )
+ using namespace dynamicgraph::sot;
+
+ TaskDynPD & task = dynamic_cast<TaskDynPD&> ( g_pool.getEntity( taskName ) );
+ assert( task.getFeatureList().size() == 1 );
+ BOOST_FOREACH( FeatureAbstract* fptr, task.getFeatureList() )
{
- const double x = positions(0,i);
- const double y = positions(1,i);
- Phi.col(i) << -1,-y,+x;
+ FeaturePoint6d* f6 = dynamic_cast< FeaturePoint6d* >( fptr );
+ assert( NULL!=f6 );
+ f6->positionSIN.recompute(controlSOUT.getTime());
+ f6->servoCurrentPosition();
+ f6->FeatureAbstract::selectionSIN = true;
}
- sotDEBUG(5) << "Psi = " << (soth::MATLAB) Phi << std::endl;
+ addContact( contactName, &task.jacobianSOUT, &task.JdotSOUT,&task.taskVectorSOUT, NULL );
}
- }
+ void SolverOpSpace::
+ addContact( const std::string & name,
+ Signal<ml::Matrix,int> * jacobianSignal,
+ Signal<ml::Matrix,int> * JdotSignal,
+ Signal<ml::Vector,int> * corrSignal,
+ Signal<ml::Matrix,int> * contactPointsSignal )
+ {
+ if( contactMap.find(name) != contactMap.end())
+ {
+ std::cerr << "!! Contact " << name << " already exists." << std::endl;
+ return;
+ }
- /* --- INIT SOLVER ------------------------------------------------------ */
- /* --- INIT SOLVER ------------------------------------------------------ */
- /* --- INIT SOLVER ------------------------------------------------------ */
+ contactMap[name].jacobianSIN
+ = matrixSINPtr( new SignalPtr<ml::Matrix,int>
+ ( jacobianSignal,
+ "sotDynInvWB("+getName()+")::input(matrix)::_"+name+"_J" ) );
+ signalRegistration( *contactMap[name].jacobianSIN );
+
+ contactMap[name].JdotSIN
+ = matrixSINPtr( new SignalPtr<ml::Matrix,int>
+ ( JdotSignal,
+ "sotDynInvWB("+getName()+")::input(matrix)::_"+name+"_Jdot" ) );
+ signalRegistration( *contactMap[name].JdotSIN );
+
+ contactMap[name].supportSIN
+ = matrixSINPtr( new SignalPtr<ml::Matrix,int>
+ ( contactPointsSignal,
+ "sotDynInvWB("+getName()+")::input(matrix)::_"+name+"_p" ) );
+ signalRegistration( *contactMap[name].supportSIN );
+
+ contactMap[name].correctorSIN
+ = vectorSINPtr( new SignalPtr<ml::Vector,int>
+ ( corrSignal,
+ "sotDynInvWB("+getName()+")::input(vector)::_"+name+"_x" ) );
+ signalRegistration( *contactMap[name].correctorSIN );
+
+ contactMap[name].forceSOUT
+ = vectorSOUTPtr( new Signal<ml::Vector,int>
+ ( "sotDynInvWB("+getName()+")::output(vector)::_"+name+"_f" ) );
+ signalRegistration( *contactMap[name].forceSOUT );
+
+ contactMap[name].fnSOUT
+ = vectorSOUTPtr( new Signal<ml::Vector,int>
+ ( "sotDynInvWB("+getName()+")::output(vector)::_"+name+"_fn" ) );
+ signalRegistration( *contactMap[name].fnSOUT );
- /** Force the update of all the task in-signals, in order to fix their
- * size for resizing the solver.
- */
- void SolverOpSpace::
- refreshTaskTime( int time )
- {
- // TODO BOOST_FOREACH
- for( StackIterator_t iter=stack.begin();stack.end()!=iter;++iter )
- {
- TaskDynPD& task = **iter;
- task.taskSOUT( time );
- }
- }
+ }
- /** Knowing the sizes of all the stages (except the task ones),
- * the function resizes the matrix and vector of all stages (except...).
- */
- void SolverOpSpace::
- initialResizeSolver( void )
- {
- const int nbContactCst = 3*nbContactBodies+nbContactPoints;
+ void SolverOpSpace::
+ removeContact( const std::string & name )
+ {
+ if( contactMap.find(name) == contactMap.end() )
+ {
+ std::cerr << "!! Contact " << name << " does not exist." << std::endl;
+ return;
+ }
- Cdyn.resize( nbDofs+6,nbParam ); bdyn.resize( nbDofs+6 );
- Ccontact.resize( 6*nbContactBodies,nbParam ); bcontact.resize( 6*nbContactBodies );
- Czmp.resize( nbContactCst,nbParam ); bzmp.resize( nbContactCst );
- Czero.resize( nbDofs,nbParam ); bzero.resize( nbDofs );
- }
+ signalDeregistration( signalShortName(contactMap[name].jacobianSIN->getName()) );
+ signalDeregistration( signalShortName(contactMap[name].supportSIN->getName()) );
+ signalDeregistration( signalShortName(contactMap[name].forceSOUT->getName()) );
+ signalDeregistration( signalShortName(contactMap[name].fnSOUT->getName()) );
+ signalDeregistration( signalShortName(contactMap[name].JdotSIN->getName()) );
+ signalDeregistration( signalShortName(contactMap[name].correctorSIN->getName()) );
+ contactMap.erase(name);
+ }
- void SolverOpSpace::
- resizeSolver( void )
- {
- sotDEBUGIN(15);
-
- assert( nbDofs>0 );
- nq = nbDofs+6; ntau=nbDofs;
- nbContactBodies = contactMap.size();
- nbContactPoints = 0;
- int range = 0;
- BOOST_FOREACH(contacts_t::value_type& pContact, contactMap)
+ void SolverOpSpace::
+ dispContacts( std::ostream& os ) const
+ {
+ os << "+-----------------\n+ Contacts\n+-----------------" << std::endl;
+ // TODO BOOST FOREACH
+ for( contacts_t::const_iterator iter=contactMap.begin();
+ iter!=contactMap.end(); ++iter )
+ {
+ os << "| " << iter->first <<std::endl;
+ }
+ }
+
+ /* --------------------------------------------------------------------- */
+ /* --- STATIC INTERNAL ------------------------------------------------- */
+ /* --------------------------------------------------------------------- */
+ namespace sotOPH
+ {
+ template< typename D1,typename D2 >
+ void preCross( const Eigen::MatrixBase<D1> & M,Eigen::MatrixBase<D2> & Tx )
{
- Contact & contact = pContact.second;
- const int nbi = contact.supportSIN->accessCopy().nbCols();
- nbContactPoints += nbi;
- int sizeVar = 6+nbi;
- contact.range = std::make_pair( range,range+sizeVar );
- range+=sizeVar;
+ EIGEN_STATIC_ASSERT_VECTOR_ONLY(Eigen::MatrixBase<D1>);
+ assert( Tx.cols()==3 && Tx.rows()==3 && M.size()==3 );
+ Tx( 0,0 ) = 0 ; Tx( 0,1 )=-M[2]; Tx( 0,2 ) = M[1];
+ Tx( 1,0 ) = M[2]; Tx( 1,1 )= 0 ; Tx( 1,2 ) =-M[0];
+ Tx( 2,0 ) =-M[1]; Tx( 2,1 )= M[0]; Tx( 2,2 ) = 0 ;
}
- nfs=6*nbContactBodies+nbContactPoints;
- nbParam = nq+ntau+nfs;
-
- const int nbDynConstraint = 3;
- /* constraint [B]efore and [A]fter the tasks. */
- const int nbCstB = nbDynConstraint, nbCstA = 1;
- const int nbCst = nbCstB+stack.size()+nbCstA;
-
- bool toBeResized =
- (hsolver==NULL
- || (int)hsolver->nbStages()!=nbCst
- || (int)hsolver->sizeProblem!=nbParam
- // || (int)hsolver->stage(1).nbConstraints()!=nbContactBodies*6
- // || (int)hsolver->stage(2).nbConstraints()!=3*nbContactBodies+nbContactPoints
- );
- if(! toBeResized )
- for( int i=0;i<(int)stack.size();++i )
- if( stack[i]->taskSOUT.accessCopy().size()
- !=hsolver->stage(nbCstB+i).nbConstraints() )
- { toBeResized = true; break; }
- if( toBeResized )
+ template< typename D1, typename D2 >
+ void computeForceNormalConversion( Eigen::MatrixBase<D1> & Ci,
+ Eigen::MatrixBase<D2> & positions )
{
- std::cout << "Resize all..." << std::endl;
- sotDEBUG(1) << "Resize all." << std::endl;
- hsolver = hcod_ptr_t(new soth::HCOD(nbParam,nbCst));
- initialResizeSolver();
+ /* General Constraint is: phi^0 = Psi.fi, with Psi = [ I; skew(OP1);
+ skew(OP2); skew(OP3); skew(OP4) ]. But phi^0 = X_c^0*phi^c (both feet
+ are 0.105cm above the ground so X_c^0 is the transformation matri from
+ actual ankle force to the actual contact force). * X_c^0*phi^c - Psi*fi
+ = 0 Since phi_{x,y,Rz}^0 ~ fi_{x,y} and phi_{z,Rx,Ry} ~ fi_{z,Rx,Ry}: *
+ Reduced Constraint Ci is applied as:[ X_c^0*phi^c - Psi*fi ]_{z,Rx,Ry} =
+ 0 [ C_phi -C_fi ]_{z,Rx,Ry}*[phi^c; fi_{z,Rx,Ry}] = 0, so Ci = [ C_phi
+ -C_fi ]_{z,Rx,Ry} with C_phi = X_c^0 and C_fi = Psi.
+ */
+
+ using namespace Eigen;
+
+ const int nbPoint = positions.cols();
+ assert( positions.rows()==3 );
+ assert( Ci.rows()==3 && Ci.cols()==6+nbPoint );
+
+ //Ci.leftCols(6) = -MatrixXd::Identity(6,6);
+ /* Ci = [ X_c^0_{z,Rx,Ry} -Psi_{z,Rx,Ry}];
+ * with X_c^0_{z,Rx,Ry} = [ 0 0 1 0 0 0; 0 -z 0 1 0 0; z 0 0 0 1 0 ]
+ * and Psi_{z,Rx,Ry} = [ 1 1 1 1; y_1 y_2 y_3 y_4; -x_1 -x_2 -x_3 -x_4 ]. */
+ const double z = positions(2,0);
+ Ci.setZero();
+ Ci(0,2)=1;
+ Ci(1,1)=-z; Ci(1,3)=1;
+ Ci(2,0)=+z; Ci(2,4)=1;
+
+ typename D1::ColsBlockXpr Phi = Ci.rightCols( nbPoint );
+ for( int i=0;i<nbPoint;++i )
+ {
+ const double x = positions(0,i);
+ const double y = positions(1,i);
+ Phi.col(i) << -1,-y,+x;
+ }
+ sotDEBUG(5) << "Psi = " << (soth::MATLAB) Phi << std::endl;
+ }
+ }
+
+
+ /* --- INIT SOLVER ------------------------------------------------------ */
+ /* --- INIT SOLVER ------------------------------------------------------ */
+ /* --- INIT SOLVER ------------------------------------------------------ */
- hsolver->pushBackStage( Cdyn, bdyn );
- hsolver->stages.back()->name = "dyn";
- hsolver->pushBackStage( Ccontact,bcontact );
- hsolver->stages.back()->name = "contact";
- hsolver->pushBackStage( Czmp, bzmp );
- hsolver->stages.back()->name = "zmp";
+ /** Force the update of all the task in-signals, in order to fix their
+ * size for resizing the solver.
+ */
+ void SolverOpSpace::
+ refreshTaskTime( int time )
+ {
+ // TODO BOOST_FOREACH
+ for( StackIterator_t iter=stack.begin();stack.end()!=iter;++iter )
+ {
+ TaskDynPD& task = **iter;
+ task.taskSOUT( time );
+ }
+ }
+
+ /** Knowing the sizes of all the stages (except the task ones),
+ * the function resizes the matrix and vector of all stages (except...).
+ */
+ void SolverOpSpace::
+ initialResizeSolver( void )
+ {
+ const int nbContactCst = 3*nbContactBodies+nbContactPoints;
+
+ Cdyn.resize( nbDofs+6,nbParam ); bdyn.resize( nbDofs+6 );
+ Ccontact.resize( 6*nbContactBodies,nbParam ); bcontact.resize( 6*nbContactBodies );
+ Czmp.resize( nbContactCst,nbParam ); bzmp.resize( nbContactCst );
+ Czero.resize( nbDofs,nbParam ); bzero.resize( nbDofs );
+ }
- Ctasks.clear(); Ctasks.resize( stack.size() );
- btasks.clear(); btasks.resize( stack.size() );
+ void SolverOpSpace::
+ resizeSolver( void )
+ {
+ sotDEBUGIN(15);
+ assert( nbDofs>0 );
+ nq = nbDofs+6; ntau=nbDofs;
+ nbContactBodies = contactMap.size();
+ nbContactPoints = 0;
+ int range = 0;
+ BOOST_FOREACH(contacts_t::value_type& pContact, contactMap)
+ {
+ Contact & contact = pContact.second;
+ const int nbi = contact.supportSIN->accessCopy().nbCols();
+ nbContactPoints += nbi;
+ int sizeVar = 6+nbi;
+ contact.range = std::make_pair( range,range+sizeVar );
+ range+=sizeVar;
+ }
+ nfs=6*nbContactBodies+nbContactPoints;
+ nbParam = nq+ntau+nfs;
+
+ const int nbDynConstraint = 3;
+ /* constraint [B]efore and [A]fter the tasks. */
+ const int nbCstB = nbDynConstraint, nbCstA = 1;
+ const int nbCst = nbCstB+stack.size()+nbCstA;
+
+ bool toBeResized =
+ (hsolver==NULL
+ || (int)hsolver->nbStages()!=nbCst
+ || (int)hsolver->sizeProblem!=nbParam
+ // || (int)hsolver->stage(1).nbConstraints()!=nbContactBodies*6
+ // || (int)hsolver->stage(2).nbConstraints()!=3*nbContactBodies+nbContactPoints
+ );
+ if(! toBeResized )
for( int i=0;i<(int)stack.size();++i )
- {
- TaskDynPD & task = *stack[i];
- const int nx = task.taskSOUT.accessCopy().size();
- Ctasks[i].resize(nx,nbParam);
- btasks[i].resize(nx);
+ if( stack[i]->taskSOUT.accessCopy().size()
+ !=hsolver->stage(nbCstB+i).nbConstraints() )
+ { toBeResized = true; break; }
- hsolver->pushBackStage( Ctasks[i],btasks[i] );
- hsolver->stages.back()->name = task.getName();
- }
+ if( toBeResized )
+ {
+ std::cout << "Resize all..." << std::endl;
+ sotDEBUG(1) << "Resize all." << std::endl;
+ hsolver = hcod_ptr_t(new soth::HCOD(nbParam,nbCst));
+ initialResizeSolver();
+
+ hsolver->pushBackStage( Cdyn, bdyn );
+ hsolver->stages.back()->name = "dyn";
+ hsolver->pushBackStage( Ccontact,bcontact );
+ hsolver->stages.back()->name = "contact";
+ hsolver->pushBackStage( Czmp, bzmp );
+ hsolver->stages.back()->name = "zmp";
+
+ Ctasks.clear(); Ctasks.resize( stack.size() );
+ btasks.clear(); btasks.resize( stack.size() );
+
+ for( int i=0;i<(int)stack.size();++i )
+ {
+ TaskDynPD & task = *stack[i];
+ const int nx = task.taskSOUT.accessCopy().size();
+ Ctasks[i].resize(nx,nbParam);
+ btasks[i].resize(nx);
- hsolver->pushBackStage( Czero, bzero );
- hsolver->stages.back()->name = "zero";
+ hsolver->pushBackStage( Ctasks[i],btasks[i] );
+ hsolver->stages.back()->name = task.getName();
+ }
- solution.resize( nbDofs );
- }
+ hsolver->pushBackStage( Czero, bzero );
+ hsolver->stages.back()->name = "zero";
- sotDEBUGOUT(15);
- }
+ solution.resize( nbDofs );
+ }
+ sotDEBUGOUT(15);
+ }
- /* --- SIGNALS ---------------------------------------------------------- */
- /* --- SIGNALS ---------------------------------------------------------- */
- /* --- SIGNALS ---------------------------------------------------------- */
- ml::Vector& SolverOpSpace::
- controlSOUT_function( ml::Vector &control, int t )
- {
- sotDEBUG(15) << " # In time = " << t << std::endl;
+ /* --- SIGNALS ---------------------------------------------------------- */
+ /* --- SIGNALS ---------------------------------------------------------- */
+ /* --- SIGNALS ---------------------------------------------------------- */
- refreshTaskTime( t );
- resizeSolver();
+ ml::Vector& SolverOpSpace::
+ controlSOUT_function( ml::Vector &control, int t )
+ {
+ sotDEBUG(15) << " # In time = " << t << std::endl;
- // if( t==1112 ) { hsolver->debugOnce(); }
+ refreshTaskTime( t );
+ resizeSolver();
- EIGEN_VECTOR_FROM_SIGNAL(b,dyndriftSIN(t));
- EIGEN_MATRIX_FROM_SIGNAL(A,matrixInertiaSIN(t));
- EIGEN_VECTOR_FROM_SIGNAL(dq,velocitySIN(t));
+ // if( t==1112 ) { hsolver->debugOnce(); }
- using namespace sotOPH;
- using namespace soth;
+ EIGEN_VECTOR_FROM_SIGNAL(b,dyndriftSIN(t));
+ EIGEN_MATRIX_FROM_SIGNAL(A,matrixInertiaSIN(t));
+ EIGEN_VECTOR_FROM_SIGNAL(dq,velocitySIN(t));
- if( dampingSIN ) //damp?
- {
- sotDEBUG(5) << "Using damping. " << std::endl;
- hsolver->setDamping( 0 );
- hsolver->useDamp( true );
- hsolver->stages.back()->damping( dampingSIN(t) );
- }
- else
- {
- sotDEBUG(5) << "Without damping. " << std::endl;
- hsolver->useDamp( false );
- }
- sotDEBUG(1) << "A = " << (MATLAB)A << std::endl;
- sotDEBUG(1) << "b = " << (MATLAB)b << std::endl;
- sotDEBUG(1) << "dq = " << (MATLAB)dq << std::endl;
-
- /* SOT:
- * -1- A ddq + b + J'f = S' tau
- * -2- J ddq = 0
- * -3- Xp f > eps
- * -i- Ji qddot = ddxi - Jidot qdot
- *
- * -1- [ A -S' J' ] [ ddq; tau; f ] = -b
- * -2- [ J 0 0 ] [ ddq; tau; f ] = 0
- * -3- [ 0 0 Xp ] [ ddq; tau; f ] > EPS
- * -3- [ Ji 0 0 ] [ ddq; tau; f ] = ddxi - Jidot qdot
- */
+ using namespace sotOPH;
+ using namespace soth;
+
+ if( dampingSIN ) //damp?
+ {
+ sotDEBUG(5) << "Using damping. " << std::endl;
+ hsolver->setDamping( 0 );
+ hsolver->useDamp( true );
+ hsolver->stages.back()->damping( dampingSIN(t) );
+ }
+ else
+ {
+ sotDEBUG(5) << "Without damping. " << std::endl;
+ hsolver->useDamp( false );
+ }
+ sotDEBUG(1) << "A = " << (MATLAB)A << std::endl;
+ sotDEBUG(1) << "b = " << (MATLAB)b << std::endl;
+ sotDEBUG(1) << "dq = " << (MATLAB)dq << std::endl;
+
+ /* SOT:
+ * -1- A ddq + b + J'f = S' tau
+ * -2- J ddq = 0
+ * -3- Xp f > eps
+ * -i- Ji qddot = ddxi - Jidot qdot
+ *
+ * -1- [ A -S' J' ] [ ddq; tau; f ] = -b
+ * -2- [ J 0 0 ] [ ddq; tau; f ] = 0
+ * -3- [ 0 0 Xp ] [ ddq; tau; f ] > EPS
+ * -3- [ Ji 0 0 ] [ ddq; tau; f ] = ddxi - Jidot qdot
+ */
#define COLS_Q leftCols( nq )
@@ -475,301 +487,302 @@ namespace sot
#define COLS(__ri,__rs) leftCols(__rs).rightCols((__rs)-(__ri))
#define ROWS(__ri,__rs) topRows(__rs).bottomRows((__rs)-(__ri))
- /* -1- */
- /* Cdyn = [ A 0 [0(6x30);-I(30x30)] J1.transpose 0 J2.transpose 0 ] */
- assert( Cdyn.rows() == A.rows() && bdyn.size() == A.rows() );
- Cdyn.COLS_Q = A;
- Cdyn.COLS_TAU.ROWS_FF.setZero();
- Cdyn.COLS_TAU.ROWS_ACT = -MatrixXd::Identity(nbDofs,nbDofs);
- BOOST_FOREACH(contacts_t::value_type& pContact, contactMap)
- {
- Contact & contact = pContact.second;
- EIGEN_MATRIX_FROM_SIGNAL(Jc,(*contact.jacobianSIN)(t));
- const int ri = contact.range.first;
- Cdyn.COLS_F.COLS(ri,ri+6) = Jc.transpose();
- }
-
- for( int i=0;i<nbDofs+6;++i ) bdyn[i] = -b[i];
- sotDEBUG(15) << "Cdyn = " << (MATLAB)Cdyn << std::endl;
- sotDEBUG(1) << "bdyn = " << bdyn << std::endl;
-
- /* -2- */
- /* Ccontact = [ Jc1 0 0 0 0 0 ; Jc2 0 0 0 0 0 ] */
- {
- assert( Ccontact.rows() == nbContactBodies*6
- && bcontact.size() == nbContactBodies*6 );
- Ccontact.COLS_TAU.setZero();
- Ccontact.COLS_F.setZero();
- int nci = 0;
+ /* -1- */
+ /* Cdyn = [ A 0 [0(6x30);-I(30x30)] J1.transpose 0 J2.transpose 0 ] */
+ assert( Cdyn.rows() == A.rows() && bdyn.size() == A.rows() );
+ Cdyn.COLS_Q = A;
+ Cdyn.COLS_TAU.ROWS_FF.setZero();
+ Cdyn.COLS_TAU.ROWS_ACT = -MatrixXd::Identity(nbDofs,nbDofs);
BOOST_FOREACH(contacts_t::value_type& pContact, contactMap)
{
- Contact& contact = pContact.second; const int n6 = nci*6;
+ Contact & contact = pContact.second;
EIGEN_MATRIX_FROM_SIGNAL(Jc,(*contact.jacobianSIN)(t));
- Ccontact.COLS_Q.ROWS(n6,n6+6) = Jc;
-
- VectorXd reference = VectorXd::Zero(6);
- if( (*contact.JdotSIN) )
- {
- sotDEBUG(5) << "Accounting for Jcontact_dot. " << std::endl;
- EIGEN_MATRIX_FROM_SIGNAL(Jcdot,(*contact.JdotSIN)(t));
- reference -= Jcdot*dq;
- }
- if( (*contact.correctorSIN) )
- {
- sotDEBUG(5) << "Accounting for contact_xddot. " << std::endl;
- EIGEN_VECTOR_FROM_SIGNAL(xdd,(*contact.correctorSIN)(t));
- reference += xdd;
- }
- for( int r=0;r<6;++r ) bcontact[n6+r] = reference[r];
- nci++;
+ const int ri = contact.range.first;
+ Cdyn.COLS_F.COLS(ri,ri+6) = Jc.transpose();
}
- sotDEBUG(15) << "Ccontact = " << (MATLAB)Ccontact << std::endl;
- sotDEBUG(1) << "bcontact = " << bcontact << std::endl;
- }
-
- /* -3- */
- /* Czmp = [ 0 0 -X_c^0_{z,Rx,Ry} Psi_{z,Rx,Ry} 0 0; 0 0 0 Iz 0 0 ] */
- {
- Czmp.setZero();
- int rows=0;
- BOOST_FOREACH(const contacts_t::value_type& pContact, contactMap)
- {
- const Contact & contact = pContact.second;
- EIGEN_MATRIX_FROM_SIGNAL(support,(*contact.supportSIN)(t));
- const int nbP = support.cols();
- const int ri = contact.range.first, rs=contact.range.second;
-
- assert( nq+ntau+rs<=Czmp.cols() && rows+3+nbP<=Czmp.rows() );
- assert( bzmp.size() == Czmp.rows() );
-
- MatrixXd::ColsBlockXpr::ColsBlockXpr::ColsBlockXpr
- ::RowsBlockXpr::RowsBlockXpr Czi
- = Czmp.COLS_F.COLS(ri,rs). ROWS(rows,rows+3);
- computeForceNormalConversion(Czi,support);
- for( int i=0;i<3;++i ) bzmp. ROWS(rows,rows+3)[i] = 0;
- for( int p=0;p<nbP;++p )
- {
- Czmp.COLS_F.COLS(ri+6,rs).ROWS(rows+3,rows+3+nbP)
- (p,p) = 1;
- bzmp.ROWS(rows+3,rows+3+nbP)
- [p] = soth::Bound(0,soth::Bound::BOUND_SUP);
- }
- rows += 3+nbP;
- }
- sotDEBUG(15) << "Czmp = " << (MATLAB)Czmp << std::endl;
- sotDEBUG(1) << "bzmp = " << bzmp << std::endl;
- }
+ for( int i=0;i<nbDofs+6;++i ) bdyn[i] = -b[i];
+ sotDEBUG(15) << "Cdyn = " << (MATLAB)Cdyn << std::endl;
+ sotDEBUG(1) << "bdyn = " << bdyn << std::endl;
- /* -Tasks 1:n- */
- /* Ctaski = [ Ji 0 0 0 0 0 ] */
- for( int i=0;i<(int)stack.size();++i )
+ /* -2- */
+ /* Ccontact = [ Jc1 0 0 0 0 0 ; Jc2 0 0 0 0 0 ] */
{
- TaskDynPD & task = * stack[i];
- MatrixXd & Ctask1 = Ctasks[i];
- VectorBound & btask1 = btasks[i];
+ assert( Ccontact.rows() == nbContactBodies*6
+ && bcontact.size() == nbContactBodies*6 );
+ Ccontact.COLS_TAU.setZero();
+ Ccontact.COLS_F.setZero();
+ int nci = 0;
+ BOOST_FOREACH(contacts_t::value_type& pContact, contactMap)
+ {
+ Contact& contact = pContact.second; const int n6 = nci*6;
+ EIGEN_MATRIX_FROM_SIGNAL(Jc,(*contact.jacobianSIN)(t));
+ Ccontact.COLS_Q.ROWS(n6,n6+6) = Jc;
- EIGEN_MATRIX_FROM_SIGNAL(Jdot,task.JdotSOUT(t));
- EIGEN_MATRIX_FROM_SIGNAL(J,task.jacobianSOUT(t));
- const sot::VectorMultiBound & ddx = task.taskSOUT(t);
+ VectorXd reference = VectorXd::Zero(6);
+ if( (*contact.JdotSIN) )
+ {
+ sotDEBUG(5) << "Accounting for Jcontact_dot. " << std::endl;
+ EIGEN_MATRIX_FROM_SIGNAL(Jcdot,(*contact.JdotSIN)(t));
+ reference -= Jcdot*dq;
+ }
+ if( (*contact.correctorSIN) )
+ {
+ sotDEBUG(5) << "Accounting for contact_xddot. " << std::endl;
+ EIGEN_VECTOR_FROM_SIGNAL(xdd,(*contact.correctorSIN)(t));
+ reference += xdd;
+ }
+ for( int r=0;r<6;++r ) bcontact[n6+r] = reference[r];
+ nci++;
+ }
+ sotDEBUG(15) << "Ccontact = " << (MATLAB)Ccontact << std::endl;
+ sotDEBUG(1) << "bcontact = " << bcontact << std::endl;
+ }
- const int nx1 = ddx.size();
+ /* -3- */
+ /* Czmp = [ 0 0 -X_c^0_{z,Rx,Ry} Psi_{z,Rx,Ry} 0 0; 0 0 0 Iz 0 0 ] */
+ {
+ Czmp.setZero();
+ int rows=0;
+ BOOST_FOREACH(const contacts_t::value_type& pContact, contactMap)
+ {
+ const Contact & contact = pContact.second;
+ EIGEN_MATRIX_FROM_SIGNAL(support,(*contact.supportSIN)(t));
+ const int nbP = support.cols();
+ const int ri = contact.range.first, rs=contact.range.second;
- sotDEBUG(5) << "ddx"<<i<<" = " << ddx << std::endl;
- sotDEBUG(25) << "J"<<i<<" = " << J << std::endl;
- sotDEBUG(45) << "Jdot"<<i<<" = " << Jdot << std::endl;
+ assert( nq+ntau+rs<=Czmp.cols() && rows+3+nbP<=Czmp.rows() );
+ assert( bzmp.size() == Czmp.rows() );
- assert( Ctask1.rows() == nx1 && btask1.size() == nx1 );
- assert( J.rows()==nx1 && J.cols()==nq && (int)ddx.size()==nx1 );
- assert( Jdot.rows()==nx1 && Jdot.cols()==nq );
- Ctask1.COLS_Q = J;
- Ctask1.COLS_TAU.setZero();
- Ctask1.COLS_F.setZero();
+ MatrixXd::ColsBlockXpr::ColsBlockXpr::ColsBlockXpr
+ ::RowsBlockXpr::RowsBlockXpr Czi
+ = Czmp.COLS_F.COLS(ri,rs). ROWS(rows,rows+3);
+ computeForceNormalConversion(Czi,support);
+ for( int i=0;i<3;++i ) bzmp. ROWS(rows,rows+3)[i] = 0;
- VectorXd ddxdrift = - (Jdot*dq);
- for( int c=0;c<nx1;++c )
- {
- if( ddx[c].getMode() == sot::MultiBound::MODE_SINGLE )
- btask1[c] = ddx[c].getSingleBound() + ddxdrift[c];
- else
+ for( int p=0;p<nbP;++p )
{
- const bool binf = ddx[c].getDoubleBoundSetup( sot::MultiBound::BOUND_INF );
- const bool bsup = ddx[c].getDoubleBoundSetup( sot::MultiBound::BOUND_SUP );
- if( binf&&bsup )
- {
- const double xi = ddx[c].getDoubleBound(sot::MultiBound::BOUND_INF);
- const double xs = ddx[c].getDoubleBound(sot::MultiBound::BOUND_SUP);
- btask1[c] = std::make_pair( xi+ddxdrift[c], xs+ddxdrift[c] );
- }
- else if( binf )
- {
- const double xi = ddx[c].getDoubleBound(sot::MultiBound::BOUND_INF);
- btask1[c] = Bound( xi+ddxdrift[c], Bound::BOUND_INF );
- }
- else
- {
- assert( bsup );
- const double xs = ddx[c].getDoubleBound(sot::MultiBound::BOUND_SUP);
- btask1[c] = Bound( xs+ddxdrift[c], Bound::BOUND_SUP );
- }
+ Czmp.COLS_F.COLS(ri+6,rs).ROWS(rows+3,rows+3+nbP)
+ (p,p) = 1;
+ bzmp.ROWS(rows+3,rows+3+nbP)
+ [p] = soth::Bound(0,soth::Bound::BOUND_SUP);
}
+ rows += 3+nbP;
}
-
- sotDEBUG(15) << "Ctask"<<i<<" = " << (MATLAB)Ctask1 << std::endl;
- sotDEBUG(1) << "btask"<<i<<" = " << btask1 << std::endl;
+ sotDEBUG(15) << "Czmp = " << (MATLAB)Czmp << std::endl;
+ sotDEBUG(1) << "bzmp = " << bzmp << std::endl;
}
- /* -Last stage- */
- /* Czero = [ [0(30x6) I(30x30)] 0 0 0 0 0 ] */
- Czero.COLS_Q.leftCols(6).setZero();
- Czero.COLS_Q.rightCols(nbDofs).setIdentity();
- Czero.COLS_TAU.setZero();
- Czero.COLS_F.setZero();
- const double Kv = breakFactorSIN(t);
- for( int i=0;i<nbDofs;++i )
- bzero[i] = -Kv*dq[i+6];
- sotDEBUG(15) << "Czero = " << (MATLAB)Czero << std::endl;
- sotDEBUG(1) << "bzero = " << bzero << std::endl;
-
- /* --- */
- sotDEBUG(1) << "Initial config." << std::endl;
- hsolver->reset();
- hsolver->setInitialActiveSet();
-
- sotDEBUG(1) << "Run for a solution." << std::endl;
- hsolver->activeSearch(solution);
- sotDEBUG(1) << "solution = " << (MATLAB)solution << std::endl;
-
- EIGEN_VECTOR_FROM_VECTOR( tau,control,nbDofs );
- tau = solution.transpose().COLS_TAU;
-
- /* --- forces signal --- */
- BOOST_FOREACH(contacts_t::value_type& pContact, contactMap)
- {
- Contact& contact = pContact.second;
- const int nbP = (*contact.supportSIN)(t).nbCols();
- const int ri = contact.range.first;
- /* range is an object of the struct Contact containing 2 integers:
- first: the position of the 1st contact in the parameters vector
- second: the position of the 2nd contact in the parameters vector */
- ml::Vector mlf6;
- EIGEN_VECTOR_FROM_VECTOR(f6,mlf6,6 );
- ml::Vector mlfn;
- EIGEN_VECTOR_FROM_VECTOR(fn,mlfn,nbP);
-
- f6 = solution.transpose().COLS_F.COLS(ri,ri+6);
- (*contact.forceSOUT) = mlf6;
- contact.forceSOUT->setTime(t);
-
- for( int i=0;i<nbP;++i ) fn[i] = solution.transpose().COLS_F[ri+6+i];
- (*contact.fnSOUT) = mlfn;
- contact.fnSOUT->setTime(t);
- }
+ /* -Tasks 1:n- */
+ /* Ctaski = [ Ji 0 0 0 0 0 ] */
+ for( int i=0;i<(int)stack.size();++i )
+ {
+ TaskDynPD & task = * stack[i];
+ MatrixXd & Ctask1 = Ctasks[i];
+ VectorBound & btask1 = btasks[i];
- /* ACC signal */
- {
- ml::Vector mlacc;
- EIGEN_VECTOR_FROM_VECTOR( acc,mlacc,nbDofs+6 );
- acc = solution.transpose().COLS_Q;
- accelerationSOUT = mlacc;
- }
+ EIGEN_MATRIX_FROM_SIGNAL(Jdot,task.JdotSOUT(t));
+ EIGEN_MATRIX_FROM_SIGNAL(J,task.jacobianSOUT(t));
+ const dg::sot::VectorMultiBound & ddx = task.taskSOUT(t);
- /* --- verif --- */
- sotDEBUG(1) << "Vdyn = " << (MATLAB)(VectorXd)(Cdyn*solution) << std::endl;
- sotDEBUG(1) << "Vcontact = " << (MATLAB)(VectorXd)(Ccontact*solution) << std::endl;
- sotDEBUG(1) << "Vzmp = " << (MATLAB)(VectorXd)(Czmp*solution) << std::endl;
+ const int nx1 = ddx.size();
- sotDEBUG(1) << "control = " << control << std::endl;
- return control;
- }
+ sotDEBUG(5) << "ddx"<<i<<" = " << ddx << std::endl;
+ sotDEBUG(25) << "J"<<i<<" = " << J << std::endl;
+ sotDEBUG(45) << "Jdot"<<i<<" = " << Jdot << std::endl;
+ assert( Ctask1.rows() == nx1 && btask1.size() == nx1 );
+ assert( J.rows()==nx1 && J.cols()==nq && (int)ddx.size()==nx1 );
+ assert( Jdot.rows()==nx1 && Jdot.cols()==nq );
+ Ctask1.COLS_Q = J;
+ Ctask1.COLS_TAU.setZero();
+ Ctask1.COLS_F.setZero();
- /* --- COMMANDS ---------------------------------------------------------- */
- /* --- COMMANDS ---------------------------------------------------------- */
- /* --- COMMANDS ---------------------------------------------------------- */
+ VectorXd ddxdrift = - (Jdot*dq);
+ for( int c=0;c<nx1;++c )
+ {
+ if( ddx[c].getMode() == dg::sot::MultiBound::MODE_SINGLE )
+ btask1[c] = ddx[c].getSingleBound() + ddxdrift[c];
+ else
+ {
+ const bool binf = ddx[c].getDoubleBoundSetup( dg::sot::MultiBound::BOUND_INF );
+ const bool bsup = ddx[c].getDoubleBoundSetup( dg::sot::MultiBound::BOUND_SUP );
+ if( binf&&bsup )
+ {
+ const double xi = ddx[c].getDoubleBound(dg::sot::MultiBound::BOUND_INF);
+ const double xs = ddx[c].getDoubleBound(dg::sot::MultiBound::BOUND_SUP);
+ btask1[c] = std::make_pair( xi+ddxdrift[c], xs+ddxdrift[c] );
+ }
+ else if( binf )
+ {
+ const double xi = ddx[c].getDoubleBound(dg::sot::MultiBound::BOUND_INF);
+ btask1[c] = Bound( xi+ddxdrift[c], Bound::BOUND_INF );
+ }
+ else
+ {
+ assert( bsup );
+ const double xs = ddx[c].getDoubleBound(dg::sot::MultiBound::BOUND_SUP);
+ btask1[c] = Bound( xs+ddxdrift[c], Bound::BOUND_SUP );
+ }
+ }
+ }
- void SolverOpSpace::
- debugOnce( void )
- {
- sot::DebugTrace::openFile("/tmp/sot.txt");
- hsolver->debugOnce();
- }
+ sotDEBUG(15) << "Ctask"<<i<<" = " << (MATLAB)Ctask1 << std::endl;
+ sotDEBUG(1) << "btask"<<i<<" = " << btask1 << std::endl;
+ }
- /* --- ENTITY ----------------------------------------------------------- */
- /* --- ENTITY ----------------------------------------------------------- */
- /* --- ENTITY ----------------------------------------------------------- */
+ /* -Last stage- */
+ /* Czero = [ [0(30x6) I(30x30)] 0 0 0 0 0 ] */
+ Czero.COLS_Q.leftCols(6).setZero();
+ Czero.COLS_Q.rightCols(nbDofs).setIdentity();
+ Czero.COLS_TAU.setZero();
+ Czero.COLS_F.setZero();
+ const double Kv = breakFactorSIN(t);
+ for( int i=0;i<nbDofs;++i )
+ bzero[i] = -Kv*dq[i+6];
+ sotDEBUG(15) << "Czero = " << (MATLAB)Czero << std::endl;
+ sotDEBUG(1) << "bzero = " << bzero << std::endl;
+
+ /* --- */
+ sotDEBUG(1) << "Initial config." << std::endl;
+ hsolver->reset();
+ hsolver->setInitialActiveSet();
+
+ sotDEBUG(1) << "Run for a solution." << std::endl;
+ hsolver->activeSearch(solution);
+ sotDEBUG(1) << "solution = " << (MATLAB)solution << std::endl;
+
+ EIGEN_VECTOR_FROM_VECTOR( tau,control,nbDofs );
+ tau = solution.transpose().COLS_TAU;
+
+ /* --- forces signal --- */
+ BOOST_FOREACH(contacts_t::value_type& pContact, contactMap)
+ {
+ Contact& contact = pContact.second;
+ const int nbP = (*contact.supportSIN)(t).nbCols();
+ const int ri = contact.range.first;
+ /* range is an object of the struct Contact containing 2 integers:
+ first: the position of the 1st contact in the parameters vector
+ second: the position of the 2nd contact in the parameters vector */
+ ml::Vector mlf6;
+ EIGEN_VECTOR_FROM_VECTOR(f6,mlf6,6 );
+ ml::Vector mlfn;
+ EIGEN_VECTOR_FROM_VECTOR(fn,mlfn,nbP);
+
+ f6 = solution.transpose().COLS_F.COLS(ri,ri+6);
+ (*contact.forceSOUT) = mlf6;
+ contact.forceSOUT->setTime(t);
+
+ for( int i=0;i<nbP;++i ) fn[i] = solution.transpose().COLS_F[ri+6+i];
+ (*contact.fnSOUT) = mlfn;
+ contact.fnSOUT->setTime(t);
+ }
- void SolverOpSpace::
- display( std::ostream& os ) const
- {
- os << "SolverOpSpace "<<getName() << ": " << nbDofs <<" joints." << std::endl;
- try{
- os <<"control = "<<controlSOUT.accessCopy() <<std::endl << std::endl;
- } catch (dynamicgraph::ExceptionSignal e) {}
- stack_t::display(os);
- dispContacts(os);
- }
-
- void SolverOpSpace::
- commandLine( const std::string& cmdLine,
- std::istringstream& cmdArgs,
- std::ostream& os )
- {
- if( cmdLine == "help" )
- {
- os << "SolverOpSpace:\n"
- << "\t- debugOnce: open trace-file for next iteration of the solver." << std::endl
- << "\t- addContact: create the contact signals, unpluged." << std::endl
- << "\t- addContactFromTask <taskName>: Add a contact from the named task." << std::endl
- << "\t- rmContact <name>: remove a contact." << std::endl
- << "\t- dispContacts: guess what?." << std::endl;
- stackCommandLine( cmdLine,cmdArgs,os );
- Entity::commandLine( cmdLine,cmdArgs,os );
- }
- else if( cmdLine == "debugOnce" )
+ /* ACC signal */
{
- debugOnce();
+ ml::Vector mlacc;
+ EIGEN_VECTOR_FROM_VECTOR( acc,mlacc,nbDofs+6 );
+ acc = solution.transpose().COLS_Q;
+ accelerationSOUT = mlacc;
}
- else if( cmdLine == "addContact" )
- {
- if( cmdArgs.good() )
- {
- std::string name; cmdArgs >> name;
- addContact( name,NULL,NULL,NULL,NULL );
- }
- else { os << "!! A name must be specified. " << std::endl; }
- }
- else if( cmdLine == "addContactFromTask" )
- {
- if( cmdArgs.good() )
- {
- std::string name; cmdArgs >> name;
- addContactFromTask( name,name );
- }
- else { os << "!! A name must be specified. " << std::endl; }
- }
- else if( cmdLine == "rmContact" )
- {
- cmdArgs >> std::ws;
- if( cmdArgs.good() )
- {
- std::string name; cmdArgs >> name;
- removeContact( name );
- }
- else { os << "!! A name must be specified. " << std::endl; }
- }
- else if( cmdLine == "dispContacts" ) { dispContacts( os ); }
- // TODO: cf sot-v1: else if( cmdLine == "listen" )
- // {
- // hsolver->notifiorRegistration( sotOpSpaceH::hsolverListener );
- // }
- else if( stackCommandLine( cmdLine,cmdArgs,os ) );
- else
- {
- Entity::commandLine( cmdLine,cmdArgs,os );
- }
- }
- } // namespace dyninv
-} // namespace sot
+ /* --- verif --- */
+ sotDEBUG(1) << "Vdyn = " << (MATLAB)(VectorXd)(Cdyn*solution) << std::endl;
+ sotDEBUG(1) << "Vcontact = " << (MATLAB)(VectorXd)(Ccontact*solution) << std::endl;
+ sotDEBUG(1) << "Vzmp = " << (MATLAB)(VectorXd)(Czmp*solution) << std::endl;
+
+ sotDEBUG(1) << "control = " << control << std::endl;
+ return control;
+ }
+
+
+ /* --- COMMANDS ---------------------------------------------------------- */
+ /* --- COMMANDS ---------------------------------------------------------- */
+ /* --- COMMANDS ---------------------------------------------------------- */
+
+ void SolverOpSpace::
+ debugOnce( void )
+ {
+ dg::sot::DebugTrace::openFile("/tmp/sot.txt");
+ hsolver->debugOnce();
+ }
+
+ /* --- ENTITY ----------------------------------------------------------- */
+ /* --- ENTITY ----------------------------------------------------------- */
+ /* --- ENTITY ----------------------------------------------------------- */
+
+ void SolverOpSpace::
+ display( std::ostream& os ) const
+ {
+ os << "SolverOpSpace "<<getName() << ": " << nbDofs <<" joints." << std::endl;
+ try{
+ os <<"control = "<<controlSOUT.accessCopy() <<std::endl << std::endl;
+ } catch (dynamicgraph::ExceptionSignal e) {}
+ stack_t::display(os);
+ dispContacts(os);
+ }
+
+ void SolverOpSpace::
+ commandLine( const std::string& cmdLine,
+ std::istringstream& cmdArgs,
+ std::ostream& os )
+ {
+ if( cmdLine == "help" )
+ {
+ os << "SolverOpSpace:\n"
+ << "\t- debugOnce: open trace-file for next iteration of the solver." << std::endl
+ << "\t- addContact: create the contact signals, unpluged." << std::endl
+ << "\t- addContactFromTask <taskName>: Add a contact from the named task." << std::endl
+ << "\t- rmContact <name>: remove a contact." << std::endl
+ << "\t- dispContacts: guess what?." << std::endl;
+ stackCommandLine( cmdLine,cmdArgs,os );
+ Entity::commandLine( cmdLine,cmdArgs,os );
+ }
+ else if( cmdLine == "debugOnce" )
+ {
+ debugOnce();
+ }
+ else if( cmdLine == "addContact" )
+ {
+ if( cmdArgs.good() )
+ {
+ std::string name; cmdArgs >> name;
+ addContact( name,NULL,NULL,NULL,NULL );
+ }
+ else { os << "!! A name must be specified. " << std::endl; }
+ }
+ else if( cmdLine == "addContactFromTask" )
+ {
+ if( cmdArgs.good() )
+ {
+ std::string name; cmdArgs >> name;
+ addContactFromTask( name,name );
+ }
+ else { os << "!! A name must be specified. " << std::endl; }
+ }
+ else if( cmdLine == "rmContact" )
+ {
+ cmdArgs >> std::ws;
+ if( cmdArgs.good() )
+ {
+ std::string name; cmdArgs >> name;
+ removeContact( name );
+ }
+ else { os << "!! A name must be specified. " << std::endl; }
+ }
+ else if( cmdLine == "dispContacts" ) { dispContacts( os ); }
+ // TODO: cf sot-v1: else if( cmdLine == "listen" )
+ // {
+ // hsolver->notifiorRegistration( sotOpSpaceH::hsolverListener );
+ // }
+ else if( stackCommandLine( cmdLine,cmdArgs,os ) );
+ else
+ {
+ Entity::commandLine( cmdLine,cmdArgs,os );
+ }
+ }
+
+ } // namespace dyninv
+ } // namespace sot
+} // namespace dynamicgraph
diff --git a/src/solver-op-space.h b/src/solver-op-space.h
new file mode 100644
index 0000000000000000000000000000000000000000..07ea42ee400427ecaaa635060473af3858ccf52c
--- /dev/null
+++ b/src/solver-op-space.h
@@ -0,0 +1,160 @@
+/*
+ * Copyright 2011, Nicolas Mansard, LAAS-CNRS
+ *
+ * This file is part of sot-dyninv.
+ * sot-dyninv is free software: you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public License
+ * as published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ * sot-dyninv is distributed in the hope that it will be
+ * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details. You should
+ * have received a copy of the GNU Lesser General Public License along
+ * with sot-dyninv. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __sot_dyninv_SolverOpSpace_H__
+#define __sot_dyninv_SolverOpSpace_H__
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined (WIN32)
+# if defined (solver_op_space_EXPORTS)
+# define SOTSOLVEROPSPACE_EXPORT __declspec(dllexport)
+# else
+# define SOTSOLVEROPSPACE_EXPORT __declspec(dllimport)
+# endif
+#else
+# define SOTSOLVEROPSPACE_EXPORT
+#endif
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+
+/* SOT */
+#include <sot-dyninv/signal-helper.h>
+#include <sot-dyninv/entity-helper.h>
+#include <sot-dyninv/stack-template.h>
+#include <sot-dyninv/task-dyn-pd.h>
+#include <soth/HCOD.hpp>
+
+namespace dynamicgraph {
+ namespace sot {
+ namespace dyninv {
+
+ /* --------------------------------------------------------------------- */
+ /* --- CLASS ----------------------------------------------------------- */
+ /* --------------------------------------------------------------------- */
+
+ class SOTSOLVEROPSPACE_EXPORT SolverOpSpace
+ :public ::dynamicgraph::Entity
+ ,public ::dynamicgraph::EntityHelper<SolverOpSpace>
+ ,public sot::Stack< TaskDynPD >
+ {
+
+ public: /* --- CONSTRUCTOR ---- */
+
+ SolverOpSpace( const std::string & name );
+
+ public: /* --- STACK INHERITANCE --- */
+
+ typedef sot::Stack<TaskDynPD> stack_t;
+ using stack_t::TaskDependancyList_t;
+ using stack_t::StackIterator_t;
+ using stack_t::StackConstIterator_t;
+ using stack_t::stack;
+
+ virtual TaskDependancyList_t getTaskDependancyList( const TaskDynPD& task );
+ virtual void addDependancy( const TaskDependancyList_t& depList );
+ virtual void removeDependancy( const TaskDependancyList_t& depList );
+ virtual void resetReady( void );
+
+ public: /* --- ENTITY INHERITANCE --- */
+
+ static const std::string CLASS_NAME;
+ virtual void display( std::ostream& os ) const;
+ virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
+
+ virtual void commandLine( const std::string& cmdLine,
+ std::istringstream& cmdArgs,
+ std::ostream& os );
+
+ public: /* --- SIGNALS --- */
+
+ DECLARE_SIGNAL_IN(matrixInertia,ml::Matrix);
+ DECLARE_SIGNAL_IN(velocity,ml::Vector);
+ DECLARE_SIGNAL_IN(dyndrift,ml::Vector);
+ DECLARE_SIGNAL_IN(damping,double);
+ DECLARE_SIGNAL_IN(breakFactor,double);
+
+ DECLARE_SIGNAL_OUT(control,ml::Vector);
+
+ DECLARE_SIGNAL(zmp,OUT,ml::Vector);
+ DECLARE_SIGNAL(acceleration,OUT,ml::Vector);
+
+ private: /* --- CONTACT POINTS --- */
+
+ typedef boost::shared_ptr<dynamicgraph::SignalPtr<ml::Matrix,int> > matrixSINPtr;
+ typedef boost::shared_ptr<dynamicgraph::SignalPtr<ml::Vector,int> > vectorSINPtr;
+ typedef boost::shared_ptr<dynamicgraph::Signal<ml::Vector,int> > vectorSOUTPtr;
+ struct Contact
+ {
+ matrixSINPtr jacobianSIN;
+ matrixSINPtr JdotSIN;
+ matrixSINPtr supportSIN;
+ vectorSINPtr correctorSIN;
+ vectorSOUTPtr forceSOUT,fnSOUT;
+ std::pair<int,int> range;
+ };
+ typedef std::map< std::string,Contact > contacts_t;
+ contacts_t contactMap;
+
+ public:
+ void addContact( const std::string & name,
+ dynamicgraph::Signal<ml::Matrix,int> * jacobianSignal,
+ dynamicgraph::Signal<ml::Matrix,int> * JdotSignal,
+ dynamicgraph::Signal<ml::Vector,int> * corrSignal,
+ dynamicgraph::Signal<ml::Matrix,int> * contactPointsSignal );
+ void addContactFromTask( const std::string & taskName, const std::string & contactName );
+ void removeContact( const std::string & name );
+ void dispContacts( std::ostream& os ) const;
+
+
+ public: /* --- COMMANDS --- */
+ void debugOnce( void );
+
+
+ private: /* --- INTERNAL COMPUTATIONS --- */
+ void refreshTaskTime( int time );
+ void initialResizeSolver( void );
+ void resizeSolver( void );
+
+ private:
+ int nbParam, nq,ntau;
+ int nbContactBodies,nbContactPoints,nfs;
+
+ typedef boost::shared_ptr<soth::HCOD> hcod_ptr_t;
+ hcod_ptr_t hsolver;
+
+ Eigen::MatrixXd Cdyn,Ccontact,Czmp,Czero;
+ soth::VectorBound bdyn,bcontact,bzmp,bzero;
+ std::vector< Eigen::MatrixXd > Ctasks;
+ std::vector< soth::VectorBound > btasks;
+
+ Eigen::VectorXd solution;
+
+ static const int nbPoint = 4;
+
+ }; // class SolverOpSpace
+
+ } // namespace dyninv
+ } // namespace sot
+} // namespace dynamicgraph
+
+
+
+#endif // #ifndef __sot_dyninv_SolverOpSpace_H__
diff --git a/src/stack-template.h b/src/stack-template.h
new file mode 100644
index 0000000000000000000000000000000000000000..9f1943444773f32f7ac021b3b06bcb00838751f0
--- /dev/null
+++ b/src/stack-template.h
@@ -0,0 +1,194 @@
+/*
+ * Copyright 2011, Nicolas Mansard, LAAS-CNRS
+ *
+ * This file is part of sot-dyninv.
+ * sot-dyninv is free software: you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public License
+ * as published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ * sot-dyninv is distributed in the hope that it will be
+ * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details. You should
+ * have received a copy of the GNU Lesser General Public License along
+ * with sot-dyninv. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __sot_dyninv_StackTemplate_H__
+#define __sot_dyninv_StackTemplate_H__
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined (WIN32)
+# if defined (dynamic_interpretor_EXPORTS)
+# define SOTSTACKTEMPLATE_EXPORT __declspec(dllexport)
+# else
+# define SOTSTACKTEMPLATE_EXPORT __declspec(dllimport)
+# endif
+#else
+# define SOTSTACKTEMPLATE_EXPORT
+#endif
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#include <sot-dyninv/signal-helper.h>
+
+namespace dynamicgraph {
+ namespace sot {
+
+ /* --------------------------------------------------------------------- */
+ /* --- CLASS ----------------------------------------------------------- */
+ /* --------------------------------------------------------------------- */
+
+ namespace dg = dynamicgraph;
+
+ template< typename TaskGeneric >
+ class SOTSTACKTEMPLATE_EXPORT Stack
+ {
+
+ protected:
+
+ typedef std::vector<TaskGeneric*> Stack_t;
+ typedef typename Stack_t::iterator StackIterator_t;
+ typedef typename Stack_t::const_iterator StackConstIterator_t;
+
+ /*! \brief List of task (controllers) managed by the stack of tasks. */
+ Stack_t stack;
+
+ /*! \brief Number of joints to be used to compute the control law. */
+ int nbDofs;
+
+ public:
+
+ /*! \brief Number of joints by default. */
+ static const unsigned int NB_JOINTS_DEFAULT; // = 48;
+
+ public: /* --- Constructors --- */
+
+ Stack( void );
+ ~Stack( void ) { /* TODO!! */ }
+
+ public: /* --- Modifiors --- */
+
+ /*! \name Methods to handle the stack.
+ @{
+ */
+
+ /*! \brief Push the task in the stack.
+ It has a lowest priority than the previous ones.
+ If this is the first task, then it has the highest
+ priority. */
+ virtual void push( TaskGeneric& task );
+ /*! \brief Pop the task from the stack.
+ This method removes the task with the smallest
+ priority in the task. The other are projected
+ in the null-space of their predecessors. */
+ virtual TaskGeneric& pop( void );
+
+ /*! \brief This method allows to know if a task exists or not */
+ virtual bool exist( const TaskGeneric& task );
+
+ /*! \brief Remove a task regardless to its position in the stack.
+ It removes also the signals connected to the output signal of this stack.*/
+ virtual void remove( const TaskGeneric& task );
+
+ /*! \brief This method makes the task to swap with the task having the
+ immediate superior priority. */
+ virtual void up( const TaskGeneric& task );
+
+ /*! \brief This method makes the task to swap with the task having the
+ immediate inferior priority. */
+ virtual void down( const TaskGeneric& task );
+
+ /*! \brief Remove all the tasks from the stack. */
+ virtual void clear( void );
+
+ /*! Change the number of DOF, ie the field nbDofs. */
+ virtual void defineNbDof( const int& nbDof );
+
+ /*! @} */
+
+ dg::SignalBase<int>* tatata;
+
+ int titit;
+ typedef std::list< const dg::SignalBase<int>* > TaskDependancyList_t;
+
+ int totol;
+
+ /*! \brief Return the signal to be added/removed from the dependancy
+ * list of the control signal. */
+ virtual TaskDependancyList_t getTaskDependancyList( const TaskGeneric& task ) = 0;
+ virtual void addDependancy( const TaskDependancyList_t& depList ) = 0;
+ virtual void removeDependancy( const TaskDependancyList_t& depList ) = 0;
+ virtual void resetReady( void ) = 0;
+
+ public: /* --- DISPLAY --- */
+
+ /*! \name Methods to display the stack of tasks.
+ @{
+ */
+
+ /*! Display the stack of tasks in text mode as a tree. */
+ virtual void display( std::ostream& os ) const;
+ /*! \brief Write the priority between tasks in the outstream os. */
+ virtual std::ostream &
+ writeGraph(const std::string & name, std::ostream & os) const;
+
+ /*! @} */
+
+ public: /* --- COMMANDS --- */
+
+ /*! @{ */
+
+ /* To use the macro, the typedef sot::Stack<TaskSpec> stack_t has to be set,
+ * as well as the EntityClassName. */
+ //#define ADD_COMMANDS_FOR_THE_STACK /// Code in the .t.cpp
+
+
+ /*! \brief This method deals with the command line.
+ The command given in argument is send to the stack of tasks by the shell.
+ The command understood by sot are:
+ <ul>
+ <li> Tasks
+ <ul>
+ <li> push <task> : Push a task in the stack (FILO).
+ <li> pop : Remove the task push in the stack.
+ <li> down <task> : Make the task have a higher priority, i.e.
+ swap with the task immediatly superior in priority.
+ <li> up <task> : Make the task have a lowest priority, i.e.
+ swap with the task immediatly inferior in priority.
+ <li> rm <task> : Remove the task from the stack.
+ </ul>
+ */
+ virtual bool stackCommandLine( const std::string& cmdLine,
+ std::istringstream& cmdArgs,
+ std::ostream& os );
+
+ void pushByTaskName( const std::string& taskName );
+ void removeByTaskName( const std::string& taskName );
+ void upByTaskName( const std::string& taskName );
+ void downByTaskName( const std::string& taskName );
+
+ /*! @} */
+
+ }; // class StackTemplate
+
+
+ template< typename TaskGeneric >
+ std::ostream&
+ operator<< ( std::ostream& os,const Stack<TaskGeneric>& sot )
+ {
+ sot.display(os);
+ return os;
+ }
+
+
+ } // namespace sot
+} // namespace dynamicgraph
+
+#include <sot-dyninv/stack-template.t.cpp>
+
+#endif // #ifndef __sot_dyninv_StackTemplate_H__
diff --git a/src/stack-template.t.cpp b/src/stack-template.t.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..86318b9354b812ff160c66f99390865f91ebe602
--- /dev/null
+++ b/src/stack-template.t.cpp
@@ -0,0 +1,371 @@
+/*
+ * Copyright 2011, Nicolas Mansard, LAAS-CNRS
+ *
+ * This file is part of sot-dyninv.
+ * sot-dyninv is free software: you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public License
+ * as published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ * sot-dyninv is distributed in the hope that it will be
+ * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details. You should
+ * have received a copy of the GNU Lesser General Public License along
+ * with sot-dyninv. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __sot_dyninv_StackTemplate_TCC__
+#define __sot_dyninv_StackTemplate_TCC__
+
+#include <dynamic-graph/pool.h>
+#include <sot/core/debug.hh>
+#include <sot/core/task-abstract.hh>
+
+namespace dynamicgraph
+{
+ namespace sot
+ {
+
+ /* --------------------------------------------------------------------- */
+ /* --- CLASS ----------------------------------------------------------- */
+ /* --------------------------------------------------------------------- */
+
+ template< typename TaskGeneric >
+ const unsigned int Stack<TaskGeneric>::NB_JOINTS_DEFAULT = 46;
+
+ /* --------------------------------------------------------------------- */
+ /* --- CONSTRUCTION ---------------------------------------------------- */
+ /* --------------------------------------------------------------------- */
+
+ template< typename TaskGeneric >
+ Stack<TaskGeneric>::
+ Stack( void )
+ :stack()
+ ,nbDofs( NB_JOINTS_DEFAULT )
+ {
+ }
+
+ /* --------------------------------------------------------------------- */
+ /* --- STACK MANIPULATION --- */
+ /* --------------------------------------------------------------------- */
+
+ template< typename TaskGeneric >
+ void Stack<TaskGeneric>::
+ push( TaskGeneric& task )
+ {
+ stack.push_back( &task );
+ addDependancy( getTaskDependancyList( task ) );
+ resetReady();
+ }
+ template< typename TaskGeneric >
+ TaskGeneric& Stack<TaskGeneric>::
+ pop( void )
+ {
+ TaskGeneric* res = stack.back();
+ stack.pop_back();
+ removeDependancy( getTaskDependancyList( *res ) );
+ resetReady();
+ return *res;
+ }
+ template< typename TaskGeneric >
+ bool Stack<TaskGeneric>::
+ exist( const TaskGeneric& key )
+ {
+ StackIterator_t it;
+ for ( it=stack.begin();stack.end()!=it;++it )
+ {
+ if( *it == &key ) { return true; }
+ }
+ return false;
+ }
+ template< typename TaskGeneric >
+ void Stack<TaskGeneric>::
+ remove( const TaskGeneric& key )
+ {
+ bool find =false;
+ StackIterator_t it;
+ for ( it=stack.begin();stack.end()!=it;++it )
+ {
+ if( *it == &key ) { find=true; break; }
+ }
+ if(! find ){ return; }
+
+ stack.erase( it );
+ removeDependancy( getTaskDependancyList( key ) );
+ resetReady();
+ }
+
+ template< typename TaskGeneric >
+ void Stack<TaskGeneric>::
+ up( const TaskGeneric& key )
+ {
+ bool find =false;
+ StackIterator_t it;
+ for ( it=stack.begin();stack.end()!=it;++it )
+ {
+ if( *it == &key ) { find=true; break; }
+ }
+ if( stack.begin()==it ) { return; }
+ if(! find ){ return; }
+
+ StackIterator_t pos=it; pos--;
+ TaskGeneric * task = *it;
+ stack.erase( it );
+ stack.insert( pos,task );
+ resetReady();
+ }
+ template< typename TaskGeneric >
+ void Stack<TaskGeneric>::
+ down( const TaskGeneric& key )
+ {
+ bool find =false;
+ StackIterator_t it;
+ for ( it=stack.begin();stack.end()!=it;++it )
+ {
+ if( *it == &key ) { find=true; break; }
+ }
+ if( stack.end()==it ) { return; }
+ if(! find ){ return; }
+
+ StackIterator_t pos=it; pos++;
+ TaskGeneric* task=*it;
+ stack.erase( it );
+ if( stack.end()==pos ){ stack.push_back(task); }
+ else
+ {
+ pos++;
+ stack.insert( pos,task );
+ }
+ resetReady();
+ }
+
+ template< typename TaskGeneric >
+ void Stack<TaskGeneric>::
+ clear( void )
+ {
+ for ( StackIterator_t it=stack.begin();stack.end()!=it;++it )
+ {
+ removeDependancy( getTaskDependancyList( **it ) );
+ }
+ stack.clear();
+ resetReady();
+ }
+
+ template< typename TaskGeneric >
+ void Stack<TaskGeneric>::
+ defineNbDof( const int& nbDof )
+ {
+ assert(nbDof >= 0);
+ nbDofs = nbDof;
+ resetReady();
+ }
+
+ /* --------------------------------------------------------------------- */
+ /* --- DISPLAY --------------------------------------------------------- */
+ /* --------------------------------------------------------------------- */
+
+ template< typename TaskGeneric >
+ void Stack<TaskGeneric>::
+ display( std::ostream& os ) const
+ {
+
+ os << "+-----------------"<<std::endl<<"+ SOT "
+ << std::endl<< "+-----------------"<<std::endl;
+ for ( StackConstIterator_t it=this->stack.begin();
+ this->stack.end()!=it;++it )
+ //BOOST_FOREACH( TaskGeneric* task, stack )
+ {
+ os << "| " << (*it)->getName() <<std::endl;
+ }
+ }
+
+
+ /* --------------------------------------------------------------------- */
+ /* --- COMMAND --------------------------------------------------------- */
+ /* --------------------------------------------------------------------- */
+
+#define ACT_BY_TASK_NAME(ACT) \
+ template< typename TaskGeneric > \
+ void Stack<TaskGeneric>:: \
+ ACT##ByTaskName( const std::string& tname ) \
+ { \
+ dg::Entity & taska = dg::g_pool.getEntity( tname ); \
+ TaskGeneric & task = dynamic_cast<TaskGeneric&>( taska ); \
+ ACT(task); \
+ }
+
+ ACT_BY_TASK_NAME(push)
+ ACT_BY_TASK_NAME(remove)
+ ACT_BY_TASK_NAME(up)
+ ACT_BY_TASK_NAME(down)
+
+ template< typename TaskGeneric >
+ bool Stack<TaskGeneric>::
+ stackCommandLine( const std::string& cmdLine,
+ std::istringstream& cmdArgs,
+ std::ostream& os )
+ {
+ sotDEBUGIN(15);
+
+ if( cmdLine == "help")
+ {
+ os << "Stack of Tasks: "<<std::endl
+ << " - push <task>"<< std::endl
+ << " - pop"<< std::endl
+ << " - down <task>"<< std::endl
+ << " - up <task>"<< std::endl
+ << " - rm <task>"<< std::endl
+ << " - clear"<< std::endl
+ << " - display"<< std::endl
+
+ << " - nbDofs <nb> "<<std::endl;
+ }
+ else if( cmdLine == "clear")
+ {
+ clear();
+ }
+ else if( cmdLine == "push")
+ {
+ std::string tname; cmdArgs >> tname;
+ pushByTaskName(tname);
+ }
+ else if( cmdLine == "up")
+ {
+ std::string tname; cmdArgs >> tname;
+ upByTaskName(tname);
+ }
+ else if( cmdLine == "down")
+ {
+ std::string tname; cmdArgs >> tname;
+ downByTaskName(tname);
+ }
+ else if( cmdLine == "rm")
+ {
+ std::string tname; cmdArgs >> tname;
+ removeByTaskName(tname);
+ }
+ else if( cmdLine == "pop")
+ {
+ TaskGeneric& task = pop();
+ os << "Remove : "<< task << std::endl;
+ }
+
+ else if( cmdLine == "nbDofs")
+ {
+ cmdArgs>>std::ws;
+ if( cmdArgs.good() )
+ {
+ unsigned int nbDof;
+ cmdArgs >> nbDof;
+ defineNbDof(nbDof);
+ } else { os << "nbDofs = "<< nbDofs <<std::endl; }
+ }
+ else if( cmdLine == "display")
+ {
+ display(os);
+ }
+ else return false;
+
+ return true;
+
+ sotDEBUGOUT(15);
+ }
+
+ template< typename TaskGeneric >
+ std::ostream& Stack<TaskGeneric>::
+ writeGraph( const std::string & name, std::ostream& os ) const
+ {
+ StackConstIterator_t iter;
+ for( iter = stack.begin(); iter!=stack.end();++iter )
+ {
+ const TaskGeneric & task = **iter;
+ StackConstIterator_t nextiter =iter;
+ nextiter++;
+
+ if (nextiter!=stack.end())
+ {
+ TaskGeneric & nexttask = **nextiter;
+ os << "\t\t\t" << task.getName() << " -> " << nexttask.getName()
+ << " [color=red]" << std::endl;
+ }
+ }
+
+ os << "\t\tsubgraph cluster_Tasks {" <<std::endl
+ << "\t\t\tsubgraph cluster_" << name << " {" << std::endl
+ << "\t\t\t\tcolor=lightsteelblue1; label=\"" << name
+ << "\"; style=filled;" << std::endl;
+
+ for( iter = stack.begin(); iter!=stack.end();++iter )
+ {
+ const TaskGeneric & task = **iter;
+ os << "\t\t\t\t" << task.getName()
+ << " [ label = \"" << task.getName() << "\" ," << std::endl
+ << "\t\t\t\t fontcolor = black, color = black, "
+ << "fillcolor = magenta, style=filled, shape=box ]" << std::endl;
+
+ }
+ os << "\t\t\t}" << std::endl;
+ os << "\t\t\t}" <<std::endl;
+ return os;
+ }
+
+
+
+
+ /* To use the macro, the typedef sot::Stack<TaskSpec> stack_t has to be set,
+ * as well as the EntityClassName. */
+#define ADD_COMMANDS_FOR_THE_STACK \
+ addCommand("dispStack", \
+ makeCommandVerbose(*this,&stack_t::display, \
+ docCommandVerbose("Guess what?"))); \
+ addCommand("up", \
+ makeCommandVoid1(*this, \
+ (void (EntityClassName::*)(const std::string&)) \
+ &stack_t::upByTaskName, \
+ docCommandVoid1("Up the named task.", \
+ "string (task name)"))); \
+ addCommand("down", \
+ makeCommandVoid1(*this, \
+ (void (EntityClassName::*)(const std::string&)) \
+ &stack_t::downByTaskName, \
+ docCommandVoid1("Down the named task.", \
+ "string (task name)"))); \
+ addCommand("push", \
+ makeCommandVoid1(*this, \
+ (void (EntityClassName::*)(const std::string&)) \
+ &stack_t::pushByTaskName, \
+ docCommandVoid1("Push the named task.", \
+ "string (task name)"))); \
+ addCommand("rm", \
+ makeCommandVoid1(*this, \
+ (void (EntityClassName::*)(const std::string&)) \
+ &stack_t::removeByTaskName, \
+ docCommandVoid1("Remove the named task.", \
+ "string (task name)"))); \
+ addCommand("clear", \
+ makeCommandVoid0(*this, \
+ (void (EntityClassName::*)(void)) \
+ &stack_t::clear, \
+ docCommandVoid0("Clear the stack from all task."))); \
+ addCommand("pop", \
+ makeCommandVoid0(*this, \
+ (void (EntityClassName::*)(void)) \
+ &stack_t::pop, \
+ docCommandVoid0("Remove the last (lowest) task of the stack."))); \
+ \
+ addCommand("setSize", \
+ makeCommandVoid1(*this, \
+ (void (EntityClassName::*)(const int&)) \
+ &stack_t::defineNbDof, \
+ docCommandVoid1("??? TODO", \
+ "int"))); \
+ addCommand("getSize", \
+ makeDirectGetter(*this,&nbDofs, \
+ docDirectGetter("size","int")))
+
+
+
+ } // namespace sot
+} // namespace dynamicgraph
+
+#endif // #ifndef __sot_dyninv_StackTemplate_TCC__
diff --git a/src/task-dyn-pd.cpp b/src/task-dyn-pd.cpp
index 7bd7e4355bcbabef9e0ac24a29ab55090ccf8eea..abb9d7eb753ad0778bd5ac6eb5137bbab04a498c 100644
--- a/src/task-dyn-pd.cpp
+++ b/src/task-dyn-pd.cpp
@@ -16,165 +16,167 @@
#include <dynamic-graph/factory.h>
-#include <sot-core/debug.h>
-#include <sot-core/feature-abstract.h>
+#include <sot/core/debug.hh>
+#include <sot/core/feature-abstract.hh>
#include <sot-dyninv/task-dyn-pd.h>
#include <sot-dyninv/commands-helper.h>
#include <boost/foreach.hpp>
-namespace sot
+namespace dynamicgraph
{
- namespace dyninv
+ namespace sot
{
-
- namespace dg = ::dynamicgraph;
- using namespace dg;
-
- /* --- DG FACTORY ------------------------------------------------------- */
- DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(TaskDynPD,"TaskDynPD");
-
- /* --- CONSTRUCTION ----------------------------------------------------- */
- /* --- CONSTRUCTION ----------------------------------------------------- */
- /* --- CONSTRUCTION ----------------------------------------------------- */
- TaskDynPD::
- TaskDynPD( const std::string & name )
- : Task(name)
-
-
- ,CONSTRUCT_SIGNAL_IN(Kv,double)
- ,CONSTRUCT_SIGNAL_IN(qdot,ml::Vector)
- ,CONSTRUCT_SIGNAL_IN(dt,double)
-
- ,CONSTRUCT_SIGNAL_OUT(errorDot,ml::Vector,
- qdotSIN<<jacobianSOUT )
- ,CONSTRUCT_SIGNAL_OUT(KvAuto,double,
- controlGainSIN)
- ,CONSTRUCT_SIGNAL_OUT(Jdot,ml::Matrix,
- jacobianSOUT)
- ,CONSTRUCT_SIGNAL_OUT(taskVector,ml::Vector,
- taskSOUT)
-
- ,previousJ(0u,0u),previousJset(false)
- {
- taskSOUT.setFunction( boost::bind(&TaskDynPD::taskSOUT_function,this,_1,_2) );
- taskSOUT.addDependency( KvSIN );
- taskSOUT.addDependency( errorDotSOUT );
-
- KvSIN.plug(&KvAutoSOUT);
-
- signalRegistration( KvSIN << qdotSIN << dtSIN
- << errorDotSOUT << KvAutoSOUT
- << JdotSOUT << taskVectorSOUT );
- }
-
-
- /* --- SIGNALS ---------------------------------------------------------- */
- /* --- SIGNALS ---------------------------------------------------------- */
- /* --- SIGNALS ---------------------------------------------------------- */
-
- ml::Vector& TaskDynPD::
- errorDotSOUT_function( ml::Vector& errorDot,int time )
+ namespace dyninv
{
- sotDEBUGIN(15);
- const ml::Vector & qdot = qdotSIN(time);
- const ml::Matrix & J = jacobianSOUT(time);
- errorDot.resize( J.nbRows() );
- J.multiply(qdot,errorDot);
+ namespace dg = ::dynamicgraph;
- sotDEBUGOUT(15);
- return errorDot;
- }
+ /* --- DG FACTORY ------------------------------------------------------- */
+ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(TaskDynPD,"TaskDynPD");
- double& TaskDynPD::
- KvAutoSOUT_function( double& Kv,int time )
- {
- sotDEBUGIN(15);
-
- const double & Kp = controlGainSIN(time);
- assert(Kp>=0);
- Kv = 2*sqrt(Kp);
-
- sotDEBUGOUT(15);
- return Kv;
- }
-
- sot::VectorMultiBound& TaskDynPD::
- taskSOUT_function( sot::VectorMultiBound& task,int time )
- {
- sotDEBUGIN(15);
+ /* --- CONSTRUCTION ----------------------------------------------------- */
+ /* --- CONSTRUCTION ----------------------------------------------------- */
+ /* --- CONSTRUCTION ----------------------------------------------------- */
+ TaskDynPD::
+ TaskDynPD( const std::string & name )
+ : Task(name)
+
+
+ ,CONSTRUCT_SIGNAL_IN(Kv,double)
+ ,CONSTRUCT_SIGNAL_IN(qdot,ml::Vector)
+ ,CONSTRUCT_SIGNAL_IN(dt,double)
+
+ ,CONSTRUCT_SIGNAL_OUT(errorDot,ml::Vector,
+ qdotSIN<<jacobianSOUT )
+ ,CONSTRUCT_SIGNAL_OUT(KvAuto,double,
+ controlGainSIN)
+ ,CONSTRUCT_SIGNAL_OUT(Jdot,ml::Matrix,
+ jacobianSOUT)
+ ,CONSTRUCT_SIGNAL_OUT(taskVector,ml::Vector,
+ taskSOUT)
+
+ ,previousJ(0u,0u),previousJset(false)
+ {
+ taskSOUT.setFunction( boost::bind(&TaskDynPD::taskSOUT_function,this,_1,_2) );
+ taskSOUT.addDependency( KvSIN );
+ taskSOUT.addDependency( errorDotSOUT );
+
+ KvSIN.plug(&KvAutoSOUT);
- const ml::Vector & e = errorSOUT(time);
- const ml::Vector & edot = errorDotSOUT(time);
- const double& Kp = controlGainSIN(time);
- const double& Kv = KvSIN(time);
-
- assert( e.size() == edot.size() );
- task .resize( e.size() );
- for( unsigned int i=0;i<task.size(); ++i )
- task[i] = - Kp*e(i) - Kv*edot(i);
-
- sotDEBUGOUT(15);
- return task;
- }
-
- ml::Matrix& TaskDynPD::
- JdotSOUT_function( ml::Matrix& Jdot,int time )
- {
- sotDEBUGIN(15);
-
- const ml::Matrix& currentJ = jacobianSOUT(time);
- const double& dt = dtSIN(time);
-
- if( previousJ.nbRows()!=currentJ.nbRows() ) previousJset = false;
-
- if( previousJset )
- {
- assert( currentJ.nbRows()==previousJ.nbRows()
- && currentJ.nbCols()==previousJ.nbCols() );
-
- Jdot .resize( currentJ.nbRows(),currentJ.nbCols() );
- Jdot = currentJ - previousJ;
- Jdot *= 1/dt;
- }
- else { Jdot.resize(currentJ.nbRows(),currentJ.nbCols() ); Jdot.fill(0); }
-
- previousJ = currentJ;
- previousJset = true;
-
- sotDEBUGOUT(15);
- return Jdot;
- }
-
- ml::Vector& TaskDynPD::
- taskVectorSOUT_function( ml::Vector& taskV, int time )
- {
- const sot::VectorMultiBound & task = taskSOUT(time);
- taskV.resize(task.size());
- for( unsigned int i=0;i<task.size(); ++i )
- taskV(i) = task[i].getSingleBound();
-
- return taskV;
- }
-
-
- /* --- ENTITY ----------------------------------------------------------- */
- /* --- ENTITY ----------------------------------------------------------- */
- /* --- ENTITY ----------------------------------------------------------- */
-
- void TaskDynPD::
- display( std::ostream& os ) const
- {
- os << "TaskDynPD " << name << ": " << std::endl;
- os << "--- LIST --- " << std::endl;
- BOOST_FOREACH( sot::FeatureAbstract* feature,featureList )
- {
- os << "-> " << feature->getName() << std::endl;
- }
- }
-
- } // namespace dyninv
-} // namespace sot
+ signalRegistration( KvSIN << qdotSIN << dtSIN
+ << errorDotSOUT << KvAutoSOUT
+ << JdotSOUT << taskVectorSOUT );
+ }
+
+
+ /* --- SIGNALS ---------------------------------------------------------- */
+ /* --- SIGNALS ---------------------------------------------------------- */
+ /* --- SIGNALS ---------------------------------------------------------- */
+
+ ml::Vector& TaskDynPD::
+ errorDotSOUT_function( ml::Vector& errorDot,int time )
+ {
+ sotDEBUGIN(15);
+
+ const ml::Vector & qdot = qdotSIN(time);
+ const ml::Matrix & J = jacobianSOUT(time);
+ errorDot.resize( J.nbRows() );
+ J.multiply(qdot,errorDot);
+
+ sotDEBUGOUT(15);
+ return errorDot;
+ }
+
+ double& TaskDynPD::
+ KvAutoSOUT_function( double& Kv,int time )
+ {
+ sotDEBUGIN(15);
+
+ const double & Kp = controlGainSIN(time);
+ assert(Kp>=0);
+ Kv = 2*sqrt(Kp);
+
+ sotDEBUGOUT(15);
+ return Kv;
+ }
+
+ dg::sot::VectorMultiBound& TaskDynPD::
+ taskSOUT_function( dg::sot::VectorMultiBound& task,int time )
+ {
+ sotDEBUGIN(15);
+
+ const ml::Vector & e = errorSOUT(time);
+ const ml::Vector & edot = errorDotSOUT(time);
+ const double& Kp = controlGainSIN(time);
+ const double& Kv = KvSIN(time);
+
+ assert( e.size() == edot.size() );
+ task .resize( e.size() );
+ for( unsigned int i=0;i<task.size(); ++i )
+ task[i] = - Kp*e(i) - Kv*edot(i);
+
+ sotDEBUGOUT(15);
+ return task;
+ }
+
+ ml::Matrix& TaskDynPD::
+ JdotSOUT_function( ml::Matrix& Jdot,int time )
+ {
+ sotDEBUGIN(15);
+
+ const ml::Matrix& currentJ = jacobianSOUT(time);
+ const double& dt = dtSIN(time);
+
+ if( previousJ.nbRows()!=currentJ.nbRows() ) previousJset = false;
+
+ if( previousJset )
+ {
+ assert( currentJ.nbRows()==previousJ.nbRows()
+ && currentJ.nbCols()==previousJ.nbCols() );
+
+ Jdot .resize( currentJ.nbRows(),currentJ.nbCols() );
+ Jdot = currentJ - previousJ;
+ Jdot *= 1/dt;
+ }
+ else { Jdot.resize(currentJ.nbRows(),currentJ.nbCols() ); Jdot.fill(0); }
+
+ previousJ = currentJ;
+ previousJset = true;
+
+ sotDEBUGOUT(15);
+ return Jdot;
+ }
+
+ ml::Vector& TaskDynPD::
+ taskVectorSOUT_function( ml::Vector& taskV, int time )
+ {
+ const dg::sot::VectorMultiBound & task = taskSOUT(time);
+ taskV.resize(task.size());
+ for( unsigned int i=0;i<task.size(); ++i )
+ taskV(i) = task[i].getSingleBound();
+
+ return taskV;
+ }
+
+
+ /* --- ENTITY ----------------------------------------------------------- */
+ /* --- ENTITY ----------------------------------------------------------- */
+ /* --- ENTITY ----------------------------------------------------------- */
+
+ void TaskDynPD::
+ display( std::ostream& os ) const
+ {
+ os << "TaskDynPD " << name << ": " << std::endl;
+ os << "--- LIST --- " << std::endl;
+ BOOST_FOREACH( dg::sot::FeatureAbstract* feature,featureList )
+ {
+ os << "-> " << feature->getName() << std::endl;
+ }
+ }
+
+ } // namespace dyninv
+ } // namespace sot
+} // namespace dynamicgraph
diff --git a/include/sot-dyninv/task-dyn-pd.h b/src/task-dyn-pd.h
similarity index 54%
rename from include/sot-dyninv/task-dyn-pd.h
rename to src/task-dyn-pd.h
index a4419a40f9108b2cba4980a629830c1eb9907c9f..9463fa826e35c9c5f803e7b90303219f7a0f125e 100644
--- a/include/sot-dyninv/task-dyn-pd.h
+++ b/src/task-dyn-pd.h
@@ -38,56 +38,56 @@
/* SOT */
#include <sot-dyninv/signal-helper.h>
#include <sot-dyninv/entity-helper.h>
-#include <sot-core/task.h>
+#include <sot/core/task.hh>
+namespace dynamicgraph {
+ namespace sot {
+ namespace dyninv {
-namespace sot {
- namespace dyninv {
+ /* --------------------------------------------------------------------- */
+ /* --- CLASS ----------------------------------------------------------- */
+ /* --------------------------------------------------------------------- */
- /* --------------------------------------------------------------------- */
- /* --- CLASS ----------------------------------------------------------- */
- /* --------------------------------------------------------------------- */
+ class SOTTASKDYNPD_EXPORT TaskDynPD
+ :public ::dynamicgraph::sot::Task
+ ,public ::dynamicgraph::EntityHelper<TaskDynPD>
+ {
- class SOTTASKDYNPD_EXPORT TaskDynPD
- :public ::sot::Task
- ,public ::dynamicgraph::EntityHelper<TaskDynPD>
- {
+ public: /* --- CONSTRUCTOR ---- */
- public: /* --- CONSTRUCTOR ---- */
+ TaskDynPD( const std::string & name );
- TaskDynPD( const std::string & name );
+ public: /* --- ENTITY INHERITANCE --- */
- public: /* --- ENTITY INHERITANCE --- */
+ static const std::string CLASS_NAME;
+ virtual void display( std::ostream& os ) const;
+ virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
- static const std::string CLASS_NAME;
- virtual void display( std::ostream& os ) const;
- virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
+ public: /* --- SIGNALS --- */
- public: /* --- SIGNALS --- */
+ DECLARE_SIGNAL_IN(Kv,double);
+ DECLARE_SIGNAL_IN(qdot,ml::Vector);
+ DECLARE_SIGNAL_IN(dt,double);
- DECLARE_SIGNAL_IN(Kv,double);
- DECLARE_SIGNAL_IN(qdot,ml::Vector);
- DECLARE_SIGNAL_IN(dt,double);
+ DECLARE_SIGNAL_OUT(errorDot,ml::Vector);
+ DECLARE_SIGNAL_OUT(KvAuto,double);
+ DECLARE_SIGNAL_OUT(Jdot,ml::Matrix);
+ DECLARE_SIGNAL_OUT(taskVector,ml::Vector);
- DECLARE_SIGNAL_OUT(errorDot,ml::Vector);
- DECLARE_SIGNAL_OUT(KvAuto,double);
- DECLARE_SIGNAL_OUT(Jdot,ml::Matrix);
- DECLARE_SIGNAL_OUT(taskVector,ml::Vector);
+ protected:
+ dynamicgraph::sot::VectorMultiBound&
+ taskSOUT_function( dynamicgraph::sot::VectorMultiBound& task,int iter );
- protected:
- sot::VectorMultiBound&
- taskSOUT_function( sot::VectorMultiBound& task,int iter );
+ protected:
+ ml::Matrix previousJ;
+ bool previousJset;
- protected:
- ml::Matrix previousJ;
- bool previousJset;
-
- }; // class TaskDynPD
-
- } // namespace dyninv
-} // namespace sot
+ }; // class TaskDynPD
+ } // namespace dyninv
+ } // namespace sot
+} // namespace dynamicgraph
#endif // #ifndef __sot_dyninv_TaskDynPD_H__
diff --git a/src/zmp-estimator.cpp b/src/zmp-estimator.cpp
index 96f67ff79bd017cf61627f16488ec62036c083c7..14f5afb71dddbf2c5d2c631ba7b7acdd8927c951 100644
--- a/src/zmp-estimator.cpp
+++ b/src/zmp-estimator.cpp
@@ -17,95 +17,98 @@
#include <sot-dyninv/zmp-estimator.h>
#include <sot-dyninv/commands-helper.h>
-#include <sot-core/debug.h>
+#include <sot/core/debug.hh>
#include <dynamic-graph/factory.h>
-namespace sot
+namespace dynamicgraph
{
- namespace dyninv
+ namespace sot
{
-
- namespace dg = ::dynamicgraph;
- using namespace dg;
- using dg::SignalBase;
-
- /* --- DG FACTORY ------------------------------------------------------- */
- DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ZmpEstimator,"ZmpEstimator");
-
- /* --- CONSTRUCTION ----------------------------------------------------- */
- /* --- CONSTRUCTION ----------------------------------------------------- */
- /* --- CONSTRUCTION ----------------------------------------------------- */
- ZmpEstimator::
- ZmpEstimator( const std::string & name )
- : Entity(name)
- ,CONSTRUCT_SIGNAL_IN(fn,ml::Matrix)
- ,CONSTRUCT_SIGNAL_IN(support,ml::Vector)
- ,CONSTRUCT_SIGNAL_OUT(zmp,ml::Vector, fnSIN << supportSIN)
- {
- signalRegistration( fnSIN << supportSIN << zmpSOUT );
- }
-
- /* --- SIGNALS ---------------------------------------------------------- */
- /* --- SIGNALS ---------------------------------------------------------- */
- /* --- SIGNALS ---------------------------------------------------------- */
-
- ml::Vector& ZmpEstimator::
- zmpSOUT_function( ml::Vector &zmp, int iter )
- {
- sotDEBUG(15) << " # In time = " << iter << std::endl;
-
- const ml::Vector& mlfn = fnSIN(iter);
- const ml::Matrix& support = supportSIN(iter);
- zmp.resize(2);
- ml::Vector mlzmp;
- mlzmp.resize(2);
- double Sfn=0.;
- for(int i=0;i<4;i++)
- {
- mlzmp(0) += mlfn(i)*support(0,i);
- mlzmp(1) += mlfn(i)*support(1,i);
- Sfn += mlfn(i);
- }
- zmp(0) = mlzmp(0)/Sfn;
- zmp(1) = mlzmp(1)/Sfn;
-
- sotDEBUG(1) << "zmp = " << zmp << std::endl;
- return zmp;
- }
-
-
- /* --- COMMANDS ---------------------------------------------------------- */
- /* --- COMMANDS ---------------------------------------------------------- */
- /* --- COMMANDS ---------------------------------------------------------- */
-
-
- /* --- ENTITY ----------------------------------------------------------- */
- /* --- ENTITY ----------------------------------------------------------- */
- /* --- ENTITY ----------------------------------------------------------- */
-
- void ZmpEstimator::
- display( std::ostream& os ) const
+ namespace dyninv
{
- os << "ZmpEstimator "<<getName() << "." << std::endl;
- }
- void ZmpEstimator::
- commandLine( const std::string& cmdLine,
- std::istringstream& cmdArgs,
- std::ostream& os )
- {
- if( cmdLine == "help" )
- {
- os << "ZmpEstimator:\n"
- << "\t- ." << std::endl;
- Entity::commandLine( cmdLine,cmdArgs,os );
- }
- else
- {
- Entity::commandLine( cmdLine,cmdArgs,os );
- }
- }
-
- } // namespace dyninv
-} // namespace sot
+ namespace dg = ::dynamicgraph;
+ using namespace dg;
+ using dg::SignalBase;
+
+ /* --- DG FACTORY ------------------------------------------------------- */
+ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ZmpEstimator,"ZmpEstimator");
+
+ /* --- CONSTRUCTION ----------------------------------------------------- */
+ /* --- CONSTRUCTION ----------------------------------------------------- */
+ /* --- CONSTRUCTION ----------------------------------------------------- */
+ ZmpEstimator::
+ ZmpEstimator( const std::string & name )
+ : Entity(name)
+ ,CONSTRUCT_SIGNAL_IN(fn,ml::Matrix)
+ ,CONSTRUCT_SIGNAL_IN(support,ml::Vector)
+ ,CONSTRUCT_SIGNAL_OUT(zmp,ml::Vector, fnSIN << supportSIN)
+ {
+ signalRegistration( fnSIN << supportSIN << zmpSOUT );
+ }
+
+ /* --- SIGNALS ---------------------------------------------------------- */
+ /* --- SIGNALS ---------------------------------------------------------- */
+ /* --- SIGNALS ---------------------------------------------------------- */
+
+ ml::Vector& ZmpEstimator::
+ zmpSOUT_function( ml::Vector &zmp, int iter )
+ {
+ sotDEBUG(15) << " # In time = " << iter << std::endl;
+
+ const ml::Vector& mlfn = fnSIN(iter);
+ const ml::Matrix& support = supportSIN(iter);
+ zmp.resize(2);
+ ml::Vector mlzmp;
+ mlzmp.resize(2);
+ double Sfn=0.;
+ for(int i=0;i<4;i++)
+ {
+ mlzmp(0) += mlfn(i)*support(0,i);
+ mlzmp(1) += mlfn(i)*support(1,i);
+ Sfn += mlfn(i);
+ }
+ zmp(0) = mlzmp(0)/Sfn;
+ zmp(1) = mlzmp(1)/Sfn;
+
+ sotDEBUG(1) << "zmp = " << zmp << std::endl;
+ return zmp;
+ }
+
+
+ /* --- COMMANDS ---------------------------------------------------------- */
+ /* --- COMMANDS ---------------------------------------------------------- */
+ /* --- COMMANDS ---------------------------------------------------------- */
+
+
+ /* --- ENTITY ----------------------------------------------------------- */
+ /* --- ENTITY ----------------------------------------------------------- */
+ /* --- ENTITY ----------------------------------------------------------- */
+
+ void ZmpEstimator::
+ display( std::ostream& os ) const
+ {
+ os << "ZmpEstimator "<<getName() << "." << std::endl;
+ }
+
+ void ZmpEstimator::
+ commandLine( const std::string& cmdLine,
+ std::istringstream& cmdArgs,
+ std::ostream& os )
+ {
+ if( cmdLine == "help" )
+ {
+ os << "ZmpEstimator:\n"
+ << "\t- ." << std::endl;
+ Entity::commandLine( cmdLine,cmdArgs,os );
+ }
+ else
+ {
+ Entity::commandLine( cmdLine,cmdArgs,os );
+ }
+ }
+
+ } // namespace dyninv
+ } // namespace sot
+} // namespace dynamicgraph
diff --git a/include/sot-dyninv/zmp-estimator.h b/src/zmp-estimator.h
similarity index 60%
rename from include/sot-dyninv/zmp-estimator.h
rename to src/zmp-estimator.h
index 3b44e080ca8d46a7098302f55cd8a38bf67ffb22..d56770931f598422612049dc4f4f0c1546137322 100644
--- a/include/sot-dyninv/zmp-estimator.h
+++ b/src/zmp-estimator.h
@@ -39,42 +39,44 @@
#include <sot-dyninv/signal-helper.h>
#include <sot-dyninv/entity-helper.h>
-namespace sot {
- namespace dyninv {
+namespace dynamicgraph {
+ namespace sot {
+ namespace dyninv {
- /* --------------------------------------------------------------------- */
- /* --- CLASS ----------------------------------------------------------- */
- /* --------------------------------------------------------------------- */
+ /* --------------------------------------------------------------------- */
+ /* --- CLASS ----------------------------------------------------------- */
+ /* --------------------------------------------------------------------- */
- class SOTZMPESTIMATOR_EXPORT ZmpEstimator
- :public ::dynamicgraph::Entity
- ,public ::dynamicgraph::EntityHelper<ZmpEstimator>
- {
+ class SOTZMPESTIMATOR_EXPORT ZmpEstimator
+ :public ::dynamicgraph::Entity
+ ,public ::dynamicgraph::EntityHelper<ZmpEstimator>
+ {
- public: /* --- CONSTRUCTOR ---- */
+ public: /* --- CONSTRUCTOR ---- */
- ZmpEstimator( const std::string & name );
+ ZmpEstimator( const std::string & name );
- public: /* --- ENTITY INHERITANCE --- */
+ public: /* --- ENTITY INHERITANCE --- */
- static const std::string CLASS_NAME;
- virtual void display( std::ostream& os ) const;
- virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
+ static const std::string CLASS_NAME;
+ virtual void display( std::ostream& os ) const;
+ virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
- virtual void commandLine( const std::string& cmdLine,
- std::istringstream& cmdArgs,
- std::ostream& os );
+ virtual void commandLine( const std::string& cmdLine,
+ std::istringstream& cmdArgs,
+ std::ostream& os );
- public: /* --- SIGNALS --- */
+ public: /* --- SIGNALS --- */
- DECLARE_SIGNAL_IN(fn,ml::Vector);
- DECLARE_SIGNAL_IN(support,ml::Matrix);
- DECLARE_SIGNAL_OUT(zmp,ml::Vector);
+ DECLARE_SIGNAL_IN(fn,ml::Vector);
+ DECLARE_SIGNAL_IN(support,ml::Matrix);
+ DECLARE_SIGNAL_OUT(zmp,ml::Vector);
- }; // class ZmpEstimator
+ }; // class ZmpEstimator
- } // namespace dyninv
-} // namespace sot
+ } // namespace dyninv
+ } // namespace sot
+} // namespace dynamicgraph