include/sot-core/solver-hierarchical-inequalities.h

-/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- * Copyright Projet Gepetto, LAAS, 2009
- *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- *
- * File:      solver-hierarchical-inequalities.h
- * Project:   SOT
- * Author:    Nicolas Mansard
- *
- * Version control
- * ===============
- *
- *  $Id$
- *
- * Description
- * ============
- *
- *
- * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
-
-#ifndef __SOT_sotSolverHierarchicalInequalities_HH__
-#define __SOT_sotSolverHierarchicalInequalities_HH__
-
-
-/* --- STD --- */
-#include <iostream>
-#include <sstream>
-#include <list>
-#include <vector>
-#include <deque>
-#include <typeinfo>
-#include <algorithm>
-
-
-#include <sot-core/rotation-simple.h>
-
-/* --------------------------------------------------------------------- */
-/* --- API ------------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-#ifndef SOTSOTH_EXPORT 
-#  if defined (WIN32)
-#    if defined (sotSOTH_EXPORTS)
-#      define SOTSOTH_EXPORT __declspec(dllexport)
-#    else
-#      define SOTSOTH_EXPORT __declspec(dllimport)
-#    endif
-#  else
-#    define SOTSOTH_EXPORT
-#  endif
-#endif
-
-/* --------------------------------------------------------------------- */
-/* --- CLASS ----------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-class SOTSOTH_EXPORT sotConstraintMem
-{
-public:
-  enum BoundSideType
-    {
-      BOUND_VOID = 0,
-      BOUND_INF = 1,
-      BOUND_SUP = 2,
-      BOUND_BOTH = 3
-    };
-  typedef std::vector<sotConstraintMem::BoundSideType> BoundSideVector;
-
-public:
-  bool active; // If the constraint is active.
-  bool equality; // If the constraint cannot be inactivate.
-  bool notToBeConsidered; // (during warm start) if constraint is unactive and not to be activated.
-
-  bubVector Ji; // The row of the jacobian.
-  double eiInf,eiSup; // The row of the error reference.
-  BoundSideType boundSide,activeSide;
-
-  bool rankIncreaser; // If the constraint is non redundant wrt to R. */
-  unsigned int constraintRow; // Num of the row where it is located in Ji.
-  unsigned int range; // Num of the col where it has been added in F. Used in H, not in S.
-  double lagrangian; // last value of the lagrangian -> negatif: should be unactivated.
-  double Ju,Jdu;  // effet of the control law in the task space.
-
-public:
-  sotConstraintMem( void )
-    :active(false),equality(0),notToBeConsidered(0)
-    ,Ji(0),eiInf(0),eiSup(0),boundSide(BOUND_VOID),activeSide(BOUND_VOID)
-    ,rankIncreaser(false),constraintRow(0),range(0),lagrangian(0)
-    ,Ju(0),Jdu(0) {}
-
-  sotConstraintMem( const sotConstraintMem& clone );
-  SOTSOTH_EXPORT friend std::ostream& operator<<( std::ostream& os,const BoundSideType& bs );
-  SOTSOTH_EXPORT friend std::ostream & operator<< (std::ostream& os,const sotConstraintMem &c );
-};
-
-
-  /* ---------------------------------------------------------- */
-  /* ---------------------------------------------------------- */
-  /* ---------------------------------------------------------- */
-
-class SOTSOTH_EXPORT sotConstraintRef
-{
-public:
-  unsigned int id;
-  sotConstraintMem::BoundSideType side;
-  sotConstraintRef( const unsigned int id_ = 0,
-                    const sotConstraintMem::BoundSideType side_
-                    =sotConstraintMem::BOUND_VOID )
-    :id(id_),side(side_) {}
-  SOTSOTH_EXPORT friend std::ostream & operator<< (std::ostream & os,
-                                    const sotConstraintRef & cr )
-  { return os << cr.side << cr.id; }
-};
-
-
-  /* ---------------------------------------------------------- */
-  /* ---------------------------------------------------------- */
-  /* ---------------------------------------------------------- */
-
-class SOTSOTH_EXPORT sotSolverHierarchicalInequalities
-{
-public: // protected:
-  typedef bub::matrix<double,bub::column_major> bubMatrixQRWide;
-  typedef bub::matrix_range<bubMatrixQRWide> bubMatrixQR;
-  typedef bub::matrix_range<const bubMatrixQRWide> bubMatrixQRConst;
-  typedef bub::triangular_adaptor<bubMatrixQR,bub::upper> bubMatrixQRTri;
-  typedef bub::triangular_adaptor<bubMatrixQRConst,bub::upper> bubMatrixQRTriConst;
-  typedef bub::indirect_array<> bubOrder;
-
-  typedef bub::matrix_indirect<bubMatrix> bubMatrixOrdered;
-  typedef bub::matrix_indirect<bubMatrixQRWide> bubMatrixQRWideOrdered;
-  typedef bub::matrix_indirect<bubMatrixQR> bubMatrixQROrdered;
-  typedef bub::triangular_adaptor<bubMatrixQROrdered,bub::upper> bubMatrixQROrderedTri;
-
-  typedef bub::matrix_indirect<const bubMatrixQRWide> bubMatrixQRWideOrderedConst;
-  typedef bub::matrix_indirect<bubMatrixQRConst> bubMatrixQROrderedConst;
-  typedef bub::triangular_adaptor<bubMatrixQROrderedConst,bub::upper> bubMatrixQROrderedTriConst;
-
-  typedef std::vector<sotConstraintMem> ConstraintList;
-  typedef std::vector<sotConstraintMem*> ConstraintRefList;
-  static double THRESHOLD_ZERO;
-
-public: // protected:
-  unsigned int nJ;
-  bubVector u0;
-
-  /* --- H data --- */
-  sotRotationComposedInExtenso& Qh;
-  bubMatrix& Rh;
-  unsigned int rankh,freeRank;
-  ConstraintList& constraintH;
-  bool Hactivation,Hinactivation;
-  unsigned int HactivationRef,HinactivationRef;
-  sotConstraintMem::BoundSideType HactivationSide;
-
-  /* --- S data --- */
-  bubMatrixQRWide QhJsU,QhJs;
-  unsigned int ranks,sizes;
-  bubOrder orderS;
-  ConstraintList constraintS;
-  ConstraintRefList constraintSactive;
-  bool Sactivation,Sinactivation;
-  unsigned int SactivationRef,SinactivationRef;
-  sotConstraintMem::BoundSideType SactivationSide;
-
-  /* --- Next step --- */
-  double SactivationScore,SinactivationScore,HactivationScore,HinactivationScore;
-
-  /* --- divers --- */
-  bubVector du,slackInf,slackSup,lagrangian;
-  sotRotationComposed lastRotation;
-  int freeRankChange;
-
-  /* --- Warm start knowledge --- */
-  enum ActivationTodoType { TODO_NOTHING, TODO_ACTIVATE, TODO_INACTIVATE };
-  std::vector<bool> initialActiveH;
-  sotConstraintMem::BoundSideVector initialSideH;
-  bubVector du0;
-  std::vector<sotConstraintRef> slackActiveSet;
-  std::vector<sotConstraintRef> toActivate,toInactivate;
-  bool warmStartReady;
-
-  /* ---------------------------------------------------------- */
-public:
-  sotSolverHierarchicalInequalities( unsigned int _nJ,
-                                     sotRotationComposedInExtenso& _Qh,
-                                     bubMatrix &_Rh,
-                                     ConstraintList &_cH )
-    :nJ(_nJ),u0(_nJ),Qh(_Qh),Rh(_Rh),rankh(0),freeRank(_nJ),constraintH(_cH)
-    ,ranks(0),warmStartReady(false)
-  {
-    u0*=0;
-  }
- private:
-  sotSolverHierarchicalInequalities( const sotSolverHierarchicalInequalities& clone )
-    : nJ(clone.nJ),Qh(clone.Qh),Rh(clone.Rh),constraintH(clone.constraintH){ /* forbiden */ }
-
- public:
-  /* ---------------------------------------------------------- */
-  void initConstraintSize( const unsigned int size );
-  void setInitialCondition( const bubVector& _u0,
-                            const unsigned int _rankh );
-  void setInitialConditionVoid( void );
-  void setNbDof( const unsigned int nJ );
-  unsigned int getNbDof( void ) { return nJ; }
-  /* ---------------------------------------------------------- */
-  void recordInitialConditions( void );
-  void computeDifferentialCondition( void );
-
-  const std::vector<sotConstraintRef> & getToActivateList( void ) const
-  { return toActivate; }
-  const std::vector<sotConstraintRef> & getToInactivateList( void ) const
-  { return toInactivate; }
-  const bubVector & getDifferentialU0( void ) const
-  {    return du0;  }
-  const std::vector<sotConstraintRef>& getSlackActiveSet( void ) const
-  { return slackActiveSet; }
-  void printDifferentialCondition( std::ostream & os ) const;
-
-  /* ---------------------------------------------------------- */
-  bub::range fullrange( void ) const { return bub::range(0,nJ); }
-  bub::range rangeh( void ) const { return bub::range(0,rankh); }
-  bub::range rangehs( void ) const { return bub::range(0,rankh+ranks); }
-  bub::range freerange( void ) const { return bub::range(rankh,nJ); }
-  bub::range freeranges( void ) const { return bub::range(rankh,rankh+ranks); }
-
-/*   bubMatrixQROrdered accessQRs( void ); */
-/*   bubMatrixQROrderedConst accessQRs( void ) const; */
-/*   bubMatrixQROrderedTri accessRs( void ); */
-  bubMatrixQROrderedTriConst accessRsConst( void ) const;
-  bub::triangular_adaptor<bub::matrix_range< const bubMatrix >,bub::upper>
-    accessRhConst( void ) const;
-  bub::triangular_adaptor<bub::matrix_range< bubMatrix >,bub::upper>
-    accessRh( void );
-  template< typename bubTemplateMatrix >
-  unsigned int rankDetermination( const bubTemplateMatrix& A,
-                                  const double threshold = THRESHOLD_ZERO );
-  /* ---------------------------------------------------------- */
-  static void displayConstraint( ConstraintList & cs );
-  void printDebug( void );
-
-  /* ---------------------------------------------------------- */
-  void warmStart( void );
-  void applyFreeSpaceMotion( const bubVector& _du );
-  void forceUpdateHierachic( ConstraintRefList& toUpdate,
-                             const sotConstraintMem::BoundSideVector& boundSide );
-  void forceDowndateHierachic( ConstraintRefList& toDowndate );
-
-  /* ---------------------------------------------------------- */
-  void solve( const bubMatrix& Jse, const bubVector& ese,
-              const bubMatrix& Jsi, const bubVector& esiInf, const bubVector& esiSup,
-              const std::vector<sotConstraintMem::BoundSideType> esiBoundSide,
-              bool pushBackAtTheEnd = true );
-  void solve( const bubMatrix& Jse, const bubVector& ese,
-              const bubMatrix& Jsi, const bubVector& esiInf, const bubVector& esiSup,
-              const sotConstraintMem::BoundSideVector & esiBoundSide,
-              const std::vector<sotConstraintRef> & slackActiveWarmStart,
-              bool pushBackAtTheEnd = true );
-
-  /* ---------------------------------------------------------- */
-  void initializeConstraintMemory( const bubMatrix& Jse, const bubVector& ese,
-                                   const bubMatrix& Jsi, const bubVector& esiInf,
-                                   const bubVector& esiSup,
-                                   const sotConstraintMem::BoundSideVector& esiBoundSide,
-                                   const std::vector<sotConstraintRef>& warmStartSide );
-  void initializeDecompositionSlack(void);
-
-  /* ---------------------------------------------------------- */
-  void updateConstraintHierarchic( const unsigned int constraintId,
-                                   const sotConstraintMem::BoundSideType side );
-  void downdateConstraintHierarchic( const unsigned int kdown );
-  void updateRankOneDowndate( void );
-  void updateRankOneUpdate( void );
-
-  /* ---------------------------------------------------------- */
-  void updateConstraintSlack( const unsigned int kup,
-                              const sotConstraintMem::BoundSideType activeSide );
-  void regularizeQhJs( void );
-  void regularizeQhJsU( void );
-  void downdateConstraintSlack( const unsigned int kdown );
-
-  /* ---------------------------------------------------------- */
-  void computeGradient( bubVector& gradientWide );
-  void computePrimal( void );
-  void computeSlack( void );
-  void computeLagrangian( void );
-
-  bool selecActivationHierarchic( double & tau );
-  bool selecInactivationHierarchic( void );
-  bool selecActivationSlack( void );
-  bool selecInactivationSlack( void );
-  void pushBackSlackToHierarchy( void );
-
-};
-
-
-#endif // #ifdef __SOT_sotSolverHierarchicalInequalities_HH__
-

