<commit_before>/*
 * Copyright 2010,
 * <NAME>,
 * <NAME>,
 *
 * CNRS/AIST
 *
 * This file is part of sot-core.
 * sot-core is free software: you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public License
 * as published by the Free Software Foundation, either version 3 of
 * the License, or (at your option) any later version.
 * sot-core is distributed in the hope that it will be
 * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
 * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.  You should
 * have received a copy of the GNU Lesser General Public License along
 * with sot-core.  If not, see <http://www.gnu.org/licenses/>.
 */

/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */

//#define VP_DEBUG
//#define VP_DEBUG_MODE 45
#include <sot/core/debug.hh>

/* SOT */
#ifdef VP_DEBUG
 class sotSOT__INIT
 {
 public:sotSOT__INIT( void ) { dynamicgraph::sot::DebugTrace::openFile(); }
 };
 sotSOT__INIT sotSOT_initiator;
#endif //#ifdef VP_DEBUG

#include <sot/core/sot.hh>
#include <sot/core/pool.hh>
#include <sot/core/task.hh>
#include <sot/core/memory-task-sot.hh>
#include <sot/core/matrix-svd.hh>
#include <sot/core/matrix-geometry.hh>
#include <sot/core/factory.hh>

using namespace std;
using namespace dynamicgraph::sot;
using namespace dynamicgraph;

#include "../src/sot/sot-command.h"

/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */


DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Sot,"SOT");


const double Sot::INVERSION_THRESHOLD_DEFAULT = 1e-4;
const unsigned int Sot::NB_JOINTS_DEFAULT = 46;

/* --------------------------------------------------------------------- */
/* --- CONSTRUCTION ---------------------------------------------------- */
/* --------------------------------------------------------------------- */
Sot::
Sot( const std::string& name )
  :Entity(name)
  ,stack()
  ,constraintList()
  ,ffJointIdFirst( FF_JOINT_ID_DEFAULT )
  ,ffJointIdLast( FF_JOINT_ID_DEFAULT+6 )

  ,nbJoints( NB_JOINTS_DEFAULT )
  ,taskGradient(0)
  ,recomputeEachTime(true)
  ,q0SIN( NULL,"sotSOT("+name+")::input(double)::q0" )
  ,inversionThresholdSIN( NULL,"sotSOT("+name+")::input(double)::damping" )
  ,constraintSOUT( boost::bind(&Sot::computeConstraintProjector,this,_1,_2),
		   sotNOSIGNAL,
		    "sotSOT("+name+")::output(matrix)::constraint" )
  ,controlSOUT( boost::bind(&Sot::computeControlLaw,this,_1,_2),
		constraintSOUT<<inversionThresholdSIN<<q0SIN,
		"sotSOT("+name+")::output(vector)::control" )
{
  inversionThresholdSIN = INVERSION_THRESHOLD_DEFAULT;

  signalRegistration( inversionThresholdSIN<<controlSOUT<<constraintSOUT<<q0SIN );

  // Commands
  //
  std::string docstring;
  // addConstraint
  docstring ="    \n"
    "    AddConstraint\n"
    "    \n"
    "      Input:\n"
    "        - a string: name of the constraint object\n"
    "    \n";
  addCommand("addConstraint",
	     new command::classSot::AddConstraint(*this, docstring));

  docstring ="    \n"
    "    setNumberDofs.\n"
    "    \n"
    "      Input:\n"
    "        - a positive integer : number of degrees of freedom of the robot.\n"
    "    \n";
  addCommand("setSize",
	     new dynamicgraph::command::Setter<Sot, unsigned int>
	     (*this, &Sot::defineNbDof, docstring));

  docstring ="    \n"
    "    push a task into the stack.\n"
    "    \n"
    "      Input:\n"
    "        - a string : Name of the task.\n"
    "    \n";
  addCommand("push",
	     new command::classSot::Push(*this, docstring));

  docstring ="    \n"
    "    remove a task into the stack.\n"
    "    \n"
    "      Input:\n"
    "        - a string : Name of the task.\n"
    "    \n";
  addCommand("remove",
	     new command::classSot::Remove(*this, docstring));

  docstring ="    \n"
    "    up a task into the stack.\n"
    "    \n"
    "      Input:\n"
    "        - a string : Name of the task.\n"
    "    \n";
  addCommand("up",
	     new command::classSot::Up(*this, docstring));

  docstring ="    \n"
    "    down a task into the stack.\n"
    "    \n"
    "      Input:\n"
    "        - a string : Name of the task.\n"
    "    \n";
  addCommand("down",
	     new command::classSot::Down(*this, docstring));

  // Display
  docstring ="    \n"
    "    display the list of tasks pushed inside the stack.\n"
    "    \n";
  addCommand("display",
	     new command::classSot::Display(*this, docstring));


  // Clear
  docstring ="    \n"
    "    clear the list of tasks pushed inside the stack.\n"
    "    \n";
  addCommand("clear",
	     new command::classSot::Clear(*this, docstring));

  // List
  docstring ="    \n"
    "    returns the list of tasks pushed inside the stack.\n"
    "    \n";
  addCommand("list",
	     new command::classSot::List(*this, docstring));


}

/* --------------------------------------------------------------------- */
/* --- STACK MANIPULATION --- */
/* --------------------------------------------------------------------- */
void Sot::
push( TaskAbstract& task )
{
  stack.push_back( &task );
  controlSOUT.addDependency( task.taskSOUT );
  controlSOUT.addDependency( task.jacobianSOUT );
  controlSOUT.setReady();
}
TaskAbstract& Sot::
pop( void )
{
  TaskAbstract* res = stack.back();
  stack.pop_back();
  controlSOUT.removeDependency( res->taskSOUT );
  controlSOUT.removeDependency( res->jacobianSOUT );
  controlSOUT.setReady();
  return *res;
}
bool Sot::
exist( const TaskAbstract& key )
{
  std::list<TaskAbstract*>::iterator it;
  for ( it=stack.begin();stack.end()!=it;++it )
    {
      if( *it == &key ) { return true; }
    }
  return false;
}
void Sot::
remove( const TaskAbstract& key )
{
  bool find =false; std::list<TaskAbstract*>::iterator it;
  for ( it=stack.begin();stack.end()!=it;++it )
    {
      if( *it == &key ) { find=true; break; }
    }
  if(! find ){ return; }

  stack.erase( it );
  removeDependency( key );
}

