Added missing cpp files.

src/solver-kine.cpp

+/*
+ * Copyright 2011, Nicolas Mansard, LAAS-CNRS
+ *
+ * This file is part of sot-dyninv.
+ * sot-dyninv 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-dyninv 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-dyninv.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#define VP_DEBUG
+#define VP_DEBUG_MODE 50
+#include <sot/core/debug.hh>
+#ifdef VP_DEBUG
+class solver_op_space__INIT
+{
+public:solver_op_space__INIT( void ) { dynamicgraph::sot::DebugTrace::openFile(); }
+};
+solver_op_space__INIT solver_op_space_initiator;
+#endif //#ifdef VP_DEBUG
+
+
+#include <sot-dyninv/solver-kine.h>
+#include <sot-dyninv/commands-helper.h>
+#include <dynamic-graph/factory.h>
+#include <boost/foreach.hpp>
+#include <sot-dyninv/commands-helper.h>
+#include <dynamic-graph/pool.h>
+#include <soth/HCOD.hpp>
+#include <sot-dyninv/task-dyn-pd.h>
+#include <sot/core/feature-point6d.hh>
+#include <sstream>
+#include <soth/Algebra.hpp>
+#include <Eigen/QR>
+#include <sot-dyninv/mal-to-eigen.h>
+
+
+namespace dynamicgraph
+{
+  namespace sot
+  {
+    namespace dyninv
+    {
+
+      namespace dg = ::dynamicgraph;
+      using namespace dg;
+      using dg::SignalBase;
+
+      /* --- DG FACTORY ------------------------------------------------------- */
+      DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SolverKine,"SolverKine");
+
+      /* --- CONSTRUCTION ----------------------------------------------------- */
+      /* --- CONSTRUCTION ----------------------------------------------------- */
+      /* --- CONSTRUCTION ----------------------------------------------------- */
+      SolverKine::
+      SolverKine( const std::string & name )
+	: Entity(name)
+	,stack_t()
+
+	,CONSTRUCT_SIGNAL_IN(damping,double)
+	,CONSTRUCT_SIGNAL_OUT(control,ml::Vector,
+			      dampingSIN )
+	,CONSTRUCT_SIGNAL(velocity,OUT,ml::Vector)
+
+	,controlFreeFloating(true)
+
+	,hsolver()
+
+	,Ctasks(),btasks()
+	,solution()
+      {
+	signalRegistration(  controlSOUT << velocitySOUT
+			    << dampingSIN );
+
+	/* Command registration. */
+	addCommand("debugOnce",
+		   makeCommandVoid0(*this,&SolverKine::debugOnce,
+				    docCommandVoid0("open trace-file for next iteration of the solver.")));
+
+	addCommand("setControlFreeFloating",
+		   makeDirectSetter(*this,&controlFreeFloating,
+				    docDirectSetter("If true, the ouput control includes the ff (ie, size nbDof). Oterwise, size is nbDof-6. FF is supposed to be at the head.","bool")));
+
+	ADD_COMMANDS_FOR_THE_STACK;
+      }
+
+      /* --- STACK ----------------------------------------------------------- */
+      /* --- STACK ----------------------------------------------------------- */
+      /* --- STACK ----------------------------------------------------------- */
+
+      SolverKine::TaskDependancyList_t SolverKine::
+      getTaskDependancyList( const TaskAbstract& task )
+      {
+	TaskDependancyList_t res;
+	res.push_back( &task.taskSOUT );
+	res.push_back( &task.jacobianSOUT );
+	return res;
+      }
+      void SolverKine::
+      addDependancy( const TaskDependancyList_t& depList )
+      {
+	BOOST_FOREACH( const SignalBase<int>* sig, depList )
+	  { controlSOUT.addDependency( *sig ); }
+      }
+      void  SolverKine::
+      removeDependancy( const TaskDependancyList_t& depList )
+      {
+	BOOST_FOREACH( const SignalBase<int>* sig, depList )
+	  { controlSOUT.removeDependency( *sig ); }
+      }
+      void SolverKine::
+      resetReady( void )
+      {
+	controlSOUT.setReady();
+      }
+
+
+      /* --- INIT SOLVER ------------------------------------------------------ */
+      /* --- INIT SOLVER ------------------------------------------------------ */
+      /* --- INIT SOLVER ------------------------------------------------------ */
+
+      /** Force the update of all the task in-signals, in order to fix their
+       * size for resizing the solver.
+       */
+      void SolverKine::
+      refreshTaskTime( int time )
+      {
+	BOOST_FOREACH( TaskAbstract* task, stack )
+	  {
+	    task->taskSOUT( time );
+	  }
+      }
+
+      /** Knowing the sizes of all the stages (except the task ones),
+       * the function resizes the matrix and vector of all stages (except...).
+       */
+      void SolverKine::
+      resizeSolver( void )
+      {
+	hsolver = hcod_ptr_t(new soth::HCOD( nbDofs,stack.size() ));
+	Ctasks.resize(stack.size());
+	btasks.resize(stack.size());
+
+	int i=0;
+	BOOST_FOREACH( TaskAbstract* task, stack )
+	  {
+	    const int nx = task->taskSOUT.accessCopy().size();
+	    Ctasks[i].resize(nx,nbDofs);
+	    btasks[i].resize(nx);
+
+	    hsolver->pushBackStage( Ctasks[i],btasks[i] );
+	    hsolver->stages.back()->name = task->getName();
+	    i++;
+	  }
+
+	solution.resize( nbDofs );
+      }
+
+      /* Return true iff the solver sizes fit to the task set. */
+      bool SolverKine::
+      checkSolverSize( void )
+      {
+	sotDEBUGIN(15);
+
+	assert( nbDofs>0 );
+
+	if(! hsolver ) return false;
+	if( stack.size() != hsolver->nbStages() ) return false;
+
+	bool toBeResized=false;
+	for( int i=0;i<(int)stack.size();++i )
+	  {
+	    assert( Ctasks[i].cols() == nbDofs && Ctasks[i].rows() == btasks[i].size() );
+	    TaskAbstract & task = *stack[i];
+	    if( btasks[i].size() != (int)task.taskSOUT.accessCopy().size() )
+	      {
+		toBeResized = true;
+		break;
+	      }
+	  }
+
+	return !toBeResized;
+      }
+
+      /* --- SIGNALS ---------------------------------------------------------- */
+      /* --- SIGNALS ---------------------------------------------------------- */
+      /* --- SIGNALS ---------------------------------------------------------- */
+
+      ml::Vector& SolverKine::
+      controlSOUT_function( ml::Vector &mlcontrol, int t )
+      {
+	sotDEBUG(15) << " # In time = " << t << std::endl;
+
+	refreshTaskTime( t );
+	if(! checkSolverSize() ) resizeSolver();
+
+	using namespace soth;
+
+	if( dampingSIN ) //damp?
+	  {
+	    sotDEBUG(5) << "Using damping. " << std::endl;
+	    /* Only damp the final stage of the stack, 'cose of the solver known limitation. */
+	    hsolver->setDamping( 0 );
+	    hsolver->useDamp( true );
+	    hsolver->stages.back()->damping( dampingSIN(t) );
+	  }
+	else
+	  {
+	    sotDEBUG(5) << "Without damping. " << std::endl;
+	    hsolver->useDamp( false );
+	  }
+
+
+	/* -Tasks 1:n- */
+	/* Ctaski = [ Ji 0 0 0 0 0 ] */
+	for( int i=0;i<(int)stack.size();++i )
+	  {
+	    TaskAbstract & task = * stack[i];
+	    MatrixXd & Ctask = Ctasks[i];
+	    VectorBound & btask = btasks[i];
+
+	    EIGEN_MATRIX_FROM_SIGNAL(J,task.jacobianSOUT(t));
+	    const dg::sot::VectorMultiBound & ddx = task.taskSOUT(t);
+	    const int nx = ddx.size();
+
+	    sotDEBUG(5) << "ddx"<<i<<" = " << ddx << std::endl;
+	    sotDEBUG(25) << "J"<<i<<" = " << J << std::endl;
+
+	    assert( Ctask.rows() == nx && btask.size() == nx );
+	    assert( J.rows()==nx && J.cols()==nbDofs && (int)ddx.size()==nx );
+
+	    Ctask = J;  COPY_MB_VECTOR_TO_EIGEN(ddx,btask);
+
+	    sotDEBUG(15) << "Ctask"<<i<<" = "     << (MATLAB)Ctask << std::endl;
+	    sotDEBUG(1) << "btask"<<i<<" = "     << btask << std::endl;
+	  }
+
+
+	/* --- */
+	sotDEBUG(1) << "Initial config." << std::endl;
+	hsolver->reset();
+	hsolver->setInitialActiveSet();
+
+	sotDEBUG(1) << "Run for a solution." << std::endl;
+	hsolver->activeSearch(solution);
+	sotDEBUG(1) << "solution = " << (MATLAB)solution << std::endl;
+
+
+	if( controlFreeFloating )
+	  {
+	    EIGEN_VECTOR_FROM_VECTOR( control,mlcontrol,nbDofs );
+	    control=solution;
+	  }
+	else
+	  {
+	    EIGEN_VECTOR_FROM_VECTOR( control,mlcontrol,nbDofs-6 );
+	    control=solution.tail( nbDofs-6 );
+	  }
+
+	sotDEBUG(1) << "control = " << mlcontrol << std::endl;
+	return mlcontrol;
+      }
+
+
+      /* --- COMMANDS ---------------------------------------------------------- */
+      /* --- COMMANDS ---------------------------------------------------------- */
+      /* --- COMMANDS ---------------------------------------------------------- */
+
+      void SolverKine::
+      debugOnce( void )
+      {
+	dg::sot::DebugTrace::openFile("/tmp/sot.txt");
+	hsolver->debugOnce();
+      }
+
+      /* --- ENTITY ----------------------------------------------------------- */
+      /* --- ENTITY ----------------------------------------------------------- */
+      /* --- ENTITY ----------------------------------------------------------- */
+
+      void SolverKine::
+      display( std::ostream& os ) const
+      {
+	os << "SolverKine "<<getName() << ": " << nbDofs <<" joints." << std::endl;
+	try{
+	  os <<"control = "<<controlSOUT.accessCopy() <<std::endl << std::endl;
+	}  catch (dynamicgraph::ExceptionSignal e) {}
+	stack_t::display(os);
+      }
+
+      void SolverKine::
+      commandLine( const std::string& cmdLine,
+		   std::istringstream& cmdArgs,
+		   std::ostream& os )
+      {
+	if( cmdLine == "help" )
+	  {
+	    os << "SolverKine:\n"
+	       << "\t- debugOnce: open trace-file for next iteration of the solver." << std::endl;
+	    stackCommandLine( cmdLine,cmdArgs,os );
+	    Entity::commandLine( cmdLine,cmdArgs,os );
+	  }
+	else if( cmdLine == "debugOnce" )
+	  {
+	    debugOnce();
+	  }
+	/* TODO: add controlFreeFloating g/setter. */
+	else if( stackCommandLine( cmdLine,cmdArgs,os ) );
+	else
+	  {
+	    Entity::commandLine( cmdLine,cmdArgs,os );
+	  }
+      }
+
+    } // namespace dyninv
+  } // namespace sot
+} // namespace dynamicgraph
+