include/sot-core/sot-core-api.h

-/*
- *  Copyright
- */
-
-#ifndef SOT_CORE_API_HH
-#define SOT_CORE_API_HH
-
-#if defined (WIN32)
-#  ifdef SOT_CORE_EXPORTS
-#    define SOT_CORE_EXPORT __declspec(dllexport)
-#  else
-#    define SOT_CORE_EXPORT __declspec(dllimport)
-#  endif
-#else
-#  define SOT_CORE_EXPORT
-#endif
-
-#endif

include/sot-core/sot-h.h

-/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- * Copyright Projet Gepetto, LAAS, 2009
- *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- *
- * File:      sot-h.h
- * Project:   SOT
- * Author:    Nicolas Mansard
- *
- * Version control
- * ===============
- *
- *  $Id$
- *
- * Description
- * ============
- *
- *
- * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
-
-
-#ifndef __SOT_sotSOTH_H__
-#define __SOT_sotSOTH_H__
-
-
-/* --------------------------------------------------------------------- */
-/* --- INCLUDE --------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-/* Matrix */
-#include <sot-core/sot.h>
-#include <sot-core/solver-hierarchical-inequalities.h>
-
-/* --------------------------------------------------------------------- */
-/* --- API ------------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-#ifndef SOTSOTH_EXPORT 
-# if defined (WIN32)
-#  if defined (sotSOTH_EXPORTS)
-#    define SOTSOTH_EXPORT __declspec(dllexport)
-#  else
-#    define SOTSOTH_EXPORT __declspec(dllimport)
-#  endif
-# else
-#  define SOTSOTH_EXPORT
-# endif
-#endif
-
-/* --------------------------------------------------------------------- */
-/* --- CLASS ----------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-class SOTSOTH_EXPORT sotSOTH
-:public sotSOT
-{
- public:
-  /*! \brief Specify the name of the class entity. */
-  static const std::string CLASS_NAME;
-  /*! \brief Returns the name of this class. */
-  virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
-
-  /* --- SPECIFIC MEM -------------------------------------------------- */
-  class sotMemoryTaskSOTH
-    : public sotTaskAbstract::sotMemoryTaskAbstract,public Entity
-    {
-    public:
-      const sotSOTH * referenceKey;
-      sotSolverHierarchicalInequalities solver;
-      ml::Matrix JK,Jff,Jact;
-    public:
-    sotMemoryTaskSOTH( const std::string & name,
-                       const sotSOTH * ref,
-                       unsigned int nJ,
-                       sotRotationComposedInExtenso& Qh,
-                       bubMatrix &Rh,
-                       sotSolverHierarchicalInequalities::ConstraintList &cH );
-
-    public: // Entity heritage
-      static const std::string CLASS_NAME;
-      virtual void display( std::ostream& os ) const;
-      virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
-      Signal< ml::Matrix,int > jacobianConstrainedSINOUT;
-      Signal< ml::Vector,int > diffErrorSINOUT;
-      virtual void commandLine( const std::string& cmdLine,std::istringstream& cmdArgs,
-                                std::ostream& os );
-
-    };
-  /* --- \ SPECIFIC MEM ------------------------------------------------ */
-
-
- protected:
-  //typedef std::vector<sotSolverHierarchicalInequalities *> SolversList;
-  //SolversList solvers;
-  sotRotationComposedInExtenso Qh;
-  bubMatrix Rh;
-  sotSolverHierarchicalInequalities::ConstraintList constraintH;
-  sotSolverHierarchicalInequalities solverNorm;
-  sotSolverHierarchicalInequalities * solverPrec;
-  bool fillMemorySignal;
-
- public:
-
-  /*! \brief Default constructor */
-  sotSOTH( const std::string& name );
-  ~sotSOTH( void );
-
- public: /* --- CONTROL --- */
-
-  /*! \name Methods to compute the control law following the
-   recursive definition of the stack of tasks.
-   @{
-  */
-
-  /*! \brief Compute the control law. */
-  virtual ml::Vector& computeControlLaw( ml::Vector& control,const int& time );
-
-  /*! @} */
-
-
- public: /* --- DISPLAY --- */
-
- public: /* --- COMMANDS --- */
-  virtual void commandLine( const std::string& cmdLine,std::istringstream& cmdArgs,
-                            std::ostream& os );
-  virtual void defineNbDof( const unsigned int& nbDof );
-
-};
-
-
-
-
-
-#endif // #ifndef __SOT_sotSOTH_H__

