IVIGIT.

CMakeLists.txt

@@ -46,6 +46,7 @@ ADD_REQUIRED_DEPENDENCY("soth >= 0.0.1")
 
 # List plug-ins that will be compiled.
 SET(libs
+  solver-op-space
   controller-pd
   task-dyn-pd
   dynamic-integrator

include/CMakeLists.txt

@@ -14,7 +14,18 @@
 # sot-dyninv. If not, see <http://www.gnu.org/licenses/>.
 
 SET(${PROJECT_NAME}_HEADERS
+	commands-helper.h
+	entity-helper.h
+	signal-helper.h
+
+	mal-to-eigen.h
+	stack-template.h
+	stack-template.t.cpp
+
 	controller-pd.h
+	task-dyn-pd.h
+	dynamic-integrator.h
+	solver-op-space.h
 )
 
 # Recreate correct path for the headers

include/sot-dyninv/solver-op-space.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_SolverOpSpace_H__
+#define __sot_dyninv_SolverOpSpace_H__
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined (WIN32)
+#  if defined (dynamic_interpretor_EXPORTS)
+#    define SOTSOLVEROPSPACE_EXPORT __declspec(dllexport)
+#  else
+#    define SOTSOLVEROPSPACE_EXPORT __declspec(dllimport)
+#  endif
+#else
+#  define SOTSOLVEROPSPACE_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 sot {
+  namespace dyninv {
+
+  /* --------------------------------------------------------------------- */
+  /* --- CLASS ----------------------------------------------------------- */
+  /* --------------------------------------------------------------------- */
+
+  class SOTSOLVEROPSPACE_EXPORT SolverOpSpace
+    :public ::dynamicgraph::Entity
+      ,public ::dynamicgraph::EntityHelper<SolverOpSpace>
+      ,public sot::Stack< TaskDynPD >
+    {
+
+      public: /* --- CONSTRUCTOR ---- */
+
+	SolverOpSpace( const std::string & name );
+
+    public: /* --- STACK INHERITANCE --- */
+
+	typedef sot::Stack<TaskDynPD> 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 TaskDynPD& 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(matrixInertia,ml::Matrix);
+	DECLARE_SIGNAL_IN(velocity,ml::Vector);
+	DECLARE_SIGNAL_IN(dyndrift,ml::Vector);
+	DECLARE_SIGNAL_IN(damping,double);
+	DECLARE_SIGNAL_IN(breakFactor,double);
+
+	DECLARE_SIGNAL_OUT(control,ml::Vector);
+
+	DECLARE_SIGNAL(zmp,OUT,ml::Vector);
+	DECLARE_SIGNAL(acceleration,OUT,ml::Vector);
+
+    private:  /* --- CONTACT POINTS --- */
+
+	typedef boost::shared_ptr<dynamicgraph::SignalPtr<ml::Matrix,int> > matrixSINPtr;
+	typedef boost::shared_ptr<dynamicgraph::SignalPtr<ml::Vector,int> > vectorSINPtr;
+	typedef boost::shared_ptr<dynamicgraph::Signal<ml::Vector,int> > vectorSOUTPtr;
+	struct Contact
+	{
+	  matrixSINPtr jacobianSIN;
+	  matrixSINPtr JdotSIN;
+	  matrixSINPtr supportSIN;
+	  vectorSINPtr correctorSIN;
+	  vectorSOUTPtr forceSOUT,fnSOUT;
+	  std::pair<int,int> range;
+	};
+	typedef std::map< std::string,Contact > contacts_t;
+	contacts_t contactMap;
+
+    public:
+	void addContact( const std::string & name,
+			 dynamicgraph::Signal<ml::Matrix,int> * jacobianSignal,
+			 dynamicgraph::Signal<ml::Matrix,int> * JdotSignal,
+			 dynamicgraph::Signal<ml::Vector,int> * corrSignal,
+			 dynamicgraph::Signal<ml::Matrix,int> * contactPointsSignal );
+	void addContactFromTask( const std::string & name );
+	void removeContact( const std::string & name );
+	void dispContacts( std::ostream& os ) const;
+
+
+    public: /* --- COMMANDS --- */
+	void debugOnce( void );
+
+
+    private: /* --- INTERNAL COMPUTATIONS --- */
+	void refreshTaskTime( int time );
+	void initialResizeSolver( void );
+	void resizeSolver( void );
+
+    private:
+	int nbParam, nq,ntau;
+	int nbContactBodies,nbContactPoints,nfs;
+
+	typedef boost::shared_ptr<soth::HCOD> hcod_ptr_t;
+	hcod_ptr_t hsolver;
+
+	Eigen::MatrixXd Cdyn,Ccontact,Czmp,Czero;
+	soth::VectorBound bdyn,bcontact,bzmp,bzero;
+	std::vector< Eigen::MatrixXd > Ctasks;
+	std::vector< soth::VectorBound > btasks;
+
+	Eigen::VectorXd solution;
+
+	static const int nbPoint = 4;
+
+     }; // class SolverOpSpace
+
+  } // namespace dyninv
+} // namespace sot
+
+
+
+#endif // #ifndef __sot_dyninv_SolverOpSpace_H__

src/dynamic_graph/sot/dyninv/__init__.py

@@ -6,3 +6,6 @@ TaskDynPD('')
 
 from dynamic_integrator import DynamicIntegrator
 DynamicIntegrator('')
+
+from solver_op_space import SolverOpSpace
+SolverOpSpace('')