IVIGIT.

CMakeLists.txt

@@ -46,15 +46,18 @@ ADD_REQUIRED_DEPENDENCY("soth >= 0.0.1")
 
 # List plug-ins that will be compiled.
 SET(libs
-	controller-pd
-	task-dyn-pd
 	dynamic-integrator
-	solver-op-space
-	solver-kine
 	zmp-estimator
 	pseudo-robot-dynamic
 	robot-dyn-simu
+
+	controller-pd
+	task-dyn-pd
 	task-dyn-joint-limits
+	solver-op-space
+
+	solver-kine
+	task-joint-limits
   )
 
 LIST(APPEND LOGGING_WATCHED_TARGETS ${libs})
@@ -64,6 +67,11 @@ SET(headers
 	entity-helper.h
 	signal-helper.h
 
+	dynamic-integrator.h
+	zmp-estimator.h
+	pseudo-robot-dynamic.h
+	robot-dyn-simu.h
+
 	mal-to-eigen.h
 	stack-template.h
 	stack-template.t.cpp
@@ -71,13 +79,11 @@ SET(headers
 	controller-pd.h
 	task-dyn-pd.h
 	task-dyn-joint-limits.h
-	dynamic-integrator.h
 	solver-op-space.h
+
+	task-joint-limits.h
 	solver-kine.h
 
-	zmp-estimator.h
-	pseudo-robot-dynamic.h
-	robot-dyn-simu.h
 )
 
 # Add subdirectories.

src/dynamic_graph/sot/dyninv/__init__.py

@@ -24,3 +24,6 @@ RobotDynSimu('')
 
 from task_dyn_joint_limits import TaskDynJointLimits
 TaskDynJointLimits('')
+
+from task_joint_limits import TaskJointLimits
+TaskJointLimits('')

src/task-joint-limits.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/>.
+ */
+
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+//#define VP_DEBUG
+//#define VP_DEBUG_MODE 15
+#include <sot/core/debug.hh>
+
+#include <dynamic-graph/factory.h>
+#include <sot-dyninv/task-joint-limits.h>
+
+
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+namespace dynamicgraph
+{
+  namespace sot
+  {
+    namespace dyninv
+    {
+
+      namespace dg = ::dynamicgraph;
+
+      /* --- DG FACTORY ------------------------------------------------------- */
+      DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(TaskJointLimits,"TaskJointLimits");
+
+      /* ---------------------------------------------------------------------- */
+      /* --- CONSTRUCTION ----------------------------------------------------- */
+      /* ---------------------------------------------------------------------- */
+
+      TaskJointLimits::
+      TaskJointLimits( const std::string & name )
+	: TaskAbstract(name)
+
+	,CONSTRUCT_SIGNAL_IN(position,ml::Vector)
+	,CONSTRUCT_SIGNAL_IN(referenceInf,ml::Vector)
+	,CONSTRUCT_SIGNAL_IN(referenceSup,ml::Vector)
+	,CONSTRUCT_SIGNAL_IN(dt,double)
+	,CONSTRUCT_SIGNAL_IN(controlGain,double)
+	,CONSTRUCT_SIGNAL_IN(selec,Flags)
+
+	,CONSTRUCT_SIGNAL_OUT(normalizedPosition,ml::Vector,
+			      positionSIN<<referenceInfSIN<<referenceSupSIN)
+	,CONSTRUCT_SIGNAL_OUT(activeSize,int,
+			      positionSIN<<selecSIN)
+      {
+	taskSOUT.setFunction( boost::bind(&TaskJointLimits::computeTask,this,_1,_2) );
+	jacobianSOUT.setFunction( boost::bind(&TaskJointLimits::computeJacobian,this,_1,_2) );
+	taskSOUT.clearDependencies();
+	taskSOUT.addDependency( referenceSupSIN );
+	taskSOUT.addDependency( referenceInfSIN );
+	taskSOUT.addDependency( dtSIN );
+	taskSOUT.addDependency( positionSIN );
+	controlGainSIN = 1.0;
+	selecSIN = true;
+	signalRegistration( referenceSupSIN << referenceInfSIN << dtSIN << selecSIN
+			    << activeSizeSOUT
+			    << controlGainSIN << positionSIN << normalizedPositionSOUT );
+      }
+
+      /* ---------------------------------------------------------------------- */
+      /* --- COMPUTATION ------------------------------------------------------ */
+      /* ---------------------------------------------------------------------- */
+      int& TaskJointLimits::
+      activeSizeSOUT_function(int& res, int time)
+      {
+	const Flags & selec = selecSIN(time);
+	const int size = positionSIN(time).size();
+	res=0;
+	for( int i=0;i<size;++i )
+	  {
+	    if(selec(i)) res++;
+	  }
+	return res;
+      }
+
+
+      dg::sot::VectorMultiBound& TaskJointLimits::
+      computeTask( dg::sot::VectorMultiBound& res,int time )
+      {
+	const ml::Vector & position = positionSIN(time);
+	const ml::Vector & refInf = referenceInfSIN(time);
+	const ml::Vector & refSup = referenceSupSIN(time);
+	const Flags & selec = selecSIN(time);
+	const double K = 1.0/(dtSIN(time)*controlGainSIN(time));
+	const int size = position.size(), activeSize=activeSizeSOUT(time);
+	sotDEBUG(35) << "position = " << position << std::endl;
+
+	res.resize(activeSize); int idx=0;
+	for( int i=0;i<size;++i )
+	  {
+	    if( selec(i) )
+	      res[idx++] = dg::sot::MultiBound( (refInf(i)-position(i))*K,
+						(refSup(i)-position(i))*K );
+	  }
+
+	sotDEBUG(15) << "taskU = "<< res << std::endl;
+	return res;
+      }
+
+      ml::Matrix& TaskJointLimits::
+      computeJacobian( ml::Matrix& J,int time )
+      {
+	const Flags & selec = selecSIN(time);
+	const int size = positionSIN(time).size(), activeSize=activeSizeSOUT(time);
+	J.resize(activeSize,size);	J.setZero();
+
+	int idx=0;
+	for( int i=0;i<size;++i )
+	  {
+	    if( selec(i) ) J(idx++,i) = 1;
+	  }
+
+	return J;
+      }
+
+      ml::Vector& TaskJointLimits::
+      normalizedPositionSOUT_function( ml::Vector& res, int time )
+      {
+	const ml::Vector & position = positionSIN(time);
+	const ml::Vector & refInf = referenceInfSIN(time);
+	const ml::Vector & refSup = referenceSupSIN(time);
+	const Flags & selec = selecSIN(time);
+	const int size = position.size(), activeSize=activeSizeSOUT(time);
+	assert( size==(int)refInf.size() && size==(int)refSup.size() );
+
+	res.resize( activeSize ); int idx=0;
+	for( int i=0;i<size;++i )
+	  { if(selec(i))
+	      res(idx++) = ( position(i)-refInf(i) ) / ( refSup(i)-refInf(i) ); }
+	return res;
+      }
+
+      /* ------------------------------------------------------------------------ */
+      /* --- DISPLAY ENTITY ----------------------------------------------------- */
+      /* ------------------------------------------------------------------------ */
+
+      void TaskJointLimits::
+      display( std::ostream& os ) const
+      {
+	os << "TaskJointLimits " << name << ": " << std::endl;
+      }
+
+    } // namespace dyninv
+  } // namespace sot
+} // namespace dynamicgraph
+

src/task-joint-limits.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_TaskJointLimits_H__
+#define __sot_dyninv_TaskJointLimits_H__
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined (WIN32)
+#  if defined (task_dyn_joint_limits_EXPORTS)
+#    define SOTTASKDYNJOINTLIMITS_EXPORT __declspec(dllexport)
+#  else
+#    define SOTTASKDYNJOINTLIMITS_EXPORT __declspec(dllimport)
+#  endif
+#else
+#  define SOTTASKDYNJOINTLIMITS_EXPORT
+#endif
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+/* SOT */
+#include <sot-dyninv/signal-helper.h>
+#include <sot-dyninv/entity-helper.h>
+#include <sot/core/task.hh>
+#include <sot/core/flags.hh>
+
+namespace dynamicgraph {
+  namespace sot {
+    namespace dyninv {
+
+      /* --------------------------------------------------------------------- */
+      /* --- CLASS ----------------------------------------------------------- */
+      /* --------------------------------------------------------------------- */
+
+
+      class SOTTASKDYNJOINTLIMITS_EXPORT TaskJointLimits
+	:public TaskAbstract
+	,public EntityHelper<TaskJointLimits>
+	{
+
+	public: /* --- CONSTRUCTOR ---- */
+
+	  TaskJointLimits( const std::string& name );
+
+	public: /* --- ENTITY INHERITANCE --- */
+
+	  static const std::string CLASS_NAME;
+	  virtual const std::string& getClassName( void ) { return CLASS_NAME; }
+	  virtual void display( std::ostream& os ) const;
+
+	public:  /* --- SIGNALS --- */
+
+	  DECLARE_SIGNAL_IN(position,ml::Vector);
+	  DECLARE_SIGNAL_IN(referenceInf,ml::Vector);
+	  DECLARE_SIGNAL_IN(referenceSup,ml::Vector);
+	  DECLARE_SIGNAL_IN(dt,double);
+	  DECLARE_SIGNAL_IN(controlGain,double);
+	  DECLARE_SIGNAL_IN(selec,Flags);
+
+	  DECLARE_SIGNAL_OUT(normalizedPosition,ml::Vector);
+	  DECLARE_SIGNAL_OUT(activeSize,int);
+
+	public:  /* --- COMPUTATION --- */
+	  dg::sot::VectorMultiBound&
+	    computeTask( dg::sot::VectorMultiBound& res,int time );
+	  ml::Matrix& computeJacobian( ml::Matrix& J,int time );
+
+	}; // class TaskJointLimits
+
+    } // namespace dyninv
+  } // namespace sot
+} // namespace dynamicgraph
+
+
+#endif // #ifndef __sot_dyninv_TaskJointLimits_H__