include/sot-core/sot.h

-/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- * Copyright Projet JRL-Japan, 2007
- *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- *
- * File:      sotSot.h
- * Project:   SOT
- * Author:    Nicolas Mansard
- *
- * Version control
- * ===============
- *
- *  $Id$
- *
- * Description
- * ============
- *
- *
- * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
-
-
-
-#ifndef __SOT_SOT_HH
-#define __SOT_SOT_HH
-
-/* --------------------------------------------------------------------- */
-/* --- INCLUDE --------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-/* Matrix */
-#include <MatrixAbstractLayer/boost.h>
-namespace ml = maal::boost;
-
-/* Classes standards. */
-#include <list>                    /* Classe std::list   */
-
-/* SOT */
-#include <sot-core/task-abstract.h>
-#include <sot-core/flags.h>
-#include <dynamic-graph/entity.h>
-#include <sot-core/constraint.h>
-
-/* --------------------------------------------------------------------- */
-/* --- API ------------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-#ifndef SOTSOT_CORE_EXPORT
-# if defined (WIN32) 
-#  if defined (sotSOT_CORE_EXPORTS) 
-#    define SOTSOT_CORE_EXPORT __declspec(dllexport)
-#  else  
-#    define SOTSOT_CORE_EXPORT __declspec(dllimport)
-#  endif 
-# else
-#  define SOTSOT_CORE_EXPORT
-# endif
-#endif
-
-/* --------------------------------------------------------------------- */
-/* --- CLASS ----------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-
-/*! @ingroup stackoftasks
-  \brief This class implements the Stack of Task.
-  It allows to deal with the priority of the controllers
-  through the shell. The controllers can be either constraints
-  either tasks.
-  
-  
-*/
-class SOTSOT_CORE_EXPORT sotSOT
-:public Entity
-{
- public:
-  /*! \brief Specify the name of the class entity. */
-  static const std::string CLASS_NAME;
- public:
-  /*! \brief Returns the name of this class. */
-  virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
-
- protected:
-
-  /*! \brief Defines a type for a list of tasks */
-  typedef std::list<sotTaskAbstract*> StackType;
-
-
-  /*! \brief This field is a list of controllers 
-   managed by the stack of tasks. */
-  StackType stack;
-
-  /*! \brief Defines a type for a list of constraints */
-  typedef std::list<sotConstraint*> ConstraintListType;
-  /*! \brief This field is a list of constraints
-    managed by the stack of tasks. */
-  ConstraintListType constraintList;
-
-  /*! \brief Defines an interval in the state vector of the robot 
-    which is the free flyer. */
-  unsigned int ffJointIdFirst,ffJointIdLast;
-  /*! \brief Defines a default joint. */
-  static const unsigned int FF_JOINT_ID_DEFAULT = 0;
-  
-  /*   double directionalThreshold; */
-  /*   bool useContiInverse; */
-  
-  /*! \brief Store the number of joints to be used in the
-    command computed by the stack of tasks. */
-  unsigned int nbJoints;
-  
-  /*! \brief Store a pointer to compute the gradient */
-  sotTaskAbstract* taskGradient;
-
-  /*! Projection used to compute the control law. */
-  ml::Matrix Proj;
-
-  /*! Force the recomputation at each step. */
-  bool recomputeEachTime;
-
- public:
-
-  /*! \brief Threshold to compute the dumped pseudo inverse. */
-  static const double INVERSION_THRESHOLD_DEFAULT; // = 1e-4;
-
-  /*   static const double DIRECTIONAL_THRESHOLD_DEFAULT = 1e-2; */
-  /*   static const bool USE_CONTI_INVERSE_DEFAULT = false; */
-
-  /*! \brief Number of joints by default. */
-  static const unsigned int NB_JOINTS_DEFAULT; // = 48;
-
-  static ml::Matrix & computeJacobianConstrained( const ml::Matrix& Jac,
-                                           const ml::Matrix& K,
-                                           ml::Matrix& JK,
-                                           ml::Matrix& Jff,
-                                           ml::Matrix& Jact );
-  static ml::Matrix & computeJacobianConstrained( const sotTaskAbstract& task,
-                                           const ml::Matrix& K );
-  static ml::Vector taskVectorToMlVector( const sotVectorMultiBound& taskVector );
-
- public:
-  
-  /*! \brief Default constructor */
-  sotSOT( const std::string& name );
-  ~sotSOT( void ) { /* TODO!! */ }
-
-  /*! \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( sotTaskAbstract& 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 sotTaskAbstract& pop( void );
-
-  /*! \brief This method allows to know if a task exists or not */
-  virtual bool exist( const sotTaskAbstract& 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 sotTaskAbstract& task );
-
-  /*! \brief This method removes the output signals depending on this task. */
-  virtual void removeDependancy( const sotTaskAbstract& key );
-
-  /*! \brief This method makes the task to swap with the task having the 
-    immediate superior priority. */
-  virtual void up( const sotTaskAbstract& task );
-
-  /*! \brief This method makes the task to swap with the task having the 
-    immediate inferior priority. */
-  virtual void down( const sotTaskAbstract& task );
-
-  /*! \brief Remove all the tasks from the stack. */
-  virtual void clear( void );
-  /*! @} */
-
-  
-  /*! \name Methods to handle the constraints. 
-    @{
-   */
-  /*! \brief Add a constraint to the stack with the current level
-   of priority. */
-  virtual void addConstraint( sotConstraint& constraint );
-  /*! \brief Remove a constraint from the stack. */
-  virtual void removeConstraint( const sotConstraint& constraint );
-  /*! \brief Remove all the constraints from the stack. */
-  virtual void clearConstraint( void );
-
-  /*! @} */
-
-  /*! \brief This method defines the part of the state vector which correspond
-   to the free flyer of the robot. */
-  virtual void defineFreeFloatingJoints( const unsigned int& jointIdFirst,
-				 const unsigned int& jointIdLast = -1 );
-  virtual void defineNbDof( const unsigned int& nbDof );
-
-  /*! @} */
- public: /* --- CONTROL --- */
-
-  /*! \name Methods to compute the control law following the
-   recursive definition of the stack of tasks. 
-   @{
-  */
-  
-  /*! \brief Compute the control law. */
-  virtual ml::Vector& computeControlLaw( ml::Vector& control,const int& time );
-
-  /*! \brief Compute the projector of the constraint. */
-  virtual ml::Matrix& computeConstraintProjector( ml::Matrix& Proj, const int& time );
-
-  /*! @} */
-
-  /*   double getDirectionalThreshold( void ){ return directionalThreshold; } */
-  /*   void setdirectionalThreshold( double th ){ directionalThreshold = th; } */
-  /*   bool getUseContiInverse( void ){ return useContiInverse; } */
-  /*   void setUseContiInverse( bool b ){ useContiInverse = b; } */
-  
-
- 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;
-  /*! Wrap the previous method around an operator. */
-  SOTSOT_CORE_EXPORT friend std::ostream& operator<< ( std::ostream& os,const sotSOT& sot );
-  /*! @} */
- public: /* --- SIGNALS --- */
-  
-  /*! \name Methods to handle signals  
-    @{
-   */
-  /*! \brief Intrinsec velocity of the robot, that is used to initialized
-   * the recurence of the SOT (e.g. velocity comming from the other
-   * OpenHRP plugins).
-   */
-  SignalPtr<ml::Vector,int> q0SIN;
-  /*! \brief This signal allow to change the threshold for the damped pseudo-inverse
-    on-line */
-  SignalPtr<double,int> inversionThresholdSIN;
-  /*! \brief This signal allow to get the result of the Constraint projector. */
-  SignalTimeDependant<ml::Matrix,int> constraintSOUT;
-  /*! \brief This signal allow to get the result of the computed control law. */
-  SignalTimeDependant<ml::Vector,int> controlSOUT;
-  /*! @} */
-
- public: /* --- COMMANDS --- */
-  /*! \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> 
-    <li> Constraints
-    <ul>
-    <li> addConstraint <constraint> : Add the constraint in the stack (FILO).
-    <li> rmConstraint <constraint> : Remove the constraint.
-    <li> clearConstraint : Remove all the constraints.
-    <li> printConstraint :
-    </ol>
-    </ul> 
-  */
-  virtual void commandLine( const std::string& cmdLine,std::istringstream& cmdArgs,
-			    std::ostream& os );
-
-  /*! \brief This method write the priority between tasks in the output stream os. */
-  virtual std::ostream & writeGraph(std::ostream & os) const;
-
-
-};
-
-
-
-
-
-#endif /* #ifndef __SOT_SOT_HH */
-
-
-
-

