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 409 additions and 2685 deletions
include/sot-core/robot-simu.h deleted 100644 → 0
View file @ f4f51423
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Copyright Projet JRL-Japan, 2007
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*
* File: RobotSimu.h
* Project: SOT
* Author: Nicolas Mansard
*
* Version control
* ===============
*
* $Id$
*
* Description
* ============
*
*
* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
#ifndef __SOT_ROBOT_SIMU_HH
#define __SOT_ROBOT_SIMU_HH
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* -- MaaL --- */
#include <MatrixAbstractLayer/boost.h>
namespace ml= maal::boost;
/* SOT */
#include <dynamic-graph/entity.h>
#include <dynamic-graph/all-signals.h>
#include <sot-core/vector-roll-pitch-yaw.h>
#include <sot-core/periodic-call.h>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (robot_simu_EXPORTS)
# define SOTROBOTSIMU_EXPORT __declspec(dllexport)
# else
# define SOTROBOTSIMU_EXPORT __declspec(dllimport)
# endif
#else
# define SOTROBOTSIMU_EXPORT
#endif
namespace sot {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SOTROBOTSIMU_EXPORT RobotSimu
:public dynamicgraph::Entity
{
public:
static const std::string CLASS_NAME;
virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
enum ForceSignalSource
{
FORCE_SIGNAL_RLEG,
FORCE_SIGNAL_LLEG,
FORCE_SIGNAL_RARM,
FORCE_SIGNAL_LARM
};
protected:
ml::Vector state;
PeriodicCall periodicCallBefore;
PeriodicCall periodicCallAfter;
bool withForceSignals[4];
public:
/* --- CONSTRUCTION --- */
RobotSimu( const std::string& name );
void setStateSize( const unsigned int size );
void setState( const ml::Vector st );
void increment( const double dt = 5e-2 );
public: /* --- DISPLAY --- */
virtual void display( std::ostream& os ) const;
SOTROBOTSIMU_EXPORT friend std::ostream& operator<< ( std::ostream& os,const RobotSimu& r )
{ r.display(os); return os;}
public: /* --- SIGNALS --- */
dynamicgraph::SignalPtr<ml::Vector,int> controlSIN;
//dynamicgraph::SignalPtr<MatrixRotation,int> attitudeSIN;
dynamicgraph::SignalPtr<ml::Vector,int> attitudeSIN;
dynamicgraph::SignalPtr<ml::Vector,int> zmpSIN;
dynamicgraph::Signal<ml::Vector,int> stateSOUT;
dynamicgraph::Signal<MatrixRotation,int> attitudeSOUT;
dynamicgraph::Signal<ml::Vector,int>* forcesSOUT[4];
dynamicgraph::Signal<ml::Vector,int> pseudoTorqueSOUT;
dynamicgraph::Signal<ml::Vector,int> previousControlSOUT;
/*! \brief The current state of the robot from the command viewpoint. */
dynamicgraph::Signal<ml::Vector,int> motorcontrolSOUT;
/*! \brief The ZMP reference send by the previous controller. */
dynamicgraph::Signal<ml::Vector,int> ZMPPreviousControllerSOUT;
public: /* --- COMMANDS --- */
virtual void commandLine( const std::string& cmdLine,std::istringstream& cmdArgs,
std::ostream& os );
};
} // namespace sot
#endif /* #ifndef __SOT_ROBOT_SIMU_HH */
include/sot-core/rotation-simple.h deleted 100644 → 0
View file @ f4f51423
#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 <sot-core/sot-core-api.h>
#include <list>
/* --------------------------------------------------------------------- */
/* --- 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 SOT_CORE_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 SOT_CORE_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 SOT_CORE_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 SOT_CORE_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 SOT_CORE_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 SOT_CORE_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__
include/sot-core/signal-cast.h deleted 100644 → 0
View file @ f4f51423
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Copyright Projet JRL-Japan, 2007
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*
* File: SignalCast.h
* Project: SOT
* Author: Nicolas Mansard
*
* Version control
* ===============
*
* $Id$
*
* Description
* ============
*
*
* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
#ifndef __SOT_SIGNAL_CAST_HH__
#define __SOT_SIGNAL_CAST_HH__
#include <sot-core/flags.h>
#include <MatrixAbstractLayer/boost.h>
#include <sot-core/matrix-homogeneous.h>
#include <sot-core/matrix-twist.h>
#include <sot-core/vector-utheta.h>
#include <sot-core/vector-quaternion.h>
#include <sot-core/vector-roll-pitch-yaw.h>
#include <sot-core/matrix-force.h>
#include <sot-core/multi-bound.h>
#include <sot-core/sot-core-api.h>
#ifdef WIN32
#include <sot-core/utils-windows.h>
#endif
#include <dynamic-graph/signal-caster.h>
/* -------------------------------------------------------------------------- */
/* --- CLASS ---------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
namespace sot {
class FeatureAbstract;
/*!
* This class is only used to group together static functions who differ by
* a template parameter. It is never actually instanced (the private constructor
* makes sure of that).
*/
template< class T >
class SignalCast
{
public:
SOT_CORE_EXPORT static T cast( std::istringstream& stringValue ) { throw 1;}
SOT_CORE_EXPORT static void disp( const T& t,std::ostream& os ) { throw 1; }
SOT_CORE_EXPORT static void trace( const T& t,std::ostream& os ) { disp(t,os); }
public:
// adapter functions for SignalCast
static boost::any cast_( std::istringstream& stringValue ) {
return boost::any_cast<T>(cast(stringValue));
}
static void disp_( const boost::any& t,std::ostream& os ) {
disp(boost::any_cast<T>(t), os);
}
static void trace_( const boost::any& t,std::ostream& os ) {
trace(boost::any_cast<T>(t),os);
}
private:
SignalCast() {}
};
/* -------------------------------------------------------------------------- */
/* --- MACROS --------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
/* Declaration macro: one instance of each class needs to be present in
* order for casts to be registered.
*/
#define SOT_SIGNAL_CAST_DECLARATION(TYPE) \
dynamicgraph::SignalCastRegisterer sotCastRegisterer_##TYPE \
(typeid(TYPE), \
SignalCast<TYPE>::disp_, \
SignalCast<TYPE>::cast_, \
SignalCast<TYPE>::trace_);
#define SOT_SIGNAL_CAST_DECLARATION_NAMED(TYPE,NAME) \
dynamicgraph::SignalCastRegisterer sotCastRegisterer_##NAME \
(typeid(TYPE), \
SignalCast<TYPE>::disp_, \
SignalCast<TYPE>::cast_, \
SignalCast<TYPE>::trace_);
/* Standard definition macros: the three functions can be specified
* in the macros. To define then in the cpp, just put ';' in the args.
*/
#define SOT_SIGNAL_CAST_FULL_DEFINITION(TYPE,CAST,DISP,TRACE) \
template<> \
class SignalCast<TYPE> \
{ \
public: \
SOT_CORE_EXPORT static TYPE cast( std::istringstream& iss ) CAST \
SOT_CORE_EXPORT static void disp( const TYPE& t,std::ostream& os ) DISP \
SOT_CORE_EXPORT static void trace( const TYPE& t,std::ostream& os ) TRACE \
public: \
static boost::any cast_( std::istringstream& stringValue ) { \
return boost::any_cast<TYPE>(cast(stringValue)); \
} \
static void disp_( const boost::any& t,std::ostream& os ) { \
disp(boost::any_cast<TYPE>(t), os); \
} \
static void trace_( const boost::any& t,std::ostream& os ) { \
trace(boost::any_cast<TYPE>(t),os); \
} \
}
/* Standard definition macros: the functions <cast> and <disp> have
* to be implemented in the cpp files. The function <trace> is
* implemented as a proxy on <disp>.
*/
#define SOT_SIGNAL_CAST_DEFINITION_HPP(TYPE) \
SOT_SIGNAL_CAST_FULL_DEFINITION(TYPE,;,;,{ disp(t,os); })
/* Lazy definition: <cast> and <disp> are to proxys on the standard
* std input (>>) and output (<<). The function <trace> has to be
* implemented in the cpp.
*/
#define SOT_SIGNAL_CAST_DEFINITION_TRACE_HPP(TYPE,TRACE) \
SOT_SIGNAL_CAST_FULL_DEFINITION(TYPE, \
{TYPE res; iss >> res; return res; }, \
{ os << t <<std::endl; }, \
TRACE )
/* Lazy lazy definition: the three functions are implemented as
* proxys on std::io operation.
*/
#define SOT_SIGNAL_CAST_DEFINITION(TYPE) \
SOT_SIGNAL_CAST_FULL_DEFINITION(TYPE, \
{TYPE res; iss >> res; return res; }, \
{ os << t <<std::endl; }, \
{ disp(t,os); })
/* Lazy definition of <cast> and <disp> with implementation of
* <trace> in the cpp.
*/
#define SOT_SIGNAL_CAST_DEFINITION_TRACE(TYPE) \
SOT_SIGNAL_CAST_FULL_DEFINITION(TYPE, \
{TYPE res; iss >> res; return res; }, \
{ os << t <<std::endl; }, \
;)
/* -------------------------------------------------------------------------- */
/* --- MACROS --------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
/* --- OTHER --- */
SOT_SIGNAL_CAST_DEFINITION(Flags);
SOT_SIGNAL_CAST_DEFINITION_TRACE(VectorMultiBound);
typedef FeatureAbstract* SignalCast_sotFeatureAbstractPtr ;
SOT_SIGNAL_CAST_DEFINITION_HPP( SignalCast_sotFeatureAbstractPtr );
SOT_SIGNAL_CAST_DEFINITION_HPP( struct timeval );
/* --- Matrices and Vectors --- */
#define SOT_SIGNAL_CAST_DEFINITION_MATRIX(TYPE) \
SOT_SIGNAL_CAST_FULL_DEFINITION(TYPE,{TYPE res; iss >> res; return res; }, \
{ SignalCast<ml::Matrix>::disp(t,os); }, \
{ SignalCast<ml::Matrix>::trace(t,os); })
//SOT_SIGNAL_CAST_DEFINITION_TRACE_HPP(TYPE,{ SignalCast<ml::Matrix>::trace(t,os); })
#define SOT_SIGNAL_CAST_DEFINITION_VECTOR(TYPE) \
SOT_SIGNAL_CAST_FULL_DEFINITION(TYPE,{TYPE res; iss >> res; return res; }, \
{ SignalCast<ml::Vector>::disp(t,os); }, \
{ SignalCast<ml::Vector>::trace(t,os); })
//SOT_SIGNAL_CAST_DEFINITION_TRACE_HPP(TYPE,{ SignalCast<ml::Vector>::trace(t,os); })
SOT_SIGNAL_CAST_FULL_DEFINITION(maal::boost::Vector,; SOT_CORE_EXPORT static maal::boost::DisplayType displayType;,;,;);
SOT_SIGNAL_CAST_FULL_DEFINITION(maal::boost::Matrix,;,;,;);
/* All the followings are defined with proxys on the equivalent
* functions ml:: based.
*/
SOT_SIGNAL_CAST_DEFINITION_VECTOR(VectorUTheta);
SOT_SIGNAL_CAST_DEFINITION_VECTOR(VectorQuaternion);
SOT_SIGNAL_CAST_DEFINITION_VECTOR(VectorRollPitchYaw);
SOT_SIGNAL_CAST_DEFINITION_MATRIX(MatrixRotation);
SOT_SIGNAL_CAST_DEFINITION_MATRIX(MatrixHomogeneous);
SOT_SIGNAL_CAST_DEFINITION_MATRIX(MatrixTwist);
SOT_SIGNAL_CAST_DEFINITION_MATRIX(MatrixForce);
} // namespace sot
#endif // #ifndef __SOT_SIGNAL_CAST_HH__
include/sot-core/solver-hierarchical-inequalities.h deleted 100644 → 0
View file @ f4f51423
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Copyright Projet Gepetto, LAAS, 2009
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*
* File: solver-hierarchical-inequalities.h
* Project: SOT
* Author: Nicolas Mansard
*
* Version control
* ===============
*
* $Id$
*
* Description
* ============
*
*
* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
#ifndef __SOT_sotSolverHierarchicalInequalities_HH__
#define __SOT_sotSolverHierarchicalInequalities_HH__
/* --- STD --- */
#include <iostream>
#include <sstream>
#include <list>
#include <vector>
#include <deque>
#include <typeinfo>
#include <algorithm>
#include <sot-core/rotation-simple.h>
#include <sot-core/sot-core-api.h>
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
namespace sot {
class SOT_CORE_EXPORT ConstraintMem
{
public:
enum BoundSideType
{
BOUND_VOID = 0,
BOUND_INF = 1,
BOUND_SUP = 2,
BOUND_BOTH = 3
};
typedef std::vector<ConstraintMem::BoundSideType> BoundSideVector;
public:
bool active; // If the constraint is active.
bool equality; // If the constraint cannot be inactivate.
bool notToBeConsidered; // (during warm start) if constraint is unactive and not to be activated.
bubVector Ji; // The row of the jacobian.
double eiInf,eiSup; // The row of the error reference.
BoundSideType boundSide,activeSide;
bool rankIncreaser; // If the constraint is non redundant wrt to R. */
unsigned int constraintRow; // Num of the row where it is located in Ji.
unsigned int range; // Num of the col where it has been added in F. Used in H, not in S.
double lagrangian; // last value of the lagrangian -> negatif: should be unactivated.
double Ju,Jdu; // effet of the control law in the task space.
public:
ConstraintMem( void )
:active(false),equality(0),notToBeConsidered(0)
,Ji(0),eiInf(0),eiSup(0),boundSide(BOUND_VOID),activeSide(BOUND_VOID)
,rankIncreaser(false),constraintRow(0),range(0),lagrangian(0)
,Ju(0),Jdu(0) {}
ConstraintMem( const ConstraintMem& clone );
SOT_CORE_EXPORT friend std::ostream& operator<<( std::ostream& os,const BoundSideType& bs );
SOT_CORE_EXPORT friend std::ostream & operator<< (std::ostream& os,const ConstraintMem &c );
};
/* ---------------------------------------------------------- */
/* ---------------------------------------------------------- */
/* ---------------------------------------------------------- */
class SOT_CORE_EXPORT ConstraintRef
{
public:
unsigned int id;
ConstraintMem::BoundSideType side;
ConstraintRef( const unsigned int id_ = 0,
const ConstraintMem::BoundSideType side_
=ConstraintMem::BOUND_VOID )
:id(id_),side(side_) {}
SOT_CORE_EXPORT friend std::ostream & operator<< (std::ostream & os,
const ConstraintRef & cr )
{ return os << cr.side << cr.id; }
};
/* ---------------------------------------------------------- */
/* ---------------------------------------------------------- */
/* ---------------------------------------------------------- */
class SOT_CORE_EXPORT SolverHierarchicalInequalities
{
public: // protected:
typedef bub::matrix<double,bub::column_major> bubMatrixQRWide;
typedef bub::matrix_range<bubMatrixQRWide> bubMatrixQR;
typedef bub::matrix_range<const bubMatrixQRWide> bubMatrixQRConst;
typedef bub::triangular_adaptor<bubMatrixQR,bub::upper> bubMatrixQRTri;
typedef bub::triangular_adaptor<bubMatrixQRConst,bub::upper> bubMatrixQRTriConst;
typedef bub::indirect_array<> bubOrder;
typedef bub::matrix_indirect<bubMatrix> bubMatrixOrdered;
typedef bub::matrix_indirect<bubMatrixQRWide> bubMatrixQRWideOrdered;
typedef bub::matrix_indirect<bubMatrixQR> bubMatrixQROrdered;
typedef bub::triangular_adaptor<bubMatrixQROrdered,bub::upper> bubMatrixQROrderedTri;
typedef bub::matrix_indirect<const bubMatrixQRWide> bubMatrixQRWideOrderedConst;
typedef bub::matrix_indirect<bubMatrixQRConst> bubMatrixQROrderedConst;
typedef bub::triangular_adaptor<bubMatrixQROrderedConst,bub::upper> bubMatrixQROrderedTriConst;
typedef std::vector<ConstraintMem> ConstraintList;
typedef std::vector<ConstraintMem*> ConstraintRefList;
static double THRESHOLD_ZERO;
public: // protected:
unsigned int nJ;
bubVector u0;
/* --- H data --- */
sotRotationComposedInExtenso& Qh;
bubMatrix& Rh;
unsigned int rankh,freeRank;
ConstraintList& constraintH;
bool Hactivation,Hinactivation;
unsigned int HactivationRef,HinactivationRef;
ConstraintMem::BoundSideType HactivationSide;
/* --- S data --- */
bubMatrixQRWide QhJsU,QhJs;
unsigned int ranks,sizes;
bubOrder orderS;
ConstraintList constraintS;
ConstraintRefList constraintSactive;
bool Sactivation,Sinactivation;
unsigned int SactivationRef,SinactivationRef;
ConstraintMem::BoundSideType SactivationSide;
/* --- Next step --- */
double SactivationScore,SinactivationScore,HactivationScore,HinactivationScore;
/* --- divers --- */
bubVector du,slackInf,slackSup,lagrangian;
sotRotationComposed lastRotation;
int freeRankChange;
/* --- Warm start knowledge --- */
enum ActivationTodoType { TODO_NOTHING, TODO_ACTIVATE, TODO_INACTIVATE };
std::vector<bool> initialActiveH;
ConstraintMem::BoundSideVector initialSideH;
bubVector du0;
std::vector<ConstraintRef> slackActiveSet;
std::vector<ConstraintRef> toActivate,toInactivate;
bool warmStartReady;
/* ---------------------------------------------------------- */
public:
SolverHierarchicalInequalities( unsigned int _nJ,
sotRotationComposedInExtenso& _Qh,
bubMatrix &_Rh,
ConstraintList &_cH )
:nJ(_nJ),u0(_nJ),Qh(_Qh),Rh(_Rh),rankh(0),freeRank(_nJ),constraintH(_cH)
,ranks(0),warmStartReady(false)
{
u0*=0;
}
private:
SolverHierarchicalInequalities( const SolverHierarchicalInequalities& clone )
: nJ(clone.nJ),Qh(clone.Qh),Rh(clone.Rh),constraintH(clone.constraintH){ /* forbiden */ }
public:
/* ---------------------------------------------------------- */
void initConstraintSize( const unsigned int size );
void setInitialCondition( const bubVector& _u0,
const unsigned int _rankh );
void setInitialConditionVoid( void );
void setNbDof( const unsigned int nJ );
unsigned int getNbDof( void ) { return nJ; }
/* ---------------------------------------------------------- */
void recordInitialConditions( void );
void computeDifferentialCondition( void );
const std::vector<ConstraintRef> & getToActivateList( void ) const
{ return toActivate; }
const std::vector<ConstraintRef> & getToInactivateList( void ) const
{ return toInactivate; }
const bubVector & getDifferentialU0( void ) const
{ return du0; }
const std::vector<ConstraintRef>& getSlackActiveSet( void ) const
{ return slackActiveSet; }
void printDifferentialCondition( std::ostream & os ) const;
/* ---------------------------------------------------------- */
bub::range fullrange( void ) const { return bub::range(0,nJ); }
bub::range rangeh( void ) const { return bub::range(0,rankh); }
bub::range rangehs( void ) const { return bub::range(0,rankh+ranks); }
bub::range freerange( void ) const { return bub::range(rankh,nJ); }
bub::range freeranges( void ) const { return bub::range(rankh,rankh+ranks); }
/* bubMatrixQROrdered accessQRs( void ); */
/* bubMatrixQROrderedConst accessQRs( void ) const; */
/* bubMatrixQROrderedTri accessRs( void ); */
bubMatrixQROrderedTriConst accessRsConst( void ) const;
bub::triangular_adaptor<bub::matrix_range< const bubMatrix >,bub::upper>
accessRhConst( void ) const;
bub::triangular_adaptor<bub::matrix_range< bubMatrix >,bub::upper>
accessRh( void );
template< typename bubTemplateMatrix >
unsigned int rankDetermination( const bubTemplateMatrix& A,
const double threshold = THRESHOLD_ZERO );
/* ---------------------------------------------------------- */
static void displayConstraint( ConstraintList & cs );
void printDebug( void );
/* ---------------------------------------------------------- */
void warmStart( void );
void applyFreeSpaceMotion( const bubVector& _du );
void forceUpdateHierachic( ConstraintRefList& toUpdate,
const ConstraintMem::BoundSideVector& boundSide );
void forceDowndateHierachic( ConstraintRefList& toDowndate );
/* ---------------------------------------------------------- */
void solve( const bubMatrix& Jse, const bubVector& ese,
const bubMatrix& Jsi, const bubVector& esiInf, const bubVector& esiSup,
const std::vector<ConstraintMem::BoundSideType> esiBoundSide,
bool pushBackAtTheEnd = true );
void solve( const bubMatrix& Jse, const bubVector& ese,
const bubMatrix& Jsi, const bubVector& esiInf, const bubVector& esiSup,
const ConstraintMem::BoundSideVector & esiBoundSide,
const std::vector<ConstraintRef> & slackActiveWarmStart,
bool pushBackAtTheEnd = true );
/* ---------------------------------------------------------- */
void initializeConstraintMemory( const bubMatrix& Jse, const bubVector& ese,
const bubMatrix& Jsi, const bubVector& esiInf,
const bubVector& esiSup,
const ConstraintMem::BoundSideVector& esiBoundSide,
const std::vector<ConstraintRef>& warmStartSide );
void initializeDecompositionSlack(void);
/* ---------------------------------------------------------- */
void updateConstraintHierarchic( const unsigned int constraintId,
const ConstraintMem::BoundSideType side );
void downdateConstraintHierarchic( const unsigned int kdown );
void updateRankOneDowndate( void );
void updateRankOneUpdate( void );
/* ---------------------------------------------------------- */
void updateConstraintSlack( const unsigned int kup,
const ConstraintMem::BoundSideType activeSide );
void regularizeQhJs( void );
void regularizeQhJsU( void );
void downdateConstraintSlack( const unsigned int kdown );
/* ---------------------------------------------------------- */
void computeGradient( bubVector& gradientWide );
void computePrimal( void );
void computeSlack( void );
void computeLagrangian( void );
bool selecActivationHierarchic( double & tau );
bool selecInactivationHierarchic( void );
bool selecActivationSlack( void );
bool selecInactivationSlack( void );
void pushBackSlackToHierarchy( void );
};
} // namespace sot
#endif // #ifdef __SOT_sotSolverHierarchicalInequalities_HH__
include/sot-core/sot-core-api.h deleted 100644 → 0
View file @ f4f51423
/*
* Copyright
*/
#ifndef SOT_CORE_API_HH
#define SOT_CORE_API_HH
#if defined (WIN32)
# ifdef sot_core_EXPORTS
# define SOT_CORE_EXPORT __declspec(dllexport)
# else
# define SOT_CORE_EXPORT __declspec(dllimport)
# endif
#else
# define SOT_CORE_EXPORT
#endif
#endif
include/sot-core/sot-h.h deleted 100644 → 0
View file @ f4f51423
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Copyright Projet Gepetto, LAAS, 2009
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*
* File: sot-h.h
* Project: SOT
* Author: Nicolas Mansard
*
* Version control
* ===============
*
* $Id$
*
* Description
* ============
*
*
* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
#ifndef __SOT_sotSOTH_H__
#define __SOT_sotSOTH_H__
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* Matrix */
#include <sot-core/sot.h>
#include <sot-core/solver-hierarchical-inequalities.h>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#ifndef SOTSOTH_EXPORT
# if defined (WIN32)
# if defined (sot_h_EXPORTS)
# define SOTSOTH_EXPORT __declspec(dllexport)
# else
# define SOTSOTH_EXPORT __declspec(dllimport)
# endif
# else
# define SOTSOTH_EXPORT
# endif
#endif
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
namespace sot {
namespace dg = dynamicgraph;
class SOTSOTH_EXPORT SotH
:public Sot
{
public:
/*! \brief Specify the name of the class entity. */
static const std::string CLASS_NAME;
/*! \brief Returns the name of this class. */
virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
/* --- SPECIFIC MEM -------------------------------------------------- */
class MemoryTaskSOTH
: public TaskAbstract::MemoryTaskAbstract,public Entity
{
public:
const SotH * referenceKey;
SolverHierarchicalInequalities solver;
ml::Matrix JK,Jff,Jact;
public:
MemoryTaskSOTH( const std::string & name,
const SotH * ref,
unsigned int nJ,
sotRotationComposedInExtenso& Qh,
bubMatrix &Rh,
SolverHierarchicalInequalities::ConstraintList &cH );
public: // Entity heritage
static const std::string CLASS_NAME;
virtual void display( std::ostream& os ) const;
virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
dg::Signal< ml::Matrix,int > jacobianConstrainedSINOUT;
dg::Signal< ml::Vector,int > diffErrorSINOUT;
virtual void commandLine( const std::string& cmdLine,std::istringstream& cmdArgs,
std::ostream& os );
};
/* --- \ SPECIFIC MEM ------------------------------------------------ */
protected:
//typedef std::vector<SolverHierarchicalInequalities *> SolversList;
//SolversList solvers;
sotRotationComposedInExtenso Qh;
bubMatrix Rh;
SolverHierarchicalInequalities::ConstraintList constraintH;
SolverHierarchicalInequalities solverNorm;
SolverHierarchicalInequalities * solverPrec;
bool fillMemorySignal;
public:
/*! \brief Default constructor */
SotH( const std::string& name );
~SotH( void );
public: /* --- CONTROL --- */
/*! \name Methods to compute the control law following the
recursive definition of the stack of tasks.
@{
*/
/*! \brief Compute the control law. */
virtual ml::Vector& computeControlLaw( ml::Vector& control,const int& time );
/*! @} */
public: /* --- DISPLAY --- */
public: /* --- COMMANDS --- */
virtual void commandLine( const std::string& cmdLine,std::istringstream& cmdArgs,
std::ostream& os );
virtual void defineNbDof( const unsigned int& nbDof );
};
} // namespace sot
#endif // #ifndef __SOT_sotSOTH_H__
include/sot-core/sot.h deleted 100644 → 0
View file @ f4f51423
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Copyright Projet JRL-Japan, 2007
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*
* File: sotSot.h
* Project: SOT
* Author: Nicolas Mansard
*
* Version control
* ===============
*
* $Id$
*
* Description
* ============
*
*
* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
#ifndef __SOT_SOT_HH
#define __SOT_SOT_HH
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* Matrix */
#include <MatrixAbstractLayer/boost.h>
namespace ml = maal::boost;
/* Classes standards. */
#include <list> /* Classe std::list */
/* SOT */
#include <sot-core/task-abstract.h>
#include <sot-core/flags.h>
#include <dynamic-graph/entity.h>
#include <sot-core/constraint.h>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#ifndef SOTSOT_CORE_EXPORT
# if defined (WIN32)
# if defined (sot_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;
/*! @ingroup stackoftasks
\brief This class implements the Stack of Task.
It allows to deal with the priority of the controllers
through the shell. The controllers can be either constraints
either tasks.
*/
class SOTSOT_CORE_EXPORT Sot
:public dg::Entity
{
public:
/*! \brief Specify the name of the class entity. */
static const std::string CLASS_NAME;
public:
/*! \brief Returns the name of this class. */
virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
protected:
/*! \brief Defines a type for a list of tasks */
typedef std::list<TaskAbstract*> StackType;
/*! \brief This field is a list of controllers
managed by the stack of tasks. */
StackType stack;
/*! \brief Defines a type for a list of constraints */
typedef std::list<Constraint*> ConstraintListType;
/*! \brief This field is a list of constraints
managed by the stack of tasks. */
ConstraintListType constraintList;
/*! \brief Defines an interval in the state vector of the robot
which is the free flyer. */
unsigned int ffJointIdFirst,ffJointIdLast;
/*! \brief Defines a default joint. */
static const unsigned int FF_JOINT_ID_DEFAULT = 0;
/* double directionalThreshold; */
/* bool useContiInverse; */
/*! \brief Store the number of joints to be used in the
command computed by the stack of tasks. */
unsigned int nbJoints;
/*! \brief Store a pointer to compute the gradient */
TaskAbstract* taskGradient;
/*! Projection used to compute the control law. */
ml::Matrix Proj;
/*! Force the recomputation at each step. */
bool recomputeEachTime;
public:
/*! \brief Threshold to compute the dumped pseudo inverse. */
static const double INVERSION_THRESHOLD_DEFAULT; // = 1e-4;
/* static const double DIRECTIONAL_THRESHOLD_DEFAULT = 1e-2; */
/* static const bool USE_CONTI_INVERSE_DEFAULT = false; */
/*! \brief Number of joints by default. */
static const unsigned int NB_JOINTS_DEFAULT; // = 48;
static ml::Matrix & computeJacobianConstrained( const ml::Matrix& Jac,
const ml::Matrix& K,
ml::Matrix& JK,
ml::Matrix& Jff,
ml::Matrix& Jact );
static ml::Matrix & computeJacobianConstrained( const TaskAbstract& task,
const ml::Matrix& K );
static ml::Vector taskVectorToMlVector( const VectorMultiBound& taskVector );
public:
/*! \brief Default constructor */
Sot( const std::string& name );
~Sot( void ) { /* TODO!! */ }
/*! \name Methods to handle the stack.
@{
*/
/*! \brief Push the task in the stack.
It has a lowest priority than the previous ones.
If this is the first task, then it has the highest
priority. */
virtual void push( TaskAbstract& task );
/*! \brief Pop the task from the stack.
This method removes the task with the smallest
priority in the task. The other are projected
in the null-space of their predecessors. */
virtual TaskAbstract& pop( void );
/*! \brief This method allows to know if a task exists or not */
virtual bool exist( const TaskAbstract& task );
/*! \brief Remove a task regardless to its position in the stack.
It removes also the signals connected to the output signal of this stack.*/
virtual void remove( const TaskAbstract& task );
/*! \brief This method removes the output signals depending on this task. */
virtual void removeDependency( const TaskAbstract& key );
/*! \brief This method makes the task to swap with the task having the
immediate superior priority. */
virtual void up( const TaskAbstract& task );
/*! \brief This method makes the task to swap with the task having the
immediate inferior priority. */
virtual void down( const TaskAbstract& task );
/*! \brief Remove all the tasks from the stack. */
virtual void clear( void );
/*! @} */
/*! \name Methods to handle the constraints.
@{
*/
/*! \brief Add a constraint to the stack with the current level
of priority. */
virtual void addConstraint( Constraint& constraint );
/*! \brief Remove a constraint from the stack. */
virtual void removeConstraint( const Constraint& constraint );
/*! \brief Remove all the constraints from the stack. */
virtual void clearConstraint( void );
/*! @} */
/*! \brief This method defines the part of the state vector which correspond
to the free flyer of the robot. */
virtual void defineFreeFloatingJoints( const unsigned int& jointIdFirst,
const unsigned int& jointIdLast = -1 );
virtual void defineNbDof( const unsigned int& nbDof );
/*! @} */
public: /* --- CONTROL --- */
/*! \name Methods to compute the control law following the
recursive definition of the stack of tasks.
@{
*/
/*! \brief Compute the control law. */
virtual ml::Vector& computeControlLaw( ml::Vector& control,const int& time );
/*! \brief Compute the projector of the constraint. */
virtual ml::Matrix& computeConstraintProjector( ml::Matrix& Proj, const int& time );
/*! @} */
/* double getDirectionalThreshold( void ){ return directionalThreshold; } */
/* void setdirectionalThreshold( double th ){ directionalThreshold = th; } */
/* bool getUseContiInverse( void ){ return useContiInverse; } */
/* void setUseContiInverse( bool b ){ useContiInverse = b; } */
public: /* --- DISPLAY --- */
/*! \name Methods to display the stack of tasks.
@{
*/
/*! Display the stack of tasks in text mode as a tree. */
virtual void display( std::ostream& os ) const;
/*! Wrap the previous method around an operator. */
SOTSOT_CORE_EXPORT friend std::ostream& operator<< ( std::ostream& os,const Sot& sot );
/*! @} */
public: /* --- SIGNALS --- */
/*! \name Methods to handle signals
@{
*/
/*! \brief Intrinsec velocity of the robot, that is used to initialized
* the recurence of the SOT (e.g. velocity comming from the other
* OpenHRP plugins).
*/
dg::SignalPtr<ml::Vector,int> q0SIN;
/*! \brief This signal allow to change the threshold for the damped pseudo-inverse
on-line */
dg::SignalPtr<double,int> inversionThresholdSIN;
/*! \brief This signal allow to get the result of the Constraint projector. */
dg::SignalTimeDependent<ml::Matrix,int> constraintSOUT;
/*! \brief This signal allow to get the result of the computed control law. */
dg::SignalTimeDependent<ml::Vector,int> controlSOUT;
/*! @} */
public: /* --- COMMANDS --- */
/*! \brief This method deals with the command line.
The command given in argument is send to the stack of tasks by the shell.
The command understood by sot are:
<ul>
<li> Tasks
<ul>
<li> push <task> : Push a task in the stack (FILO).
<li> pop : Remove the task push in the stack.
<li> down <task> : Make the task have a higher priority, i.e.
swap with the task immediatly superior in priority.
<li> up <task> : Make the task have a lowest priority, i.e.
swap with the task immediatly inferior in priority.
<li> rm <task> : Remove the task from the stack.
</ul>
<li> Constraints
<ul>
<li> addConstraint <constraint> : Add the constraint in the stack (FILO).
<li> rmConstraint <constraint> : Remove the constraint.
<li> clearConstraint : Remove all the constraints.
<li> printConstraint :
</ol>
</ul>
*/
virtual void commandLine( const std::string& cmdLine,std::istringstream& cmdArgs,
std::ostream& os );
/*! \brief This method write the priority between tasks in the output stream os. */
virtual std::ostream & writeGraph(std::ostream & os) const;
};
} // namespace sot
#endif /* #ifndef __SOT_SOT_HH */
include/sot-core/vector-constant.h deleted 100644 → 0
View file @ f4f51423
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Copyright Projet JRL-Japan, 2007
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*
* File: vector-constant.h
* Project: SOT
* Author: Nicolas Mansard
*
* Version control
* ===============
*
* $Id$
*
* Description
* ============
*
*
* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
#include <dynamic-graph/entity.h>
#include <dynamic-graph/all-signals.h>
/* Matrix */
#include <MatrixAbstractLayer/boost.h>
namespace ml = maal::boost;
/* --------------------------------------------------------------------- */
/* --- VECTOR ---------------------------------------------------------- */
/* --------------------------------------------------------------------- */
namespace sot{
namespace dg = dynamicgraph;
class VectorConstant
: public dg::Entity
{
static const std::string CLASS_NAME;
virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
int rows;
double color;
public:
VectorConstant( const std::string& name )
:Entity( name )
,rows(0),color(0.)
,SOUT( "sotVectorConstant("+name+")::output(vector)::out" )
{
SOUT.setDependencyType( dg::TimeDependency<int>::BOOL_DEPENDENT );
signalRegistration( SOUT );
}
virtual ~VectorConstant( void ){}
dg::SignalTimeDependent<ml::Vector,int> SOUT;
virtual void commandLine( const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os );
};
} // namespace sot
include/sot-core/vector-quaternion.h deleted 100644 → 0
View file @ f4f51423
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Copyright Projet JRL-Japan, 2007
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*
* File: vector-quaternion.h
* Project: SOT
* Author: Nicolas Mansard
*
* Version control
* ===============
*
* $Id$
*
* Description
* ============
*
*
* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
#ifndef __SOT_VECTOR_QUATERNION_H__
#define __SOT_VECTOR_QUATERNION_H__
/* --- SOT --- */
#include <sot-core/vector-rotation.h>
#include <sot-core/sot-core-api.h>
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
namespace sot {
class SOT_CORE_EXPORT VectorQuaternion
: public sotVectorRotation
{
public:
VectorQuaternion( void ) : sotVectorRotation() { ml::Vector::resize(4); }
virtual ~VectorQuaternion( void ) { }
virtual sotVectorRotation& fromMatrix( const MatrixRotation& rot );
virtual MatrixRotation& toMatrix( MatrixRotation& rot ) const;
sotVectorRotation& fromVector( const VectorUTheta& ut );
VectorQuaternion& conjugate(VectorQuaternion& res) const;
VectorQuaternion& multiply(const VectorQuaternion& q2, VectorQuaternion& res) const;
};
}
#endif /* #ifndef __SOT_VECTOR_QUATERNION_H__ */
include/sot-core/vector-roll-pitch-yaw.h deleted 100644 → 0
View file @ f4f51423
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Copyright Projet JRL-Japan, 2007
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*
* File: vector-roll-pitch-yaw.h
* Project: SOT
* Author: Nicolas Mansard
*
* Version control
* ===============
*
* $Id$
*
* Description
* ============
*
*
* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
#ifndef __SOT_VECTOR_ROLLPITCHYAW_H__
#define __SOT_VECTOR_ROLLPITCHYAW_H__
/* --- SOT --- */
#include <sot-core/vector-rotation.h>
#include <sot-core/sot-core-api.h>
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
namespace sot {
class SOT_CORE_EXPORT VectorRollPitchYaw
: public sotVectorRotation
{
public:
VectorRollPitchYaw( void ) : sotVectorRotation() { }
virtual ~VectorRollPitchYaw( void ) { }
virtual sotVectorRotation& fromMatrix( const MatrixRotation& rot );
virtual MatrixRotation& toMatrix( MatrixRotation& rot ) const;
};
} // namespace sot
#endif /* #ifndef __SOT_VECTOR_ROLLPITCHYAW_H__ */
include/sot-core/vector-rotation.h deleted 100644 → 0
View file @ f4f51423
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Copyright Projet JRL-Japan, 2007
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*
* File: vector-rotation.h
* Project: SOT
* Author: Nicolas Mansard
*
* Version control
* ===============
*
* $Id$
*
* Description
* ============
*
*
* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
#ifndef __SOT_VECTOR_ROTATION_H__
#define __SOT_VECTOR_ROTATION_H__
/* Matrix */
#include <MatrixAbstractLayer/boost.h>
namespace ml = maal::boost;
/* --- SOT --- */
#include <sot-core/matrix-rotation.h>
#include <sot-core/sot-core-api.h>
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
namespace sot {
class SOT_CORE_EXPORT sotVectorRotation
: public ml::Vector
{
public:
sotVectorRotation( void ) : ml::Vector(3) { fill(0.); }
virtual ~sotVectorRotation( void ) { }
virtual sotVectorRotation& fromMatrix( const MatrixRotation& rot ) = 0;
virtual MatrixRotation& toMatrix( MatrixRotation& rot ) const = 0;
};
} // namespace sot
#endif /* #ifndef __SOT_VECTOR_ROTATION_H__ */
include/sot-core/vector-to-rotation.h deleted 100644 → 0
View file @ f4f51423
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Copyright Projet JRL-Japan, 2007
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*
* File: vector-to-rotation.h
* Project: SOT
* Author: Nicolas Mansard
*
* Version control
* ===============
*
* $Id$
*
* Description
* ============
*
*
* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
#ifndef __SOTVECTORTOMATRIX_HH
#define __SOTVECTORTOMATRIX_HH
#include <dynamic-graph/entity.h>
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/all-signals.h>
#include <sot-core/matrix-rotation.h>
/* Matrix */
#include <MatrixAbstractLayer/boost.h>
namespace ml = maal::boost;
/* STD */
#include <vector>
/* --------------------------------------------------------------------- */
/* --- VECTOR ---------------------------------------------------------- */
/* --------------------------------------------------------------------- */
namespace sot {
namespace dg = dynamicgraph;
class VectorToRotation
: public dg::Entity
{
static const std::string CLASS_NAME;
virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
enum sotAxis
{
AXIS_X
,AXIS_Y
,AXIS_Z
};
unsigned int size;
std::vector< sotAxis > axes;
public:
VectorToRotation( const std::string& name );
virtual ~VectorToRotation( void ){}
dg::SignalPtr<ml::Vector,int> SIN;
dg::SignalTimeDependent<MatrixRotation,int> SOUT;
MatrixRotation& computeRotation( const ml::Vector& angles,
MatrixRotation& res );
virtual void commandLine( const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os );
};
} // namespace sot
#endif // #ifndef __SOTVECTORTOMATRIX_HH
include/sot-core/vector-utheta.h deleted 100644 → 0
View file @ f4f51423
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Copyright Projet JRL-Japan, 2007
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*
* File: vector-utheta.h
* Project: SOT
* Author: Nicolas Mansard
*
* Version control
* ===============
*
* $Id$
*
* Description
* ============
*
*
* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
#ifndef __SOT_VECTOR_UTHETA_H__
#define __SOT_VECTOR_UTHETA_H__
/* --- SOT --- */
#include <sot-core/vector-rotation.h>
#include <sot-core/sot-core-api.h>
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
namespace sot {
class SOT_CORE_EXPORT VectorUTheta
: public sotVectorRotation
{
public:
VectorUTheta( void ) : sotVectorRotation() { }
virtual ~VectorUTheta( void ) { }
virtual sotVectorRotation& fromMatrix( const MatrixRotation& rot );
virtual MatrixRotation& toMatrix( MatrixRotation& rot ) const;
};
} // namespace sot
#endif /* #ifndef __SOT_VECTOR_UTHETA_H__ */
include/sot-core/weighted-sot.h deleted 100644 → 0
View file @ f4f51423
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Copyright Projet JRL-Japan, 2007
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*
* File: weighted-sot.h
* Project: SOT
* Author: Nicolas Mansard
*
* Version control
* ===============
*
* $Id$
*
* Description
* ============
*
*
* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
#ifndef __SOT_WSOT_HH
#define __SOT_WSOT_HH
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* Matrix */
#include <MatrixAbstractLayer/boost.h>
namespace ml = maal::boost;
/* Classes standards. */
#include <list> /* Classe std::list */
/* SOT */
#include <sot-core/sot.h>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (weighted_sot_EXPORTS)
# define SOTWEIGHTEDSOT_CORE_EXPORT __declspec(dllexport)
# else
# define SOTWEIGHTEDSOT_CORE_EXPORT __declspec(dllimport)
# endif
#else
# define SOTWEIGHTEDSOT_CORE_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/*! @ingroup stackoftasks
\brief This class implements the Stack of Task.
It allows to deal with the priority of the controllers
through the shell. The controllers can be either constraints
either tasks.
*/
namespace sot {
namespace dg = dynamicgraph;
class SOTWEIGHTEDSOT_CORE_EXPORT WeightedSot
:public Sot
{
public:
/*! \brief Specify the name of the class entity. */
static const std::string CLASS_NAME;
/*! \brief Returns the name of this class. */
virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
public:
/*! \brief Default constructor */
WeightedSot( const std::string& name );
public: /* --- CONTROL --- */
/*! \name Methods to compute the control law following the
recursive definition of the stack of tasks.
@{
*/
/*! \brief Compute the control law using weighted inverse. */
ml::Matrix& computeSquareRootInvWeight( ml::Matrix& res,const int& time );
ml::Vector& computeWeightedControlLaw( ml::Vector& control,const int& time );
ml::Matrix& computeConstrainedWeight( ml::Matrix& KAK,const int& time );
/*! @} */
public: /* --- SIGNALS --- */
/*! \name Methods to handle signals
@{
*/
dg::SignalPtr<ml::Matrix,int> weightSIN;
dg::SignalTimeDependent<ml::Matrix,int> constrainedWeightSOUT;
dg::SignalPtr<ml::Matrix,int> constrainedWeightSIN;
dg::SignalTimeDependent<ml::Matrix,int> squareRootInvWeightSOUT;
dg::SignalPtr<ml::Matrix,int> squareRootInvWeightSIN;
/*! @} */
};
} // namespace sot
#endif /* #ifndef __SOT_SOT_HH */
include/sot/core/abstract-sot-external-interface.hh 0 → 100644
View file @ 1c36e63b
/*
* Copyright 2011,
* Olivier Stasse, CNRS
*
* CNRS
*
*/
#ifndef ABSTRACT_SOT_EXTERNAL_INTERFACE_HH
#define ABSTRACT_SOT_EXTERNAL_INTERFACE_HH
#include <map>
#include <sot/core/api.hh>
#include <string>
#include <vector>
namespace dynamicgraph {
namespace sot {
class SOT_CORE_EXPORT NamedVector {
private:
std::string name_;
std::vector<double> values_;
public:
NamedVector() {}
~NamedVector() {}
const std::string &getName() const { return name_; }
void setName(const std::string &aname) { name_ = aname; }
const std::vector<double> &getValues() const { return values_; }
void setValues(const std::vector<double> &values) { values_ = values; }
};
typedef NamedVector SensorValues;
typedef NamedVector ControlValues;
class SOT_CORE_EXPORT AbstractSotExternalInterface {
public:
AbstractSotExternalInterface() {}
virtual ~AbstractSotExternalInterface() {}
virtual void setupSetSensors(
std::map<std::string, SensorValues> &sensorsIn) = 0;
virtual void nominalSetSensors(
std::map<std::string, SensorValues> &sensorsIn) = 0;
virtual void cleanupSetSensors(
std::map<std::string, SensorValues> &sensorsIn) = 0;
virtual void getControl(std::map<std::string, ControlValues> &) = 0;
virtual void setSecondOrderIntegration(void) = 0;
virtual void setNoIntegration(void) = 0;
};
} // namespace sot
} // namespace dynamicgraph
typedef dynamicgraph::sot::AbstractSotExternalInterface *
createSotExternalInterface_t();
typedef void destroySotExternalInterface_t(
dynamicgraph::sot::AbstractSotExternalInterface *);
#endif // ABSTRACT_SOT_EXTERNAL_INTERFACE_HH
include/sot-core/additional-functions.h → include/sot/core/additional-functions.hh
View file @ 1c36e63b
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Copyright Projet JRL-Japan, 2007
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
/*
* Copyright 2010,
* François Bleibel,
* Olivier Stasse,
*
* File: additional-functions.h
* Project: SOT
* Author: François Bleibel
* CNRS/AIST
*
* Version control
* ===============
*
* $Id$
*
* Description
* ============
*
*
* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
*/
/* SOT */
#include <dynamic-graph/signal-base.h>
#include <sot-core/exception-factory.h>
#include <dynamic-graph/pool.h>
#include <sot-core/pool.h>
#include <sot-core/sot-core-api.h>
#include <dynamic-graph/signal-base.h>
#include <sot/core/exception-factory.hh>
#include <sot/core/pool.hh>
#include "sot/core/api.hh"
/* --- STD --- */
#include <string>
#include <map>
#include <sstream>
#include <string>
/* --- BOOST --- */
#include <boost/function.hpp>
#include <boost/bind.hpp>
#include <boost/function.hpp>
namespace dynamicgraph {
namespace sot {
/*! @ingroup factory
\brief This helper class dynamically overloads the "new" shell command
to allow creation of tasks and features as well as entities.
*/
class AdditionalFunctions
{
public:
AdditionalFunctions();
~AdditionalFunctions();
static void cmdNew( const std::string& cmd,std::istringstream& args,
std::ostream& os );
static void cmdMatrixDisplay( const std::string& cmd,std::istringstream& args,
std::ostream& os );
static void cmdFlagSet( const std::string& cmd,std::istringstream& args,
std::ostream& os );
class AdditionalFunctions {
public:
AdditionalFunctions();
~AdditionalFunctions();
static void cmdFlagSet(const std::string &cmd, std::istringstream &args,
std::ostream &os);
};
} // namespace sot
} /* namespace sot */
} /* namespace dynamicgraph */
include/sot/core/admittance-control-op-point.hh 0 → 100644
View file @ 1c36e63b
/*
* Copyright 2019
*
* LAAS-CNRS
*
* Noëlie Ramuzat
* This file is part of sot-core.
* See license file.
*/
#ifndef __sot_core_admittance_control_op_point_H__
#define __sot_core_admittance_control_op_point_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined(WIN32)
#if defined(admittance_control_op_point_EXPORTS)
#define ADMITTANCECONTROLOPPOINT_EXPORT __declspec(dllexport)
#else
#define ADMITTANCECONTROLOPPOINT_EXPORT __declspec(dllimport)
#endif
#else
#define ADMITTANCECONTROLOPPOINT_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <dynamic-graph/signal-helper.h>
#include <sot/core/matrix-geometry.hh>
#include "pinocchio/spatial/force.hpp"
#include "pinocchio/spatial/motion.hpp"
#include "pinocchio/spatial/se3.hpp"
namespace dynamicgraph {
namespace sot {
namespace core {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/**
* @brief Admittance controller for an operational point wrt to a force sensor.
* It can be a point of the model (hand) or not (created operational
* point: an object in the hand of the robot) Which is closed to a force sensor
* (for instance the right or left wrist ft)
*
* This entity computes a velocity reference for an operational point based
* on the force error in the world frame :
* w_dq = integral(Kp(w_forceDes-w_force)) + Kd (w_dq)
*
*/
class ADMITTANCECONTROLOPPOINT_EXPORT AdmittanceControlOpPoint
: public ::dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
AdmittanceControlOpPoint(const std::string &name);
/**
* @brief Initialize the entity
*
* @param[in] dt Time step of the control
*/
void init(const double &dt);
/* --- SIGNALS --- */
/// \brief Gain (6d) for the integration of the error on the force
DECLARE_SIGNAL_IN(Kp, dynamicgraph::Vector);
/// \brief Derivative gain (6d) for the error on the force
DECLARE_SIGNAL_IN(Kd, dynamicgraph::Vector);
/// \brief Value of the saturation to apply on the velocity output
DECLARE_SIGNAL_IN(dqSaturation, dynamicgraph::Vector);
/// \brief 6d force given by the sensor in its local frame
DECLARE_SIGNAL_IN(force, dynamicgraph::Vector);
/// \brief 6d desired force of the end-effector in the world frame
DECLARE_SIGNAL_IN(w_forceDes, dynamicgraph::Vector);
/// \brief Current position (matrixHomogeneous) of the given operational
/// point
DECLARE_SIGNAL_IN(opPose, dynamicgraph::sot::MatrixHomogeneous);
/// \brief Current position (matrixHomogeneous) of the given force sensor
DECLARE_SIGNAL_IN(sensorPose, dynamicgraph::sot::MatrixHomogeneous);
/// \brief 6d force given by the sensor in the world frame
DECLARE_SIGNAL_INNER(w_force, dynamicgraph::Vector);
/// \brief Internal intergration computed in the world frame
DECLARE_SIGNAL_INNER(w_dq, dynamicgraph::Vector);
/// \brief Velocity reference for the end-effector in the local frame
DECLARE_SIGNAL_OUT(dq, dynamicgraph::Vector);
/* --- COMMANDS --- */
/**
* @brief Reset the velocity
*/
void resetDq();
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream &os) const;
protected:
/// Dimension of the force signals and of the output
int m_n;
/// True if the entity has been successfully initialized
bool m_initSucceeded;
/// Internal state
dynamicgraph::Vector m_w_dq;
/// Time step of the control
double m_dt;
// Weight of the end-effector
double m_mass;
}; // class AdmittanceControlOpPoint
} // namespace core
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_core_admittance_control_op_point_H__
include/sot/core/api.hh 0 → 100644
View file @ 1c36e63b
/*
* Copyright 2010,
* François Bleibel,
* Olivier Stasse,
*
* CNRS/AIST
*
*/
#ifndef SOT_CORE_API_HH
#define SOT_CORE_API_HH
#if defined(WIN32)
#ifdef sot_core_EXPORTS
#define SOT_CORE_EXPORT __declspec(dllexport)
#else
#define SOT_CORE_EXPORT __declspec(dllimport)
#endif
#else
#define SOT_CORE_EXPORT
#endif
#endif
include/sot/core/binary-int-to-uint.hh 0 → 100644
View file @ 1c36e63b
/*
* Copyright 2010,
* François Bleibel,
* Olivier Stasse,
*
* CNRS/AIST
*
*/
#ifndef __SOT_BINARY_INT_TO_UINT_HH__
#define __SOT_BINARY_INT_TO_UINT_HH__
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* SOT */
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
#include <sot/core/exception-task.hh>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined(WIN32)
#if defined(binary_int_to_uint_EXPORTS)
#define SOTBINARYINTTOUINT_EXPORT __declspec(dllexport)
#else
#define SOTBINARYINTTOUINT_EXPORT __declspec(dllimport)
#endif
#else
#define SOTBINARYINTTOUINT_EXPORT
#endif
namespace dynamicgraph {
namespace sot {
class SOTBINARYINTTOUINT_EXPORT BinaryIntToUint : public dynamicgraph::Entity {
public:
static const std::string CLASS_NAME;
virtual const std::string &getClassName(void) const { return CLASS_NAME; }
/* --- SIGNALS ------------------------------------------------------------ */
public:
dynamicgraph::SignalPtr<int, int> binaryIntSIN;
dynamicgraph::SignalTimeDependent<unsigned, int> binaryUintSOUT;
public:
BinaryIntToUint(const std::string &name);
virtual ~BinaryIntToUint() {}
virtual unsigned &computeOutput(unsigned &res, int time);
virtual void display(std::ostream &os) const;
};
} /* namespace sot */
} /* namespace dynamicgraph */
#endif
include/sot/core/binary-op.hh 0 → 100644
View file @ 1c36e63b
/*
* Copyright 2010,
* François Bleibel,
* Olivier Stasse,
*
* CNRS/AIST
*
*/
#ifndef SOT_CORE_BINARYOP_HH
#define SOT_CORE_BINARYOP_HH
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* Matrix */
#include <dynamic-graph/linear-algebra.h>
/* SOT */
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
#include <sot/core/flags.hh>
#include <sot/core/matrix-geometry.hh>
#include <sot/core/pool.hh>
/* STD */
#include <boost/function.hpp>
#include <string>
namespace dynamicgraph {
namespace sot {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
template <typename Operator>
class BinaryOp : public Entity {
Operator op;
typedef typename Operator::Tin1 Tin1;
typedef typename Operator::Tin2 Tin2;
typedef typename Operator::Tout Tout;
typedef BinaryOp<Operator> Self;
public: /* --- CONSTRUCTION --- */
static std::string getTypeIn1Name(void) { return Operator::nameTypeIn1(); }
static std::string getTypeIn2Name(void) { return Operator::nameTypeIn2(); }
static std::string getTypeOutName(void) { return Operator::nameTypeOut(); }
static const std::string CLASS_NAME;
virtual const std::string &getClassName() const { return CLASS_NAME; }
std::string getDocString() const { return op.getDocString(); }
BinaryOp(const std::string &name)
: Entity(name),
SIN1(NULL, BinaryOp::CLASS_NAME + "(" + name + ")::input(" +
getTypeIn1Name() + ")::sin1"),
SIN2(NULL, CLASS_NAME + "(" + name + ")::input(" + getTypeIn2Name() +
")::sin2"),
SOUT(boost::bind(&Self::computeOperation, this, _1, _2), SIN1 << SIN2,
CLASS_NAME + "(" + name + ")::output(" + getTypeOutName() +
")::sout") {
signalRegistration(SIN1 << SIN2 << SOUT);
op.addSpecificCommands(*this, commandMap);
}
virtual ~BinaryOp(void){};
public: /* --- SIGNAL --- */
SignalPtr<Tin1, int> SIN1;
SignalPtr<Tin2, int> SIN2;
SignalTimeDependent<Tout, int> SOUT;
protected:
Tout &computeOperation(Tout &res, int time) {
const Tin1 &x1 = SIN1(time);
const Tin2 &x2 = SIN2(time);
op(x1, x2, res);
return res;
}
};
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef SOT_CORE_BINARYOP_HH