ivigit.

src/contact-selecter.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 contacter_op_space__INIT
+{
+public:contacter_op_space__INIT( void )
+  {
+    //    dynamicgraph::sot::DebugTrace::openFile("/tmp/dynred.txt");
+  }
+};
+contacter_op_space__INIT contacter_op_space_initiator;
+#endif //#ifdef VP_DEBUG
+
+#include <sot-dyninv/commands-helper.h>
+#include <sot-dyninv/contact-selecter.h>
+#include <dynamic-graph/factory.h>
+#include <boost/foreach.hpp>
+
+namespace dynamicgraph
+{
+  namespace sot
+  {
+    namespace dyninv
+    {
+#include <Eigen/Cholesky>
+
+      namespace dg = ::dynamicgraph;
+      using namespace dg;
+      using dg::SignalBase;
+
+      /* --- DG FACTORY ------------------------------------------------------- */
+      DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ContactSelecter,"ContactSelecter");
+
+      /* --- CONSTRUCTION ----------------------------------------------------- */
+      /* --- CONSTRUCTION ----------------------------------------------------- */
+      /* --- CONSTRUCTION ----------------------------------------------------- */
+      ContactSelecter::
+      ContactSelecter( const std::string & name )
+	: Entity(name)
+
+	,CONSTRUCT_SIGNAL_OUT(trigger,int,sotNOSIGNAL)
+	,contacts()
+	,solver(NULL)
+
+      {
+	signalRegistration( triggerSOUT );
+	initCommands();
+      }
+
+      void ContactSelecter::
+      initCommands( void )
+      {
+	using namespace dynamicgraph::command;
+
+	addCommand("setContact",
+		   makeCommandVoid2(*this,&ContactSelecter::setContact,
+				    docCommandVoid2("Create the contact, and give the name of the task that reprensent the contact function.",
+						    "contact name (string)",
+						    "name of the task definining the contact (string)")));
+
+	addCommand("setTask",
+		   makeCommandVoid2(*this,&ContactSelecter::setTask,
+				    docCommandVoid2("Create the contact, and give the name of the task that is orthogonal to the contact. The task will be active when the input boolean is false.",
+						    "contact name (string)",
+						    "name of the task definining the orthogonal (string)")));
+
+	addCommand("setContactAndTask",
+		   makeCommandVoid3(*this,&ContactSelecter::setContactAndTask,
+				    docCommandVoid3("Create the contact, and give the name of both the contact task and the orthogonal task.",
+						    "contact name (string)",
+						    "name of the task definining the contact (string)",
+						    "name of the task definining the orthogonal (string)")));
+
+	addCommand("setSolver",
+		   makeCommandVoid1(*this,&ContactSelecter::setSolverReference,
+				    docCommandVoid1("Give the name of the solver",
+						    "solver name (string)")));
+
+	addCommand("setContactStatus",
+		   makeCommandVoid2(*this,&ContactSelecter::setContactStatus,
+				    docCommandVoid2("Explicitely specify the status",
+						    "contact name (string)",
+						    "status (bool)")));
+
+	addCommand("verbose",
+		   makeDirectSetter(*this, &verbose,
+				    docDirectSetter("verbose","bool")));
+	
+
+      }
+
+      /* --- COMMANDS ---------------------------------------------------------- */
+      /* --- COMMANDS ---------------------------------------------------------- */
+      /* --- COMMANDS ---------------------------------------------------------- */
+
+      void ContactSelecter::
+      setSolverReference( const std::string& solverName )
+      {
+	solver = dynamic_cast<Solver_ptr> ( &g_pool().getEntity(solverName) );
+      }
+
+      ContactSelecter::ContactInfo::
+      ContactInfo( const std::string & contactName,
+		   const std::string & contactTaskName,
+		   const std::string & taskName,
+		   ContactSelecter& mother)
+	:contactTaskName(contactTaskName)
+	,complementaryTaskName(taskName)
+	,contactName( contactName )
+	,status(false)
+	,activationSIN( NULL,"ContactSelecter("+mother.getName()+")::input(bool)::"+contactName+"Activation" )
+	,supportSIN( NULL,"ContactSelecter("+mother.getName()+")::input(matrix)::"+contactName+"Support" )
+      {
+	mother.signalRegistration( activationSIN << supportSIN );
+	mother.triggerSOUT.addDependency( activationSIN );
+      }
+
+      void ContactSelecter::
+      setContact( const std::string & contactName, const std::string & contactTaskName )
+      {
+	contacts[contactName] = ContactInfo_ptr( new ContactInfo(contactName,contactTaskName,"",*this) );
+      }
+
+      void ContactSelecter::
+      setTask( const std::string & contactName, const std::string & taskName )
+      {
+	contacts[contactName] = ContactInfo_ptr( new ContactInfo(contactName,"",taskName,*this) );
+      }
+      void ContactSelecter::
+      setContactAndTask( const std::string & contactName,
+			 const std::string & contactTaskName,
+			 const std::string & taskName )
+      {
+	contacts[contactName]
+	  = ContactInfo_ptr( new ContactInfo(contactName,contactTaskName,taskName,*this) );
+      }
+      void ContactSelecter::
+      setContactStatus(  const std::string & contactName,
+			 const bool& status )
+      {
+	contacts[contactName]->status = status;
+      }
+      bool ContactSelecter::
+      getContactStatus(  const std::string & contactName )
+      {
+	return contacts[contactName]->status;
+      }
+
+
+      /* --- SIGNALS ---------------------------------------------------------- */
+      /* --- SIGNALS ---------------------------------------------------------- */
+      /* --- SIGNALS ---------------------------------------------------------- */
+
+      int& ContactSelecter::
+      triggerSOUT_function( int&,int t)
+      {
+	BOOST_FOREACH( ContactInfo_map::value_type iter, contacts)
+	  {
+	    ContactInfo_ptr contact = iter.second;
+	    bool newStatus = contact->activationSIN(t);
+	    if( newStatus!=contact->status )
+	      {
+		assert( solver!=NULL);
+		if( newStatus )  // Activate the contact
+		  {
+		    if( verbose)
+		      std::cout << "Iter " << t
+				<< ":  Add the contact " << contact->contactName << std::endl;
+
+		    /* Add the contact to the solver. */
+		    if( contact->contactTaskName.size()>0 )
+		      {
+			solver->addContactFromTask(contact->contactTaskName,contact->contactName);
+			solver->getSupportSIN(contact->contactName)->plug( &contact->supportSIN );
+			//solver->getSupportSIN(contact->contactName)->setReady();
+			contact->supportSIN.setReady();
+		      }
+		    /* Remove the complementary task from the solver. */
+		    if( contact->complementaryTaskName.size()>0 )
+		      {
+			TaskDynPD & task = dynamic_cast<TaskDynPD&>
+			  ( g_pool().getEntity(contact->complementaryTaskName) );
+			solver->remove(task);
+		      }
+		  }
+		else             // Unactive the contact
+		  {
+		    if( verbose)
+		      std::cout << "Iter " << t
+				<< ":  Rm the contact " << contact->contactName << std::endl;
+
+		    /* Add the complementary task too the solver. */
+		    if( contact->complementaryTaskName.size()>0 )
+		      {
+			TaskDynPD & task = dynamic_cast<TaskDynPD&>
+			  ( g_pool().getEntity(contact->complementaryTaskName) );
+			solver->push(task);
+		      }
+
+		    /* Remove the contact from the solver. */
+		    if( contact->contactTaskName.size()>0 )
+		      {
+			solver->removeContact(contact->contactName);
+		      }
+
+		  }
+
+		contact->status = newStatus;
+	      }
+	  }
+
+      }
+      
+      void ContactSelecter::display( std::ostream& os ) const
+      {
+	using std::endl;
+	os << "Contact&task selecter '" << getName() << "':" << endl
+	   << " - attached solver: " << solver->getName() << endl
+	   << " - contacts:" << endl;
+	
+	BOOST_FOREACH( ContactInfo_map::value_type iter, contacts)
+	  {
+	    ContactInfo_ptr contact = iter.second;
+	    os << "   * " << contact->contactName << ": "
+	       << "contact task '" << contact->contactTaskName <<"'"
+	       << "   -- complementary task '" << contact->complementaryTaskName <<"'"
+	       << endl;
+	  }
+      }
+
+    } // namespace dyninv
+  } // namespace sot
+} // namespace dynamicgraph
+