include/sot-core/task-multi-bound.h

-/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- * Copyright Projet Gepetto, Laas, CNRS, 2009
- *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- *
- * File:      task-multi-bound.h
- * Project:   SOT
- * Author:    Nicolas Mansard
- *
- * Version control
- * ===============
- *
- *  $Id$
- *
- * Description
- * ============
- *
- *
- * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
-
-
-
-#ifndef __SOT_sotMultiBound_H__
-#define __SOT_sotMultiBound_H__
-
-/* --------------------------------------------------------------------- */
-/* --- INCLUDE --------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-/* STD */
-#include <string>
-#include <vector>
-#include <iostream>
-
-/* SOT */
-#include <sot-core/sot-core-api.h>
-#include <sot-core/exception-task.h>
-
-/* --------------------------------------------------------------------- */
-/* --- CLASS ----------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-class SOT_CORE_EXPORT sotMultiBound
-{
- public:
-  enum MultiBoundModeType { MODE_SINGLE, MODE_DOUBLE };
-  enum SupInfType { BOUND_SUP,BOUND_INF };
-
- public:// protected:
-  MultiBoundModeType mode;
-  double boundSingle;
-  double boundSup,boundInf;
-  bool boundSupSetup,boundInfSetup;
-
- public:
-  sotMultiBound( const double x = 0.);
-  sotMultiBound( const double xi,const double xs );
-  sotMultiBound( const double x,const SupInfType bound );
-  sotMultiBound( const sotMultiBound& clone );
-
- public: // Acessors
-  MultiBoundModeType getMode( void ) const;
-  double getSingleBound( void ) const;
-  double getDoubleBound( const SupInfType bound ) const;
-  bool getDoubleBoundSetup( const SupInfType bound ) const;
-
- public: // Modifiors
-  void setDoubleBound( SupInfType boundType,double boundValue );
-  void unsetDoubleBound( SupInfType boundType );
-  void setSingleBound( double boundValue );
-
- public:
-  SOT_CORE_EXPORT friend std::ostream& operator<< ( std::ostream& os, const sotMultiBound & m  );
-  SOT_CORE_EXPORT friend std::istream& operator>> ( std::istream& is, sotMultiBound & m  );
-};
-
-/* --------------------------------------------------------------------- */
-typedef std::vector< sotMultiBound > sotVectorMultiBound;
-SOT_CORE_EXPORT std::ostream& operator<< (std::ostream& os, const sotVectorMultiBound& v );
-SOT_CORE_EXPORT std::istream& operator>> (std::istream& os, sotVectorMultiBound& v );
-
-
-
-#endif // #ifndef __SOT_sotMultiBound_H__

include/sot-core/vector-constant.h

-/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- * Copyright Projet JRL-Japan, 2007
- *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- *
- * File:      vector-constant.h
- * Project:   SOT
- * Author:    Nicolas Mansard
- *
- * Version control
- * ===============
- *
- *  $Id$
- *
- * Description
- * ============
- *
- *
- * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
-
-#include <dynamic-graph/entity.h>
-
-#include <dynamic-graph/all-signals.h>
-
-/* Matrix */
-#include <MatrixAbstractLayer/boost.h>
-namespace ml = maal::boost;
-
-/* --------------------------------------------------------------------- */
-/* --- VECTOR ---------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-class sotVectorConstant
-: public Entity
-{
-  static const std::string CLASS_NAME;
-  virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
-
-  int rows;
-  double color;
-
-public:
-  sotVectorConstant( const std::string& name )
-    :Entity( name )
-    ,rows(0),color(0.)
-    ,SOUT( "sotVectorConstant("+name+")::output(vector)::out" )
-    {
-      SOUT.setDependancyType( TimeDependancy<int>::BOOL_DEPENDANT );
-      signalRegistration( SOUT );
-    }
-
-  virtual ~sotVectorConstant( void ){}
-
-  SignalTimeDependant<ml::Vector,int> SOUT;
-
-  virtual void commandLine( const std::string& cmdLine,
-			    std::istringstream& cmdArgs, 
-			    std::ostream& os );
-
-};
-    
-
-
-
-
-
-

include/sot-core/vector-quaternion.h

-/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- * Copyright Projet JRL-Japan, 2007
- *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- *
- * File:      vector-quaternion.h
- * Project:   SOT
- * Author:    Nicolas Mansard
- *
- * Version control
- * ===============
- *
- *  $Id$
- *
- * Description
- * ============
- *
- *
- * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
-
-#ifndef __SOT_VECTOR_QUATERNION_H__
-#define __SOT_VECTOR_QUATERNION_H__
-
-/* --- SOT --- */
-#include <sot-core/vector-rotation.h>
-#include <sot-core/sot-core-api.h>
-
-/* --------------------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-class SOT_CORE_EXPORT sotVectorQuaternion
-: public sotVectorRotation
-{
- public: 
-
-  sotVectorQuaternion( void ) : sotVectorRotation() { ml::Vector::resize(4); }
-  virtual ~sotVectorQuaternion( void ) { }
-
-  virtual sotVectorRotation& fromMatrix( const sotMatrixRotation& rot );
-  virtual sotMatrixRotation& toMatrix( sotMatrixRotation& rot ) const;
-
-  sotVectorRotation& fromVector( const sotVectorUTheta& ut );
-
-  sotVectorQuaternion& conjugate(sotVectorQuaternion& res) const;
-  sotVectorQuaternion& multiply(const sotVectorQuaternion& q2, sotVectorQuaternion& res) const;
-
-};
-    
-
-
-#endif /* #ifndef __SOT_VECTOR_QUATERNION_H__ */

