Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • ostasse/sot-core
  • gsaurel/sot-core
  • stack-of-tasks/sot-core
3 results
Show changes
Showing
with 0 additions and 3256 deletions
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Copyright Projet JRL-Japan, 2007
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*
* File: binary-op.h
* Project: SOT
* Author: Nicolas Mansard
*
* Version control
* ===============
*
* $Id$
*
* Description
* ============
*
*
* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
#ifndef __SOT_BINARYOP_H__
#define __SOT_BINARYOP_H__
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* Matrix */
#include <MatrixAbstractLayer/boost.h>
namespace ml = maal::boost;
/* SOT */
#include <sot-core/flags.h>
#include <dynamic-graph/entity.h>
#include <sot-core/pool.h>
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/all-signals.h>
#include <sot-core/vector-quaternion.h>
/* STD */
#include <string>
#include <boost/function.hpp>
namespace sot {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
template< class Tin1,class Tin2,class Tout,typename Operator >
class BinaryOp
:public Entity
{
Operator op;
public: /* --- CONSTRUCTION --- */
static std::string getTypeIn1Name( void ) { return "UnknownIn1"; }
static std::string getTypeIn2Name( void ) { return "UnknownIn2"; }
static std::string getTypeOutName( void ) { return "UnknownOut"; }
static const std::string CLASS_NAME;
BinaryOp( const std::string& name )
: Entity(name)
,SIN1(NULL,BinaryOp::CLASS_NAME+"("+name+")::input("+getTypeIn1Name()+")::in1")
,SIN2(NULL,CLASS_NAME+"("+name+")::input("+getTypeIn2Name()+")::in2")
,SOUT( boost::bind(&BinaryOp<Tin1,Tin2,Tout,Operator>::computeOperation,this,_1,_2),
SIN1<<SIN2,CLASS_NAME+"("+name+")::output("+getTypeOutName()+")::out")
{
signalRegistration( SIN1<<SIN2<<SOUT );
}
virtual ~BinaryOp( void ) {};
public: /* --- SIGNAL --- */
SignalPtr<Tin1,int> SIN1;
SignalPtr<Tin2,int> SIN2;
SignalTimeDependant<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;
}
public: /* --- PARAMS --- */
virtual void commandLine( const std::string& cmdLine,std::istringstream& cmdArgs,
std::ostream& os );
};
} // namespace sot
#endif // #ifndef __SOT_BINARYOP_H__
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Copyright Projet Lagadic, 2005
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*
* File: debug.h
* Project: STack of Tasks
* Author: Nicolas Mansard
*
* Version control
* ===============
*
* $Id$
*
* Description
* ============
*
* Macro de trace et de debugage
*
* - TRACAGE: TRACE et ERROR_TRACE fonctionnent comme des printf
* avec retour chariot en fin de fonction.
* CERROR et CTRACE fonctionnent comme les flux de sortie
* C++ cout et cerr.
* - DEBUGAGE: DEBUG_TRACE(niv, et DERROR_TRACE(niv, fonctionnent
* comme des printf, n'imprimant que si le niveau de trace 'niv' est
* superieur au mode de debugage VP_DEBUG_MODE.
* CDEBUG(niv) fonctionne comme le flux de sortie C++ cout.
* DEBUG_ENABLE(niv) vaut 1 ssi le niveau de tracage 'niv'
* est superieur au mode de debugage DEBUG_MODE. Il vaut 0 sinon.
* - PROG DEFENSIVE: DEFENSIF(a) vaut a ssi le mode defensif est active,
* et vaut 0 sinon.
*
* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
#ifndef __VS_DEBUG_HH
#define __VS_DEBUG_HH
#include <stdio.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <stdarg.h>
#include <sot-core/sot-core-api.h>
namespace sot {
/* -------------------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
#ifndef VP_DEBUG_MODE
#define VP_DEBUG_MODE 0
#endif
#ifndef VP_TEMPLATE_DEBUG_MODE
#define VP_TEMPLATE_DEBUG_MODE 0
#endif
#define SOT_COMMON_TRACES do { \
va_list arg; \
va_start(arg,format); \
vsnprintf( charbuffer,SIZE,format,arg ); \
va_end(arg); \
outputbuffer << tmpbuffer.str() << charbuffer <<std::endl; \
} while(0)
class SOT_CORE_EXPORT DebugTrace
{
public:
static const int SIZE = 512;
std::stringstream tmpbuffer;
std::ostream& outputbuffer;
char charbuffer[SIZE+1];
int traceLevel;
int traceLevelTemplate;
DebugTrace( std::ostream& os ): outputbuffer(os) {}
inline void trace( const int level,const char* format,...)
{ if( level<=traceLevel ) SOT_COMMON_TRACES; tmpbuffer.str(""); }
inline void trace( const char* format,...){ SOT_COMMON_TRACES; tmpbuffer.str(""); }
inline void trace( const int level=-1 )
{ if( level<=traceLevel ) outputbuffer << tmpbuffer.str(); tmpbuffer.str(""); }
inline void traceTemplate( const int level,const char* format,...)
{ if( level<=traceLevelTemplate ) SOT_COMMON_TRACES; tmpbuffer.str(""); }
inline void traceTemplate( const char* format,...)
{ SOT_COMMON_TRACES; tmpbuffer.str(""); }
inline DebugTrace& pre( const std::ostream& dummy ) { return *this; }
inline DebugTrace& pre( const std::ostream& dummy,int level )
{ traceLevel = level; return *this; }
/* inline DebugTrace& preTemplate( const std::ostream& dummy,int level ) */
/* { traceLevelTemplate = level; return *this; } */
static const char * DEBUG_FILENAME_DEFAULT;
static void openFile( const char * filename = DEBUG_FILENAME_DEFAULT );
static void closeFile( const char * filename = DEBUG_FILENAME_DEFAULT );
};
SOT_CORE_EXPORT extern DebugTrace sotDEBUGFLOW;
SOT_CORE_EXPORT extern DebugTrace sotERRORFLOW;
#ifdef VP_DEBUG
#define sotPREDEBUG __FILE__ << ": " <<__FUNCTION__ \
<< "(#" << __LINE__ << ") :"
#define sotPREERROR "\t!! "<<__FILE__ << ": " <<__FUNCTION__ \
<< "(#" << __LINE__ << ") :"
# define sotDEBUG(level) if( (level>VP_DEBUG_MODE)||(!sotDEBUGFLOW.outputbuffer.good()) ) ;\
else sotDEBUGFLOW.outputbuffer << sotPREDEBUG
# define sotDEBUGMUTE(level) if( (level>VP_DEBUG_MODE)||(!sotDEBUGFLOW.outputbuffer.good()) ) ;\
else sotDEBUGFLOW.outputbuffer
# define sotERROR if(!sotDEBUGFLOW.outputbuffer.good()); else sotERRORFLOW.outputbuffer << sotPREERROR
# define sotDEBUGF if(!sotDEBUGFLOW.outputbuffer.good()); else sotDEBUGFLOW.pre(sotDEBUGFLOW.tmpbuffer<<sotPREDEBUG,VP_DEBUG_MODE).trace
# define sotERRORF if(!sotDEBUGFLOW.outputbuffer.good()); else sotERRORFLOW.pre(sotERRORFLOW.tmpbuffer<<sotPREERROR).trace
// TEMPLATE
# define sotTDEBUG(level) if( (level>VP_TEMPLATE_DEBUG_MODE)||(!sotDEBUGFLOW.outputbuffer.good()) ) ;\
else sotDEBUGFLOW.outputbuffer << sotPREDEBUG
# define sotTDEBUGF if(!sotDEBUGFLOW.outputbuffer.good()); else sotDEBUGFLOW.pre(sotDEBUGFLOW.tmpbuffer<<sotPREDEBUG,VP_TEMPLATE_DEBUG_MODE).trace
inline bool sotDEBUG_ENABLE( const int & level ) { return level<=VP_DEBUG_MODE; }
inline bool sotTDEBUG_ENABLE( const int & level ) { return level<=VP_TEMPLATE_DEBUG_MODE; }
/* -------------------------------------------------------------------------- */
#else // #ifdef VP_DEBUG
#define sotPREERROR "\t!! "<<__FILE__ << ": " <<__FUNCTION__ \
<< "(#" << __LINE__ << ") :"
# define sotDEBUG(level) if( 1 ) ; else std::cout
# define sotDEBUGMUTE(level) if( 1 ) ; else std::cout
# define sotERROR sotERRORFLOW.outputbuffer << sotPREERROR
inline void sotDEBUGF( const int level,const char* format,...) { return; }
inline void sotDEBUGF( const char* format,...) { return; }
inline void sotERRORF( const int level,const char* format,...) { return; }
inline void sotERRORF( const char* format,...) { return; }
// TEMPLATE
# define sotTDEBUG(level) if( 1 ) ; else std::cout
inline void sotTDEBUGF( const int level,const char* format,...) { return; }
inline void sotTDEBUGF( const char* format,...) { return; }
#define sotDEBUG_ENABLE(level) false
#define sotTDEBUG_ENABLE(level) false
#endif // #ifdef VP_DEBUG
/* -------------------------------------------------------------------------- */
#define sotDEBUGIN(level) sotDEBUG(level) << "# In {" << std::endl
#define sotDEBUGOUT(level) sotDEBUG(level) << "# Out }" << std::endl
#define sotDEBUGINOUT(level) sotDEBUG(level) << "# In/Out { }" << std::endl
#define sotTDEBUGIN(level) sotTDEBUG(level) << "# In {" << std::endl
#define sotTDEBUGOUT(level) sotTDEBUG(level) << "# Out }" << std::endl
#define sotTDEBUGINOUT(level) sotTDEBUG(level) << "# In/Out { }" << std::endl
} // namespace sot
#endif /* #ifdef __VS_DEBUG_HH */
/*
* Local variables:
* c-basic-offset: 4
* End:
*/
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Copyright Projet JRL-Japan, 2007
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*
* File: derivator.h
* Project: SOT
* Author: Nicolas Mansard
*
* Version control
* ===============
*
* $Id$
*
* Description
* ============
*
*
* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
#ifndef __SOT_DERIVATOR_H__
#define __SOT_DERIVATOR_H__
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* Matrix */
#include <MatrixAbstractLayer/boost.h>
namespace ml = maal::boost;
/* SOT */
#include <sot-core/flags.h>
#include <sot-core/pool.h>
#include <dynamic-graph/entity.h>
#include <dynamic-graph/all-signals.h>
#include <sot-core/vector-quaternion.h>
/* STD */
#include <string>
namespace sot {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
template< class T >
class sotDerivator
:public Entity
{
protected:
T memory;
bool initialized;
double timestep;
static const double TIMESTEP_DEFAULT ; //= 1.;
public: /* --- CONSTRUCTION --- */
static std::string getTypeName( void ) { return "Unknown"; }
static const std::string CLASS_NAME;
sotDerivator( const std::string& name )
: Entity(name)
,memory(),initialized(false)
,timestep(TIMESTEP_DEFAULT)
,SIN(NULL,"sotDerivator<"+getTypeName()+">("+name+")::input("+getTypeName()+")::in")
,SOUT( boost::bind(&sotDerivator<T>::computeDerivation,this,_1,_2),
SIN,"sotDerivator<"+getTypeName()+">("+name+")::output("+getTypeName()+")::out")
,timestepSIN("sotDerivator<"+getTypeName()+">("+name+")::input(double)::dt")
{
signalRegistration( SIN<<SOUT<<timestepSIN );
timestepSIN.setReferenceNonConstant( &timestep );
timestepSIN.setKeepReference(true);
}
virtual ~sotDerivator( void ) {};
public: /* --- SIGNAL --- */
SignalPtr<T,int> SIN;
SignalTimeDependant<T,int> SOUT;
Signal<double,int> timestepSIN;
protected:
T& computeDerivation( T& res,int time )
{
if(initialized)
{
res = memory; res *= -1;
memory = SIN(time);
res += memory;
if( timestep!=1. ) res*=timestep;
} else {
initialized = true;
memory = SIN(time);
res = memory;
res *= 0;
}
return res;
}
public: /* --- PARAMS --- */
/* virtual void commandLine( const std::string& cmdLine,std::istringstream& cmdArgs, */
/* std::ostream& os ) {} */
};
} // using namespace sot
#endif // #ifndef __SOT_DERIVATOR_H__
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Copyright Projet JRL-Japan, 2007
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*
* File: exception-traces.h
* Project: SOT
* Author: Nicolas Mansard
*
* Version control
* ===============
*
* $Id$
*
* Description
* ============
*
*
* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
#ifndef __SOT_TRACES_EXCEPTION_H
#define __SOT_TRACES_EXCEPTION_H
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <sot-core/exception-abstract.h>
#include <sot-core/sot-core-api.h>
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
namespace sot {
/* \class sotExceptionTraces
*/
class SOT_CORE_EXPORT sotExceptionTraces
:public ExceptionAbstract
{
public:
enum ErrorCodeEnum
{
GENERIC = ExceptionAbstract::TRACES
,NOT_OPEN
};
static const std::string EXCEPTION_NAME;
virtual const std::string& getExceptionName( void ) const { return EXCEPTION_NAME; }
public:
sotExceptionTraces ( const sotExceptionTraces::ErrorCodeEnum& errcode,
const std::string & msg = "" );
sotExceptionTraces( const sotExceptionTraces::ErrorCodeEnum& errcode,
const std::string & msg,const char* format, ... );
virtual ~sotExceptionTraces( void ){}
};
} // namespace sot
#endif /* #ifndef __SOT_TRACES_EXCEPTION_H */
/*
* Local variables:
* c-basic-offset: 2
* End:
*/
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Copyright Projet JRL-Japan, 2007
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*
* File: factory.h
* Project: SOT
* Author: Nicolas Mansard
*
* Version control
* ===============
*
* $Id$
*
* Description
* ============
*
*
* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
#ifndef __SOT_FACTORY_HH__
#define __SOT_FACTORY_HH__
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --- STD --- */
#include <map>
#include <string>
/* --- SOT --- */
#include <sot-core/exception-factory.h>
#include <sot-core/sot-core-api.h>
#include <dynamic-graph/factory.h>
namespace sot {
class sotFeatureAbstract;
class sotTaskAbstract;
/* --------------------------------------------------------------------- */
/* --- FACTORY ---------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/*! @ingroup factory
\brief This class implements the factory that allows creation of
objects loaded from dynamic link libraries. Objects are referenced by their
class names, and can be created by passing this string to one of the three
public "new" methods (newEntity, newFeature or newTask).
The factory instance (singleton) is publicly available under the name sotFactory
(include factory.h). A task, feature or entity can register itself by
using the SOT_FACTORY_{ENTITY,TASK,FEATURE}_PLUGIN macro. See sotTask.cpp for
an example.
*/
class SOT_CORE_EXPORT sotFactoryStorage
{
public:
typedef sotFeatureAbstract* (*FeatureConstructor_ptr)( const std::string& );
typedef sotTaskAbstract* (*TaskConstructor_ptr)( const std::string& );
protected:
typedef std::map< std::string,TaskConstructor_ptr > TaskMap;
typedef std::map< std::string,FeatureConstructor_ptr > FeatureMap;
FeatureMap featureMap;
TaskMap taskMap;
public:
~sotFactoryStorage( void );
void registerTask( const std::string& entname,TaskConstructor_ptr ent );
sotTaskAbstract* newTask( const std::string& name,const std::string& objname );
bool existTask( const std::string& name, TaskMap::iterator& entPtr );
bool existTask( const std::string& name );
void registerFeature( const std::string& entname,FeatureConstructor_ptr ent );
sotFeatureAbstract* newFeature( const std::string& name,const std::string& objname );
bool existFeature( const std::string& name, FeatureMap::iterator& entPtr );
bool existFeature( const std::string& name );
void commandLine( const std::string& cmdLine,std::istringstream& cmdArgs,
std::ostream& os );
};
SOT_CORE_EXPORT extern sotFactoryStorage sotFactory;
/* --- REGISTERER ----------------------------------------------------------- */
/* --- REGISTERER ----------------------------------------------------------- */
/* --- REGISTERER ----------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
/* --- ENTITY REGISTERER ---------------------------------------------------- */
/* -------------------------------------------------------------------------- */
typedef dynamicgraph::EntityRegisterer sotEntityRegisterer;
#define SOT_FACTORY_ENTITY_PLUGIN(sotClassType,className) \
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(sotClassType, className)
/* -------------------------------------------------------------------------- */
/* --- FEATURE REGISTERER --------------------------------------------------- */
/* -------------------------------------------------------------------------- */
class SOT_CORE_EXPORT sotFeatureRegisterer
{
private:
sotFeatureRegisterer( void );
public:
sotFeatureRegisterer( const std::string& featureClassName,
sotFactoryStorage::FeatureConstructor_ptr maker);
};
#define SOT_FACTORY_FEATURE_PLUGIN(sotFeatureType,className) \
const std::string sotFeatureType::CLASS_NAME = className; \
extern "C" { \
sotFeatureAbstract *sotFeatureMaker##_##sotFeatureType( const std::string& objname )\
{ \
return new sotFeatureType( objname ); \
} \
sotFeatureRegisterer reg##_##sotFeatureType( className, \
&sotFeatureMaker##_##sotFeatureType ); \
} \
/* -------------------------------------------------------------------------- */
/* --- TASK REGISTERER ------------------------------------------------------ */
/* -------------------------------------------------------------------------- */
class SOT_CORE_EXPORT sotTaskRegisterer
{
private:
sotTaskRegisterer( void );
public:
sotTaskRegisterer( const std::string& taskClassName,
sotFactoryStorage::TaskConstructor_ptr maker);
};
#define SOT_FACTORY_TASK_PLUGIN(sotTaskType,className) \
const std::string sotTaskType::CLASS_NAME = className; \
extern "C" { \
sotTaskAbstract *sotTaskMaker##_##sotTaskType( const std::string& objname ) \
{ \
return new sotTaskType( objname ); \
} \
sotTaskRegisterer reg##_##sotTaskType( className,&sotTaskMaker##_##sotTaskType ); \
} \
} // namespace sot
#endif /* #ifndef __SOT_FACTORY_HH__ */
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Copyright Projet JRL-Japan, 2007
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*
* File: feature-abstract.h
* Project: SOT
* Author: Nicolas Mansard
*
* Version control
* ===============
*
* $Id$
*
* Description
* ============
*
*
* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
#ifndef __SOT_FEATURE_ABSTRACT_H__
#define __SOT_FEATURE_ABSTRACT_H__
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* Matrix */
#include <MatrixAbstractLayer/boost.h>
namespace ml = maal::boost;
/* SOT */
#include <sot-core/flags.h>
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
#include <sot-core/sot-core-api.h>
/* STD */
#include <string>
using namespace dynamicgraph;
namespace sot {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/*! \class sotFeatureAbstract
\ingroup features
\brief This class gives the abstract definition of a feature.
A feature is a data evolving according to time.
It is defined by a vector \f${\bf s}(t) \in \mathbb{R}^n \f$ where \f$ t \f$ is the time.
By default a feature has a desired \f${\bf s}^*(t) \f$.
The feature is in charge of collecting its own current state.
A feature is supposed to compute an error between its current state and the desired one:
\f$ e(t) = {\bf s}^*(t) - {\bf s}(t) \f$.
A feature is supposed to compute a Jacobian according to the robot state vector
\f$ \frac{\delta {\bf s}(t)}{\delta {\bf q}(t)}\f$.
*/
class SOT_CORE_EXPORT sotFeatureAbstract
:public Entity
{
public:
/*! \brief Store the name of the class. */
static const std::string CLASS_NAME;
/*! \brief Returns the name class. */
virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
/*! \brief Register the feature in the stack of tasks. */
void featureRegistration( void );
public:
/*! \brief Default constructor: the name of the class should be given. */
sotFeatureAbstract( const std::string& name );
/*! \brief Default destructor */
virtual ~sotFeatureAbstract( void ) {};
/*! \name Methods returning the dimension of the feature.
@{ */
/*! \brief Verbose method.
\par res: The integer in which the dimension will be return.
\par time: The time at which the feature should be considered.
\return Dimension of the feature.
\note Be careful with features changing their dimension according to time.
*/
virtual unsigned int& getDimension( unsigned int& res,int time ) = 0;
/*! \brief Short method
\par time: The time at which the feature should be considered.
\return Dimension of the feature.
\note Be careful with features changing their dimension according to time.
*/
inline unsigned int getDimension( int time )
{unsigned int res; getDimension(res,time); return res;}
/*! \brief Shortest method
\return Dimension of the feature.
\note The feature is not changing its dimension according to time.
*/
inline unsigned int getDimension( void ) const
{ return dimensionSOUT; }
/*! @} */
/*! \name Methods to control internal computation.
The main idea is that some feature may have a lower frequency
than the internal control loop. In this case, the methods for
computation are called only when needed.
@{*/
/*! \brief Compute the error between the desired feature and
the current value of the feature measured or deduced from the robot state.
\par[out] res: The error will be set into res.
\par[in] time: The time at which the error is computed.
\return The vector res with the appropriate value.
*/
virtual ml::Vector& computeError( ml::Vector& res,int time ) = 0;
/*! \brief Compute the Jacobian of the error according the robot state.
\par[out] res: The matrix in which the error will be written.
\par[in] time: The time at which the Jacobian is computed \f$ {\bf J}(q(t)) \f$.
\return The matrix res with the appropriate values.
*/
virtual ml::Matrix& computeJacobian( ml::Matrix& res,int time ) = 0;
/*! \brief Reevaluate the current value of the feature
according to external measurement provided through a mailbox,
or deduced from the estimated state of the robot at the time specified.
\par[out] res: The vector in which the value will be written.
\par[in] time: The time at which the feature is evaluated \f$ {\bf s}(t)) \f$.
\return The vector res with the appropriate values.
*/
virtual ml::Vector& computeActivation( ml::Vector& res,int time ) = 0;
/*! @} */
/* --- SIGNALS ------------------------------------------------------------ */
public:
/*! \name Signals
@{
*/
/*! \name Input signals:
@{ */
/*! \brief This signal specifies the desired value \f$ {\bf s}^*(t) \f$ */
SignalPtr< sotFeatureAbstract*,int > desiredValueSIN;
/*! \brief This vector specifies which dimension are used to perform the computation.
For instance let us assume that the feature is a 3D point. If only the Y-axis should
be used for computing error, activation and Jacobian, then the vector to specify
is \f$ [ 0 1 0] \f$.*/
SignalPtr< sotFlags,int > selectionSIN;
/*! @} */
/*! \name Output signals:
@{ */
/*! \brief This signal returns the error between the desired value and
the current value : \f$ {\bf s}^*(t) - {\bf s}(t)\f$ */
SignalTimeDependant<ml::Vector,int> errorSOUT;
/*! \brief This signal returns the Jacobian of the current value
according to the robot state: \f$ J(t) = \frac{\delta{\bf s}^*(t)}{\delta {\bf q}(t)}\f$ */
SignalTimeDependant<ml::Matrix,int> jacobianSOUT;
/*! \brief Compute the new value of the feature \f$ {\bf s}(t)\f$ */
SignalTimeDependant<ml::Vector,int> activationSOUT;
/*! \brief Returns the dimension of the feature as an output signal. */
SignalTimeDependant<unsigned int,int> dimensionSOUT;
/*! \brief This method write a graph description on the file named FileName. */
virtual std::ostream & writeGraph(std::ostream & os) const;
/*! @} */
};
} // namespace sot
#endif // #ifndef __SOT_FEATURE_ABSTRACT_H__
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Copyright Projet JRL-Japan, 2007
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*
* File: feature-point6d.h
* Project: SOT
* Author: Nicolas Mansard
*
* Version control
* ===============
*
* $Id$
*
* Description
* ============
*
*
* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
#ifndef __SOT_FEATURE_POINT6D_HH__
#define __SOT_FEATURE_POINT6D_HH__
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* SOT */
#include <sot-core/feature-abstract.h>
#include <sot-core/exception-task.h>
#include <sot-core/matrix-homogeneous.h>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (sotFeaturePoint6d_EXPORTS)
# define SOTFEATUREPOINT6D_EXPORT __declspec(dllexport)
# else
# define SOTFEATUREPOINT6D_EXPORT __declspec(dllimport)
# endif
#else
# define SOTFEATUREPOINT6D_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
namespace sot {
/*!
\class sotFeaturePoint6d
\brief Class that defines point-3d control feature
*/
class SOTFEATUREPOINT6D_EXPORT sotFeaturePoint6d
: public sotFeatureAbstract
{
public:
static const std::string CLASS_NAME;
virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
protected:
enum ComputationFrameType
{
FRAME_DESIRED
,FRAME_CURRENT
};
static const ComputationFrameType COMPUTATION_FRAME_DEFAULT;
ComputationFrameType computationFrame;
/* --- SIGNALS ------------------------------------------------------------ */
public:
SignalPtr< sotMatrixHomogeneous,int > positionSIN;
SignalPtr< ml::Matrix,int > articularJacobianSIN;
using sotFeatureAbstract::desiredValueSIN;
using sotFeatureAbstract::selectionSIN;
using sotFeatureAbstract::jacobianSOUT;
using sotFeatureAbstract::errorSOUT;
using sotFeatureAbstract::activationSOUT;
public:
sotFeaturePoint6d( const std::string& name );
virtual ~sotFeaturePoint6d( void ) {}
virtual unsigned int& getDimension( unsigned int & dim, int time );
virtual ml::Vector& computeError( ml::Vector& res,int time );
virtual ml::Matrix& computeJacobian( ml::Matrix& res,int time );
virtual ml::Vector& computeActivation( ml::Vector& res,int time );
/** Static Feature selection. */
inline static sotFlags selectX( void ) { return FLAG_LINE_1; }
inline static sotFlags selectY( void ) { return FLAG_LINE_2; }
inline static sotFlags selectZ( void ) { return FLAG_LINE_3; }
inline static sotFlags selectRX( void ) { return FLAG_LINE_4; }
inline static sotFlags selectRY( void ) { return FLAG_LINE_5; }
inline static sotFlags selectRZ( void ) { return FLAG_LINE_6; }
inline static sotFlags selectTranslation( void ) { return sotFlags(7); }
inline static sotFlags selectRotation( void ) { return sotFlags(56); }
virtual void display( std::ostream& os ) const;
virtual void commandLine( const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os );
} ;
} // namespace sot
#endif // #ifndef __SOT_FEATURE_POINT6D_HH__
/*
* Local variables:
* c-basic-offset: 2
* End:
*/
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Copyright Projet JRL-Japan, 2008
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*
* File: fir-filter.h
* Project: SOT
* Author: Paul Evrard
*
* Version control
* ===============
*
* $Id$
*
* Description
* ============
*
*
* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
#ifndef __SOT_FIRFILTER_HH__
#define __SOT_FIRFILTER_HH__
#include <cassert>
#include <vector>
#include <algorithm>
#include <iterator>
//#include <boost/circular_buffer.hpp>
#include <MatrixAbstractLayer/boost.h>
namespace ml = maal::boost;
#include <dynamic-graph/entity.h>
#include <dynamic-graph/all-signals.h>
namespace detail
{
/* GRMBL boost-sandox::circular_buffer smells... Why keep using 1.33?!
* As a workaround, only the part of circular_buffer's interface used here is implemented.
* Ugly, fatty piece of code.
*/
template<class T>
class circular_buffer
{
public:
circular_buffer(): buf(1), start(0), numel(0) {}
void push_front(const T& data) {
if(start){ --start; } else { start = buf.size()-1; }
buf[start] = data;
if(numel < buf.size()){ ++numel; }
}
void reset_capacity(size_t n) {
buf.resize(n);
start = 0;
numel = 0;
}
void reset_capacity(size_t n, const T& el) {
buf.clear();
buf.resize(n, el);
start = 0;
numel = 0;
}
T& operator[](size_t i) {
assert((i<numel) && "Youre accessing an empty buffer");
size_t index = (start+i) % buf.size();
return buf[index];
}
size_t size() const { return numel; }
private:
std::vector<T> buf;
size_t start;
size_t numel;
};
}
template<class sigT, class coefT>
class sotFIRFilter
: public Entity
{
public:
virtual const std::string& getClassName() const { return Entity::getClassName(); }
static std::string getTypeName( void ) { return "Unknown"; }
static const std::string CLASS_NAME;
public:
sotFIRFilter( const std::string& name )
: Entity(name)
, SIN(NULL,"sotFIRFilter("+name+")::input(T)::in")
, SOUT(boost::bind(&sotFIRFilter::compute,this,_1,_2),
SIN,
"sotFIRFilter("+name+")::output(T)::out")
{
signalRegistration( SIN<<SOUT );
}
virtual ~sotFIRFilter() {}
virtual sigT& compute( sigT& res,int time )
{
const sigT& in = SIN.access( time );
reset_signal( res, in );
data.push_front( in );
size_t SIZE = std::min(data.size(), coefs.size());
for(size_t i = 0; i < SIZE; ++i) {
res += coefs[i] * data[i];
}
return res;
}
virtual void commandLine( const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os )
{
if(cmdLine == "setCoefs") {
coefT tmp;
std::vector<coefT> ncoefs;
while(cmdArgs>>tmp){ ncoefs.push_back(tmp); }
if(!ncoefs.empty()){
coefs.swap(ncoefs);
data.reset_capacity(coefs.size());
}
else {
std::copy( coefs.begin(), coefs.end(),
std::ostream_iterator<coefT>(os, " ") );
os << std::endl;
}
}
else if(cmdLine == "printBuffer") {
for(size_t i = 0; i < data.size(); ++i){ os << data[i] << std::endl; }
}
else { Entity::commandLine( cmdLine, cmdArgs, os); }
}
static void reset_signal(sigT& res, const sigT& sample) { }
public:
SignalPtr<sigT, int> SIN;
SignalTimeDependant<sigT, int> SOUT;
private:
std::vector<coefT> coefs;
detail::circular_buffer<sigT> data;
};
#endif
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Copyright Projet JRL-Japan, 2007
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*
* File: flags.h
* Project: SOT
* Author: Nicolas Mansard
*
* Version control
* ===============
*
* $Id$
*
* Description
* ============
*
*
* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
#ifndef __SOT_FLAGS_H
#define __SOT_FLAGS_H
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* STD */
#include <vector>
#include <iostream>
/* SOT */
#include <sot-core/sot-core-api.h>
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
namespace sot {
class SOT_CORE_EXPORT sotFlags
{
protected:
std::vector<char> flags;
bool reverse;
char operator[]( const unsigned int& i ) const;
public:
sotFlags( const bool& b=false ) ;
sotFlags( const char& c ) ;
sotFlags( const int& c4 ) ;
void add( const char& c ) ;
void add( const int& c4 ) ;
sotFlags operator! (void) const;
SOT_CORE_EXPORT friend sotFlags operator& ( const sotFlags& f1,const sotFlags& f2 ) ;
SOT_CORE_EXPORT friend sotFlags operator| ( const sotFlags& f1,const sotFlags& f2 ) ;
sotFlags& operator&= ( const sotFlags& f2 ) ;
sotFlags& operator|= ( const sotFlags& f2 ) ;
SOT_CORE_EXPORT friend sotFlags operator& ( const sotFlags& f1,const bool& b ) ;
SOT_CORE_EXPORT friend sotFlags operator| ( const sotFlags& f1,const bool& b ) ;
sotFlags& operator&= ( const bool& b );
sotFlags& operator|= ( const bool& b );
SOT_CORE_EXPORT friend std::ostream& operator<< (std::ostream& os, const sotFlags& fl );
SOT_CORE_EXPORT friend char operator>> (const sotFlags& flags, const int& i);
SOT_CORE_EXPORT friend std::istream& operator>> (std::istream& is, sotFlags& fl );
bool operator() (const int& i) const;
operator bool (void) const;
void unset( const unsigned int & i );
void set( const unsigned int & i );
public: /* Selec "matlab-style" : 1:15, 1:, :45 ... */
static void readIndexMatlab( std::istream & iss,
unsigned int & indexStart,
unsigned int & indexEnd,
bool& unspecifiedEnd );
static sotFlags readIndexMatlab( std::istream & iss );
};
SOT_CORE_EXPORT extern const sotFlags FLAG_LINE_1;
SOT_CORE_EXPORT extern const sotFlags FLAG_LINE_2;
SOT_CORE_EXPORT extern const sotFlags FLAG_LINE_3;
SOT_CORE_EXPORT extern const sotFlags FLAG_LINE_4;
SOT_CORE_EXPORT extern const sotFlags FLAG_LINE_5;
SOT_CORE_EXPORT extern const sotFlags FLAG_LINE_6;
SOT_CORE_EXPORT extern const sotFlags FLAG_LINE_7;
SOT_CORE_EXPORT extern const sotFlags FLAG_LINE_8;
} // namespace sot
#endif /* #ifndef __SOT_FLAGS_H */
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Copyright Projet JRL-Japan, 2007
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*
* File: integrator-euler.h
* Project: SOT
* Author: Paul Evrard and Nicolas Mansard
*
* Version control
* ===============
*
* $Id$
*
* Description
* ============
*
*
* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
#ifndef __SOT_INTEGRATOR_EULER_H__
#define __SOT_INTEGRATOR_EULER_H__
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* SOT */
#include <sot-core/integrator-abstract.h>
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
namespace sot {
/*!
* \class sotIntegratorEuler
* \brief integrates an ODE using a naive Euler integration.
* TODO: change the integration method. For the moment, the highest
* derivative of the output signal is computed using the
* previous values of the other derivatives and the input
* signal, then integrated n times, which will most certainly
* induce a huge drift for ODEs with a high order at the denominator.
*/
template<class sigT,class coefT>
class sotIntegratorEuler
: public sotIntegratorAbstract<sigT,coefT>
{
public:
virtual const std::string& getClassName( void ) const { return Entity::getClassName(); }
static std::string getTypeName( void ) { return "Unknown"; }
static const std::string CLASS_NAME;
public:
using sotIntegratorAbstract<sigT,coefT>::SIN;
using sotIntegratorAbstract<sigT,coefT>::SOUT;
using sotIntegratorAbstract<sigT,coefT>::numerator;
using sotIntegratorAbstract<sigT,coefT>::denominator;
public:
sotIntegratorEuler( const std::string& name )
: sotIntegratorAbstract<sigT,coefT>( name )
{
SOUT.addDependancy(SIN);
}
virtual ~sotIntegratorEuler( void ) {}
protected:
std::vector<sigT> inputMemory;
std::vector<sigT> outputMemory;
public:
sigT& integrate( sigT& res, int time )
{
sotDEBUG(15)<<"# In {"<<std::endl;
const double dt = 0.005;
const double invdt = 200;
sigT sum;
sigT tmp1, tmp2;
const std::vector<coefT>& num = numerator;
const std::vector<coefT>& denom = denominator;
// Step 1
tmp1 = inputMemory[0];
inputMemory[0] = SIN.access(time);
sum.resize(tmp1.size());
sum = denom[0] * inputMemory[0];
// End of step 1. Here, sum is b_0 X
// Step 2
int denomsize = denom.size();
for(int i = 1; i < denomsize; ++i)
{
tmp2 = inputMemory[i-1] - tmp1;
tmp2 *= invdt;
tmp1 = inputMemory[i];
inputMemory[i] = tmp2;
sum += (denom[i] * inputMemory[i]);
}
// End of step 2. Here, sum is b_m * d(m)X / dt^m + ... - b_0 X
// Step 3
int numsize = num.size() - 1;
for(int i = 0; i < numsize; ++i)
{
sum -= (num[i] * outputMemory[i]);
}
// End of step 3. Here, sum is b_m * d(m)X / dt^m + ... - b_0 X - a_0 Y - ... a_n-1 d(n-1)Y / dt^(n-1)
// Step 4
outputMemory[numsize] = sum;
for(int i = numsize - 1; i >= 0; --i)
{
outputMemory[i] += (outputMemory[i+1] * dt);
}
// End of step 4. The ODE is integrated
inputMemory[0] = SIN.access(time);
res = outputMemory[0];
sotDEBUG(15)<<"# Out }"<<std::endl;
return res;
}
};
} // namespace sot
#endif
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Copyright Projet JRL-Japan, 2007
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*
* File: macros-signal.h
* Project: SOT
* Author: Nicolas Mansard
*
* Version control
* ===============
*
* $Id$
*
* Description
* ============
*
*
* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
/* --- GENERIC MACROS ------------------------------------------------------- */
/* --- GENERIC MACROS ------------------------------------------------------- */
/* --- GENERIC MACROS ------------------------------------------------------- */
#define SOT_CALL_SIG(sotName,sotType) \
boost::bind(&Signal<sotType,int>::access, \
&sotName,_2)
/* --- STATIC MACROS -------------------------------------------------------- */
/* --- STATIC MACROS -------------------------------------------------------- */
/* --- STATIC MACROS -------------------------------------------------------- */
#define SOT_INIT_SIGNAL_1(sotFunction, \
sotArg1,sotArg1Type) \
boost::bind(&sotFunction, \
SOT_CALL_SIG(sotArg1,sotArg1Type),_1), \
sotArg1
#define SOT_INIT_SIGNAL_2(sotFunction, \
sotArg1,sotArg1Type, \
sotArg2,sotArg2Type) \
boost::bind(&sotFunction, \
SOT_CALL_SIG(sotArg1,sotArg1Type), \
SOT_CALL_SIG(sotArg2,sotArg2Type),_1), \
sotArg1<<sotArg2
#define SOT_INIT_SIGNAL_3(sotFunction, \
sotArg1,sotArg1Type, \
sotArg2,sotArg2Type, \
sotArg3,sotArg3Type) \
boost::bind(&sotFunction, \
SOT_CALL_SIG(sotArg1,sotArg1Type), \
SOT_CALL_SIG(sotArg2,sotArg2Type), \
SOT_CALL_SIG(sotArg3,sotArg3Type),_1), \
sotArg1<<sotArg2<<sotArg3
#define SOT_INIT_SIGNAL_4(sotFunction, \
sotArg1,sotArg1Type, \
sotArg2,sotArg2Type, \
sotArg3,sotArg3Type, \
sotArg4,sotArg4Type) \
boost::bind(&sotFunction, \
SOT_CALL_SIG(sotArg1,sotArg1Type), \
SOT_CALL_SIG(sotArg2,sotArg2Type), \
SOT_CALL_SIG(sotArg3,sotArg3Type), \
SOT_CALL_SIG(sotArg4,sotArg4Type),_1), \
sotArg1<<sotArg2<<sotArg3<<sotArg4
#define SOT_INIT_SIGNAL_5(sotFunction, \
sotArg1,sotArg1Type, \
sotArg2,sotArg2Type, \
sotArg3,sotArg3Type, \
sotArg4,sotArg4Type, \
sotArg5,sotArg5Type) \
boost::bind(&sotFunction, \
SOT_CALL_SIG(sotArg1,sotArg1Type), \
SOT_CALL_SIG(sotArg2,sotArg2Type), \
SOT_CALL_SIG(sotArg3,sotArg3Type), \
SOT_CALL_SIG(sotArg4,sotArg4Type), \
SOT_CALL_SIG(sotArg5,sotArg5Type),_1), \
sotArg1<<sotArg2<<sotArg3<<sotArg4<<sotArg5
#define SOT_INIT_SIGNAL_6(sotFunction, \
sotArg1,sotArg1Type, \
sotArg2,sotArg2Type, \
sotArg3,sotArg3Type, \
sotArg4,sotArg4Type, \
sotArg5,sotArg5Type, \
sotArg6,sotArg6Type) \
boost::bind(&sotFunction, \
SOT_CALL_SIG(sotArg1,sotArg1Type), \
SOT_CALL_SIG(sotArg2,sotArg2Type), \
SOT_CALL_SIG(sotArg3,sotArg3Type), \
SOT_CALL_SIG(sotArg4,sotArg4Type), \
SOT_CALL_SIG(sotArg5,sotArg5Type), \
SOT_CALL_SIG(sotArg6,sotArg6Type),_1), \
sotArg1<<sotArg2<<sotArg3<<sotArg4<<sotArg5<<sotArg6
#define SOT_INIT_SIGNAL_7(sotFunction, \
sotArg1,sotArg1Type, \
sotArg2,sotArg2Type, \
sotArg3,sotArg3Type, \
sotArg4,sotArg4Type, \
sotArg5,sotArg5Type, \
sotArg6,sotArg6Type, \
sotArg7,sotArg7Type) \
boost::bind(&sotFunction, \
SOT_CALL_SIG(sotArg1,sotArg1Type), \
SOT_CALL_SIG(sotArg2,sotArg2Type), \
SOT_CALL_SIG(sotArg3,sotArg3Type), \
SOT_CALL_SIG(sotArg4,sotArg4Type), \
SOT_CALL_SIG(sotArg5,sotArg5Type), \
SOT_CALL_SIG(sotArg6,sotArg6Type), \
SOT_CALL_SIG(sotArg7,sotArg7Type),_1), \
sotArg1<<sotArg2<<sotArg3<<sotArg4<<sotArg5<<sotArg6<<sotArg7
/* --- MEMBERS MACROS ------------------------------------------------------- */
/* --- MEMBERS MACROS ------------------------------------------------------- */
/* --- MEMBERS MACROS ------------------------------------------------------- */
#define SOT_MEMBER_SIGNAL_1(sotFunction, \
sotArg1,sotArg1Type) \
boost::bind(&sotFunction,this, \
SOT_CALL_SIG(sotArg1,sotArg1Type),_1), \
sotArg1
#define SOT_MEMBER_SIGNAL_2(sotFunction, \
sotArg1,sotArg1Type, \
sotArg2,sotArg2Type) \
boost::bind(&sotFunction,this, \
SOT_CALL_SIG(sotArg1,sotArg1Type), \
SOT_CALL_SIG(sotArg2,sotArg2Type),_1), \
sotArg1<<sotArg2
#define SOT_MEMBER_SIGNAL_5(sotFunction, \
sotArg1,sotArg1Type, \
sotArg2,sotArg2Type, \
sotArg3,sotArg3Type, \
sotArg4,sotArg4Type, \
sotArg5,sotArg5Type) \
boost::bind(&sotFunction,this, \
SOT_CALL_SIG(sotArg1,sotArg1Type), \
SOT_CALL_SIG(sotArg2,sotArg2Type), \
SOT_CALL_SIG(sotArg3,sotArg3Type), \
SOT_CALL_SIG(sotArg4,sotArg4Type), \
SOT_CALL_SIG(sotArg5,sotArg5Type),_1), \
sotArg1<<sotArg2<<sotArg3<<sotArg4<<sotArg5
#define SOT_MEMBER_SIGNAL_6(sotFunction, \
sotArg1,sotArg1Type, \
sotArg2,sotArg2Type, \
sotArg3,sotArg3Type, \
sotArg4,sotArg4Type, \
sotArg5,sotArg5Type, \
sotArg6,sotArg6Type) \
boost::bind(&sotFunction,this, \
SOT_CALL_SIG(sotArg1,sotArg1Type), \
SOT_CALL_SIG(sotArg2,sotArg2Type), \
SOT_CALL_SIG(sotArg3,sotArg3Type), \
SOT_CALL_SIG(sotArg4,sotArg4Type), \
SOT_CALL_SIG(sotArg5,sotArg5Type), \
SOT_CALL_SIG(sotArg6,sotArg6Type),_1), \
sotArg1<<sotArg2<<sotArg3<<sotArg4<<sotArg5<<sotArg6
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Copyright Projet JRL-Japan, 2007
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*
* File: matrix-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;
/* --------------------------------------------------------------------- */
/* --- MATRIX ---------------------------------------------------------- */
/* --------------------------------------------------------------------- */
namespace sot {
class sotMatrixConstant
: public Entity
{
public:
static const std::string CLASS_NAME;
virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
int rows,cols;
double color;
public:
sotMatrixConstant( const std::string& name )
:Entity( name )
,rows(0),cols(0),color(0.)
,SOUT( "sotMatrixConstant("+name+")::output(matrix)::out" )
{
SOUT.setDependancyType( TimeDependancy<int>::BOOL_DEPENDANT );
signalRegistration( SOUT );
}
virtual ~sotMatrixConstant( void ){}
SignalTimeDependant<ml::Matrix,int> SOUT;
virtual void commandLine( const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os );
};
} // namespace sot
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Copyright Projet JRL-Japan, 2007
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*
* File: matrix-force.h
* Project: SOT
* Author: Nicolas Mansard
*
* Version control
* ===============
*
* $Id$
*
* Description
* ============
*
*
* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
#ifndef __SOT_MATRIX_FORCE_H__
#define __SOT_MATRIX_FORCE_H__
/* --- Matrix --- */
#include <MatrixAbstractLayer/boost.h>
#include <sot-core/sot-core-api.h>
namespace ml = maal::boost;
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
namespace sot {
class sotMatrixHomogeneous;
class sotMatrixTwist;
class SOT_CORE_EXPORT sotMatrixForce
: public ml::Matrix
{
public:
sotMatrixForce( void ) : ml::Matrix(6,6) { setIdentity(); }
~sotMatrixForce( void ) { }
explicit sotMatrixForce( const sotMatrixHomogeneous& M )
: ml::Matrix(6,6)
{ buildFrom(M); }
sotMatrixForce& buildFrom( const sotMatrixHomogeneous& trans );
sotMatrixForce& operator=( const ml::Matrix& );
sotMatrixForce&
inverse( sotMatrixForce& invMatrix ) const ;
inline sotMatrixForce inverse( void ) const
{ sotMatrixForce Ainv; return inverse(Ainv); }
sotMatrixTwist& transpose( sotMatrixTwist& Vt ) const;
sotMatrixTwist transpose( void ) const;
};
} // namespace sot
#endif /* #ifndef __SOT_MATRIX_FORCE_H__ */
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Copyright Projet JRL-Japan, 2007
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*
* File: matrix-homogeneous.h
* Project: SOT
* Author: Nicolas Mansard
*
* Version control
* ===============
*
* $Id$
*
* Description
* ============
*
*
* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
#ifndef __SOT_MATRIX_HOMOGENEOUS_H__
#define __SOT_MATRIX_HOMOGENEOUS_H__
/* --- Matrix --- */
#include <MatrixAbstractLayer/boost.h>
#include <sot-core/sot-core-api.h>
namespace ml = maal::boost;
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
namespace sot {
class sotMatrixRotation;
class SOT_CORE_EXPORT sotMatrixHomogeneous
: public ml::Matrix
{
public:
sotMatrixHomogeneous( void ) : ml::Matrix(4,4) { setIdentity(); }
~sotMatrixHomogeneous( void ) { }
sotMatrixHomogeneous& buildFrom( const sotMatrixRotation& rot, const ml::Vector& trans );
sotMatrixRotation& extract( sotMatrixRotation& rot ) const;
ml::Vector& extract( ml::Vector& trans ) const;
sotMatrixHomogeneous& operator=( const ml::Matrix& );
sotMatrixHomogeneous&
inverse( sotMatrixHomogeneous& invMatrix ) const ;
inline sotMatrixHomogeneous inverse( void ) const
{ sotMatrixHomogeneous Ainv; return inverse(Ainv); }
ml::Vector& multiply( const ml::Vector& v1,ml::Vector& res ) const;
inline ml::Vector multiply( const ml::Vector& v1 ) const
{ ml::Vector res; return multiply(v1,res); }
using ml::Matrix::multiply;
};
} // namespace sot
#endif /* #ifndef __SOT_MATRIX_HOMOGENEOUS_H__ */
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Copyright Projet JRL-Japan, 2007
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*
* File: matrix-rotation.h
* Project: SOT
* Author: Nicolas Mansard
*
* Version control
* ===============
*
* $Id$
*
* Description
* ============
*
*
* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
#ifndef __SOT_MATRIX_ROTATION_H__
#define __SOT_MATRIX_ROTATION_H__
/* --- Matrix --- */
#include <sot-core/sot-core-api.h>
#include <MatrixAbstractLayer/boost.h>
namespace ml = maal::boost;
namespace sot {
class sotVectorUTheta;
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SOT_CORE_EXPORT sotMatrixRotation
: public ml::Matrix
{
public:
sotMatrixRotation( void ) : ml::Matrix(3,3) { setIdentity(); }
~sotMatrixRotation( void ) { }
void fromVector( sotVectorUTheta& );
sotMatrixRotation& operator= ( sotVectorUTheta&th ) { fromVector(th); return *this; }
};
} // namespace sot
#endif /* #ifndef __SOT_MATRIX_ROTATION_H__ */
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Copyright Projet JRL-Japan, 2007
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*
* File: matrix-twist.h
* Project: SOT
* Author: Nicolas Mansard
*
* Version control
* ===============
*
* $Id$
*
* Description
* ============
*
*
* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
#ifndef __SOT_MATRIX_TWIST_H__
#define __SOT_MATRIX_TWIST_H__
/* --- Matrix --- */
#include <MatrixAbstractLayer/boost.h>
#include <sot-core/sot-core-api.h>
namespace ml = maal::boost;
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
namespace sot {
class sotMatrixHomogeneous;
class sotMatrixForce;
class SOT_CORE_EXPORT sotMatrixTwist
: public ml::Matrix
{
public:
sotMatrixTwist( void ) : ml::Matrix(6,6) { setIdentity(); }
~sotMatrixTwist( void ) { }
explicit sotMatrixTwist( const sotMatrixHomogeneous& M )
: ml::Matrix(6,6)
{ buildFrom(M); }
sotMatrixTwist& buildFrom( const sotMatrixHomogeneous& trans );
sotMatrixTwist& operator=( const ml::Matrix& );
sotMatrixTwist&
inverse( sotMatrixTwist& invMatrix ) const ;
inline sotMatrixTwist inverse( void ) const
{ sotMatrixTwist Ainv; return inverse(Ainv); }
sotMatrixForce& transpose( sotMatrixForce& Vt ) const;
sotMatrixForce transpose( void ) const;
};
} // namespace sot
#endif /* #ifndef __SOT_MATRIX_TWIST_H__ */
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Copyright Projet Gepetto, LAAS, CNRS, 2009
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*
* File: memory-task-sot.h
* Project: SOT
* Author: Nicolas Mansard
*
* Version control
* ===============
*
* $Id$
*
* Description
* ============
*
*
* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
#ifndef __SOT_MEMORY_TASK_HH
#define __SOT_MEMORY_TASK_HH
#include <sot-core/task-abstract.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 ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
namespace sot {
class SOTSOT_CORE_EXPORT sotMemoryTaskSOT
: public sotTaskAbstract::sotMemoryTaskAbstract, public Entity
{
public:// protected:
/* Internal memory to reduce the dynamic allocation at task resolution. */
ml::MatrixSvd Jt; //( nJ,mJ );
ml::Matrix Jp;
ml::Matrix PJp;
ml::Matrix Jff; //( nJ,FF_SIZE ); // Free-floating part
ml::Matrix Jact; //( nJ,mJ ); // Activated part
ml::Matrix JK; //(nJ,mJ);
ml::Matrix U,V;
ml::Vector S;
public:
/* mJ is the number of actuated joints, nJ the number of feature in the task,
* and ffsize the number of unactuated DOF. */
sotMemoryTaskSOT( const std::string & name,const unsigned int nJ=0,
const unsigned int mJ=0,const unsigned int ffsize =0 );
virtual void initMemory( const unsigned int nJ,
const unsigned int mJ,
const unsigned int ffsize );
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; }
public: /* --- SIGNALS --- */
Signal< ml::Matrix,int > jacobianInvSINOUT;
Signal< ml::Matrix,int > jacobianConstrainedSINOUT;
Signal< ml::Matrix,int > jacobianProjectedSINOUT;
Signal< ml::Matrix,int > singularBaseImageSINOUT;
Signal< unsigned int,int > rankSINOUT;
public: /* --- PARAMS --- */
virtual void commandLine( const std::string& cmdLine,std::istringstream& cmdArgs,
std::ostream& os );
};
} //namespace sot
#endif // __SOT_MEMORY_TASK_HH
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* 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 ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
namespace sot {
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 );
} // namespace sot
#endif // #ifndef __SOT_sotMultiBound_H__
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Copyright Projet JRL-Japan, 2007
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*
* File: op-point-modifier.h
* Project: SOT
* Author: Nicolas Mansard
*
* Version control
* ===============
*
* $Id$
*
* Description
* ============
*
*
* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
#ifndef __SOT_OP_POINT_MODIFIOR_H__
#define __SOT_OP_POINT_MODIFIOR_H__
#include <dynamic-graph/entity.h>
#include <dynamic-graph/all-signals.h>
#include <sot-core/debug.h>
#include <sot-core/matrix-homogeneous.h>
/* Matrix */
#include <MatrixAbstractLayer/boost.h>
namespace ml = maal::boost;
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (sotOpPointModifior_EXPORTS)
# define SOTOPPOINTMODIFIOR_EXPORT __declspec(dllexport)
# else
# define SOTOPPOINTMODIFIOR_EXPORT __declspec(dllimport)
# endif
#else
# define SOTOPPOINTMODIFIOR_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- VECTOR ---------------------------------------------------------- */
/* --------------------------------------------------------------------- */
namespace sot {
class SOTOPPOINTMODIFIOR_EXPORT sotOpPointModifior
: public Entity
{
public:
static const std::string CLASS_NAME;
virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
sotMatrixHomogeneous transformation;
public:
SignalPtr<ml::Matrix,int> jacobianSIN;
SignalPtr<sotMatrixHomogeneous,int> positionSIN;
SignalTimeDependant<ml::Matrix,int> jacobianSOUT;
SignalTimeDependant<sotMatrixHomogeneous,int> positionSOUT;
public:
sotOpPointModifior( const std::string& name );
virtual ~sotOpPointModifior( void ){}
ml::Matrix& computeJacobian( ml::Matrix& res,const int& time );
sotMatrixHomogeneous& computePosition( sotMatrixHomogeneous& res,const int& time );
void setTransformation( const sotMatrixHomogeneous& tr );
virtual void commandLine( const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os );
};
} // namespace sot
#endif // __SOT_OP_POINT_MODIFIOR_H__
#ifndef __SOT_sotRotationSimple_HH__
#define __SOT_sotRotationSimple_HH__
/* --- BOOST --- */
#include <MatrixAbstractLayer/boost.h>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <sot-core/debug.h>
#include <list>
/* --------------------------------------------------------------------- */
/* --- 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 ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
namespace bub = boost::numeric::ublas;
typedef bub::vector<double> bubVector;
typedef bub::matrix<double> bubMatrix;
template< class bubTemplateMatrix >
void bubClear( const bubTemplateMatrix& m )
{
//std::fill(m.begin(),m.end(),0.0);
}
template< class bubTemplateMatrix >
void bubClearMatrix( const bubTemplateMatrix& m )
{
//m
}
/* --- DEBUG ---------------------------------------------------------------- */
/* --- DEBUG ---------------------------------------------------------------- */
/* --- DEBUG ---------------------------------------------------------------- */
class sotRotationSimple;
class SOTSOTH_EXPORT MATLAB
{
public:
static bool fullPrec;
std::string str;
friend std::ostream & operator << (std::ostream & os, const MATLAB & m )
{return os << m.str; }
template< typename bubTemplateMatrix >
void initFromBubMatrix( const bubTemplateMatrix& m1)
{
fullPrec=false;
std::ostringstream os; os << "[ ";
std::ostringstream ostmp;
for( unsigned int i=0;i<m1.size1();++i )
{
for( unsigned int j=0;j<m1.size2();++j )
{
if( m1(i,j)<0 ) ostmp << "-"; else ostmp << " ";
if(fullPrec||fabs(m1(i,j))>1e-6) ostmp << fabs(m1(i,j));
else { ostmp << "0"; }
if( m1.size2()!=j+1 )
{
ostmp << ",";
const int size = ostmp.str().length();
for( unsigned int i=size;i<8;++i) ostmp<<" ";
ostmp << "\t";
}
os << ostmp.str(); ostmp.str("") ;
}
if( m1.size1()!=i+1 ) { os << " ;" << std::endl<<" "; }
else { os << " ];"; }
}
str = os.str();
}
MATLAB( const bubVector& v1 )
{
std::ostringstream os; os << "[ ";
for( unsigned int i=0;i<v1.size();++i )
{
{os << " "; double a=v1(i); os << a;}
//DEBUGos << v1(i);
if( v1.size()!=i+1 ) { os << ", "; }
}
os << "]';";
str = os.str();
}
MATLAB( const bub::indirect_array<>& v1 )
{
std::ostringstream os; os << "[ ";
for( unsigned int i=0;i<v1.size();++i )
{
os << v1(i);
if( v1.size()!=i+1 ) { os << ", "; }
}
os << "]";
str = os.str();
}
MATLAB( const bubMatrix& m1)
{initFromBubMatrix(m1);}
MATLAB( const sotRotationSimple& m1,const unsigned int nJ );
};
template< typename bubTemplateMatrix >
void randMatrix( bubTemplateMatrix& M,const unsigned int row,const unsigned int col )
{
M.resize(row,col);
for( unsigned int i=0;i<row;++i )
for( unsigned int j=0;j<col;++j )
{ M(i,j) = ((rand()+0.0)/RAND_MAX*2)-1.; }
}
template< typename bubTemplateVector >
void randVector( bubTemplateVector& M,const unsigned int row)
{
M.resize(row);
for( unsigned int i=0;i<row;++i )
{ M(i) = ((rand()+0.0)/RAND_MAX*2)-1.; }
}
/* ---------------------------------------------------------- */
/* --- SIMPLE ROTATION -------------------------------------- */
/* ---------------------------------------------------------- */
#define SOT_ROTATION_SIMPLE_MULTIPLY(MATRIX_TYPE) \
virtual void multiplyRight( MATRIX_TYPE& M ) const = 0; /* M:=P.M */ \
virtual void multiplyLeft( MATRIX_TYPE& M ) const= 0; /* M:=M.P */
#define SOT_ROTATION_SIMPLE_MULTIPLY_TRANSPOSE(MATRIX_TYPE) \
virtual void multiplyRightTranspose( MATRIX_TYPE& M ) const = 0; /* M:=P'.M */ \
virtual void multiplyLeftTranspose( MATRIX_TYPE& M ) const= 0; /* M:=M.P' */
/* Define the virtual link with template operators. */
#define SOT_ROTATION_DERIVATED_MULTIPLY_VECTOR(VECTOR_TYPE) \
virtual void multiplyRight( VECTOR_TYPE& M ) const \
{ multiplyRightVectorTemplate( M ); } \
virtual void multiplyLeft( VECTOR_TYPE& M ) const \
{ multiplyLeftVectorTemplate( M ); }
#define SOT_ROTATION_DERIVATED_MULTIPLY_MATRIX(MATRIX_TYPE) \
virtual void multiplyRight( MATRIX_TYPE& M ) const \
{ multiplyRightMatrixTemplate( M ); } \
virtual void multiplyLeft( MATRIX_TYPE& M ) const \
{ multiplyLeftMatrixTemplate( M ); } \
virtual void multiplyRightTranspose( MATRIX_TYPE& M ) const \
{ multiplyRightTransposeMatrixTemplate( M ); } \
virtual void multiplyLeftTranspose( MATRIX_TYPE& M ) const \
{ multiplyLeftTransposeMatrixTemplate( M ); }
typedef bub::matrix<double,bub::column_major> __SRS_matcolmaj ;
typedef bub::matrix_range<__SRS_matcolmaj> __SRS_rang_matcolmaj;
typedef bub::triangular_adaptor<bub::matrix_range<bub::matrix<double,bub::column_major> >,bub::upper > __SRS_triadup_rang_matcolmaj;
typedef bub::triangular_adaptor<bub::matrix_range< bubMatrix >,bub::upper> __SRS_triadup_rang_mat;
typedef bub::triangular_adaptor<bub::matrix_range<bub::triangular_matrix<double,bub::upper> >,bub::upper > __SRS_triadup_rang_triup ;
typedef bub::triangular_matrix<double,bub::upper> __SRS_triup ;
typedef bub::matrix_column<__SRS_matcolmaj> __SRS_col_matcolmaj;
#define SOT_ROTATION_SIMPLE_TEMPLATE_VECTOR_LIST(A) \
A(bubVector); A(bub::vector_range<bubVector>) ; A(bub::matrix_column<bubMatrix>); A(bub::vector_range<bub::matrix_column<bubMatrix> >); A(__SRS_col_matcolmaj);
#define SOT_ROTATION_SIMPLE_TEMPLATE_MATRIX_LIST(A) \
A(bubMatrix); A(bub::matrix_range<bubMatrix>);A(__SRS_matcolmaj);A( __SRS_rang_matcolmaj);A( __SRS_triadup_rang_matcolmaj);A( __SRS_triadup_rang_mat); A(__SRS_triadup_rang_triup ); A(__SRS_triup);
/* Virtual Pure. */
class SOTSOTH_EXPORT sotRotationSimple
{
public:
virtual ~sotRotationSimple( void ) {}
public:
/* --- STANDARD (FULL-RANGE) MULTIPLICATIONS. */
/* Vector multiplications */
SOT_ROTATION_SIMPLE_TEMPLATE_VECTOR_LIST(SOT_ROTATION_SIMPLE_MULTIPLY);
/* Matrix multiplications */
SOT_ROTATION_SIMPLE_TEMPLATE_MATRIX_LIST(SOT_ROTATION_SIMPLE_MULTIPLY);
SOT_ROTATION_SIMPLE_TEMPLATE_MATRIX_LIST(SOT_ROTATION_SIMPLE_MULTIPLY_TRANSPOSE);
/* --- (LIMITED-)RANGE MULTIPLICATIONS. */
/* Vector multiplications */
/* Multiply M := Q*[0;I;0]*M. */
template<typename bubTemplateVectorIN,typename bubTemplateVectorOUT >
void multiplyRangeRight( const bubTemplateVectorIN& M,
bubTemplateVectorOUT& res,
const unsigned int zeroBefore,
const unsigned int zeroAfter ) const
{
const unsigned int m=M.size();
const unsigned int mres=m+zeroBefore+zeroAfter;
res.resize( mres ); bubClear(res);
bub::vector_range< bubTemplateVectorOUT > Mrange
(res,bub::range(zeroBefore,zeroBefore+m));
Mrange.assign(M);
multiplyRight(Mrange);
}
/* Multiply M := Q*[0;I;0]*M. */
template<typename bubTemplateVector>
void multiplyRangeRight( bubTemplateVector& M,
const unsigned int zeroBefore,
const unsigned int zeroAfter ) const
{
bub::project(M,bub::range(0,zeroBefore))
.assign( bub::zero_vector<double>(zeroBefore) );
bub::project(M,bub::range(M.size()-zeroAfter,M.size()))
.assign( bub::zero_vector<double>(zeroAfter) );
multiplyRight(M);
}
/* Multiply M := M*Q*[0 I 0]. */
template<typename bubTemplateVectorIN,typename bubTemplateVectorOUT>
void multiplyRangeLeft( const bubTemplateVectorIN& M,
bubTemplateVectorOUT& res,
const unsigned int zeroBefore,
const unsigned int zeroAfter ) const
{
bubVector acopy = M; multiplyLeft(acopy);
const unsigned int mres = M.size() - zeroBefore - zeroAfter;
res.resize(mres); res.assign( bub::project( acopy,bub::range(zeroBefore,mres)) );
}
/* Multiply M := M*Q*[0 I 0]. */
/* template<typename bubTemplateVector> */
/* void multiplyRangeLeft( bubTemplateVector& m, */
/* const unsigned int zeroBefore, */
/* const unsigned int zeroAfter ) const */
/* { */
/* multiplyLeft(m); */
/* if(zeroBefore>0) */
/* bub::project( m,bub::range(0,zeroBefore)) *= 0; */
/* if(zeroAfter>0) */
/* bub::project( m,bub::range(m.size()-zeroAfter,m.size())) *= 0; */
/* } */
// virtual void multiplyRangeLeft( const bubVector& M,
// bubVector& res,
// const unsigned int zeroBefore,
// const unsigned int zeroAfter ) const = 0;
// virtual void multiplyRangeLeft( bubVector& M,
// const unsigned int zeroBefore,
// const unsigned int zeroAfter ) const = 0;
/* Multiply M := [0;I;0]*Q'*M = M*Q*[0 I 0]. */
template<typename bubTemplateVector>
bub::vector_range<bubTemplateVector>
multiplyRangeLeft( bubTemplateVector& m,
const unsigned int zeroBefore,
const unsigned int zeroAfter ) const
{
//sotDEBUG(1)<<"Q = " << *this << std::endl;
multiplyLeft(m);
if(zeroBefore>0)
bub::project( m,bub::range(0,zeroBefore))
.assign(bub::zero_vector<double>(zeroBefore));
if(zeroAfter>0)
bub::project( m,bub::range(m.size()-zeroAfter,m.size()))
.assign(bub::zero_vector<double>(zeroAfter));
return bub::vector_range<bubTemplateVector>(m,bub::range(zeroBefore,m.size()-zeroAfter));
}
/* Matrix multiplications */
/* Multiply M := Q*[0;I;0]*M. */
virtual void multiplyRangeRight( const bubMatrix& M,
bubMatrix& res,
const unsigned int zeroBefore,
const unsigned int zeroAfter ) const
{
const unsigned int m=M.size1(); const unsigned int nJ = M.size2();
const unsigned int mres=m+zeroBefore+zeroAfter;
res.resize( mres,nJ ); res = bub::zero_matrix<double>(mres,nJ);
bub::matrix_range< bubMatrix > Mrange(res,bub::range(zeroBefore,zeroBefore+m),
bub::range(0,nJ));
Mrange.assign(M); multiplyRight(Mrange);
}
/* Multiply M := Q*[0;I;0]*M. */
virtual void multiplyRangeRight( bubMatrix& M,
const unsigned int zeroBefore,
const unsigned int zeroAfter ) const
{
const unsigned int nJ = M.size2(); bub::range fullCol(0,nJ);
bub::project(M,bub::range(0,zeroBefore),fullCol)
= bub::zero_matrix<double>(zeroBefore,nJ);
const unsigned int mp = M.size1()-zeroBefore-zeroAfter;
bub::project(M,bub::range(zeroBefore+mp,zeroBefore+mp+zeroAfter),fullCol)
= bub::zero_matrix<double>(zeroAfter,nJ);
multiplyRight(M);
}
public:
virtual std::ostream & display( std::ostream & os ) const = 0;
friend std::ostream& operator << ( std::ostream & os,const sotRotationSimple& Q ) { return Q.display(os); }
};
/* ---------------------------------------------------------- */
/* ---------------------------------------------------------- */
/* ---------------------------------------------------------- */
class SOTSOTH_EXPORT sotRotationSimpleHouseholder
: public sotRotationSimple
{
public: // protected:
bubVector v;
double beta;
public:
sotRotationSimpleHouseholder( void ) { v.resize(0);}
sotRotationSimpleHouseholder( const unsigned int n )
{ v.resize(n); v = bub::zero_vector<double>(n); beta=0;}
sotRotationSimpleHouseholder( const bubVector& _v, const double& _beta )
{ v= _v; beta=_beta; }
sotRotationSimpleHouseholder( const bubMatrix& _v, const double& _beta )
{ v.resize(_v.size1()); v = bub::row(_v,0); beta=_beta; }
virtual ~sotRotationSimpleHouseholder( void ) {}
public:
SOT_ROTATION_SIMPLE_TEMPLATE_VECTOR_LIST(SOT_ROTATION_DERIVATED_MULTIPLY_VECTOR)
SOT_ROTATION_SIMPLE_TEMPLATE_MATRIX_LIST(SOT_ROTATION_DERIVATED_MULTIPLY_MATRIX)
/* --- VECTOR --- */
template< typename bubTemplateVector>
void multiplyRightVectorTemplate( bubTemplateVector& a ) const // P*a
{
const unsigned int m=a.size();
const unsigned int kv = v.size();
const unsigned int m_kv = m-kv;
double w; w=0;
// w <- b v' A = b [ 0 ... 0 1 v' ] a
for( unsigned int i=0;i<kv;++i )
{ w += v(i)*a(i+m_kv); }
w*=beta;
// a <- a - v w = [ 0; ... ; 0; 1; v ] w
for( unsigned int i=0;i<kv;++i )
{ a(i+m_kv)-=v(i)*w; }
}
template< typename bubTemplateVector>
void multiplyLeftVectorTemplate( bubTemplateVector& m ) const // a*P=P*a
{ multiplyRightVectorTemplate(m); }
/* --- MATRIX --- */
template< typename bubTemplateMatrix >
void multiplyRightMatrixTemplate( bubTemplateMatrix& A ) const // P*A
// P*A = (I-b.v.v') A = A-b.v.(v'.A)
{
const unsigned int m=A.size1(), n=A.size2();
const unsigned int kv = v.size();
const unsigned int m_kv = m-kv;
bubVector w(n); w.clear();
// W <- b v' A = b [ 0 ... 0 1 v' ] A
for( unsigned int j=0;j<n;++j )
{
for( unsigned int i=0;i<kv;++i )
{ w(j) += v(i)*A(i+m_kv,j); }
w(j)*=beta;
}
// A <- A - v w = [ 0; ... ; 0; 1; v ] w
for( unsigned int i=0;i<kv;++i )
for( unsigned int j=0;j<n;++j )
{ A(i+m_kv,j)-=v(i)*w(j); }
}
template< typename bubTemplateMatrix >
void multiplyLeftMatrixTemplate( bubTemplateMatrix& A ) const // A*P
// A*P = A.(I-b.v.v') = A-b.(A.v).v'
{
const unsigned int m=A.size1(), n=A.size2();
const unsigned int kv = v.size();
const unsigned int n_kv = n-kv;
bubVector w(m); w.clear();
// W <- b A v = b A [ 0; ...; 0; 1; v ]
for( unsigned int i=0;i<m;++i )
{
for( unsigned int j=0;j<kv;++j )
{ w(i) += A(i,n_kv+j)*v(j); }
w(i)*=beta;
}
//sotDEBUG(1)<<"whh = " << (MATLAB)w << std::endl;
// A <- A - w v' = w [ 0 ... 0 1 v' ]
for( unsigned int j=0;j<kv;++j )
for( unsigned int i=0;i<m;++i )
{ A(i,j+n_kv)-=w(i)*v(j); }
}
template< typename bubTemplateMatrix >
void multiplyRightTransposeMatrixTemplate( bubTemplateMatrix& M ) const // P'*A=P*A
{ multiplyRightMatrixTemplate(M); }
template< typename bubTemplateMatrix>
void multiplyLeftTransposeMatrixTemplate( bubTemplateMatrix& M ) const // A*P'=A*P
{ multiplyLeftMatrixTemplate(M); }
/* --- DIPLAY ----------------------------------------------- */
virtual std::ostream& display( std::ostream & os ) const
{ return os << "[b=" << beta << "] " << (MATLAB)v; }
/* --- HOUSEHOLDER EXTRACTION ------------------------------- */
/* After the execution of the function, x is the corresponding
* househould vector h so that x-b.h'.h.x = [ || x ||; 0 ].
* Additionally, return the norm of original x. */
template<typename bubTemplateVector>
static double householderExtraction( bubTemplateVector& x,double& beta,
const double THRESHOLD_ZERO=1e-15 )
{
const unsigned int n=x.size();
double sigma=0;
for( unsigned int i=1;i<n;++i ) // sigma = x(2:n)^T x(2:n)
{const double& xi=x(i); sigma+=xi*xi;}
if( fabs(sigma)<THRESHOLD_ZERO ) // Vector already nullified x=[||x||; 0].
{
const double norm = x(0); // return a norm ~homogeneous to x(0)
beta = 0; x(0)=1;
return norm;
}
else
{
double& v1=x(0);
const double x1sq=v1*v1;
const double mu= sqrt(x1sq+sigma); // mu = ||x||
int signRes;
if(v1<=0) { v1 -= mu; signRes = 1; }
else { v1 = -sigma/(v1+mu); signRes = -1; }
const double v1sq=v1*v1;
const double v1inv=1/v1;
beta=2*v1sq/(sigma+v1sq);
for( unsigned int i=0;i<n;++i )
{ x(i) *= v1inv; }
return mu;
}
}
};
/* ---------------------------------------------------------- */
/* ---------------------------------------------------------- */
/* ---------------------------------------------------------- */
class SOTSOTH_EXPORT sotRotationSimpleGiven
: public sotRotationSimple
{
public: // protected:
double cosF,sinF;
unsigned int idx1,idx2; // The matrix is I but on R(idxN,idxN), N={1,2}.
public: /* --- CONSTRUCTORS ------------------------------------------------- */
sotRotationSimpleGiven( void )
: cosF(1),sinF(0),idx1(0),idx2(0) {}
sotRotationSimpleGiven( const double _cosF,const double _sinF,
const unsigned int _idx1,const unsigned int _idx2 )
: cosF(_cosF),sinF(_sinF),idx1(_idx1),idx2(_idx2) {}
/* Intialize for nullification of Rx.U: nullify R(r,c2) using R(r,c1). */
// For nullification from the right.
sotRotationSimpleGiven( unsigned int row, unsigned int col1,
unsigned int col2, const bubMatrix& Rx )
{ nullifyRow(Rx,row,col1,col2); }
// For nullification from the right.
sotRotationSimpleGiven( const bubMatrix& Rx, unsigned int row1,
unsigned int row2, unsigned int col )
{ nullifyColumn(Rx,row1,row2,col); }
virtual ~sotRotationSimpleGiven( void ) {}
public: /* --- MULTIPLIERS -------------------------------------------------- */
SOT_ROTATION_SIMPLE_TEMPLATE_VECTOR_LIST(SOT_ROTATION_DERIVATED_MULTIPLY_VECTOR)
SOT_ROTATION_SIMPLE_TEMPLATE_MATRIX_LIST(SOT_ROTATION_DERIVATED_MULTIPLY_MATRIX)
/* --- VECTORS --- */
template< typename bubTemplateVector >
void multiplyLeftVectorTemplate( bubTemplateVector & v ) const // v <- v.U=U'.v
{
double & R1 = v(idx1); double & R2 = v(idx2);
const double r1 = R1; const double r2 = R2;
R1 = r1*cosF-r2*sinF; R2 = r1*sinF+r2*cosF;
} /// A VERIFIER DANS GOLUB!
template< typename bubTemplateVector >
void multiplyRightVectorTemplate( bubTemplateVector & v ) const // v <- U.v
{
double & R1 = v(idx1); double & R2 = v(idx2);
const double r1 = R1; const double r2 = R2;
R1 = r1*cosF+r2*sinF; R2 = -r1*sinF+r2*cosF;
}
/* --- LEFT MATRIX --- */
template< typename bubTemplateMatrix >
void multiplyLeftMatrixTemplate( bubTemplateMatrix & M ) const // M <- M.U
{
const unsigned int n = M.size1();
for( unsigned int row=0;row<n;++row )
{
double & R1 = M(row,idx1); double & R2 = M(row,idx2);
const double r1 = R1; const double r2 = R2;
R1 = r1*cosF-r2*sinF; R2 = r1*sinF+r2*cosF;
}
}
void multiplyLeftMatrixTemplate( bub::triangular_matrix<double,bub::upper> & M ) const
{
const unsigned int n = std::min( std::min(idx1,idx2),M.size1());
for( unsigned int row=0;row<n;++row )
{
double & R1 = M(row,idx1); double & R2 = M(row,idx2);
const double r1 = R1; const double r2 = R2;
R1 = r1*cosF-r2*sinF; R2 = r1*sinF+r2*cosF;
}
}
template< typename bubTemplateMatrix > // M <- M.U
void multiplyLeftMatrixTemplate( bub::triangular_adaptor<bubTemplateMatrix,bub::upper> & M ) const
{
const unsigned int n = std::min( std::min(idx1,idx2)+1,M.size1());
for( unsigned int row=0;row<n;++row )
{
double & R1 = M(row,idx1); double & R2 = M(row,idx2);
const double r1 = R1; const double r2 = R2;
R1 = r1*cosF-r2*sinF; R2 = r1*sinF+r2*cosF;
}
}
/* --- RIGHT MATRIX --- */
template< typename bubTemplateMatrix >
void multiplyRightMatrixTemplate( bubTemplateMatrix & M ) const // M <- U.M
{
const unsigned int m = M.size2();
for( unsigned int col=0;col<m;++col )
{
double & R1 = M(idx1,col); double & R2 = M(idx2,col);
const double r1 = R1; const double r2 = R2;
R1 = r1*cosF+r2*sinF; R2 = -r1*sinF+r2*cosF;
}
}
void multiplyRightMatrixTemplate( bub::triangular_matrix<double,bub::upper> & M ) const
{
const unsigned int m = std::min( std::min(idx1,idx2),M.size2());
for( unsigned int col=0;col<m;++col )
{
double & R1 = M(idx1,col); double & R2 = M(idx2,col);
const double r1 = R1; const double r2 = R2;
R1 = r1*cosF+r2*sinF; R2 = -r1*sinF+r2*cosF;
}
}
template< typename bubTemplateMatrix > // M <- M.U'
void multiplyRightMatrixTemplate( bub::triangular_adaptor<bubTemplateMatrix,bub::upper> & M ) const
{
const unsigned int m = std::min( std::min(idx1,idx2)+1,M.size2());
for( unsigned int col=0;col<m;++col )
{
double & R1 = M(idx1,col); double & R2 = M(idx2,col);
const double r1 = R1; const double r2 = R2;
R1 = r1*cosF+r2*sinF; R2 = -r1*sinF+r2*cosF;
}
}
/* --- LEFT TRANSPOSE MATRIX --- */
template< typename bubTemplateMatrix >
void multiplyLeftTransposeMatrixTemplate( bubTemplateMatrix & M ) const // M <- M.U
{
const unsigned int n = M.size1();
for( unsigned int row=0;row<n;++row )
{
double & R1 = M(row,idx1); double & R2 = M(row,idx2);
const double r1 = R1; const double r2 = R2;
R1 = r1*cosF+r2*sinF; R2 = -r1*sinF+r2*cosF;
}
}
void multiplyLeftTransposeMatrixTemplate( bub::triangular_matrix<double,bub::upper> & M ) const
{
const unsigned int n = std::min( std::min(idx1,idx2),M.size1());
for( unsigned int row=0;row<n;++row )
{
double & R1 = M(row,idx1); double & R2 = M(row,idx2);
const double r1 = R1; const double r2 = R2;
R1 = r1*cosF+r2*sinF; R2 = -r1*sinF+r2*cosF;
}
}
template< typename bubTemplateMatrix > // M <- M.U
void multiplyLeftTransposeMatrixTemplate( bub::triangular_adaptor<bubTemplateMatrix,bub::upper> & M ) const
{
const unsigned int n = std::min( std::min(idx1,idx2)+1,M.size1());
for( unsigned int row=0;row<n;++row )
{
double & R1 = M(row,idx1); double & R2 = M(row,idx2);
const double r1 = R1; const double r2 = R2;
R1 = r1*cosF+r2*sinF; R2 = -r1*sinF+r2*cosF;
}
}
/* --- RIGHT TRANSPOSE MATRIX --- */
template< typename bubTemplateMatrix >
void multiplyRightTransposeMatrixTemplate( bubTemplateMatrix & M ) const // M <- U.M
{
const unsigned int m = M.size2();
for( unsigned int col=0;col<m;++col )
{
double & R1 = M(idx1,col); double & R2 = M(idx2,col);
const double r1 = R1; const double r2 = R2;
R1 = r1*cosF-r2*sinF; R2 = r1*sinF+r2*cosF;
}
}
void multiplyRightTransposeMatrixTemplate( bub::triangular_matrix<double,bub::upper> & M ) const
{
/* const unsigned int m = std::min( std::min(idx1,idx2),M.size2()); */
/* for( unsigned int col=0;col<m;++col ) */
/* { */
/* double & R1 = M(idx1,col); double & R2 = M(idx2,col); */
/* const double r1 = R1; const double r2 = R2; */
/* R1 = r1*cosF-r2*sinF; R2 = r1*sinF+r2*cosF; */
/* } */
std::cerr << "Not implemented yet (sotRotationSimple l" << __LINE__
<< ")." << std::endl;
throw "Not implemented yet.";
}
template< typename bubTemplateMatrix > // M <- M.U'
void multiplyRightTransposeMatrixTemplate( bub::triangular_adaptor<bubTemplateMatrix,bub::upper> & M ) const
{
/* const unsigned int m1 = std::min(idx1,idx2); */
/* for( unsigned int col=m1;col<M.size2();++col ) */
/* { */
/* double & R1 = M(idx1,col); double & R2 = M(idx2,col); */
/* const double r1 = R1; const double r2 = R2; */
/* R1 = r1*cosF-r2*sinF; R2 = r1*sinF+r2*cosF; */
/* } */
std::cerr << "Not implemented yet (sotRotationSimple l" << __LINE__
<< ")." << std::endl;
throw "Not implemented yet.";
}
/* --- DISPLAY --- */
virtual std::ostream& display( std::ostream& os ) const
{ return os << "GR["<<idx1<<","<<idx2<<"]("<<cosF<<","<<sinF<<")"; }
/* --- GIVEN ROTATION EXTRACTION --- */
/* On line <v>, find the coeff a&b so that (v.R)([col1 col2]) = [ x 0 ]. */
template< typename bubTemplateVector >
void nullifyFromRight( const bubTemplateVector & v,
unsigned int col1, unsigned int col2 )
{
const double &a=v(col1); const double &b=v(col2); double tau;
if( fabs(b)>fabs(a) )
{ tau=-a/b; sinF=1/sqrt(1+tau*tau); cosF=sinF*tau; }
else
{ tau=-b/a; cosF=1/sqrt(1+tau*tau); sinF=cosF*tau; }
idx1=col1; idx2=col2;
}
void inverse( void ) { sinF *= -1; }
template< typename bubTemplateVector >
void nullifyFromLeft( const bubTemplateVector & v,
unsigned int col1, unsigned int col2 )
{ nullifyFromRight(v,col1,col2); inverse(); }
template< typename bubTemplateMatrix >
void nullifyRow( const bubTemplateMatrix& M, unsigned int row,
unsigned int col1, unsigned int col2 )
{ nullifyFromRight(bub::row(M,row),col1,col2); }
template< typename bubTemplateMatrix >
void nullifyColumn( const bubTemplateMatrix& M, unsigned int row1,
unsigned int row2, unsigned int col )
{ nullifyFromLeft(bub::column(M,col),row1,row2); }
};
/* ---------------------------------------------------------- */
/* ---------------------------------------------------------- */
/* ---------------------------------------------------------- */
class SOTSOTH_EXPORT sotRotationComposed
:public sotRotationSimple
{
public: // protected
std::list< sotRotationSimple* > listRotationSimple;
std::list< sotRotationSimpleHouseholder > listHouseholder;
std::list< sotRotationSimpleGiven > listGivenRotation;
public:
sotRotationComposed( void )
:listRotationSimple(0),listHouseholder(0),listGivenRotation(0) {}
sotRotationComposed( const sotRotationComposed& clone )
:listRotationSimple(0),listHouseholder(0),listGivenRotation(0)
{
pushBack(clone);
}
virtual ~sotRotationComposed( void )
{ listRotationSimple.clear(); listGivenRotation.clear(); listHouseholder.clear();}
public:
template< typename TemplateRotation>
void pushBack( const TemplateRotation& R )
{ std::cerr << "Error: simple rotation not defined yet. " << std::endl;
throw "Error: simple rotation not defined yet. "; }
void pushBack( const sotRotationSimpleGiven & R )
{
listGivenRotation.push_back(R);
listRotationSimple.push_back(&listGivenRotation.back());
}
void pushBack( const sotRotationSimpleHouseholder & R )
{
listHouseholder.push_back(R);
listRotationSimple.push_back(&listHouseholder.back());
}
void pushBack( const sotRotationSimple * R )
{
const sotRotationSimpleGiven * GR = dynamic_cast<const sotRotationSimpleGiven *>(R);
if( GR ) pushBack(*GR);
else
{
const sotRotationSimpleHouseholder* HH = dynamic_cast<const sotRotationSimpleHouseholder *>(R);
if( HH ) pushBack(*HH);
else
{
std::cerr << "Error: this rotation cast not defined yet. " << std::endl;
throw "Error: simple rotation not defined yet. ";
}
}
}
void pushBack( const sotRotationComposed & R )
{
//sotDEBUG(15) << "PB size clone = " << R.listRotationSimple.size() << std::endl;
for( std::list<sotRotationSimple*>::const_iterator Pi = R.listRotationSimple.begin();
Pi!=R.listRotationSimple.end(); ++Pi )
{
const sotRotationSimple* R = *Pi;
const sotRotationSimpleHouseholder * Rh = dynamic_cast<const sotRotationSimpleHouseholder*>(R);
const sotRotationSimpleGiven * Rg = dynamic_cast<const sotRotationSimpleGiven*>(R);
if(NULL!=Rh) pushBack(*Rh);
else if(NULL!=Rg)
{
pushBack(*Rg);
//sotDEBUG(55) << "PB GR " << *Rg << std::endl;
}
else
{
std::cerr << "Error: this rotation PB not defined yet. " << std::endl;
throw "Error: simple rotation not defined yet. ";
}
}
}
void popBack( void )
{
sotRotationSimple * R = listRotationSimple.back();
listRotationSimple.pop_back();
if( dynamic_cast<sotRotationSimpleHouseholder*>(R) ) { listHouseholder.pop_back(); }
if( dynamic_cast<sotRotationSimpleGiven*>(R) ) { listGivenRotation.pop_back(); }
}
void clear( void )
{
listRotationSimple.clear();
listHouseholder.clear();
listGivenRotation.clear();
}
template< typename bubTemplateMatrix >
void householderQRinit( const bubTemplateMatrix & RQ,
const bubVector & betas,
const int nbVector=-1 )
{
const unsigned int nToProceed
=((nbVector<0)
?std::min(RQ.size1(),RQ.size2())
:(unsigned int)nbVector);
const unsigned int m=RQ.size1();
for( unsigned int i=0;i<nToProceed;++i )
{
bubVector v(m-i); v(0)=1; // TODO: optimize to save the nTP allocations of v.
for( unsigned int j=1;j<m-i;++j ) v(j)=RQ(j+i,i);
this->pushBack( sotRotationSimpleHouseholder(v,betas(i)) );
}
}
enum GivenRotationModifior { GR_FIRST,GR_SECOND,GR_BOTH };
void resetGRindex( const GivenRotationModifior modifior,
const unsigned int k1,const unsigned int k2=1 )
{
for( std::list<sotRotationSimpleGiven>::iterator Gi
= listGivenRotation.begin();
Gi!=listGivenRotation.end(); ++Gi )
{ switch(modifior)
{ case GR_FIRST: Gi->idx1=k1; break;
case GR_SECOND: Gi->idx2=k1; break;
case GR_BOTH: Gi->idx1=k1; Gi->idx2=k2;break;
}
}
}
void increaseGRindex( const GivenRotationModifior modifior,
const unsigned int k1,const unsigned int k2=1 )
{
for( std::list<sotRotationSimpleGiven>::iterator Gi
= listGivenRotation.begin();
Gi!=listGivenRotation.end(); ++Gi )
{ switch(modifior)
{ case GR_FIRST: Gi->idx1+=k1; break;
case GR_SECOND: Gi->idx2+=k1; break;
case GR_BOTH: Gi->idx1+=k1; Gi->idx2+=k2;break;
}
}
}
/* --- MULTIPLIERS --- */
SOT_ROTATION_SIMPLE_TEMPLATE_VECTOR_LIST(SOT_ROTATION_DERIVATED_MULTIPLY_VECTOR)
SOT_ROTATION_SIMPLE_TEMPLATE_MATRIX_LIST(SOT_ROTATION_DERIVATED_MULTIPLY_MATRIX)
template< typename bubTemplateVector >
void multiplyRightVectorTemplate( bubTemplateVector & v ) const // v <- U.v
{ multiplyRightTemplate(v); }
template< typename bubTemplateVector >
void multiplyLeftVectorTemplate( bubTemplateVector & v ) const // v <- U.v
{ multiplyLeftTemplate(v); }
template< typename bubTemplateMatrix >
void multiplyRightMatrixTemplate( bubTemplateMatrix & M ) const // M <- U.M
{ multiplyRightTemplate(M); }
template< typename bubTemplateMatrix >
void multiplyLeftMatrixTemplate( bubTemplateMatrix & M ) const // M <- U.M
{ multiplyLeftTemplate(M); }
template< typename bubTemplateMatrix >
void multiplyRightTransposeMatrixTemplate( bubTemplateMatrix & M ) const // M <- U.M
{ multiplyRightTransposeTemplate(M); }
template< typename bubTemplateMatrix >
void multiplyLeftTransposeMatrixTemplate( bubTemplateMatrix & M ) const // M <- U.M
{ multiplyLeftTransposeTemplate(M); }
template< typename bubTemplate >
void multiplyLeftTemplate( bubTemplate & Rx ) const // Rx <- Rx*U1*...*Un
{
for( std::list<sotRotationSimple*>::const_iterator Pi
= listRotationSimple.begin();
Pi!=listRotationSimple.end(); ++Pi )
{ (*Pi)->multiplyLeft(Rx); }
}
template< typename bubTemplate >
void multiplyRightTemplate( bubTemplate & Rx ) const // Rx <- U1*...*Un*Rx
{
for( std::list<sotRotationSimple*>::const_reverse_iterator Pi
= listRotationSimple.rbegin();
Pi!=listRotationSimple.rend(); ++Pi )
{ (*Pi)->multiplyRight(Rx); }
}
template< typename bubTemplate >
void multiplyLeftTransposeTemplate( bubTemplate & Rx ) const // Rx <- Rx*Un*...*U1
{
for( std::list<sotRotationSimple*>::const_reverse_iterator Pi
= listRotationSimple.rbegin();
Pi!=listRotationSimple.rend(); ++Pi )
{ (*Pi)->multiplyLeftTranspose(Rx); }
}
template< typename bubTemplate >
void multiplyRightTransposeTemplate( bubTemplate & Rx ) const // Rx <- Un*...*U1*Rx
{
for( std::list<sotRotationSimple*>::const_iterator Pi
= listRotationSimple.begin();
Pi!=listRotationSimple.end(); ++Pi )
{ (*Pi)->multiplyRightTranspose(Rx); }
}
/* --- DISPLAY --- */
virtual std::ostream& display( std::ostream& os ) const
{
for( std::list<sotRotationSimple*>::const_iterator Pi = listRotationSimple.begin();
Pi!=listRotationSimple.end(); ++Pi )
{
os << (**Pi) << " ";
}
if( listRotationSimple.empty() ) os << "Identity" << std::endl;
return os;
}
/* --- EXTRACTION --- */
template< typename bubTemplateMatrix >
void householderTrigonalisation( bubTemplateMatrix &R )
{
const unsigned int m=R.size1();
const unsigned int n=R.size2();
for( unsigned int i=0;i<std::min(m,n);++i )
{
bub::matrix_column<bubMatrix> m(R,i);
this->multiplyLeft(m);
bub::vector_range<bub::matrix_column<bubMatrix> > mdown(m,bub::range(i,5));
double beta;
R(i,i) = sotRotationSimpleHouseholder::householderExtraction(mdown,beta);
this->pushBack(sotRotationSimpleHouseholder(mdown,beta));
}
}
/* Rx has a shape like [Rr Rh], with Rr (full-rank)-triangular up, and
* Rh (generic-shape) rectangular.
* Regularize to [R0 Rh0], where column <xn> of Rh0 is null.
*/
template< typename bubTemplateMatrix >
void regularizeRankDeficientTriangle( bubTemplateMatrix & Rx, const unsigned int xn=0 )
{
const unsigned int n = Rx.size1();
for( unsigned int iter =1; iter<=n;iter++ )
{
//sotDEBUG(45) << "Rx"<<iter << " = " << (MATLAB)Rx << std::endl;
sotRotationSimpleGiven Un;
Un.nullifyFromRight(bub::row(Rx,n-iter),n-iter,n+xn);
Un.multiplyLeft(Rx);
//sotDEBUG(45) << "U"<<iter << " = " << Un << std::endl;
if(fabsf(Un.sinF)>1e-9)pushBack(Un);
}
}
template< typename bubTemplateMatrix >
void regularizeRankDeficientTriangle( bubTemplateMatrix & Rx,
bub::indirect_array<> orderCol,
const unsigned int xn )
{
sotDEBUG(1)<<"order="<<(MATLAB)orderCol<<std::endl;
sotDEBUG(45) << "Rx = " << (MATLAB)Rx << std::endl;
const unsigned int n = Rx.size1();
sotDEBUG(1)<<std::endl;
for( unsigned int iter =1; iter<=n;iter++ )
{
sotDEBUG(45) << "Rx"<<iter << " = " << (MATLAB)Rx << std::endl;
sotRotationSimpleGiven Un;
sotDEBUG(1)<<n<<std::endl;
sotDEBUG(1)<<iter<<std::endl;
sotDEBUG(1)<<n-iter<<std::endl;
sotDEBUG(1)<<(MATLAB)bub::row(Rx,n-iter)<<std::endl;
sotDEBUG(1)<<(MATLAB)bub::row(Rx,n-iter)
<<" ["<<orderCol(n-iter)<<","<<xn<<"]"<<std::endl;
/* Nullify line <n-iter> on column <xn> using the corresponding <n-iter>
* column in the ordered matrix (ie orderCol(n-iter) in the non-ordered
* matrix. */
Un.nullifyFromRight(bub::row(Rx,n-iter),orderCol(n-iter),xn);
sotDEBUG(1)<<std::endl;
if(fabsf(Un.sinF)>1e-9)
{
sotDEBUG(1)<<std::endl;
Un.multiplyLeft(Rx);
sotDEBUG(1)<<std::endl;
pushBack(Un);
sotDEBUG(1)<<std::endl;
}
sotDEBUG(45) << "U"<<iter << " = " << Un << std::endl;
}
}
};
/* ---------------------------------------------------------- */
/* ---------------------------------------------------------- */
/* ---------------------------------------------------------- */
class SOTSOTH_EXPORT sotRotationComposedInExtenso
:public sotRotationSimple
{
public: // protected
bubMatrix Q;
public:
sotRotationComposedInExtenso( const unsigned int nJ )
:Q(bub::identity_matrix<double>(nJ,nJ)) {}
sotRotationComposedInExtenso( const sotRotationComposedInExtenso& clone )
:Q(clone.Q)
{
}
virtual ~sotRotationComposedInExtenso( void )
{ }
public:
template< typename TemplateRotation>
void pushBack( const TemplateRotation& R )
{ // Q=U1*...*Un <- Q*Un+1
R.multiplyLeft(Q);
}
void pushBack( const sotRotationSimple * R )
{
R->multiplyLeft(Q);
}
void clear( const int newSize = -1 )
{
unsigned int nJ = Q.size1();
if( newSize>0 ) { nJ=(unsigned int)newSize; Q.resize(nJ,nJ); }
Q.assign( bub::identity_matrix<double>(nJ,nJ) );
}
unsigned int getSize( void ) { return Q.size1(); }
void resize( unsigned int nJ )
{
Q.resize(nJ,nJ);
Q.assign(bub::identity_matrix<double>(nJ,nJ));
}
template< typename bubTemplateMatrix >
void householderQRinit( const bubTemplateMatrix & RQ,
const bubVector & betas,
const int nbVector=-1 )
{
sotRotationComposed Qlist; Qlist.householderQRinit(RQ,betas,nbVector);
pushBack(Qlist);
}
/* --- MULTIPLIERS --- */
SOT_ROTATION_SIMPLE_TEMPLATE_VECTOR_LIST(SOT_ROTATION_DERIVATED_MULTIPLY_VECTOR)
SOT_ROTATION_SIMPLE_TEMPLATE_MATRIX_LIST(SOT_ROTATION_DERIVATED_MULTIPLY_MATRIX)
template< typename bubTemplateVector >
void multiplyRightVectorTemplate( bubTemplateVector & v ) const // v <- U.v
{
bubVector copy(v);
v.assign(bub::prod(Q,copy));
}
template< typename bubTemplateVector >
void multiplyLeftVectorTemplate( bubTemplateVector & v ) const // v <- U.v
{
bubVector copy(v);
//sotDEBUG(1)<<"Rxcopy = " << (MATLAB)copy<<std::endl;
//sotDEBUG(1)<<"Q = " << (MATLAB)Q<<std::endl;
v.assign(bub::prod(copy,Q));
//sotDEBUG(1)<<"Q'J' = JQ = " << (MATLAB)v<<std::endl;
}
template< typename bubTemplateMatrix >
void multiplyRightMatrixTemplate( bubTemplateMatrix & M ) const // M <- U.M
{
bubMatrix copy(M);
M.assign(bub::prod(Q,copy));
}
template< typename bubTemplateMatrix >
void multiplyLeftMatrixTemplate( bubTemplateMatrix & M ) const // M <- U.M
{
bubMatrix copy(M);
M.assign(bub::prod(copy,Q));
}
template< typename bubTemplateMatrix >
void multiplyRightTransposeMatrixTemplate( bubTemplateMatrix & M ) const // M <- U.M
{
bubMatrix copy(M);
M.assign(bub::prod(bub::trans(Q),copy));
}
template< typename bubTemplateMatrix >
void multiplyLeftTransposeMatrixTemplate( bubTemplateMatrix & M ) const // M <- U.M
{
bubMatrix copy(M);
M.assign(bub::prod(copy,bub::trans(Q)));
}
/* --- (LIMITED-)RANGE MULTIPLICATIONS. */
/* Vector multiplications */
/* Multiply M := Q*[0;I;0]*M. */
template<typename bubTemplateVectorIN,typename bubTemplateVectorOUT >
void multiplyRangeRight( const bubTemplateVectorIN& M,
bubTemplateVectorOUT& res,
const unsigned int zeroBefore,
const unsigned int zeroAfter ) const
{
const unsigned int nonZero = Q.size2()-zeroAfter-zeroBefore;
bub::matrix_range<bubMatrix>
Q0I0(Q,bub::range(0,Q.size1()),bub::range(zeroBefore,zeroBefore+nonZero));
res.resize( Q.size1() );
res.assign( bub::prod(Q0I0,M) );
}
/* Multiply M := Q*[0;I;0]*M. */
template<typename bubTemplateVector>
void multiplyRangeRight( bubTemplateVector& M,
const unsigned int zeroBefore,
const unsigned int zeroAfter ) const
{
const unsigned int nonZero = Q.size2()-zeroAfter-zeroBefore;
bub::project(M,bub::range(0,zeroBefore))
.assign(bub::zero_vector<double>(zeroBefore));
bub::project(M,bub::range(M.size()-zeroAfter,M.size()))
.assign(bub::zero_vector<double>(zeroAfter));
bubVector Min( bub::vector_range<bubTemplateVector>
( M,bub::range(zeroBefore,zeroBefore+nonZero)) );
bub::matrix_range<const bubMatrix>
Q0I0(Q,bub::range(0,Q.size1()),bub::range(zeroBefore,zeroBefore+nonZero));
//sotDEBUG(1) << "Qoio = " << (MATLAB)Q0I0 << std::endl;
//sotDEBUG(1) << "m = " << (MATLAB)Min << std::endl;
M.assign( bub::prod(Q0I0,Min));
}
/* Multiply M := M'*Q*[0;I;0]. */
template<typename bubTemplateVectorIN,typename bubTemplateVectorOUT>
void multiplyRangeLeft( const bubTemplateVectorIN& M,
bubTemplateVectorOUT& res,
const unsigned int zeroBefore,
const unsigned int zeroAfter ) const
{
const unsigned int nonZero = Q.size2()-zeroAfter-zeroBefore;
bub::matrix_range<bubMatrix>
Q0I0(Q,bub::range(0,Q.size1()),bub::range(zeroBefore,zeroBefore+nonZero));
res.assign( bub::prod(M,Q0I0));
}
/* Multiply M := [0;I;0]*Q'*M = M'*Q*[0 I 0]. */
template<typename bubTemplateVector>
bub::vector_range<bubTemplateVector>
multiplyRangeLeft( bubTemplateVector& m,
const unsigned int zeroBefore,
const unsigned int zeroAfter ) const
{
const unsigned int nonZero = Q.size2()-zeroAfter-zeroBefore;
const bub::matrix_range<const bubMatrix>
Q0I0(Q,bub::range(0,Q.size1()),
bub::range(zeroBefore,zeroBefore+nonZero));
bub::vector_range<bubTemplateVector> Mnonzero(m,bub::range(zeroBefore,
zeroBefore+nonZero));
bubVector Min(Mnonzero);
//sotDEBUG(55) << "Qoio = " << (MATLAB)Q0I0 << std::endl;
//sotDEBUG(55) << "m = " << (MATLAB)Mnonzero << std::endl;
Min.assign(bub::prod(m,Q0I0));
Mnonzero.assign(Min);
if(zeroBefore>0)
bub::project( m,bub::range(0,zeroBefore))
.assign(bub::zero_vector<double>(zeroBefore));
if(zeroAfter>0)
bub::project( m,bub::range(m.size()-zeroAfter,m.size()))
.assign(bub::zero_vector<double>(zeroAfter));
return Mnonzero;
}
/* --- DISPLAY --- */
virtual std::ostream& display( std::ostream& os ) const
{
return os<<Q;
}
};
#endif //ifndef __SOT_sotRotationSimple_HH__