src/contact-selecter.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_ContactSelecter_H__
+#define __sot_dyninv_ContactSelecter_H__
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined (WIN32)
+#  if defined (solver_op_space_EXPORTS)
+#    define SOTCONTACTSELECTER_EXPORT __declspec(dllexport)
+#  else
+#    define SOTCONTACTSELECTER_EXPORT __declspec(dllimport)
+#  endif
+#else
+#  define SOTCONTACTSELECTER_EXPORT
+#endif
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+
+/* SOT */
+#include <sot-dyninv/signal-helper.h>
+#include <sot-dyninv/entity-helper.h>
+#include <sot-dyninv/solver-dyn-reduced.h>
+#include <map>
+
+namespace dynamicgraph {
+  namespace sot {
+    namespace dyninv {
+
+      /* --------------------------------------------------------------------- */
+      /* --- CLASS ----------------------------------------------------------- */
+      /* --------------------------------------------------------------------- */
+
+      class SOTCONTACTSELECTER_EXPORT ContactSelecter
+	:public ::dynamicgraph::Entity
+	,public ::dynamicgraph::EntityHelper<ContactSelecter>
+	{
+
+	public: /* --- CONSTRUCTOR ---- */
+
+	  ContactSelecter( const std::string & name );
+
+	public: /* --- ENTITY INHERITANCE --- */
+
+	  static const std::string CLASS_NAME;
+	  virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
+	  virtual void display( std::ostream& os ) const;
+	  void initCommands( void );
+
+	public:  /* --- SIGNALS --- */
+
+	  DECLARE_SIGNAL_OUT(trigger,int);
+
+	public: /* --- COMMANDS --- */
+
+	  void setSolverReference( const std::string& solverName );
+	  void setContact( const std::string & contactName, const std::string & contactTaskName );
+	  void setTask( const std::string & contactName, const std::string & taskName );
+	  void setContactAndTask( const std::string & contactName,
+				  const std::string & contactTaskName,
+				  const std::string & taskName );
+	  void setContactStatus( const std::string & contactName,
+				 const bool & status );
+	  bool getContactStatus( const std::string & contactName );
+
+	public:
+
+	  struct ContactInfo
+	  {
+	    std::string contactTaskName;
+	    std::string complementaryTaskName;
+	    std::string contactName;
+	    bool status;
+	    DECLARE_SIGNAL_IN(activation,bool);
+	    DECLARE_SIGNAL_IN(support,ml::Matrix);
+	    ContactInfo( const std::string & contactName,
+			 const std::string & contactTaskName,
+			 const std::string & complementaryTaskName,
+			 ContactSelecter& mother);
+	  };
+	  typedef boost::shared_ptr<ContactInfo> ContactInfo_ptr;
+	  typedef std::map<std::string, ContactInfo_ptr> ContactInfo_map;
+
+	  typedef SolverDynReduced Solver;
+	  typedef Solver* Solver_ptr;
+
+	private:
+	  ContactInfo_map contacts;
+	  Solver_ptr solver;
+	  bool verbose;
+
+	}; // class ContactSelecter
+
+    } // namespace dyninv
+  } // namespace sot
+} // namespace dynamicgraph
+
+
+
+#endif // #ifndef __sot_dyninv_ContactSelecter_H__