include/sot-core/vector-roll-pitch-yaw.h

-/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- * Copyright Projet JRL-Japan, 2007
- *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- *
- * File:      vector-roll-pitch-yaw.h
- * Project:   SOT
- * Author:    Nicolas Mansard
- *
- * Version control
- * ===============
- *
- *  $Id$
- *
- * Description
- * ============
- *
- *
- * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
-
-#ifndef __SOT_VECTOR_ROLLPITCHYAW_H__
-#define __SOT_VECTOR_ROLLPITCHYAW_H__
-
-/* --- SOT --- */
-#include <sot-core/vector-rotation.h>
-#include <sot-core/sot-core-api.h>
-
-/* --------------------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-class SOT_CORE_EXPORT sotVectorRollPitchYaw
-: public sotVectorRotation
-{
- public: 
-
-  sotVectorRollPitchYaw( void ) : sotVectorRotation() { }
-  virtual ~sotVectorRollPitchYaw( void ) { }
-
-  virtual sotVectorRotation& fromMatrix( const sotMatrixRotation& rot );
-  virtual sotMatrixRotation& toMatrix( sotMatrixRotation& rot ) const;
-
-};
-    
-
-
-#endif /* #ifndef __SOT_VECTOR_ROLLPITCHYAW_H__ */

include/sot-core/vector-rotation.h

-/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- * Copyright Projet JRL-Japan, 2007
- *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- *
- * File:      vector-rotation.h
- * Project:   SOT
- * Author:    Nicolas Mansard
- *
- * Version control
- * ===============
- *
- *  $Id$
- *
- * Description
- * ============
- *
- *
- * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
-
-#ifndef __SOT_VECTOR_ROTATION_H__
-#define __SOT_VECTOR_ROTATION_H__
-
-/* Matrix */
-#include <MatrixAbstractLayer/boost.h>
-namespace ml = maal::boost;
-
-/* --- SOT --- */
-#include <sot-core/matrix-rotation.h>
-#include <sot-core/sot-core-api.h>
-
-/* --------------------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-class SOT_CORE_EXPORT sotVectorRotation
-: public ml::Vector
-{
- public: 
-
-  sotVectorRotation( void ) : ml::Vector(3) { fill(0.); } 
-  virtual ~sotVectorRotation( void ) { }
-
-  virtual sotVectorRotation& fromMatrix( const sotMatrixRotation& rot ) = 0;
-  virtual sotMatrixRotation& toMatrix( sotMatrixRotation& rot ) const = 0;
-};
-    
-
-#endif /* #ifndef __SOT_VECTOR_ROTATION_H__ */
-
-
-
-

include/sot-core/vector-to-rotation.h

-/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- * Copyright Projet JRL-Japan, 2007
- *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- *
- * File:      vector-to-rotation.h
- * Project:   SOT
- * Author:    Nicolas Mansard
- *
- * Version control
- * ===============
- *
- *  $Id$
- *
- * Description
- * ============
- *
- *
- * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
-
-#ifndef __SOTVECTORTOMATRIX_HH
-#define __SOTVECTORTOMATRIX_HH
-
-#include <dynamic-graph/entity.h>
-#include <dynamic-graph/all-signals.h>
-#include <dynamic-graph/all-signals.h>
-#include <sot-core/matrix-rotation.h>
-
-/* Matrix */
-#include <MatrixAbstractLayer/boost.h>
-namespace ml = maal::boost;
-
-/* STD */ 
-#include <vector>
-
-/* --------------------------------------------------------------------- */
-/* --- VECTOR ---------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-class sotVectorToRotation
-: public Entity
-{
-  static const std::string CLASS_NAME;
-  virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
-
-  enum sotAxis
-    { 
-      AXIS_X
-      ,AXIS_Y
-      ,AXIS_Z
-    };
-
-  unsigned int size;
-  std::vector< sotAxis > axes;
-  
-
-public:
-  sotVectorToRotation( const std::string& name );
-
-  virtual ~sotVectorToRotation( void ){}
-
-  SignalPtr<ml::Vector,int> SIN;
-  SignalTimeDependant<sotMatrixRotation,int> SOUT;
-
-  sotMatrixRotation& computeRotation( const ml::Vector& angles,
-				      sotMatrixRotation& res );
-
-
-  virtual void commandLine( const std::string& cmdLine,
-			    std::istringstream& cmdArgs, 
-			    std::ostream& os );
-
-};
-    
-
-
-
-
-#endif // #ifndef __SOTVECTORTOMATRIX_HH
-
-

include/sot-core/vector-utheta.h

-/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- * Copyright Projet JRL-Japan, 2007
- *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- *
- * File:      vector-utheta.h
- * Project:   SOT
- * Author:    Nicolas Mansard
- *
- * Version control
- * ===============
- *
- *  $Id$
- *
- * Description
- * ============
- *
- *
- * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
-
-#ifndef __SOT_VECTOR_UTHETA_H__
-#define __SOT_VECTOR_UTHETA_H__
-
-/* --- SOT --- */
-#include <sot-core/vector-rotation.h>
-#include <sot-core/sot-core-api.h>
-
-/* --------------------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-class SOT_CORE_EXPORT sotVectorUTheta
-: public sotVectorRotation
-{
- public: 
-
-  sotVectorUTheta( void ) : sotVectorRotation() { }
-  virtual ~sotVectorUTheta( void ) { }
-
-  virtual sotVectorRotation& fromMatrix( const sotMatrixRotation& rot );
-  virtual sotMatrixRotation& toMatrix( sotMatrixRotation& rot ) const;
-
-};
-    
-
-
-#endif /* #ifndef __SOT_VECTOR_UTHETA_H__ */

include/sot-core/weighted-sot.h