void Sot::
removeDependency( const TaskAbstract& key )
{
  controlSOUT.removeDependency( key.taskSOUT );
  controlSOUT.removeDependency( key.jacobianSOUT );
  controlSOUT.setReady();
}

void Sot::
up( const TaskAbstract& key )
{
  bool find =false; std::list<TaskAbstract*>::iterator it;
  for ( it=stack.begin();stack.end()!=it;++it )
    {
      if( *it == &key ) { find=true; break; }
    }
  if( stack.begin()==it ) { return; }
  if(! find ){ return; }

  std::list<TaskAbstract*>::iterator pos=it; pos--;
  TaskAbstract * task = *it;
  stack.erase( it );
  stack.insert( pos,task );
  controlSOUT.setReady();
}
void Sot::
down( const TaskAbstract& key )
{
  bool find =false; std::list<TaskAbstract*>::iterator it;
  for ( it=stack.begin();stack.end()!=it;++it )
    {
      if( *it == &key ) { find=true; break; }
    }
  if( stack.end()==it ) { return; }
  if(! find ){ return; }

  std::list<TaskAbstract*>::iterator pos=it; pos++;
  TaskAbstract* task=*it;
  stack.erase( it );
  if( stack.end()==pos ){ stack.push_back(task); }
  else
    {
      pos++;
      stack.insert( pos,task );
    }
  controlSOUT.setReady();
}

void Sot::
clear( void )
{
  for (  std::list<TaskAbstract*>::iterator it=stack.begin();stack.end()!=it;++it )
    {
      removeDependency( **it );
    }
  stack.clear();
  controlSOUT.setReady();
}

/* --------------------------------------------------------------------- */
/* --- CONSTRAINTS ----------------------------------------------------- */
/* --------------------------------------------------------------------- */

void Sot::
addConstraint( Constraint& constraint )
{
  constraintList.push_back( &constraint );
  constraintSOUT.addDependency( constraint.jacobianSOUT );
}

void Sot::
removeConstraint( const Constraint& key )
{
  bool find =false; ConstraintListType::iterator it;
  for ( it=constraintList.begin();constraintList.end()!=it;++it )
    {
      if( *it == &key ) { find=true; break; }
    }
  if(! find ){ return; }

  constraintList.erase( it );

  constraintSOUT.removeDependency( key.jacobianSOUT );
}
void Sot::
clearConstraint( void )
{
  for (  ConstraintListType::iterator it=constraintList.begin();
	 constraintList.end()!=it;++it )
    {
      constraintSOUT.removeDependency( (*it)->jacobianSOUT );
    }
  constraintList.clear();
}

void Sot::
defineFreeFloatingJoints( const unsigned int& first,const unsigned int& last )
{
  ffJointIdFirst = first ;
  if( last>0 ) ffJointIdLast=last ;
  else ffJointIdLast=ffJointIdFirst+6;
}

void Sot::
defineNbDof( const unsigned int& nbDof )
{
  nbJoints = nbDof;
  constraintSOUT.setReady();
  controlSOUT.setReady();
}

/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */

dynamicgraph::Matrix & Sot::
computeJacobianConstrained( const dynamicgraph::Matrix& Jac,
                            const dynamicgraph::Matrix& K,
                            dynamicgraph::Matrix& JK,
                            dynamicgraph::Matrix& Jff,
                            dynamicgraph::Matrix& Jact )
{
  const int nJ = Jac.rows();
  const int mJ = K.cols();
  const int nbConstraints = Jac.cols() - mJ;

  if (nbConstraints == 0) {
    JK = Jac;
    return JK;
  }
  JK.resize(nJ, mJ);
  JK.noalias() = Jac.leftCols (nbConstraints) * K;
  JK.noalias() += Jac.rightCols (Jac.cols() - nbConstraints);

  return JK;
}


dynamicgraph::Matrix & Sot::
computeJacobianConstrained( const TaskAbstract& task,
                            const dynamicgraph::Matrix& K )
{
  const dynamicgraph::Matrix &Jac = task.jacobianSOUT.accessCopy ();
  MemoryTaskSOT * mem = dynamic_cast<MemoryTaskSOT *>( task.memoryInternal );
  if( NULL==mem ) throw; // TODO
  dynamicgraph::Matrix &Jff = mem->Jff;
  dynamicgraph::Matrix &Jact = mem->Jact;
  dynamicgraph::Matrix &JK = mem->JK;
  return computeJacobianConstrained(Jac,K,JK,Jff,Jact);
}

static void computeJacobianActivated( Task* taskSpec,
				      dynamicgraph::Matrix& Jt,
				      const int& iterTime )
{
  if( NULL!=taskSpec )
    {
      const Flags& controlSelec = taskSpec->controlSelectionSIN( iterTime );
      sotDEBUG(25) << "Control selection = " << controlSelec <<endl;
      if( controlSelec )
	{
	  if(!controlSelec)
	    {
	      sotDEBUG(15) << "Control selection."<<endl;
	      for( int i=0;i<Jt.cols();++i )
		{
		  if(! controlSelec(i) )
		    {for( int j=0;j<Jt.rows();++j ) { Jt(j,i)=0.; }}
		}
	    }
	  else
	    {
	      sotDEBUG(15) << "S is equal to Id."<<endl;
	    }
	}
      else
	{
	  sotDEBUG(15) << "Task not activated."<<endl;
	  Jt *= 0;
	}
    }
  else { /* No selection specification: nothing to do. */ }
}


/* --------------------------------------------------------------------- */
/* --- CONTROL --------------------------------------------------------- */
/* --------------------------------------------------------------------- */