src/solver-kine.h

+/*
+ * Copyright 2011, Nicolas Mansard, LAAS-CNRS
+ *
+ * This file is part of sot-dyninv.
+ * sot-dyninv 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-dyninv 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-dyninv.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __sot_dyninv_SolverKine_H__
+#define __sot_dyninv_SolverKine_H__
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined (WIN32)
+#  if defined (solver_kine_EXPORTS)
+#    define SOTSOLVERKINE_EXPORT __declspec(dllexport)
+#  else
+#    define SOTSOLVERKINE_EXPORT __declspec(dllimport)
+#  endif
+#else
+#  define SOTSOLVERKINE_EXPORT
+#endif
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+
+/* SOT */
+#include <sot-dyninv/signal-helper.h>
+#include <sot-dyninv/entity-helper.h>
+#include <sot-dyninv/stack-template.h>
+#include <sot-dyninv/task-dyn-pd.h>
+#include <soth/HCOD.hpp>
+
+namespace dynamicgraph {
+  namespace sot {
+    namespace dyninv {
+
+      /* --------------------------------------------------------------------- */
+      /* --- CLASS ----------------------------------------------------------- */
+      /* --------------------------------------------------------------------- */
+
+      class SOTSOLVERKINE_EXPORT SolverKine
+	:public ::dynamicgraph::Entity
+	,public ::dynamicgraph::EntityHelper<SolverKine>
+	,public sot::Stack< TaskAbstract >
+	{
+
+	public: /* --- CONSTRUCTOR ---- */
+
+	  SolverKine( const std::string & name );
+
+	public: /* --- STACK INHERITANCE --- */
+
+	  typedef sot::Stack<TaskAbstract> stack_t;
+	  using stack_t::TaskDependancyList_t;
+	  using stack_t::StackIterator_t;
+	  using stack_t::StackConstIterator_t;
+	  using stack_t::stack;
+
+	  virtual TaskDependancyList_t getTaskDependancyList( const TaskAbstract& task );
+	  virtual void addDependancy( const TaskDependancyList_t& depList );
+	  virtual void removeDependancy( const TaskDependancyList_t& depList );
+	  virtual void resetReady( void );
+
+	public: /* --- ENTITY INHERITANCE --- */
+
+	  static const std::string CLASS_NAME;
+	  virtual void display( std::ostream& os ) const;
+	  virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
+
+	  virtual void commandLine( const std::string& cmdLine,
+				    std::istringstream& cmdArgs,
+				    std::ostream& os );
+
+	public:  /* --- SIGNALS --- */
+
+	  DECLARE_SIGNAL_IN(damping,double);
+	  DECLARE_SIGNAL_OUT(control,ml::Vector);
+	  DECLARE_SIGNAL(velocity,OUT,ml::Vector);
+
+	public: /* --- COMMANDS --- */
+	  void debugOnce( void );
+	  bool controlFreeFloating;
+
+	private: /* --- INTERNAL COMPUTATIONS --- */
+	  void refreshTaskTime( int time );
+	  bool checkSolverSize( void );
+	  void resizeSolver( void );
+
+	private:
+	  typedef boost::shared_ptr<soth::HCOD> hcod_ptr_t;
+	  hcod_ptr_t hsolver;
+
+	  std::vector< Eigen::MatrixXd > Ctasks;
+	  std::vector< soth::VectorBound > btasks;
+
+	  Eigen::VectorXd solution;
+
+	}; // class SolverKine
+
+    } // namespace dyninv
+  } // namespace sot
+} // namespace dynamicgraph
+
+
+
+#endif // #ifndef __sot_dyninv_SolverKine_H__