Add TaskDynPassingPoint as a dynamic task

There is meta_task_dyn_passing_point to help in the initialization
of this task

AUTHORS

@@ -3,3 +3,4 @@ This package was written by and with the assistance of:
 * Nicolas Mansard       nicolas.mansard@laas.fr
 * Layale Saab           lsaab@laas.fr
 * Olivier Stasse	olivier.stasse@aist.go.jp
+* Oscar E. Ramos P.     oramos@laas.fr
\ No newline at end of file

CMakeLists.txt

@@ -56,6 +56,7 @@ SET(libs
 	task-dyn-joint-limits
 	task-dyn-limits
 	task-dyn-inequality
+	task-dyn-passing-point
 	solver-op-space
 	solver-dyn-reduced
 
@@ -89,6 +90,7 @@ SET(headers
 	task-dyn-joint-limits.h
 	task-dyn-limits.h
 	task-dyn-inequality.h
+	task-dyn-passing-point.h
 	solver-op-space.h
 	solver-dyn-reduced.h
 
@@ -107,6 +109,7 @@ SET(headers
 list(APPEND task-dyn-limits_plugins_dependencies task-dyn-pd)
 list(APPEND task-dyn-inequality_plugins_dependencies task-dyn-pd)
 list(APPEND task-dyn-joint-limits_plugins_dependencies task-dyn-pd)
+list(APPEND task-dyn-passing-point_plugins_dependencies task-dyn-pd)
 list(APPEND pseudo-robot-dynamic_plugins_dependencies dynamic-integrator)
 list(APPEND contact-selecter_plugins_dependencies solver-dyn-reduced)
 

src/CMakeLists.txt

@@ -73,6 +73,7 @@ ENDFOREACH(lib)
 INSTALL(FILES
   ${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/dyninv/__init__.py
   ${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/dyninv/meta_task_dyn_6d.py
+  ${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/dyninv/meta_task_dyn_passing_point.py
   ${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/dyninv/meta_tasks_dyn.py
   ${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/dyninv/meta_tasks_dyn_relative.py
   ${CMAKE_CURRENT_BINARY_DIR}/../python/robot_specific.py

src/dynamic_graph/sot/dyninv/__init__.py

@@ -9,6 +9,7 @@ from zmp_estimator import ZmpEstimator
 from robot_dyn_simu import RobotDynSimu
 from task_dyn_joint_limits import TaskDynJointLimits
 from task_dyn_limits import TaskDynLimits
+from task_dyn_passing_point import TaskDynPassingPoint
 from task_joint_limits import TaskJointLimits
 from task_inequality import TaskInequality
 from feature_projected_line import FeatureProjectedLine

src/dynamic_graph/sot/dyninv/meta_task_dyn_passing_point.py

+from dynamic_graph import plug
+from dynamic_graph.sot.core import GainAdaptive, OpPointModifier
+from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple,rotate, rpy2tr
+from dynamic_graph.sot.core.meta_task_6d import MetaTask6d
+from dynamic_graph.sot.dyninv import TaskDynPassingPoint
+
+from numpy import matrix, identity, zeros, eye, array, sqrt, radians, arccos, linalg, inner
+
+
+class MetaTaskDynPassingPoint(MetaTask6d):
+    def createTask(self):
+        self.task = TaskDynPassingPoint('task'+self.name)
+        self.task.dt.value = 1e-3
+    def createGain(self):
+        pass
+    def plugEverything(self):
+        self.feature.setReference(self.featureDes.name)
+        plug(self.dyn.signal(self.opPoint), self.feature.signal('position'))
+        plug(self.dyn.signal('J'+self.opPoint), self.feature.signal('Jq'))
+        self.task.add(self.feature.name)
+        plug(self.dyn.velocity, self.task.qdot)
+        ''' Dummy initialization '''
+        self.task.duration.value = 1
+        self.task.velocityDes.value = (0,0,0,0,0,0)
+        self.task.initialTime.value = 0
+        #self.task.currentTime.value = 0
+    
+    def __init__(self,*args):
+        MetaTask6d.__init__(self,*args)
+
+
+def goto6dPP(task, position, velocity, duration, current):
+    if isinstance(position,matrix): position = vectorToTuple(position)
+    if( len(position)==3 ):
+        ''' If only position specification '''
+        M=eye(4)
+        M[0:3,3] = position
+    else:
+        ''' If there is position and orientation '''
+        M = array( rpy2tr(*position[3:7]) )
+        M[0:3,3] = position[0:3]
+    task.feature.selec.value = "111111"
+    task.task.controlSelec.value = "111111"
+    task.featureDes.position.value = matrixToTuple(M)
+    task.task.duration.value = duration
+    task.task.initialTime.value = current
+    task.task.velocityDes.value = velocity
+    task.task.resetJacobianDerivative()
+
+
+def gotoNdPP(task, position, velocity, selec, duration, current):
+    if isinstance(position,matrix): position = vectorToTuple(position)
+    if( len(position)==3 ):
+        M=eye(4)
+        M[0:3,3] = position
+    else:
+        M = array( rpy2tr(*position[3:7]) )
+        M[0:3,3] = position[0:3]
+    if isinstance(selec,str):  
+        task.feature.selec.value = selec
+        task.task.controlSelec.value = selec
+    else:
+        task.feature.selec.value = toFlags(selec)
+        task.task.controlSelec.value = toFlags(selec)
+    task.featureDes.position.value = matrixToTuple(M)
+    task.task.duration.value = duration
+    task.task.initialTime.value = current
+    task.task.velocityDes.value = velocity
+    task.task.resetJacobianDerivative()

src/task-dyn-passing-point.cpp

+/*
+ * Copyright 2012, Oscar E. Ramos Ponce, 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 <dynamic-graph/factory.h>
+
+#include <sot/core/debug.hh>
+#include <sot/core/feature-abstract.hh>
+
+#include <sot-dyninv/task-dyn-passing-point.h>
+#include <sot-dyninv/commands-helper.h>
+
+#include <boost/foreach.hpp>
+#include <Eigen/Dense>
+
+
+namespace dynamicgraph
+{
+  namespace sot
+  {
+    namespace dyninv
+    {
+
+      namespace dg = ::dynamicgraph;
+
+      /* --- DG FACTORY ------------------------------------------------------- */
+      DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(TaskDynPassingPoint,"TaskDynPassingPoint");
+
+      /* ---------------------------------------------------------------------- */
+      /* --- CONSTRUCTION ----------------------------------------------------- */
+      /* ---------------------------------------------------------------------- */
+
+      TaskDynPassingPoint::
+      TaskDynPassingPoint( const std::string & name )
+	: TaskDynPD(name)
+
+	,CONSTRUCT_SIGNAL_IN(velocityDes, ml::Vector)
+	,CONSTRUCT_SIGNAL_IN(duration, double)
+	,CONSTRUCT_SIGNAL_IN(initialTime, double)
+
+	,CONSTRUCT_SIGNAL_OUT(velocityCurrent, ml::Vector,
+			      qdotSIN<<jacobianSOUT )
+	,CONSTRUCT_SIGNAL_OUT(velocityDesired, ml::Vector,
+			      velocityDesSIN<<controlSelectionSIN )
+
+	,previousTask(0u)
+      {
+	taskSOUT.setFunction( boost::bind(&TaskDynPassingPoint::computeTaskSOUT,this,_1,_2) );
+	taskSOUT.addDependency( velocityCurrentSOUT );
+	taskSOUT.addDependency( durationSIN );
+	taskSOUT.addDependency( velocityDesiredSOUT );
+
+	signalRegistration( velocityDesSIN << durationSIN << initialTimeSIN
+			    << velocityCurrentSOUT << velocityDesiredSOUT );
+
+      }
+
+
+      /* ---------------------------------------------------------------------- */
+      /* --- SIGNALS ---------------------------------------------------------- */
+      /* ---------------------------------------------------------------------- */
+
+      /* Current velocity using the Jacobian: dx_0 = J*dq */
+      ml::Vector& TaskDynPassingPoint::
+      velocityCurrentSOUT_function( ml::Vector& velocityCurrent, int time )
+      {
+      	sotDEBUGIN(15);
+
+      	const ml::Vector & qdot = qdotSIN(time);
+      	const ml::Matrix & J    = jacobianSOUT(time);
+      	velocityCurrent.resize( J.nbRows() );
+      	J.multiply(qdot, velocityCurrent);
+
+      	sotDEBUGOUT(15);
+      	return velocityCurrent;
+      }
+
+
+      /* Select the correct components of the desired velocity according to 'selec' */
+      ml::Vector& TaskDynPassingPoint::
+      velocityDesiredSOUT_function( ml::Vector& velocityDesired, int time )
+      {
+      	sotDEBUGIN(15);
+
+	const ml::Vector & velocityDesiredFull = velocityDesSIN(time);
+	const Flags &fl = controlSelectionSIN(time);
+
+	unsigned int dim = 0;
+	for( int i=0;i<6;++i ) if( fl(i) ) dim++;
+	velocityDesired.resize( dim );
+
+	unsigned int cursor = 0;
+	for( unsigned int i=0;i<6;++i )
+	  { if( fl(i) ) velocityDesired(cursor++) = velocityDesiredFull(i); }
+
+      	sotDEBUGOUT(15);
+      	return velocityDesired;
+      }
+
+
+      /* Task computation */
+      dg::sot::VectorMultiBound& TaskDynPassingPoint::
+      computeTaskSOUT( dg::sot::VectorMultiBound& task, int time )
+      {
+	sotDEBUGIN(15);
+
+	const double& fullDuration     = durationSIN(time);
+	const double& n0               = initialTimeSIN(time);
+	const double& dt               = dtSIN(time);
+	const ml::Vector & e           = errorSOUT(time);
+	const ml::Vector & vel_current = velocityCurrentSOUT(time);
+	const ml::Vector & vel_desired = velocityDesiredSOUT(time);
+
+	double T = fabs( fullDuration-(time-n0)*dt );
+	//double T = fullDuration-(time-n0)*dt;
+	//std::cout << "duration left: " << T << std::endl;
+	
+	assert( e.size() == vel_current.size()  && e.size() == vel_desired.size() );
+	task.resize( e.size() );
+
+	if(previousTask.size() != task.size())
+	  previousTask.resize( e.size() );
+
+	/* --- Using a pseudoinverse --- */
+	// Eigen::MatrixXd M_t = Eigen::MatrixXd::Zero(2*e.size(),2*e.size());
+	// Eigen::VectorXd V_ddx;
+	// Eigen::VectorXd b;
+
+	// V_ddx.resize(2*e.size());
+	// b.resize(2*e.size());
+
+	// for( unsigned int i=0; i<task.size(); ++i)
+	//   {
+	//     M_t(i,i) = T*T/3.0;
+	//     M_t(i,i+e.size())= T*T/6.0;
+	//     M_t(i+e.size(),i) = T/2.0;
+	//     M_t(i+e.size(),i+e.size()) = T/2;
+
+	//     b(i) = -e(i) - vel_current(i)*T;
+	//     b(i+e.size()) = vel_desired(i)-vel_current(i);
+	//   }
+
+	// //V_ddx = M_t.colPivHouseholderQr().solve(b);
+	// Eigen::ColPivHouseholderQR<Eigen::MatrixXd> colPiv(M_t);
+	// colPiv.setThreshold(1e-3);
+	// V_ddx = colPiv.solve(b);
+
+	// V_ddx = M_t.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
+
+	// std::cout << " == M_t:" << std::endl << M_t << std::endl;
+	// std::cout << " == b:" << std::endl << b << std::endl;
+	// std::cout << " == V_ddx:" << std::endl<< V_ddx << std::endl;
+
+	// for (unsigned int i=0; i<task.size(); ++i)
+	//   {
+	//     task[i] = V_ddx(i);
+	//   }
+	
+	/* --- Computing ddx(0) "manually" --- */
+	for( unsigned int i=0;i<task.size(); ++i )
+	  {
+	    if (T>dt)
+	      {
+		task[i] = - 6*e(i)/(T*T) - 2/T*(vel_desired(i)+2*vel_current(i)) ;
+		previousTask(i) = - 6*e(i)/(T*T) - 2/T*(vel_desired(i)+2*vel_current(i)) ;
+	      }
+	    else
+	      {
+		task[i] = previousTask(i);
+		//task[i] = 0;
+	      }
+	  }
+
+	sotDEBUGOUT(15);
+	return task;
+      }
+
+
+      /* ---------------------------------------------------------------------- */
+      /* --- ENTITY ----------------------------------------------------------- */
+      /* ---------------------------------------------------------------------- */
+
+      void TaskDynPassingPoint::
+      display( std::ostream& os ) const
+      {
+      	os << "TaskDynPassingPoint " << name << ": " << std::endl;
+      	os << "--- LIST ---  " << std::endl;
+      	BOOST_FOREACH( dg::sot::FeatureAbstract* feature,featureList )
+      	  {
+      	    os << "-> " << feature->getName() << std::endl;
+      	  }
+      }
+
+    } // namespace dyninv
+  } // namespace sot
+} // namespace dynamicgraph
+

src/task-dyn-passing-point.h

+/*
+ * Copyright 2012, Oscar E. Ramos Ponce, 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/>.
+ */
+
+/*! \file task-dyn-passing-point.h                                                                                                                                                                                 
+  \brief Defines a task based on time constraints as well as the initial 
+  and final position and velocity.
+*/
+
+
+#ifndef __sot_dyninv_TaskDynPassingPoint_H__
+#define __sot_dyninv_TaskDynPassingPoint_H__
+
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined (WIN32)
+#  if defined (task_dyn_passing_point_EXPORTS)
+#    define SOTTASKDYNPASSINGPOINT_EXPORT __declspec(dllexport)
+#  else
+#    define SOTTASKDYNPASSINGPOINT_EXPORT __declspec(dllimport)
+#  endif
+#else
+#  define SOTTASKDYNPASSINGPOINT_EXPORT
+#endif
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+
+/* SOT */
+#include <sot-dyninv/signal-helper.h>
+#include <sot-dyninv/entity-helper.h>
+#include <sot-dyninv/task-dyn-pd.h>
+#include <sot/core/task.hh>
+
+namespace dynamicgraph {
+  namespace sot {
+    namespace dyninv {
+
+      /* --------------------------------------------------------------------- */
+      /* --- CLASS ----------------------------------------------------------- */
+      /* --------------------------------------------------------------------- */
+
+
+      class SOTTASKDYNPASSINGPOINT_EXPORT TaskDynPassingPoint
+	:public ::dynamicgraph::sot::dyninv::TaskDynPD
+	,public ::dynamicgraph::EntityHelper<TaskDynPassingPoint>
+	{
+
+	public:
+	  /*! Constructor                                                                                                                                                                               
+	    @param name: Name of the task (string)                                                                                                             
+	  */
+	  TaskDynPassingPoint( const std::string & name );
+
+	  /* --- ENTITY INHERITANCE --- */
+	  typedef TaskDynPassingPoint EntityClassName;
+	  static const std::string CLASS_NAME;
+	  virtual void display( std::ostream& os ) const;
+	  virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
+
+	  /* --- SIGNALS --- */
+	  DECLARE_SIGNAL_IN(velocityDes, ml::Vector);
+	  DECLARE_SIGNAL_IN(duration, double);
+	  DECLARE_SIGNAL_IN(initialTime, double);
+
+	  DECLARE_SIGNAL_OUT(velocityCurrent, ml::Vector);
+	  DECLARE_SIGNAL_OUT(velocityDesired, ml::Vector);
+
+
+	protected:
+	  /* --- COMPUTATION --- */
+	  dg::sot::VectorMultiBound& computeTaskSOUT( dg::sot::VectorMultiBound& task, int iter );
+
+	private:
+	  ml::Vector previousTask;
+
+	}; // class  TaskDynPassingPoint
+
+    } // namespace dyninv
+  } // namespace sot
+} // namespace dynamicgraph
+
+
+#endif // #ifndef __sot_dyninv_TaskDynPassingPoint_H__