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
Hide whitespace changes
Inline Side-by-side
Showing
with 0 additions and 3158 deletions
include/sot-core/binary-op.h deleted 100644 → 0
View file @ c7ed0e8e
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* 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 <sot-core/vector-quaternion.h>
/* STD */
#include <string>
#include <boost/function.hpp>
namespace sot {
namespace dg = dynamicgraph;
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
template< class Tin1,class Tin2,class Tout,typename Operator >
class BinaryOp
:public dg::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 )
: dg::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 --- */
dg::SignalPtr<Tin1,int> SIN1;
dg::SignalPtr<Tin2,int> SIN2;
dg::SignalTimeDependent<Tout,int> SOUT;
protected:
Tout& computeOperation( Tout& res,int time )
{
const Tin1 &x1 = SIN1(time);
const Tin2 &x2 = SIN2(time);
op(x1,x2,res);
return res;
}
public: /* --- PARAMS --- */
virtual void commandLine( const std::string& cmdLine,std::istringstream& cmdArgs,
std::ostream& os );
};
} // namespace sot
#endif // #ifndef __SOT_BINARYOP_H__
include/sot-core/debug.h deleted 100644 → 0
View file @ c7ed0e8e
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* 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)||(!sot::sotDEBUGFLOW.outputbuffer.good()) ) ;\
else sot::sotDEBUGFLOW.outputbuffer << sotPREDEBUG
# define sotDEBUGMUTE(level) if( (level>VP_DEBUG_MODE)||(!sot::sotDEBUGFLOW.outputbuffer.good()) ) ;\
else sot::sotDEBUGFLOW.outputbuffer
# define sotERROR if(!sot::sotDEBUGFLOW.outputbuffer.good()); else sot::sotERRORFLOW.outputbuffer << sotPREERROR
# define sotDEBUGF if(!sot::sotDEBUGFLOW.outputbuffer.good()); else sot::sotDEBUGFLOW.pre(sot::sotDEBUGFLOW.tmpbuffer<<sotPREDEBUG,VP_DEBUG_MODE).trace
# define sotERRORF if(!sot::sotDEBUGFLOW.outputbuffer.good()); else sot::sotERRORFLOW.pre(sot::sotERRORFLOW.tmpbuffer<<sotPREERROR).trace
// TEMPLATE
# define sotTDEBUG(level) if( (level>VP_TEMPLATE_DEBUG_MODE)||(!sot::sotDEBUGFLOW.outputbuffer.good()) ) ;\
else sot::sotDEBUGFLOW.outputbuffer << sotPREDEBUG
# define sotTDEBUGF if(!sot::sotDEBUGFLOW.outputbuffer.good()); else sot::sotDEBUGFLOW.pre(sot::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:
*/
include/sot-core/derivator.h deleted 100644 → 0
View file @ c7ed0e8e
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* 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 {
namespace dg = dynamicgraph;
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
template< class T >
class Derivator
:public dg::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;
Derivator( const std::string& name )
: dg::Entity(name)
,memory(),initialized(false)
,timestep(TIMESTEP_DEFAULT)
,SIN(NULL,"sotDerivator<"+getTypeName()+">("+name+")::input("+getTypeName()+")::in")
,SOUT( boost::bind(&Derivator<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 ~Derivator( void ) {};
public: /* --- SIGNAL --- */
dg::SignalPtr<T,int> SIN;
dg::SignalTimeDependent<T,int> SOUT;
dg::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__
include/sot-core/factory.h deleted 100644 → 0
View file @ c7ed0e8e
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* 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 FeatureAbstract;
class TaskAbstract;
/* --------------------------------------------------------------------- */
/* --- 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 Task.cpp for
an example.
*/
class SOT_CORE_EXPORT FactoryStorage
{
public:
typedef FeatureAbstract* (*FeatureConstructor_ptr)( const std::string& );
typedef TaskAbstract* (*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:
~FactoryStorage( void );
void registerTask( const std::string& entname,TaskConstructor_ptr ent );
TaskAbstract* 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 );
FeatureAbstract* 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 FactoryStorage 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,
FactoryStorage::FeatureConstructor_ptr maker);
};
#define SOT_FACTORY_FEATURE_PLUGIN(sotFeatureType,className) \
const std::string sotFeatureType::CLASS_NAME = className; \
extern "C" { \
FeatureAbstract *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,
FactoryStorage::TaskConstructor_ptr maker);
};
#define SOT_FACTORY_TASK_PLUGIN(sotTaskType,className) \
const std::string sotTaskType::CLASS_NAME = className; \
extern "C" { \
TaskAbstract *sotTaskMaker##_##sotTaskType( const std::string& objname ) \
{ \
return new sotTaskType( objname ); \
} \
sotTaskRegisterer reg##_##sotTaskType( className,&sotTaskMaker##_##sotTaskType ); \
} \
} // namespace sot
#endif /* #ifndef __SOT_FACTORY_HH__ */
include/sot-core/feature-abstract.h deleted 100644 → 0
View file @ c7ed0e8e
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* 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>
namespace sot {
namespace dg = dynamicgraph;
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/*! \class FeatureAbstract
\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 FeatureAbstract
:public dg::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. */
FeatureAbstract( const std::string& name );
/*! \brief Default destructor */
virtual ~FeatureAbstract( 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$ */
dg::SignalPtr< FeatureAbstract*,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$.*/
dg::SignalPtr< Flags,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$ */
dg::SignalTimeDependent<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$ */
dg::SignalTimeDependent<ml::Matrix,int> jacobianSOUT;
/*! \brief Compute the new value of the feature \f$ {\bf s}(t)\f$ */
dg::SignalTimeDependent<ml::Vector,int> activationSOUT;
/*! \brief Returns the dimension of the feature as an output signal. */
dg::SignalTimeDependent<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__
include/sot-core/feature-point6d.h deleted 100644 → 0
View file @ c7ed0e8e
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* 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 {
namespace dg = dynamicgraph;
/*!
\class FeaturePoint6d
\brief Class that defines point-3d control feature
*/
class SOTFEATUREPOINT6D_EXPORT FeaturePoint6d
: public FeatureAbstract
{
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:
dg::SignalPtr< MatrixHomogeneous,int > positionSIN;
dg::SignalPtr< ml::Matrix,int > articularJacobianSIN;
using FeatureAbstract::desiredValueSIN;
using FeatureAbstract::selectionSIN;
using FeatureAbstract::jacobianSOUT;
using FeatureAbstract::errorSOUT;
using FeatureAbstract::activationSOUT;
public:
FeaturePoint6d( const std::string& name );
virtual ~FeaturePoint6d( 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 Flags selectX( void ) { return FLAG_LINE_1; }
inline static Flags selectY( void ) { return FLAG_LINE_2; }
inline static Flags selectZ( void ) { return FLAG_LINE_3; }
inline static Flags selectRX( void ) { return FLAG_LINE_4; }
inline static Flags selectRY( void ) { return FLAG_LINE_5; }
inline static Flags selectRZ( void ) { return FLAG_LINE_6; }
inline static Flags selectTranslation( void ) { return Flags(7); }
inline static Flags selectRotation( void ) { return Flags(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:
*/
include/sot-core/fir-filter.h deleted 100644 → 0
View file @ c7ed0e8e
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* 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 dg = dynamicgraph;
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 FIRFilter
: public dg::Entity
{
public:
virtual const std::string& getClassName() const { return dg::Entity::getClassName(); }
static std::string getTypeName( void ) { return "Unknown"; }
static const std::string CLASS_NAME;
public:
FIRFilter( const std::string& name )
: dg::Entity(name)
, SIN(NULL,"sotFIRFilter("+name+")::input(T)::in")
, SOUT(boost::bind(&FIRFilter::compute,this,_1,_2),
SIN,
"sotFIRFilter("+name+")::output(T)::out")
{
signalRegistration( SIN<<SOUT );
}
virtual ~FIRFilter() {}
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 { dg::Entity::commandLine( cmdLine, cmdArgs, os); }
}
static void reset_signal(sigT& res, const sigT& sample) { }
public:
dg::SignalPtr<sigT, int> SIN;
dg::SignalTimeDependent<sigT, int> SOUT;
private:
std::vector<coefT> coefs;
detail::circular_buffer<sigT> data;
};
#endif
include/sot-core/flags.h deleted 100644 → 0
View file @ c7ed0e8e
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* 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 Flags
{
protected:
std::vector<char> flags;
bool reverse;
char operator[]( const unsigned int& i ) const;
public:
Flags( const bool& b=false ) ;
Flags( const char& c ) ;
Flags( const int& c4 ) ;
void add( const char& c ) ;
void add( const int& c4 ) ;
Flags operator! (void) const;
SOT_CORE_EXPORT friend Flags operator& ( const Flags& f1,const Flags& f2 ) ;
SOT_CORE_EXPORT friend Flags operator| ( const Flags& f1,const Flags& f2 ) ;
Flags& operator&= ( const Flags& f2 ) ;
Flags& operator|= ( const Flags& f2 ) ;
SOT_CORE_EXPORT friend Flags operator& ( const Flags& f1,const bool& b ) ;
SOT_CORE_EXPORT friend Flags operator| ( const Flags& f1,const bool& b ) ;
Flags& operator&= ( const bool& b );
Flags& operator|= ( const bool& b );
SOT_CORE_EXPORT friend std::ostream& operator<< (std::ostream& os, const Flags& fl );
SOT_CORE_EXPORT friend char operator>> (const Flags& flags, const int& i);
SOT_CORE_EXPORT friend std::istream& operator>> (std::istream& is, Flags& 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 Flags readIndexMatlab( std::istream & iss );
};
SOT_CORE_EXPORT extern const Flags FLAG_LINE_1;
SOT_CORE_EXPORT extern const Flags FLAG_LINE_2;
SOT_CORE_EXPORT extern const Flags FLAG_LINE_3;
SOT_CORE_EXPORT extern const Flags FLAG_LINE_4;
SOT_CORE_EXPORT extern const Flags FLAG_LINE_5;
SOT_CORE_EXPORT extern const Flags FLAG_LINE_6;
SOT_CORE_EXPORT extern const Flags FLAG_LINE_7;
SOT_CORE_EXPORT extern const Flags FLAG_LINE_8;
} // namespace sot
#endif /* #ifndef __SOT_FLAGS_H */
include/sot-core/import-default-paths.h.cmake deleted 100644 → 0
View file @ c7ed0e8e
// -*- c++ -*-
#ifndef SOT_FACTORY_COMMAND_IMPORT_DEFAULT_PATHS_H
# define SOT_FACTORY_COMMAND_IMPORT_DEFAULT_PATHS_H
/// Default script path as known by CMake at configure time.
# define SOT_IMPORT_DEFAULT_PATHS @SOT_IMPORT_DEFAULT_PATHS@
#endif //! SOT_FACTORY_COMMAND_IMPORT_DEFAULT_PATHS_H
include/sot-core/import.h deleted 100644 → 0
View file @ c7ed0e8e
// -*- c++ -*-
#ifndef SOT_FACTORY_COMMAND_IMPORT_H
# define SOT_FACTORY_COMMAND_IMPORT_H
# include <iosfwd>
# include <string>
# include <vector>
class sotInterpretor;
namespace sot
{
namespace command
{
namespace
{
extern std::vector<std::string> importPaths;
} // end of anonymous namespace.
/// \brief Implement sot interpretor import command.
///
/// The import command sources a file and searches automatically
/// for it in the importPaths.
void import (sotInterpretor& interpretor,
const std::string& cmdLine,
std::istringstream& cmdArg,
std::ostream& os);
/// \brief Implement sot interpretor pushImportPaths command.
///
/// Append a path to importPaths.
void pushImportPaths (sotInterpretor& interpretor,
const std::string& cmdLine,
std::istringstream& cmdArg,
std::ostream& os);
/// \brief Implement sot interpretor popImportPaths command.
///
/// Drop the last path of importPaths.
void popImportPaths (sotInterpretor& interpretor,
const std::string& cmdLine,
std::istringstream& cmdArg,
std::ostream& os);
} // end of namespace command.
} // end of namespace sot.
#endif //! SOT_FACTORY_COMMAND_IMPORT_H
include/sot-core/integrator-euler.h deleted 100644 → 0
View file @ c7ed0e8e
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* 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 {
namespace dg = dynamicgraph;
/*!
* \class IntegratorEuler
* \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 IntegratorEuler
: public IntegratorAbstract<sigT,coefT>
{
public:
virtual const std::string& getClassName( void ) const { return dg::Entity::getClassName(); }
static std::string getTypeName( void ) { return "Unknown"; }
static const std::string CLASS_NAME;
public:
using IntegratorAbstract<sigT,coefT>::SIN;
using IntegratorAbstract<sigT,coefT>::SOUT;
using IntegratorAbstract<sigT,coefT>::numerator;
using IntegratorAbstract<sigT,coefT>::denominator;
public:
IntegratorEuler( const std::string& name )
: IntegratorAbstract<sigT,coefT>( name )
{
SOUT.addDependency(SIN);
}
virtual ~IntegratorEuler( 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
include/sot-core/macros-signal.h deleted 100644 → 0
View file @ c7ed0e8e
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* 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
include/sot-core/matrix-constant.h deleted 100644 → 0
View file @ c7ed0e8e
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* 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 {
namespace dg = dynamicgraph;
class MatrixConstant
: public dg::Entity
{
public:
static const std::string CLASS_NAME;
virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
int rows,cols;
double color;
public:
MatrixConstant( const std::string& name )
:Entity( name )
,rows(0),cols(0),color(0.)
,SOUT( "sotMatrixConstant("+name+")::output(matrix)::out" )
{
SOUT.setDependencyType( dg::TimeDependency<int>::BOOL_DEPENDENT );
signalRegistration( SOUT );
}
virtual ~MatrixConstant( void ){}
dg::SignalTimeDependent<ml::Matrix,int> SOUT;
virtual void commandLine( const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os );
};
} // namespace sot
include/sot-core/matrix-force.h deleted 100644 → 0
View file @ c7ed0e8e
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* 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 MatrixHomogeneous;
class MatrixTwist;
class SOT_CORE_EXPORT MatrixForce
: public ml::Matrix
{
public:
MatrixForce( void ) : ml::Matrix(6,6) { setIdentity(); }
~MatrixForce( void ) { }
explicit MatrixForce( const MatrixHomogeneous& M )
: ml::Matrix(6,6)
{ buildFrom(M); }
MatrixForce& buildFrom( const MatrixHomogeneous& trans );
MatrixForce& operator=( const ml::Matrix& );
MatrixForce&
inverse( MatrixForce& invMatrix ) const ;
inline MatrixForce inverse( void ) const
{ MatrixForce Ainv; return inverse(Ainv); }
MatrixTwist& transpose( MatrixTwist& Vt ) const;
MatrixTwist transpose( void ) const;
};
} // namespace sot
#endif /* #ifndef __SOT_MATRIX_FORCE_H__ */
include/sot-core/matrix-homogeneous.h deleted 100644 → 0
View file @ c7ed0e8e
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* 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 MatrixRotation;
class SOT_CORE_EXPORT MatrixHomogeneous
: public ml::Matrix
{
public:
MatrixHomogeneous( void ) : ml::Matrix(4,4) { setIdentity(); }
~MatrixHomogeneous( void ) { }
MatrixHomogeneous& buildFrom( const MatrixRotation& rot, const ml::Vector& trans );
MatrixRotation& extract( MatrixRotation& rot ) const;
ml::Vector& extract( ml::Vector& trans ) const;
MatrixHomogeneous& operator=( const ml::Matrix& );
MatrixHomogeneous&
inverse( MatrixHomogeneous& invMatrix ) const ;
inline MatrixHomogeneous inverse( void ) const
{ MatrixHomogeneous 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__ */
include/sot-core/matrix-rotation.h deleted 100644 → 0
View file @ c7ed0e8e
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* 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 VectorUTheta;
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SOT_CORE_EXPORT MatrixRotation
: public ml::Matrix
{
public:
MatrixRotation( void ) : ml::Matrix(3,3) { setIdentity(); }
~MatrixRotation( void ) { }
void fromVector( VectorUTheta& );
MatrixRotation& operator= ( VectorUTheta&th ) { fromVector(th); return *this; }
};
} // namespace sot
#endif /* #ifndef __SOT_MATRIX_ROTATION_H__ */
include/sot-core/matrix-twist.h deleted 100644 → 0
View file @ c7ed0e8e
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* 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 MatrixHomogeneous;
class MatrixForce;
class SOT_CORE_EXPORT MatrixTwist
: public ml::Matrix
{
public:
MatrixTwist( void ) : ml::Matrix(6,6) { setIdentity(); }
~MatrixTwist( void ) { }
explicit MatrixTwist( const MatrixHomogeneous& M )
: ml::Matrix(6,6)
{ buildFrom(M); }
MatrixTwist& buildFrom( const MatrixHomogeneous& trans );
MatrixTwist& operator=( const ml::Matrix& );
MatrixTwist&
inverse( MatrixTwist& invMatrix ) const ;
inline MatrixTwist inverse( void ) const
{ MatrixTwist Ainv; return inverse(Ainv); }
MatrixForce& transpose( MatrixForce& Vt ) const;
MatrixForce transpose( void ) const;
};
} // namespace sot
#endif /* #ifndef __SOT_MATRIX_TWIST_H__ */
include/sot-core/memory-task-sot.h deleted 100644 → 0
View file @ c7ed0e8e
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* 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 {
namespace dg = dynamicgraph;
class SOTSOT_CORE_EXPORT MemoryTaskSOT
: public TaskAbstract::MemoryTaskAbstract, public dg::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. */
MemoryTaskSOT( 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 --- */
dg::Signal< ml::Matrix,int > jacobianInvSINOUT;
dg::Signal< ml::Matrix,int > jacobianConstrainedSINOUT;
dg::Signal< ml::Matrix,int > jacobianProjectedSINOUT;
dg::Signal< ml::Matrix,int > singularBaseImageSINOUT;
dg::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
include/sot-core/op-point-modifier.h deleted 100644 → 0
View file @ c7ed0e8e
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* 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 (OpPointModifier_EXPORTS)
# define SOTOPPOINTMODIFIER_EXPORT __declspec(dllexport)
# else
# define SOTOPPOINTMODIFIER_EXPORT __declspec(dllimport)
# endif
#else
# define SOTOPPOINTMODIFIER_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- VECTOR ---------------------------------------------------------- */
/* --------------------------------------------------------------------- */
namespace sot {
namespace dg = dynamicgraph;
class SOTOPPOINTMODIFIER_EXPORT OpPointModifier
: public dg::Entity
{
public:
static const std::string CLASS_NAME;
virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
MatrixHomogeneous transformation;
public:
dg::SignalPtr<ml::Matrix,int> jacobianSIN;
dg::SignalPtr<MatrixHomogeneous,int> positionSIN;
dg::SignalTimeDependent<ml::Matrix,int> jacobianSOUT;
dg::SignalTimeDependent<MatrixHomogeneous,int> positionSOUT;
public:
OpPointModifier( const std::string& name );
virtual ~OpPointModifier( void ){}
ml::Matrix& computeJacobian( ml::Matrix& res,const int& time );
MatrixHomogeneous& computePosition( MatrixHomogeneous& res,const int& time );
void setTransformation( const MatrixHomogeneous& tr );
virtual void commandLine( const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os );
};
} // namespace sot
#endif // __SOT_OP_POINT_MODIFIOR_H__
include/sot-core/rotation-simple.h deleted 100644 → 0
View file @ c7ed0e8e
#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 RotationSimple;
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 RotationSimple& 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 RotationSimple
{
public:
virtual ~RotationSimple( 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 RotationSimple& Q ) { return Q.display(os); }
};
/* ---------------------------------------------------------- */
/* ---------------------------------------------------------- */
/* ---------------------------------------------------------- */
class SOTSOTH_EXPORT sotRotationSimpleHouseholder
: public RotationSimple
{
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 RotationSimple
{
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 RotationSimple
{
public: // protected
std::list< RotationSimple* > 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 RotationSimple * 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<RotationSimple*>::const_iterator Pi = R.listRotationSimple.begin();
Pi!=R.listRotationSimple.end(); ++Pi )
{
const RotationSimple* 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 )
{
RotationSimple * 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<RotationSimple*>::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<RotationSimple*>::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<RotationSimple*>::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<RotationSimple*>::const_iterator Pi
= listRotationSimple.begin();
Pi!=listRotationSimple.end(); ++Pi )
{ (*Pi)->multiplyRightTranspose(Rx); }
}
/* --- DISPLAY --- */
virtual std::ostream& display( std::ostream& os ) const
{
for( std::list<RotationSimple*>::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 RotationSimple
{
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 RotationSimple * 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__