-/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- * Copyright Projet JRL-Japan, 2007
- *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- *
- * File:      weighted-sot.h
- * Project:   SOT
- * Author:    Nicolas Mansard
- *
- * Version control
- * ===============
- *
- *  $Id$
- *
- * Description
- * ============
- *
- *
- * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
-
-
-
-#ifndef __SOT_WSOT_HH
-#define __SOT_WSOT_HH
-
-/* --------------------------------------------------------------------- */
-/* --- INCLUDE --------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-/* Matrix */
-#include <MatrixAbstractLayer/boost.h>
-namespace ml = maal::boost;
-
-/* Classes standards. */
-#include <list>                    /* Classe std::list   */
-
-/* SOT */
-#include <sot-core/sot.h>
-
-/* --------------------------------------------------------------------- */
-/* --- API ------------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-#if defined (WIN32)
-#  if defined (sotWeightedSOT_CORE_EXPORTS)
-#    define SOTWEIGHTEDSOT_CORE_EXPORT __declspec(dllexport)
-#  else
-#    define SOTWEIGHTEDSOT_CORE_EXPORT __declspec(dllimport)
-#  endif
-#else
-#  define SOTWEIGHTEDSOT_CORE_EXPORT
-#endif
-
-/* --------------------------------------------------------------------- */
-/* --- CLASS ----------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-
-/*! @ingroup stackoftasks
-  \brief This class implements the Stack of Task.
-  It allows to deal with the priority of the controllers
-  through the shell. The controllers can be either constraints
-  either tasks.
-
-
-*/
-class SOTWEIGHTEDSOT_CORE_EXPORT sotWeightedSOT
-:public sotSOT
-{
- public:
-  /*! \brief Specify the name of the class entity. */
-  static const std::string CLASS_NAME;
-  /*! \brief Returns the name of this class. */
-  virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
-
- public:
-  /*! \brief Default constructor */
-  sotWeightedSOT( const std::string& name );
-
- public: /* --- CONTROL --- */
-
-  /*! \name Methods to compute the control law following the
-   recursive definition of the stack of tasks.
-   @{
-  */
-
-  /*! \brief Compute the control law using weighted inverse. */
-  ml::Matrix& computeSquareRootInvWeight( ml::Matrix& res,const int& time );
-  ml::Vector& computeWeightedControlLaw( ml::Vector& control,const int& time );
-  ml::Matrix& computeConstrainedWeight( ml::Matrix& KAK,const int& time );
-
-  /*! @} */
-
- public: /* --- SIGNALS --- */
-
-  /*! \name Methods to handle signals
-    @{
-   */
-  SignalPtr<ml::Matrix,int> weightSIN;
-  SignalTimeDependant<ml::Matrix,int> constrainedWeightSOUT;
-  SignalPtr<ml::Matrix,int> constrainedWeightSIN;
-  SignalTimeDependant<ml::Matrix,int> squareRootInvWeightSOUT;
-  SignalPtr<ml::Matrix,int> squareRootInvWeightSIN;
-  /*! @} */
-
-};
-
-
-
-
-
-#endif /* #ifndef __SOT_SOT_HH */
-
-
-
-

include/sot/core/abstract-sot-external-interface.hh

+/*
+ * Copyright 2011,
+ * Olivier Stasse, CNRS
+ *
+ * CNRS
+ *
+ */
+
+#ifndef ABSTRACT_SOT_EXTERNAL_INTERFACE_HH
+#define ABSTRACT_SOT_EXTERNAL_INTERFACE_HH
+
+#include <map>
+#include <sot/core/api.hh>
+#include <string>
+#include <vector>
+
+namespace dynamicgraph {
+namespace sot {
+
+class SOT_CORE_EXPORT NamedVector {
+ private:
+  std::string name_;
+  std::vector<double> values_;
+
+ public:
+  NamedVector() {}
+  ~NamedVector() {}
+
+  const std::string &getName() const { return name_; }
+
+  void setName(const std::string &aname) { name_ = aname; }
+
+  const std::vector<double> &getValues() const { return values_; }
+
+  void setValues(const std::vector<double> &values) { values_ = values; }
+};
+typedef NamedVector SensorValues;
+typedef NamedVector ControlValues;
+
+class SOT_CORE_EXPORT AbstractSotExternalInterface {
+ public:
+  AbstractSotExternalInterface() {}
+
+  virtual ~AbstractSotExternalInterface() {}
+
+  virtual void setupSetSensors(
+      std::map<std::string, SensorValues> &sensorsIn) = 0;
+
+  virtual void nominalSetSensors(
+      std::map<std::string, SensorValues> &sensorsIn) = 0;
+
+  virtual void cleanupSetSensors(
+      std::map<std::string, SensorValues> &sensorsIn) = 0;
+
+  virtual void getControl(std::map<std::string, ControlValues> &) = 0;
+  virtual void setSecondOrderIntegration(void) = 0;
+  virtual void setNoIntegration(void) = 0;
+};
+}  // namespace sot
+}  // namespace dynamicgraph
+
+typedef dynamicgraph::sot::AbstractSotExternalInterface *
+createSotExternalInterface_t();
+typedef void destroySotExternalInterface_t(
+    dynamicgraph::sot::AbstractSotExternalInterface *);
+
+#endif  // ABSTRACT_SOT_EXTERNAL_INTERFACE_HH

include/sot-core/additional-functions.h → include/sot/core/additional-functions.hh

-/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- * Copyright Projet JRL-Japan, 2007
- *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+/*
+ * Copyright 2010,
+ * François Bleibel,
+ * Olivier Stasse,
  *
- * File:      additional-functions.h
- * Project:   SOT
- * Author:    François Bleibel
+ * CNRS/AIST
  *
- * Version control
- * ===============
- *
- *  $Id$
- *
- * Description
- * ============
- *
- *
- * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+ */
 
 /* SOT */
-#include <dynamic-graph/signal-base.h>
-#include <sot-core/exception-factory.h>
 #include <dynamic-graph/pool.h>
-#include <sot-core/pool.h>
-#include <sot-core/sot-core-api.h>
+#include <dynamic-graph/signal-base.h>
+
+#include <sot/core/exception-factory.hh>
+#include <sot/core/pool.hh>
+
+#include "sot/core/api.hh"
 
 /* --- STD --- */
-#include <string>
 #include <map>
 #include <sstream>
+#include <string>
 
 /* --- BOOST --- */
-#include <boost/function.hpp>
 #include <boost/bind.hpp>