//#define WITH_CHRONO


#ifdef  WITH_CHRONO
	#ifndef WIN32
	#include <sys/time.h>
	#else /*WIN32*/
	#include <sot/core/utils-windows.hh>
	#endif /*WIN32*/
#endif /*WITH_CHRONO*/


#ifdef  WITH_CHRONO
#   define sotINIT_CHRONO1 struct timeval t0,t1; double dt
#   define sotSTART_CHRONO1  gettimeofday(&t0,NULL)
#   define sotCHRONO1 \
      gettimeofday(&t1,NULL);\
      dt = ( (t1.tv_sec-t0.tv_sec) * 1000.* 1000.\
	     + (t1.tv_usec-t0.tv_usec+0.)  );\
      sotDEBUG(1) << "dt: "<< dt / 1000. << std::endl
#   define sotINITPARTCOUNTERS  struct timeval tpart0
#   define sotSTARTPARTCOUNTERS  gettimeofday(&tpart0,NULL)
#   define sotCOUNTER(nbc1,nbc2) \
	  gettimeofday(&tpart##nbc2,NULL); \
	  dt##nbc2 += ( (tpart##nbc2.tv_sec-tpart##nbc1.tv_sec) * 1000.* 1000. \
		   + (tpart##nbc2.tv_usec-tpart##nbc1.tv_usec+0.)  )
#   define sotINITCOUNTER(nbc1) \
   struct timeval tpart##nbc1; double dt##nbc1=0;
#   define sotPRINTCOUNTER(nbc1)  sotDEBUG(1) << "dt" << nbc1 << " = " << dt##nbc1 << std::endl
#else // #ifdef  WITH_CHRONO
#   define sotINIT_CHRONO1
#   define sotSTART_CHRONO1
#   define sotCHRONO1
#   define sotINITPARTCOUNTERS
#   define sotSTARTPARTCOUNTERS
#   define sotCOUNTER(nbc1,nbc2)
#   define sotINITCOUNTER(nbc1)
#   define sotPRINTCOUNTER(nbc1)
#endif // #ifdef  WITH_CHRONO

dynamicgraph::Vector Sot::
taskVectorToMlVector( const VectorMultiBound& taskVector )
{
  dynamicgraph::Vector res(taskVector.size()); res.setZero();
  unsigned int i=0;
  
  for( VectorMultiBound::const_iterator iter=taskVector.begin();
       iter!=taskVector.end();++iter,++i ) {
    res(i)=iter->getSingleBound();
  }
  return res;
}

dynamicgraph::Vector& Sot::
computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
{
  sotDEBUGIN(15);

  sotINIT_CHRONO1; sotINITPARTCOUNTERS;
  sotINITCOUNTER(1); sotINITCOUNTER(2);sotINITCOUNTER(3);sotINITCOUNTER(4);
  sotINITCOUNTER(5); sotINITCOUNTER(6);sotINITCOUNTER(7);sotINITCOUNTER(8);

  sotSTART_CHRONO1;
  sotSTARTPARTCOUNTERS;

  const double &th = inversionThresholdSIN(iterTime);
  const dynamicgraph::Matrix &K = constraintSOUT(iterTime);
  const int mJ = K.cols(); // number dofs - number constraints

  try {
    control = q0SIN( iterTime );
    sotDEBUG(15) << "initial velocity q0 = " << control << endl;
    if( mJ!=control.size() ) { control.resize( mJ ); control.setZero(); }
  }
  catch (...)
    {
      if( mJ!=control.size() ) { control.resize( mJ );}
      control.setZero();
      sotDEBUG(25) << "No initial velocity." <<endl;
    }

  sotDEBUGF(5, " --- Time %d -------------------", iterTime );
  unsigned int iterTask = 0;
  for( StackType::iterator iter = stack.begin(); iter!=stack.end();++iter )
    {
      sotDEBUGF(5,"Rank %d.",iterTask);
      TaskAbstract & task = **iter;
      sotDEBUG(15) << "Task: e_" << task.getName() << std::endl;
      const dynamicgraph::Matrix &Jac = task.jacobianSOUT(iterTime);
      const dynamicgraph::Vector err = taskVectorToMlVector(task.taskSOUT(iterTime));
      sotCOUNTER(0,1); // Direct Dynamic

      unsigned int rankJ;
      const int nJ = Jac.rows(); // number dofs

      /* Init memory. */
      MemoryTaskSOT * mem = dynamic_cast<MemoryTaskSOT *>( task.memoryInternal );
      if( NULL==mem )
        {
          if( NULL!=task.memoryInternal ) delete task.memoryInternal;
          mem = new MemoryTaskSOT( task.getName()+"_memSOT",nJ,mJ );
          task.memoryInternal = mem;
        }

      dynamicgraph::Matrix &Jp = mem->Jp;
      dynamicgraph::Matrix &V = mem->V;
      dynamicgraph::Matrix &JK = mem->JK;
      dynamicgraph::Matrix &Jt = mem->Jt;
      MemoryTaskSOT::SVD_t& svd = mem->svd;

      Jp.resize( mJ,nJ );
      V.resize( mJ,mJ );
      Jt.resize( nJ,mJ );
      JK.resize( nJ,mJ );

      if( (recomputeEachTime)
          ||(task.jacobianSOUT.getTime()>mem->jacobianInvSINOUT.getTime())
          ||(mem->jacobianInvSINOUT.accessCopy().rows()!=mJ)
          ||(mem->jacobianInvSINOUT.accessCopy().cols()!=nJ)
          ||(task.jacobianSOUT.getTime()>mem->jacobianConstrainedSINOUT.getTime())
          ||(task.jacobianSOUT.getTime()>mem->rankSINOUT.getTime())
          ||(task.jacobianSOUT.getTime()>mem->singularBaseImageSINOUT.getTime()) )
    {
	sotDEBUG(2) <<"Recompute inverse."<<endl;

	/* --- FIRST ALLOCS --- */
	dynamicgraph::Vector &S = mem->S;

	sotDEBUG(1) << "Size = "
		    << S.size() + mem->Jff.cols()*mem->Jff.rows()
	  + mem->Jact.cols()*mem->Jact.rows() << std::endl;

	sotDEBUG(1) << std::endl;
	S.resize( std::min(nJ,mJ) );
	sotDEBUG(1) << "nJ=" << nJ << " " << "Jac.cols()=" << Jac.cols()
		    <<" "<< "mJ=" << mJ<<std::endl;
	mem->Jff.resize( nJ,Jac.cols()-mJ ); // number dofs, number constraints
	sotDEBUG(1) << std::endl;
	mem->Jact.resize( nJ,mJ );
	sotDEBUG(1) << std::endl;
	sotDEBUG(1) << "Size = "
		    << S.size() + mem->Jff.cols()*mem->Jff.rows()
	  + mem->Jact.cols()*mem->Jact.rows() << std::endl;

	/***/sotCOUNTER(1,2); // first allocs

	/* --- COMPUTE JK --- */
	computeJacobianConstrained( task,K );
	/***/sotCOUNTER(2,3); // compute JK

	/* --- COMPUTE Jt --- */
	if( 0<iterTask ) Jt = JK*Proj; else { Jt = JK; }
	/***/sotCOUNTER(3,4); // compute Jt

	/* --- COMPUTE S --- */
	computeJacobianActivated( dynamic_cast<Task*>( &task ),Jt,iterTime );
	/***/sotCOUNTER(4,5); // Jt*S
	
	/* --- PINV --- */
        svd.compute (Jt);
        Eigen::dampedInverse (svd, Jp, th);
        V.noalias() = svd.matrixV();
        // TODO I think variable S could be removed as it is not used when
        // not recomputing the pseudo inverse.
        S.noalias() = svd.singularValues();
	/***/sotCOUNTER(5,6); // PINV
	sotDEBUG(2) << "V after dampedInverse." << V <<endl;
	/* --- RANK --- */
	{
          rankJ = 0;
          while ( rankJ < svd.singularValues().size()
              &&  th    < svd.singularValues()[rankJ])
          { ++rankJ; }
	}

	sotDEBUG(45) << "control"<<iterTask<<" = "<<control<<endl;
	sotDEBUG(25) << "J"<<iterTask<<" = "<<Jac<<endl;
	sotDEBUG(25) << "JK"<<iterTask<<" = "<<JK<<endl;
	sotDEBUG(25) << "Jt"<<iterTask<<" = "<<Jt<<endl;
	sotDEBUG(15) << "Jp"<<iterTask<<" = "<<Jp<<endl;
	sotDEBUG(15) << "e"<<iterTask<<" = "<<err<<endl;
	sotDEBUG(45) << "JJp"<<iterTask<<" = "<< JK*Jp <<endl;
	//sotDEBUG(45) << "U"<<iterTask<<" = "<< U<<endl;
	sotDEBUG(45) << "S"<<iterTask<<" = "<< S<<endl;
	sotDEBUG(45) << "V"<<iterTask<<" = "<< V<<endl;
	sotDEBUG(45) << "U"<<iterTask<<" = "<< svd.matrixU()<<endl;

	mem->jacobianInvSINOUT = Jp;
	mem->jacobianInvSINOUT.setTime( iterTime );
	mem->jacobianConstrainedSINOUT = JK;
	mem->jacobianConstrainedSINOUT.setTime( iterTime );
	mem->jacobianProjectedSINOUT = Jt;
	mem->jacobianProjectedSINOUT.setTime( iterTime );
	mem->singularBaseImageSINOUT = V;
	mem->singularBaseImageSINOUT.setTime( iterTime );
	mem->rankSINOUT = rankJ;
	mem->rankSINOUT.setTime( iterTime );

	sotDEBUG(25)<<"Inverse recomputed."<<endl;
      } else {
	sotDEBUG(2)<<"Inverse not recomputed."<<endl;
	rankJ = mem->rankSINOUT.accessCopy();
	Jp = mem->jacobianInvSINOUT.accessCopy();
	V = mem->singularBaseImageSINOUT.accessCopy();
	JK = mem->jacobianConstrainedSINOUT.accessCopy ();
	Jt = mem->jacobianProjectedSINOUT.accessCopy ();
      }
      /***/sotCOUNTER(6,7); // TRACE


      /* --- COMPUTE QDOT AND P --- */
      /*DEBUG: normally, the first iter (ie the test below)
      * is the same than the other, starting with control_0 = q0SIN. */
      if( iterTask==0 ) control += Jp*err; else
	control += Jp*(err - JK*control);
      /***/sotCOUNTER(7,8); // QDOT

      /* --- OPTIMAL FORM: To debug. --- */
      if( 0==iterTask )
	{ Proj.resize( mJ,mJ ); Proj.setIdentity(); }

//        {
// 	double *p,*v1,*v2,*vtmp1,*vtmp2;
// 	p = traits::matrix_storage(Proj.matrix);
// 	v1 = traits::matrix_storage(V.matrix);
// 	v2 = traits::matrix_storage(V.matrix);
// 	vtmp1 = traits::matrix_storage(V.matrix);
// 	/***/sotCOUNTER(6,7); // Ppre

// 	for( unsigned int i=0;i<mJ;++i )
// 	  {
// 	    vtmp2 = traits::matrix_storage(V.matrix);
// 	    for( unsigned int j=0;j<mJ;++j )
// 	      {
// 		v1 = vtmp1;   v2 = vtmp2;
// 		for( unsigned int k=0;k<rankJ;++k )
// 		  {
// 		    (*p) -=( *v1) * (*v2);
// 		    v2++;v1++;<commit_msg>Minor optimization of Sot::computeControlLaw<commit_after>/*
 * Copyright 2010,
 * <NAME>,
 * <NAME>,
 *
 * CNRS/AIST
 *
 * This file is part of sot-core.
 * sot-core is free software: you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public License
 * as published by the Free Software Foundation, either version 3 of
 * the License, or (at your option) any later version.
 * sot-core is distributed in the hope that it will be
 * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
 * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.  You should
 * have received a copy of the GNU Lesser General Public License along
 * with sot-core.  If not, see <http://www.gnu.org/licenses/>.
 */

/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */

//#define VP_DEBUG
//#define VP_DEBUG_MODE 45
#include <sot/core/debug.hh>

/* SOT */
#ifdef VP_DEBUG
 class sotSOT__INIT
 {
 public:sotSOT__INIT( void ) { dynamicgraph::sot::DebugTrace::openFile(); }
 };
 sotSOT__INIT sotSOT_initiator;
#endif //#ifdef VP_DEBUG

#include <sot/core/sot.hh>
#include <sot/core/pool.hh>
#include <sot/core/task.hh>
#include <sot/core/memory-task-sot.hh>
#include <sot/core/matrix-svd.hh>
#include <sot/core/matrix-geometry.hh>
#include <sot/core/factory.hh>

using namespace std;
using namespace dynamicgraph::sot;
using namespace dynamicgraph;

#include "../src/sot/sot-command.h"

/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */


DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Sot,"SOT");


const double Sot::INVERSION_THRESHOLD_DEFAULT = 1e-4;
const unsigned int Sot::NB_JOINTS_DEFAULT = 46;

/* --------------------------------------------------------------------- */
/* --- CONSTRUCTION ---------------------------------------------------- */
/* --------------------------------------------------------------------- */
Sot::
Sot( const std::string& name )
  :Entity(name)
  ,stack()
  ,constraintList()
  ,ffJointIdFirst( FF_JOINT_ID_DEFAULT )
  ,ffJointIdLast( FF_JOINT_ID_DEFAULT+6 )

  ,nbJoints( NB_JOINTS_DEFAULT )
  ,taskGradient(0)
  ,recomputeEachTime(true)
  ,q0SIN( NULL,"sotSOT("+name+")::input(double)::q0" )
  ,inversionThresholdSIN( NULL,"sotSOT("+name+")::input(double)::damping" )
  ,constraintSOUT( boost::bind(&Sot::computeConstraintProjector,this,_1,_2),
		   sotNOSIGNAL,
		    "sotSOT("+name+")::output(matrix)::constraint" )
  ,controlSOUT( boost::bind(&Sot::computeControlLaw,this,_1,_2),
		constraintSOUT<<inversionThresholdSIN<<q0SIN,
		"sotSOT("+name+")::output(vector)::control" )
{
  inversionThresholdSIN = INVERSION_THRESHOLD_DEFAULT;

  signalRegistration( inversionThresholdSIN<<controlSOUT<<constraintSOUT<<q0SIN );

  // Commands
  //
  std::string docstring;
  // addConstraint
  docstring ="    \n"
    "    AddConstraint\n"
    "    \n"
    "      Input:\n"
    "        - a string: name of the constraint object\n"
    "    \n";
  addCommand("addConstraint",
	     new command::classSot::AddConstraint(*this, docstring));

  docstring ="    \n"
    "    setNumberDofs.\n"
    "    \n"
    "      Input:\n"
    "        - a positive integer : number of degrees of freedom of the robot.\n"
    "    \n";
  addCommand("setSize",
	     new dynamicgraph::command::Setter<Sot, unsigned int>
	     (*this, &Sot::defineNbDof, docstring));

  docstring ="    \n"
    "    push a task into the stack.\n"
    "    \n"
    "      Input:\n"
    "        - a string : Name of the task.\n"
    "    \n";
  addCommand("push",
	     new command::classSot::Push(*this, docstring));

  docstring ="    \n"
    "    remove a task into the stack.\n"
    "    \n"
    "      Input:\n"
    "        - a string : Name of the task.\n"
    "    \n";
  addCommand("remove",
	     new command::classSot::Remove(*this, docstring));

  docstring ="    \n"
    "    up a task into the stack.\n"
    "    \n"
    "      Input:\n"
    "        - a string : Name of the task.\n"
    "    \n";
  addCommand("up",
	     new command::classSot::Up(*this, docstring));

  docstring ="    \n"
    "    down a task into the stack.\n"
    "    \n"
    "      Input:\n"
    "        - a string : Name of the task.\n"
    "    \n";
  addCommand("down",
	     new command::classSot::Down(*this, docstring));

  // Display
  docstring ="    \n"
    "    display the list of tasks pushed inside the stack.\n"
    "    \n";
  addCommand("display",
	     new command::classSot::Display(*this, docstring));


  // Clear
  docstring ="    \n"
    "    clear the list of tasks pushed inside the stack.\n"
    "    \n";
  addCommand("clear",
	     new command::classSot::Clear(*this, docstring));

  // List
  docstring ="    \n"
    "    returns the list of tasks pushed inside the stack.\n"
    "    \n";
  addCommand("list",
	     new command::classSot::List(*this, docstring));


}

/* --------------------------------------------------------------------- */
/* --- STACK MANIPULATION --- */
/* --------------------------------------------------------------------- */
void Sot::
push( TaskAbstract& task )
{
  stack.push_back( &task );
  controlSOUT.addDependency( task.taskSOUT );
  controlSOUT.addDependency( task.jacobianSOUT );
  controlSOUT.setReady();
}
TaskAbstract& Sot::
pop( void )
{
  TaskAbstract* res = stack.back();
  stack.pop_back();
  controlSOUT.removeDependency( res->taskSOUT );
  controlSOUT.removeDependency( res->jacobianSOUT );
  controlSOUT.setReady();
  return *res;
}
bool Sot::
exist( const TaskAbstract& key )
{
  std::list<TaskAbstract*>::iterator it;
  for ( it=stack.begin();stack.end()!=it;++it )
    {
      if( *it == &key ) { return true; }
    }
  return false;
}
void Sot::
remove( const TaskAbstract& key )
{
  bool find =false; std::list<TaskAbstract*>::iterator it;
  for ( it=stack.begin();stack.end()!=it;++it )
    {
      if( *it == &key ) { find=true; break; }
    }
  if(! find ){ return; }

  stack.erase( it );
  removeDependency( key );
}

void Sot::
removeDependency( const TaskAbstract& key )
{
  controlSOUT.removeDependency( key.taskSOUT );
  controlSOUT.removeDependency( key.jacobianSOUT );
  controlSOUT.setReady();
}

void Sot::
up( const TaskAbstract& key )
{
  bool find =false; std::list<TaskAbstract*>::iterator it;
  for ( it=stack.begin();stack.end()!=it;++it )
    {
      if( *it == &key ) { find=true; break; }
    }
  if( stack.begin()==it ) { return; }
  if(! find ){ return; }

  std::list<TaskAbstract*>::iterator pos=it; pos--;
  TaskAbstract * task = *it;
  stack.erase( it );
  stack.insert( pos,task );
  controlSOUT.setReady();
}
void Sot::
down( const TaskAbstract& key )
{
  bool find =false; std::list<TaskAbstract*>::iterator it;
  for ( it=stack.begin();stack.end()!=it;++it )
    {
      if( *it == &key ) { find=true; break; }
    }
  if( stack.end()==it ) { return; }
  if(! find ){ return; }

  std::list<TaskAbstract*>::iterator pos=it; pos++;
  TaskAbstract* task=*it;
  stack.erase( it );
  if( stack.end()==pos ){ stack.push_back(task); }
  else
    {
      pos++;
      stack.insert( pos,task );
    }
  controlSOUT.setReady();
}

void Sot::
clear( void )
{
  for (  std::list<TaskAbstract*>::iterator it=stack.begin();stack.end()!=it;++it )
    {
      removeDependency( **it );
    }
  stack.clear();
  controlSOUT.setReady();
}

/* --------------------------------------------------------------------- */
/* --- CONSTRAINTS ----------------------------------------------------- */
/* --------------------------------------------------------------------- */

void Sot::
addConstraint( Constraint& constraint )
{
  constraintList.push_back( &constraint );
  constraintSOUT.addDependency( constraint.jacobianSOUT );
}

void Sot::
removeConstraint( const Constraint& key )
{
  bool find =false; ConstraintListType::iterator it;
  for ( it=constraintList.begin();constraintList.end()!=it;++it )
    {
      if( *it == &key ) { find=true; break; }
    }
  if(! find ){ return; }

  constraintList.erase( it );

  constraintSOUT.removeDependency( key.jacobianSOUT );
}
void Sot::
clearConstraint( void )
{
  for (  ConstraintListType::iterator it=constraintList.begin();
	 constraintList.end()!=it;++it )
    {
      constraintSOUT.removeDependency( (*it)->jacobianSOUT );
    }
  constraintList.clear();
}

void Sot::
defineFreeFloatingJoints( const unsigned int& first,const unsigned int& last )
{
  ffJointIdFirst = first ;
  if( last>0 ) ffJointIdLast=last ;
  else ffJointIdLast=ffJointIdFirst+6;
}

void Sot::
defineNbDof( const unsigned int& nbDof )
{
  nbJoints = nbDof;
  constraintSOUT.setReady();
  controlSOUT.setReady();
}

/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */

dynamicgraph::Matrix & Sot::
computeJacobianConstrained( const dynamicgraph::Matrix& Jac,
                            const dynamicgraph::Matrix& K,
                            dynamicgraph::Matrix& JK,
                            dynamicgraph::Matrix& Jff,
                            dynamicgraph::Matrix& Jact )
{
  const int nJ = Jac.rows();
  const int mJ = K.cols();
  const int nbConstraints = Jac.cols() - mJ;

  if (nbConstraints == 0) {
    JK = Jac;
    return JK;
  }
  JK.resize(nJ, mJ);
  JK.noalias() = Jac.leftCols (nbConstraints) * K;
  JK.noalias() += Jac.rightCols (Jac.cols() - nbConstraints);

  return JK;
}


dynamicgraph::Matrix & Sot::
computeJacobianConstrained( const TaskAbstract& task,
                            const dynamicgraph::Matrix& K )
{
  const dynamicgraph::Matrix &Jac = task.jacobianSOUT.accessCopy ();
  MemoryTaskSOT * mem = dynamic_cast<MemoryTaskSOT *>( task.memoryInternal );
  if( NULL==mem ) throw; // TODO
  dynamicgraph::Matrix &Jff = mem->Jff;
  dynamicgraph::Matrix &Jact = mem->Jact;
  dynamicgraph::Matrix &JK = mem->JK;
  return computeJacobianConstrained(Jac,K,JK,Jff,Jact);
}

static void computeJacobianActivated( Task* taskSpec,
				      dynamicgraph::Matrix& Jt,
				      const int& iterTime )
{
  if( NULL!=taskSpec )
    {
      const Flags& controlSelec = taskSpec->controlSelectionSIN( iterTime );
      sotDEBUG(25) << "Control selection = " << controlSelec <<endl;
      if( controlSelec )
	{
	  if(!controlSelec)
	    {
	      sotDEBUG(15) << "Control selection."<<endl;
	      for( int i=0;i<Jt.cols();++i )
		{
		  if(! controlSelec(i) )
		    { Jt.col (i).setZero(); }
		}
	    }
	  else
	    {
	      sotDEBUG(15) << "S is equal to Id."<<endl;
	    }
	}
      else
	{
	  sotDEBUG(15) << "Task not activated."<<endl;
	  Jt *= 0;
	}
    }
  else { /* No selection specification: nothing to do. */ }
}


/* --------------------------------------------------------------------- */
/* --- CONTROL --------------------------------------------------------- */
/* --------------------------------------------------------------------- */


//#define WITH_CHRONO


#ifdef  WITH_CHRONO
	#ifndef WIN32
	#include <sys/time.h>
	#else /*WIN32*/
	#include <sot/core/utils-windows.hh>
	#endif /*WIN32*/
#endif /*WITH_CHRONO*/


#ifdef  WITH_CHRONO
#   define sotINIT_CHRONO1 struct timeval t0,t1; double dt
#   define sotSTART_CHRONO1  gettimeofday(&t0,NULL)
#   define sotCHRONO1 \
      gettimeofday(&t1,NULL);\
      dt = ( (t1.tv_sec-t0.tv_sec) * 1000.* 1000.\
	     + (t1.tv_usec-t0.tv_usec+0.)  );\
      sotDEBUG(1) << "dt: "<< dt / 1000. << std::endl
#   define sotINITPARTCOUNTERS  struct timeval tpart0
#   define sotSTARTPARTCOUNTERS  gettimeofday(&tpart0,NULL)
#   define sotCOUNTER(nbc1,nbc2) \
	  gettimeofday(&tpart##nbc2,NULL); \
	  dt##nbc2 += ( (tpart##nbc2.tv_sec-tpart##nbc1.tv_sec) * 1000.* 1000. \
		   + (tpart##nbc2.tv_usec-tpart##nbc1.tv_usec+0.)  )
#   define sotINITCOUNTER(nbc1) \
   struct timeval tpart##nbc1; double dt##nbc1=0;
#   define sotPRINTCOUNTER(nbc1)  sotDEBUG(1) << "dt" << nbc1 << " = " << dt##nbc1 << std::endl
#else // #ifdef  WITH_CHRONO
#   define sotINIT_CHRONO1
#   define sotSTART_CHRONO1
#   define sotCHRONO1
#   define sotINITPARTCOUNTERS
#   define sotSTARTPARTCOUNTERS
#   define sotCOUNTER(nbc1,nbc2)
#   define sotINITCOUNTER(nbc1)
#   define sotPRINTCOUNTER(nbc1)
#endif // #ifdef  WITH_CHRONO

dynamicgraph::Vector Sot::
taskVectorToMlVector( const VectorMultiBound& taskVector )
{
  dynamicgraph::Vector res(taskVector.size()); res.setZero();
  unsigned int i=0;
  
  for( VectorMultiBound::const_iterator iter=taskVector.begin();
       iter!=taskVector.end();++iter,++i ) {
    res(i)=iter->getSingleBound();
  }
  return res;
}

dynamicgraph::Vector& Sot::
computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
{
  sotDEBUGIN(15);

  sotINIT_CHRONO1; sotINITPARTCOUNTERS;
  sotINITCOUNTER(1); sotINITCOUNTER(2);sotINITCOUNTER(3);sotINITCOUNTER(4);
  sotINITCOUNTER(5); sotINITCOUNTER(6);sotINITCOUNTER(7);sotINITCOUNTER(8);

  sotSTART_CHRONO1;
  sotSTARTPARTCOUNTERS;

  const double &th = inversionThresholdSIN(iterTime);
  const dynamicgraph::Matrix &K = constraintSOUT(iterTime);
  const int mJ = K.cols(); // number dofs - number constraints

  try {
    control = q0SIN( iterTime );
    sotDEBUG(15) << "initial velocity q0 = " << control << endl;
    if( mJ!=control.size() ) { control.resize( mJ ); control.setZero(); }
  }
  catch (...)
    {
      if( mJ!=control.size() ) { control.resize( mJ );}
      control.setZero();
      sotDEBUG(25) << "No initial velocity." <<endl;
    }

  sotDEBUGF(5, " --- Time %d -------------------", iterTime );
  unsigned int iterTask = 0;
  for( StackType::iterator iter = stack.begin(); iter!=stack.end();++iter )
    {
      sotDEBUGF(5,"Rank %d.",iterTask);
      TaskAbstract & task = **iter;
      sotDEBUG(15) << "Task: e_" << task.getName() << std::endl;
      const dynamicgraph::Matrix &Jac = task.jacobianSOUT(iterTime);
      const dynamicgraph::Vector err = taskVectorToMlVector(task.taskSOUT(iterTime));
      sotCOUNTER(0,1); // Direct Dynamic

      unsigned int rankJ;
      const int nJ = Jac.rows(); // number dofs

      /* Init memory. */
      MemoryTaskSOT * mem = dynamic_cast<MemoryTaskSOT *>( task.memoryInternal );
      if( NULL==mem )
        {
          if( NULL!=task.memoryInternal ) delete task.memoryInternal;
          mem = new MemoryTaskSOT( task.getName()+"_memSOT",nJ,mJ );
          task.memoryInternal = mem;
        }

      dynamicgraph::Matrix &Jp = mem->Jp;
      dynamicgraph::Matrix &V = mem->V;
      dynamicgraph::Matrix &JK = mem->JK;
      dynamicgraph::Matrix &Jt = mem->Jt;
      MemoryTaskSOT::SVD_t& svd = mem->svd;

      Jp.resize( mJ,nJ );
      V.resize( mJ,mJ );
      Jt.resize( nJ,mJ );
      JK.resize( nJ,mJ );

      if( (recomputeEachTime)
          ||(task.jacobianSOUT.getTime()>mem->jacobianInvSINOUT.getTime())
          ||(mem->jacobianInvSINOUT.accessCopy().rows()!=mJ)
          ||(mem->jacobianInvSINOUT.accessCopy().cols()!=nJ)
          ||(task.jacobianSOUT.getTime()>mem->jacobianConstrainedSINOUT.getTime())
          ||(task.jacobianSOUT.getTime()>mem->rankSINOUT.getTime())
          ||(task.jacobianSOUT.getTime()>mem->singularBaseImageSINOUT.getTime()) )
    {
	sotDEBUG(2) <<"Recompute inverse."<<endl;

	/* --- FIRST ALLOCS --- */
	dynamicgraph::Vector &S = mem->S;

	sotDEBUG(1) << "Size = "
		    << S.size() + mem->Jff.cols()*mem->Jff.rows()
	  + mem->Jact.cols()*mem->Jact.rows() << std::endl;

	sotDEBUG(1) << std::endl;
	S.resize( std::min(nJ,mJ) );
	sotDEBUG(1) << "nJ=" << nJ << " " << "Jac.cols()=" << Jac.cols()
		    <<" "<< "mJ=" << mJ<<std::endl;
	mem->Jff.resize( nJ,Jac.cols()-mJ ); // number dofs, number constraints
	sotDEBUG(1) << std::endl;
	mem->Jact.resize( nJ,mJ );
	sotDEBUG(1) << std::endl;
	sotDEBUG(1) << "Size = "
		    << S.size() + mem->Jff.cols()*mem->Jff.rows()
	  + mem->Jact.cols()*mem->Jact.rows() << std::endl;

	/***/sotCOUNTER(1,2); // first allocs

	/* --- COMPUTE JK --- */
	computeJacobianConstrained( task,K );
	/***/sotCOUNTER(2,3); // compute JK

	/* --- COMPUTE Jt --- */
	if( 0<iterTask ) Jt.noalias() = JK*Proj; else { Jt = JK; }
	/***/sotCOUNTER(3,4); // compute Jt

	/* --- COMPUTE S --- */
	computeJacobianActivated( dynamic_cast<Task*>( &task ),Jt,iterTime );
	/***/sotCOUNTER(4,5); // Jt*S
	
	/* --- PINV --- */
        svd.compute (Jt);
        Eigen::dampedInverse (svd, Jp, th);
        V.noalias() = svd.matrixV();
        // TODO I think variable S could be removed as it is not used when
        // not recomputing the pseudo inverse.
        S.noalias() = svd.singularValues();
	/***/sotCOUNTER(5,6); // PINV
	sotDEBUG(2) << "V after dampedInverse." << V <<endl;
	/* --- RANK --- */
	{
          rankJ = 0;
          while ( rankJ < svd.singularValues().size()
              &&  th    < svd.singularValues()[rankJ])
          { ++rankJ; }
	}

	sotDEBUG(45) << "control"<<iterTask<<" = "<<control<<endl;
	sotDEBUG(25) << "J"<<iterTask<<" = "<<Jac<<endl;
	sotDEBUG(25) << "JK"<<iterTask<<" = "<<JK<<endl;
	sotDEBUG(25) << "Jt"<<iterTask<<" = "<<Jt<<endl;
	sotDEBUG(15) << "Jp"<<iterTask<<" = "<<Jp<<endl;
	sotDEBUG(15) << "e"<<iterTask<<" = "<<err<<endl;
	sotDEBUG(45) << "JJp"<<iterTask<<" = "<< JK*Jp <<endl;
	//sotDEBUG(45) << "U"<<iterTask<<" = "<< U<<endl;
	sotDEBUG(45) << "S"<<iterTask<<" = "<< S<<endl;
	sotDEBUG(45) << "V"<<iterTask<<" = "<< V<<endl;
	sotDEBUG(45) << "U"<<iterTask<<" = "<< svd.matrixU()<<endl;

	mem->jacobianInvSINOUT = Jp;
	mem->jacobianInvSINOUT.setTime( iterTime );
	mem->jacobianConstrainedSINOUT = JK;
	mem->jacobianConstrainedSINOUT.setTime( iterTime );
	mem->jacobianProjectedSINOUT = Jt;
	mem->jacobianProjectedSINOUT.setTime( iterTime );
	mem->singularBaseImageSINOUT = V;
	mem->singularBaseImageSINOUT.setTime( iterTime );
	mem->rankSINOUT = rankJ;
	mem->rankSINOUT.setTime( iterTime );

	sotDEBUG(25)<<"Inverse recomputed."<<endl;
      } else {
	sotDEBUG(2)<<"Inverse not recomputed."<<endl;
	rankJ = mem->rankSINOUT.accessCopy();
	Jp = mem->jacobianInvSINOUT.accessCopy();
	V = mem->singularBaseImageSINOUT.accessCopy();
	JK = mem->jacobianConstrainedSINOUT.accessCopy ();
	Jt = mem->jacobianProjectedSINOUT.accessCopy ();
      }
      /***/sotCOUNTER(6,7); // TRACE


      /* --- COMPUTE QDOT AND P --- */
      /*DEBUG: normally, the first iter (ie the test below)
      * is the same than the other, starting with control_0 = q0SIN. */
      if( iterTask==0 ) control.noalias() += Jp*err;
      else              control           += Jp*(err - JK*control);
      /***/sotCOUNTER(7,8); // QDOT

      /* --- OPTIMAL FORM: To debug. --- */
      if( 0==iterTask )
	{ Proj.resize( mJ,mJ ); Proj.setIdentity(); }

//        {
// 	double *p,*v1,*v2,*vtmp1,*vtmp2;
// 	p = traits::matrix_storage(Proj.matrix);
// 	v1 = traits::matrix_storage(V.matrix);
// 	v2 = traits::matrix_storage(V.matrix);
// 	vtmp1 = traits::matrix_storage(V.matrix);
// 	/***/sotCOUNTER(6,7); // Ppre

// 	for( unsigned int i=0;i<mJ;++i )
// 	  {
// 	    vtmp2 = traits::matrix_storage(V.matrix);
// 	    for( unsigned int j=0;j<mJ;++j )
// 	      {
// 		v1 = vtmp1;   v2 = vtmp2;
// 		for( unsigned int k=0;k<rankJ;++k )
// 		  {
// 		    (*p) -=( *v1) * (*v2);
// 		    v2++;v1++;
{'fraction_non_alphanumeric': 0.23289007547363558, 'fraction_numerical': 0.010268521846280227, 'mean_word_length': 2.3747725894481504, 'pattern_counts': {'":': 0, '<': 514, '<?xml version=': 0, '>': 151, 'https://': 0, 'lorem ipsum': 0, 'www.': 2, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '1949268', 'n_tokens_mistral': 14138, 'n_tokens_neox': 13570, 'n_words': 2582}