+#include <boost/function.hpp>
+
+namespace dynamicgraph {
+namespace sot {
 
 /*! @ingroup factory
   \brief This helper class dynamically overloads the "new" shell command
   to allow creation of tasks and features as well as entities.
  */
-class sotAdditionalFunctions
-{
-public:
-	sotAdditionalFunctions();
-	~sotAdditionalFunctions();
-	static void cmdNew( const std::string& cmd,std::istringstream& args,
-						   std::ostream& os );
-	static void cmdMatrixDisplay( const std::string& cmd,std::istringstream& args,
-						   std::ostream& os );
-	static void cmdFlagSet( const std::string& cmd,std::istringstream& args,
-						   std::ostream& os );
+class AdditionalFunctions {
+ public:
+  AdditionalFunctions();
+  ~AdditionalFunctions();
+  static void cmdFlagSet(const std::string &cmd, std::istringstream &args,
+                         std::ostream &os);
 };
+
+} /* namespace sot */
+} /* namespace dynamicgraph */

include/sot/core/admittance-control-op-point.hh

+/*
+ * Copyright 2019
+ *
+ * LAAS-CNRS
+ *
+ * Noëlie Ramuzat
+ * This file is part of sot-core.
+ * See license file.
+ */
+
+#ifndef __sot_core_admittance_control_op_point_H__
+#define __sot_core_admittance_control_op_point_H__
+
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined(WIN32)
+#if defined(admittance_control_op_point_EXPORTS)
+#define ADMITTANCECONTROLOPPOINT_EXPORT __declspec(dllexport)
+#else
+#define ADMITTANCECONTROLOPPOINT_EXPORT __declspec(dllimport)
+#endif
+#else
+#define ADMITTANCECONTROLOPPOINT_EXPORT
+#endif
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#include <dynamic-graph/signal-helper.h>
+
+#include <sot/core/matrix-geometry.hh>
+
+#include "pinocchio/spatial/force.hpp"
+#include "pinocchio/spatial/motion.hpp"
+#include "pinocchio/spatial/se3.hpp"
+
+namespace dynamicgraph {
+namespace sot {
+namespace core {
+
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+/**
+ * @brief  Admittance controller for an operational point wrt to a force sensor.
+ *         It can be a point of the model (hand) or not (created operational
+ * point: an object in the hand of the robot) Which is closed to a force sensor
+ * (for instance the right or left wrist ft)
+ *
+ *  This entity computes a velocity reference for an operational point based
+ *  on the force error in the world frame :
+ *  w_dq = integral(Kp(w_forceDes-w_force)) + Kd (w_dq)
+ *
+ */
+class ADMITTANCECONTROLOPPOINT_EXPORT AdmittanceControlOpPoint
+    : public ::dynamicgraph::Entity {
+  DYNAMIC_GRAPH_ENTITY_DECL();
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  /* --- CONSTRUCTOR ---- */
+  AdmittanceControlOpPoint(const std::string &name);
+  /**
+   * @brief      Initialize the entity
+   *
+   * @param[in]  dt  Time step of the control
+   */
+  void init(const double &dt);
+
+  /* --- SIGNALS --- */
+  /// \brief  Gain (6d) for the integration of the error on the force
+  DECLARE_SIGNAL_IN(Kp, dynamicgraph::Vector);
+  /// \brief  Derivative gain (6d) for the error on the force
+  DECLARE_SIGNAL_IN(Kd, dynamicgraph::Vector);
+  /// \brief  Value of the saturation to apply on the velocity output
+  DECLARE_SIGNAL_IN(dqSaturation, dynamicgraph::Vector);
+  /// \brief  6d force given by the sensor in its local frame
+  DECLARE_SIGNAL_IN(force, dynamicgraph::Vector);
+  /// \brief  6d desired force of the end-effector in the world frame
+  DECLARE_SIGNAL_IN(w_forceDes, dynamicgraph::Vector);
+  /// \brief  Current position (matrixHomogeneous) of the given operational
+  /// point
+  DECLARE_SIGNAL_IN(opPose, dynamicgraph::sot::MatrixHomogeneous);
+  /// \brief  Current position (matrixHomogeneous) of the given force sensor
+  DECLARE_SIGNAL_IN(sensorPose, dynamicgraph::sot::MatrixHomogeneous);
+
+  /// \brief  6d force given by the sensor in the world frame
+  DECLARE_SIGNAL_INNER(w_force, dynamicgraph::Vector);
+  /// \brief  Internal intergration computed in the world frame
+  DECLARE_SIGNAL_INNER(w_dq, dynamicgraph::Vector);
+
+  /// \brief  Velocity reference for the end-effector in the local frame
+  DECLARE_SIGNAL_OUT(dq, dynamicgraph::Vector);
+
+  /* --- COMMANDS --- */
+  /**
+   * @brief      Reset the velocity
+   */
+  void resetDq();
+
+  /* --- ENTITY INHERITANCE --- */
+  virtual void display(std::ostream &os) const;
+
+ protected:
+  /// Dimension of the force signals and of the output
+  int m_n;
+  /// True if the entity has been successfully initialized
+  bool m_initSucceeded;
+  /// Internal state
+  dynamicgraph::Vector m_w_dq;
+  /// Time step of the control
+  double m_dt;
+  // Weight of the end-effector
+  double m_mass;
+
+};  // class AdmittanceControlOpPoint
+
+}  // namespace core
+}  // namespace sot
+}  // namespace dynamicgraph
+
+#endif  // #ifndef __sot_core_admittance_control_op_point_H__

include/sot/core/api.hh

+/*
+ * Copyright 2010,
+ * François Bleibel,
+ * Olivier Stasse,
+ *
+ * CNRS/AIST
+ *
+ */
+
+#ifndef SOT_CORE_API_HH
+#define SOT_CORE_API_HH
+
+#if defined(WIN32)
+#ifdef sot_core_EXPORTS
+#define SOT_CORE_EXPORT __declspec(dllexport)
+#else
+#define SOT_CORE_EXPORT __declspec(dllimport)
+#endif
+#else
+#define SOT_CORE_EXPORT
+#endif
+
+#endif

include/sot/core/binary-int-to-uint.hh

+/*
+ * Copyright 2010,
+ * François Bleibel,
+ * Olivier Stasse,
+ *
+ * CNRS/AIST
+ *
+ */
+
+#ifndef __SOT_BINARY_INT_TO_UINT_HH__
+#define __SOT_BINARY_INT_TO_UINT_HH__
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+/* SOT */
+#include <dynamic-graph/all-signals.h>
+#include <dynamic-graph/entity.h>
+
+#include <sot/core/exception-task.hh>
+
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined(WIN32)
+#if defined(binary_int_to_uint_EXPORTS)
+#define SOTBINARYINTTOUINT_EXPORT __declspec(dllexport)
+#else
+#define SOTBINARYINTTOUINT_EXPORT __declspec(dllimport)
+#endif
+#else
+#define SOTBINARYINTTOUINT_EXPORT
+#endif
+
+namespace dynamicgraph {
+namespace sot {
+
+class SOTBINARYINTTOUINT_EXPORT BinaryIntToUint : public dynamicgraph::Entity {
+ public:
+  static const std::string CLASS_NAME;
+  virtual const std::string &getClassName(void) const { return CLASS_NAME; }
+
+  /* --- SIGNALS ------------------------------------------------------------ */
+ public:
+  dynamicgraph::SignalPtr<int, int> binaryIntSIN;
+  dynamicgraph::SignalTimeDependent<unsigned, int> binaryUintSOUT;
+
+ public:
+  BinaryIntToUint(const std::string &name);
+  virtual ~BinaryIntToUint() {}
+
+  virtual unsigned &computeOutput(unsigned &res, int time);
+
+  virtual void display(std::ostream &os) const;
+};
+
+} /* namespace sot */
+} /* namespace dynamicgraph */
+
+#endif

include/sot/core/binary-op.hh

+/*
+ * Copyright 2010,
+ * François Bleibel,
+ * Olivier Stasse,
+ *
+ * CNRS/AIST
+ *
+ */
+
+#ifndef SOT_CORE_BINARYOP_HH
+#define SOT_CORE_BINARYOP_HH
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+/* Matrix */
+#include <dynamic-graph/linear-algebra.h>
+
+/* SOT */
+#include <dynamic-graph/all-signals.h>
+#include <dynamic-graph/entity.h>
+
+#include <sot/core/flags.hh>
+#include <sot/core/matrix-geometry.hh>
+#include <sot/core/pool.hh>
+
+/* STD */
+#include <boost/function.hpp>
+#include <string>
+
+namespace dynamicgraph {
+namespace sot {
+
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+template <typename Operator>
+class BinaryOp : public Entity {
+  Operator op;
+  typedef typename Operator::Tin1 Tin1;
+  typedef typename Operator::Tin2 Tin2;
+  typedef typename Operator::Tout Tout;
+  typedef BinaryOp<Operator> Self;
+
+ public: /* --- CONSTRUCTION --- */
+  static std::string getTypeIn1Name(void) { return Operator::nameTypeIn1(); }
+  static std::string getTypeIn2Name(void) { return Operator::nameTypeIn2(); }
+  static std::string getTypeOutName(void) { return Operator::nameTypeOut(); }
+  static const std::string CLASS_NAME;
+  virtual const std::string &getClassName() const { return CLASS_NAME; }
+  std::string getDocString() const { return op.getDocString(); }
+
+  BinaryOp(const std::string &name)
+      : Entity(name),
+        SIN1(NULL, BinaryOp::CLASS_NAME + "(" + name + ")::input(" +
+                       getTypeIn1Name() + ")::sin1"),
+        SIN2(NULL, CLASS_NAME + "(" + name + ")::input(" + getTypeIn2Name() +
+                       ")::sin2"),
+        SOUT(boost::bind(&Self::computeOperation, this, _1, _2), SIN1 << SIN2,
+             CLASS_NAME + "(" + name + ")::output(" + getTypeOutName() +
+                 ")::sout") {
+    signalRegistration(SIN1 << SIN2 << SOUT);
+    op.addSpecificCommands(*this, commandMap);
+  }
+
+  virtual ~BinaryOp(void){};
+
+ public: /* --- SIGNAL --- */
+  SignalPtr<Tin1, int> SIN1;
+  SignalPtr<Tin2, int> SIN2;
+  SignalTimeDependent<Tout, int> SOUT;
+
+ protected:
+  Tout &computeOperation(Tout &res, int time) {
+    const Tin1 &x1 = SIN1(time);
+    const Tin2 &x2 = SIN2(time);
+    op(x1, x2, res);
+    return res;
+  }
+};
+}  // namespace sot
+}  // namespace dynamicgraph
+
+#endif  // #ifndef SOT_CORE_BINARYOP_HH

include/sot/core/causal-filter.hh

+#ifndef _SOT_CORE_CAUSAL_FILTER_H_
+#define _SOT_CORE_CAUSAL_FILTER_H_
+/*
+ * Copyright 2017-, Rohan Budhirja, LAAS-CNRS
+ *
+ * This file is part of sot-torque-control.
+ * sot-torque-control 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-torque-control 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-torque-control.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+#include <Eigen/Core>
+
+/** \addtogroup Filters
+    \section subsec_causalfilter CausalFilter
+    Filter data with an IIR or FIR filter.
+
+    Filter a data sequence, \f$x\f$, using a digital filter.
+    The filter is a direct form II transposed implementation
+    of the standard difference equation.
+    This means that the filter implements:
+
+    \f$ a[0]*y[N] = b[0]*x[N] + b[1]*x[N-1] + ... + b[m-1]*x[N-(m-1)]
+    - a[1]*y[N-1] - ... - a[n-1]*y[N-(n-1)] \f$
+
+    where \f$m\f$ is the degree of the numerator,
+    \f$n\f$ is the degree of the denominator,
+    and \f$N\f$ is the sample number
+
+
+ */
+namespace dynamicgraph {
+namespace sot {
+
+class CausalFilter {
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  /** --- CONSTRUCTOR ----
+      \param[in] timestep
+      \param[in] xSize
+      \param[in] filter_numerator
+      \param[in] filter_denominator
+
+      xSize is
+  */
+  CausalFilter(const double &timestep, const int &xSize,
+               const Eigen::VectorXd &filter_numerator,
+               const Eigen::VectorXd &filter_denominator);
+
+  void get_x_dx_ddx(const Eigen::VectorXd &base_x,
+                    Eigen::VectorXd &x_output_dx_ddx);
+
+  void switch_filter(const Eigen::VectorXd &filter_numerator,
+                     const Eigen::VectorXd &filter_denominator);
+
+ private:
+  /// sampling timestep of the input signal
+  double m_dt;
+  /// Size
+  int m_x_size;
+  /// Size of the numerator \f$m\f$
+  Eigen::VectorXd::Index m_filter_order_m;
+  /// Size of the denominator \f$n\f$
+  Eigen::VectorXd::Index m_filter_order_n;
+
+  /// Coefficients of the numerator \f$b\f$
+  Eigen::VectorXd m_filter_numerator;
+  /// Coefficients of the denominator \f$a\f$
+  Eigen::VectorXd m_filter_denominator;
+  bool m_first_sample;
+  ///
+  int m_pt_numerator;
+  int m_pt_denominator;
+  Eigen::MatrixXd m_input_buffer;
+  Eigen::MatrixXd m_output_buffer;
+};  // class CausalFilter
+}  // namespace sot
+}  // namespace dynamicgraph
+#endif /* _SOT_CORE_CAUSAL_FILTER_H_ */

include/sot/core/clamp-workspace.hh

+/*
+ * Copyright 2010,
+ * François Bleibel,
+ * Olivier Stasse,
+ *
+ * CNRS/AIST
+ *
+ */
+
+#ifndef __SOT_CLAMP_WORKSPACE_HH__
+#define __SOT_CLAMP_WORKSPACE_HH__
+
+/* STL */
+#include <utility>
+
+/* Matrix */
+#include <dynamic-graph/linear-algebra.h>
+
+/* SOT */
+#include <dynamic-graph/all-signals.h>
+#include <dynamic-graph/entity.h>
+
+#include <sot/core/exception-task.hh>
+#include <sot/core/matrix-geometry.hh>
+
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined(WIN32)
+#if defined(clamp_workspace_EXPORTS)
+#define SOTCLAMPWORKSPACE_EXPORT __declspec(dllexport)
+#else
+#define SOTCLAMPWORKSPACE_EXPORT __declspec(dllimport)
+#endif
+#else
+#define SOTCLAMPWORKSPACE_EXPORT
+#endif
+
+namespace dynamicgraph {
+namespace sot {
+
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+class SOTCLAMPWORKSPACE_EXPORT ClampWorkspace : public dynamicgraph::Entity {
+ public:
+  static const std::string CLASS_NAME;
+  virtual const std::string &getClassName(void) const { return CLASS_NAME; }
+
+  /* --- SIGNALS ------------------------------------------------------------ */
+ public:
+  dynamicgraph::SignalPtr<MatrixHomogeneous, int> positionrefSIN;
+  dynamicgraph::SignalPtr<MatrixHomogeneous, int> positionSIN;
+  dynamicgraph::SignalTimeDependent<dynamicgraph::Matrix, int> alphaSOUT;
+  dynamicgraph::SignalTimeDependent<dynamicgraph::Matrix, int> alphabarSOUT;
+  dynamicgraph::SignalTimeDependent<MatrixHomogeneous, int> handrefSOUT;
+
+ public:
+  ClampWorkspace(const std::string &name);
+  virtual ~ClampWorkspace(void) {}
+
+  void update(int time);
+
+  virtual dynamicgraph::Matrix &computeOutput(dynamicgraph::Matrix &res,
+                                              int time);
+  virtual dynamicgraph::Matrix &computeOutputBar(dynamicgraph::Matrix &res,
+                                                 int time);
+  virtual MatrixHomogeneous &computeRef(MatrixHomogeneous &res, int time);
+
+  virtual void display(std::ostream &) const;
+
+ private:
+  int timeUpdate;
+
+  dynamicgraph::Matrix alpha;
+  dynamicgraph::Matrix alphabar;
+  MatrixHomogeneous prefMp;
+  dynamicgraph::Vector pd;
+  MatrixRotation Rd;
+  MatrixHomogeneous handref;
+
+  double beta;
+  double scale;
+  double dm_min;
+  double dm_max;
+  double dm_min_yaw;
+  double dm_max_yaw;
+  double theta_min;
+  double theta_max;
+  int mode;
+
+  enum { FRAME_POINT, FRAME_REF } frame;
+
+  std::pair<double, double> bounds[3];
+};
+
+} /* namespace sot */
+} /* namespace dynamicgraph */
+
+#endif