Move signals errordotSIN and errordotSOUT up to class FeatureAbstract.

  errordotSOUT callback computeErrorDot copies components of input signal
  errordotSIN of reference feature defined by selectionSIN flag to output
  signal errordotSOUT. This was previously performed by FeatureGeneric.

  Remove calls to withErrorDot, result is always true.

doc/pictures/feature.fig

@@ -7,104 +7,108 @@ A4
 Single
 -2
 1200 2
-6 910 413 4728 1522
-2 1 0 1 0 7 50 -1 -1 0.000 0 0 -1 1 0 2
-	1 1 1.00 59.15 118.32
-	 999 991 1797 991
+6 6120 6030 12690 8145
 2 2 0 1 0 7 50 -1 -1 0.000 0 0 -1 0 0 5
-	 1797 413 3797 413 3797 1522 1797 1522 1797 413
+	 8930 6070 12665 6070 12665 8145 8930 8145 8930 6070
 2 1 0 1 0 7 50 -1 -1 0.000 0 0 -1 1 0 2
-	1 1 1.00 59.15 118.32
-	 3797 1389 4595 1389
+	1 1 1.00 110.67 221.33
+	 7470 6485 8930 6485
 2 1 0 1 0 7 50 -1 -1 0.000 0 0 -1 1 0 2
-	1 1 1.00 59.15 118.32
-	 3797 1035 4595 1035
+	1 1 1.00 110.67 221.33
+	 6357 7149 8930 7149
 2 1 0 1 0 7 50 -1 -1 0.000 0 0 -1 1 0 2
-	1 1 1.00 59.15 118.32
-	 3797 679 4595 679
-4 1 0 50 -1 2 13 0.0000 4 150 1470 2775 1035 FeatureAbstract\001
-4 2 0 50 -1 0 11 0.0000 4 135 690 1664 900 selection\001
-4 0 0 50 -1 0 11 0.0000 4 135 765 3885 1301 dimension\001
-4 0 0 50 -1 0 11 0.0000 4 180 630 3885 945 jacobian\001
-4 0 0 50 -1 0 11 0.0000 4 90 360 3885 591 error\001
+	1 1 1.00 110.67 221.33
+	 6357 7896 8930 7896
+4 2 0 50 -1 0 22 0.0000 4 255 2520 8681 6983 articularJacobian\001
+4 2 0 50 -1 0 22 0.0000 4 330 1230 8681 7730 jacobian\001
+4 2 0 50 -1 0 22 0.0000 4 330 1170 8681 6319 position\001
+4 1 0 50 -1 2 26 0.0000 4 300 2940 10756 6651 FeaturePoint6d\001
 -6
-6 540 4905 7380 7020
-6 540 4950 7380 6795
-6 540 5535 2115 6075
-6 630 5535 1845 5850
-4 2 0 50 -1 0 22 0.0000 4 255 1170 1823 5813 errordot\001
+6 8910 3735 10485 4275
+6 9000 3735 10215 4050
+4 2 0 50 -1 0 22 0.0000 4 255 1170 10193 4013 errordot\001
 -6
 2 1 0 1 0 7 50 -1 -1 0.000 0 0 -1 1 0 2
 	1 1 1.00 110.67 221.33
-	 578 5979 2072 5979
+	 8948 4179 10442 4179
 -6
-6 5805 4995 7380 5535
-6 5895 4995 7110 5310
-4 2 0 50 -1 0 22 0.0000 4 255 1170 7088 5273 errordot\001
+6 3600 3285 5175 3825
+6 3690 3285 4905 3600
+4 2 0 50 -1 0 22 0.0000 4 255 1170 4883 3563 errordot\001
 -6
 2 1 0 1 0 7 50 -1 -1 0.000 0 0 -1 1 0 2
 	1 1 1.00 110.67 221.33
-	 5843 5439 7337 5439
+	 3638 3729 5132 3729
 -6
-2 1 0 1 0 7 50 -1 -1 0.000 0 0 -1 1 0 2
-	1 1 1.00 110.67 221.33
-	 578 5315 2072 5315
+6 540 6300 2115 6795
 2 1 0 1 0 7 50 -1 -1 0.000 0 0 -1 1 0 2
 	1 1 1.00 110.67 221.33
 	 578 6726 2072 6726
-4 2 0 50 -1 0 22 0.0000 4 180 705 1823 5149 error\001
 4 2 0 50 -1 0 22 0.0000 4 330 1230 1823 6560 jacobian\001
-4 1 0 50 -1 2 26 0.0000 4 300 2955 3898 5481 FeatureGeneric\001
-4 0 0 50 -1 0 18 0.0000 4 195 1335 2487 5951 errSOUT =\001
-4 0 0 50 -1 0 18 0.0000 4 195 2685 2487 6320 errorSIN - ref.errorSIN\001
-4 0 0 50 -1 0 18 0.0000 4 255 2205 2487 6726 jacSOUT = jacSIN\001
 -6
-2 2 0 1 0 7 50 -1 -1 0.000 0 0 -1 0 0 5
-	 2072 4945 5807 4945 5807 7020 2072 7020 2072 4945
--6
-6 6120 6030 12690 8145
-2 2 0 1 0 7 50 -1 -1 0.000 0 0 -1 0 0 5
-	 8930 6070 12665 6070 12665 8145 8930 8145 8930 6070
+6 540 5175 2115 5625
 2 1 0 1 0 7 50 -1 -1 0.000 0 0 -1 1 0 2
 	1 1 1.00 110.67 221.33
-	 7470 6485 8930 6485
-2 1 0 1 0 7 50 -1 -1 0.000 0 0 -1 1 0 2
-	1 1 1.00 110.67 221.33
-	 6357 7149 8930 7149
+	 578 5540 2072 5540
+4 2 0 50 -1 0 22 0.0000 4 180 705 1823 5374 error\001
+-6
+6 945 495 1800 765
 2 1 0 1 0 7 50 -1 -1 0.000 0 0 -1 1 0 2
-	1 1 1.00 110.67 221.33
-	 6357 7896 8930 7896
-4 2 0 50 -1 0 22 0.0000 4 255 2520 8681 6983 articularJacobian\001
-4 2 0 50 -1 0 22 0.0000 4 330 1230 8681 7730 jacobian\001
-4 2 0 50 -1 0 22 0.0000 4 330 1170 8681 6319 position\001
-4 1 0 50 -1 2 26 0.0000 4 300 2940 10756 6651 FeaturePoint6d\001
+	1 1 1.00 59.15 118.32
+	 999 721 1797 721
+4 2 0 50 -1 0 11 0.0000 4 135 690 1664 630 selection\001
 -6
 2 1 0 1 0 7 50 -1 -1 0.000 0 0 -1 1 0 3
 	0 0 1.00 110.67 221.33
-	 3234 1522 3234 2435 5226 2435
+	 3234 1522 3234 1935 5226 1935
 2 1 0 1 0 7 50 -1 -1 0.000 0 0 -1 1 0 2
-	1 1 1.00 110.67 221.33
-	 3732 3348 5226 3348
+	0 0 1.00 110.67 221.33
+	 3825 4950 6471 4344
+2 1 0 1 0 7 50 -1 -1 0.000 0 0 -1 1 0 2
+	0 0 1.00 110.67 221.33
+	 10710 6075 7965 4344
+2 2 0 1 0 7 50 -1 -1 0.000 0 0 -1 0 0 5
+	 2072 4945 5807 4945 5807 7020 2072 7020 2072 4945
 2 2 0 1 0 7 50 -1 -1 0.000 0 0 -1 0 0 5
-	 5226 2269 8961 2269 8961 4344 5226 4344 5226 2269
+	 5175 1665 8961 1665 8961 4344 5175 4344 5175 1665
 2 1 0 1 0 7 50 -1 -1 0.000 0 0 -1 1 0 2
 	1 1 1.00 110.67 221.33
-	 8961 4095 10455 4095
+	 3732 2700 5226 2700
 2 1 0 1 0 7 50 -1 -1 0.000 0 0 -1 1 0 2
 	1 1 1.00 110.67 221.33
-	 8961 3431 10455 3431
+	 8961 3510 10455 3510
 2 1 0 1 0 7 50 -1 -1 0.000 0 0 -1 1 0 2
 	1 1 1.00 110.67 221.33
-	 8961 2767 10455 2767
+	 8961 2846 10455 2846
 2 1 0 1 0 7 50 -1 -1 0.000 0 0 -1 1 0 2
-	0 0 1.00 110.67 221.33
-	 3825 4950 6471 4344
+	1 1 1.00 110.67 221.33
+	 8961 2182 10455 2182
+2 2 0 1 0 7 50 -1 -1 0.000 0 0 -1 0 0 5
+	 1797 413 3797 413 3797 1522 1797 1522 1797 413
 2 1 0 1 0 7 50 -1 -1 0.000 0 0 -1 1 0 2
-	0 0 1.00 110.67 221.33
-	 10710 6075 7965 4344
-4 1 0 50 -1 2 26 0.0000 4 300 3105 7052 3431 FeatureAbstract\001
-4 2 0 50 -1 0 22 0.0000 4 255 1305 4977 3182 selection\001
-4 0 0 50 -1 0 22 0.0000 4 255 1545 9127 3929 dimension\001
-4 0 0 50 -1 0 22 0.0000 4 330 1230 9127 3265 jacobian\001
-4 0 0 50 -1 0 22 0.0000 4 180 705 9127 2601 error\001
-4 2 0 50 -1 0 22 0.0000 4 255 1365 4977 2186 reference\001
+	1 1 1.00 59.15 118.32
+	 3797 1389 4595 1389
+2 1 0 1 0 7 50 -1 -1 0.000 0 0 -1 1 0 2
+	1 1 1.00 59.15 118.32
+	 3797 1035 4595 1035
+2 1 0 1 0 7 50 -1 -1 0.000 0 0 -1 1 0 2
+	1 1 1.00 59.15 118.32
+	 3797 679 4595 679
+2 1 0 1 0 7 50 -1 -1 0.000 0 0 -1 1 0 2
+	1 1 1.00 59.15 118.32
+	 999 1306 1797 1306
+4 1 0 50 -1 2 26 0.0000 4 300 2955 3898 5481 FeatureGeneric\001
+4 0 0 50 -1 0 18 0.0000 4 195 1335 2487 5951 errSOUT =\001
+4 0 0 50 -1 0 18 0.0000 4 195 2685 2487 6320 errorSIN - ref.errorSIN\001
+4 0 0 50 -1 0 18 0.0000 4 255 2205 2487 6726 jacSOUT = jacSIN\001
+4 1 0 50 -1 2 26 0.0000 4 300 3105 7052 2340 FeatureAbstract\001
+4 2 0 50 -1 0 22 0.0000 4 255 1365 4977 1845 reference\001
+4 2 0 50 -1 0 22 0.0000 4 255 1305 4977 2475 selection\001
+4 0 0 50 -1 0 22 0.0000 4 255 1545 9127 3344 dimension\001
+4 0 0 50 -1 0 22 0.0000 4 330 1230 9127 2680 jacobian\001
+4 0 0 50 -1 0 22 0.0000 4 180 705 9127 2016 error\001
+4 1 0 50 -1 2 13 0.0000 4 150 1470 2775 1035 FeatureAbstract\001
+4 0 0 50 -1 0 11 0.0000 4 135 765 3885 1301 dimension\001
+4 0 0 50 -1 0 11 0.0000 4 180 630 3885 945 jacobian\001
+4 0 0 50 -1 0 11 0.0000 4 90 360 3885 591 error\001
+4 2 0 50 -1 0 11 0.0000 4 135 600 1664 1215 errordot\001

include/sot/core/feature-abstract.hh

@@ -35,6 +35,7 @@ namespace ml = maal::boost;
 #include <dynamic-graph/entity.h>
 #include <sot/core/pool.hh>
 #include "sot/core/api.hh"
+#include "sot/core/deprecated.hh"
 
 /* STD */
 #include <string>
@@ -66,29 +67,6 @@ namespace dynamicgraph {
       evolution of the feature with respect to time: \f$\frac{\partial
       f}{\partial t}\f$.
 
-      \par Time derivative of the error
-
-      Some Feature types provide as output signal the derivative of the
-      error of the feature (<c>errordotSOUT</c>). The value is computed by
-      method computeErrorDot. In this case, these feature types should
-      \li have attribute
-      \code
-      dg::SignalTimeDependent< ml::Vector,int > errordotSOUT;
-      \endcode
-      \li implement method
-      \code
-      virtual bool withErrorDot() const { return true; }
-      \endcode
-      \li implement method
-      \code
-      virtual SignalTimeDependent<ml::Vector,int>& getErrorDot()
-      {
-      return errordotSOUT;
-      }
-      \endcode
-
-      See dynamicgraph::sot::FeatureGeneric for an example.
-
       \image html pictures/feature.png "Feature diagram: Feature types derive from FeatureAbstract. Each feature has a reference of the same type and compute an error by comparing errorSIN signals from itself and from the reference." width=15cm
     */
     class SOT_CORE_EXPORT FeatureAbstract
@@ -162,6 +140,12 @@ namespace dynamicgraph {
       */
       virtual ml::Matrix& computeJacobian( ml::Matrix& res,int time ) = 0;
 
+      /// Callback for signal errordotSOUT
+      ///
+      /// Copy components of the input signal errordotSIN defined by selection
+      /// flag selectionSIN.
+      virtual ml::Vector& computeErrorDot (ml::Vector& res,int time);
+
       /*! @} */
 
       /* --- SIGNALS ------------------------------------------------------------ */
@@ -178,6 +162,10 @@ namespace dynamicgraph {
 	be used for computing error, activation and Jacobian, then the vector to specify
 	is \f$ [ 0 1 0] \f$.*/
       SignalPtr< Flags,int > selectionSIN;
+
+      /// Derivative of the reference value.
+      SignalPtr< ml::Vector,int > errordotSIN;
+
       /*! @} */
 
       /*! \name Output signals:
@@ -187,6 +175,9 @@ namespace dynamicgraph {
 	the current value : \f$ {\bf s}^*(t) - {\bf s}(t)\f$ */
       SignalTimeDependent<ml::Vector,int> errorSOUT;
 
+      /// Derivative of the reference value.
+      SignalTimeDependent< ml::Vector,int > errordotSOUT;
+
       /*! \brief This signal returns the Jacobian of the current value
 	according to the robot state: \f$ J(t) = \frac{\delta{\bf s}^*(t)}{\delta {\bf q}(t)}\f$ */
       SignalTimeDependent<ml::Matrix,int> jacobianSOUT;
@@ -198,8 +189,14 @@ namespace dynamicgraph {
       virtual std::ostream & writeGraph(std::ostream & os) const;
 
       /// Return true for children that provide the errordot output signal
-      virtual bool withErrorDot( void ) const { return false; }
-      virtual SignalTimeDependent<ml::Vector,int>& getErrorDot( void ) {throw;}
+      virtual bool withErrorDot( void ) const SOT_CORE_DEPRECATED
+      {
+	return true;
+      }
+      virtual SignalTimeDependent<ml::Vector,int>& getErrorDot()
+      {
+	return errordotSOUT;
+      }
 
       /*! @} */
 
@@ -286,13 +283,6 @@ namespace dynamicgraph {
     /* To force a ; */bool NO_REFERENCE
     /* END OF define DECLARE_REFERENCE_FUNCTIONS */
 
-#define WITH_ERRORDOT  \
-      virtual bool withErrorDot() const { return true; }   \
-      virtual SignalTimeDependent<ml::Vector,int>& getErrorDot() { return errordotSOUT; } \
-      dg::SignalTimeDependent< ml::Vector,int > errordotSOUT;   \
-      virtual ml::Vector& computeErrorDot( ml::Vector& res,int time )
-
-
   } // namespace sot
 } // namespace dynamicgraph
 

include/sot/core/feature-generic.hh

@@ -87,9 +87,6 @@ class SOTFEATUREGENERIC_EXPORT FeatureGeneric
   /*! \brief Input for the error. */
   dg::SignalPtr< ml::Vector,int > errorSIN;
 
-  /*! \brief Input for the errordot. */
-  dg::SignalPtr< ml::Vector,int > errordotSIN;
-
   /*! \brief Input for the Jacobian. */
   dg::SignalPtr< ml::Matrix,int > jacobianSIN;
 
@@ -126,8 +123,6 @@ class SOTFEATUREGENERIC_EXPORT FeatureGeneric
   /*! \brief Compute the Jacobian of the value according to the robot state.. */
   virtual ml::Matrix& computeJacobian( ml::Matrix& res,int time );
 
-  WITH_ERRORDOT;
-
   /*! @} */
 
   /*! \brief Display the information related to this generic implementation. */

src/feature/feature-abstract.cpp

@@ -20,10 +20,12 @@
 
 #include <sot/core/feature-abstract.hh>
 #include <sot/core/pool.hh>
+#include "sot/core/debug.hh"
+#include "sot/core/exception-feature.hh"
 #include <dynamic-graph/all-commands.h>
 
 using namespace dynamicgraph::sot;
-namespace dg = dynamicgraph;
+using dynamicgraph::sot::ExceptionFeature;
 
 const std::string 
 FeatureAbstract::CLASS_NAME = "FeatureAbstract";
@@ -33,6 +35,7 @@ FeatureAbstract::
 FeatureAbstract( const std::string& name ) 
   :Entity(name)
    ,selectionSIN(NULL,"sotFeatureAbstract("+name+")::input(flag)::selec")
+  ,errordotSIN (NULL,"sotFeatureAbstract("+name+")::input(vector)::errordotIN" )
    ,errorSOUT( boost::bind(&FeatureAbstract::computeError,this,_1,_2),
 	       selectionSIN,
 	       "sotFeatureAbstract("+name+")::output(vector)::error" )
@@ -42,6 +45,9 @@ FeatureAbstract( const std::string& name )
    ,dimensionSOUT( boost::bind(&FeatureAbstract::getDimension,this,_1,_2),
 		   selectionSIN,
 		   "sotFeatureAbstract("+name+")::output(uint)::dim" )
+  ,errordotSOUT (boost::bind(&FeatureAbstract::computeErrorDot,this,_1,_2),
+		 selectionSIN << errordotSIN,
+		 "sotFeatureAbstract("+name+")::output(vector)::errordot" )
 {
   selectionSIN = true;
   signalRegistration( selectionSIN
@@ -97,3 +103,35 @@ getReferenceByName() const
 {
   if( isReferenceSet() ) return getReferenceAbstract()->getName(); else return "none";
 }
+
+ml::Vector& FeatureAbstract::
+computeErrorDot( ml::Vector& res,int time )
+{ 
+  const Flags &fl = selectionSIN.access(time);
+  const unsigned int & dim = dimensionSOUT(time);
+
+  unsigned int curr = 0;
+  res.resize( dim );
+
+  sotDEBUG(25) << "Dim = " << dim << std::endl;
+
+  if( isReferenceSet () && getReferenceAbstract ()->errordotSIN.isPluged ())
+    {
+      const ml::Vector& errdotDes = getReferenceAbstract ()->errordotSIN(time);
+      sotDEBUG(15) << "Err* = " << errdotDes;
+      if( errdotDes.size()<dim )
+	{ SOT_THROW ExceptionFeature( ExceptionFeature::UNCOMPATIBLE_SIZE,
+					 "Error: dimension uncompatible with des->errorIN size."
+					 " (while considering feature <%s>).",getName().c_str() ); }
+
+      for( unsigned int i=0;i<errdotDes.size();++i ) if( fl(i) ) 
+	if( fl(i) ) res( curr++ ) = errdotDes(i);
+    }
+  else
+    {
+      for( unsigned int i=0;i<dim;++i )
+	if( fl(i) ) res( curr++ ) = 0.0;
+    }
+  
+  return res; 
+}

src/feature/feature-generic.cpp

@@ -44,11 +44,7 @@ FeatureGeneric( const string& pointName )
   : FeatureAbstract( pointName )
     ,dimensionDefault(0)
     ,errorSIN( NULL,"sotFeatureGeneric("+name+")::input(vector)::errorIN" )
-    ,errordotSIN( NULL,"sotFeatureGeneric("+name+")::input(vector)::errordotIN" )
     ,jacobianSIN( NULL,"sotFeatureGeneric("+name+")::input(matrix)::jacobianIN" )
-    ,errordotSOUT(  boost::bind(&FeatureGeneric::computeErrorDot,this,_1,_2),
-		    selectionSIN << errordotSIN,
-		    "sotFeatureAbstract("+name+")::output(vector)::errordot" )
 
 {
   jacobianSOUT.addDependency( jacobianSIN );
@@ -135,39 +131,6 @@ computeError( ml::Vector& res,int time )
 
 }
 
-ml::Vector& FeatureGeneric::
-computeErrorDot( ml::Vector& res,int time )
-{ 
-  const Flags &fl = selectionSIN.access(time);
-  const unsigned int & dim = dimensionSOUT(time);
-
-  unsigned int curr = 0;
-  res.resize( dim );
-
-  sotDEBUG(25) << "Dim = " << dim << endl;
-
-  if( isReferenceSet() && getReference()->errordotSIN.isPluged())
-    {
-      const ml::Vector& errdotDes = getReference()->errordotSIN(time);
-      sotDEBUG(15) << "Err* = " << errdotDes;
-      if( errdotDes.size()<dim )
-	{ SOT_THROW ExceptionFeature( ExceptionFeature::UNCOMPATIBLE_SIZE,
-					 "Error: dimension uncompatible with des->errorIN size."
-					 " (while considering feature <%s>).",getName().c_str() ); }
-
-      for( unsigned int i=0;i<errdotDes.size();++i ) if( fl(i) ) 
-	if( fl(i) ) res( curr++ ) = errdotDes(i);
-    }
-  else
-    {
-      for( unsigned int i=0;i<dim;++i )
-	if( fl(i) ) res( curr++ ) = 0.0;
-    }
-  
-  return res; 
-}
-
-
 ml::Matrix& FeatureGeneric::
 computeJacobian( ml::Matrix& res,int time )
 { 

src/task/task.cpp

@@ -110,8 +110,7 @@ addFeature( FeatureAbstract& s )
   featureList.push_back(&s);
   jacobianSOUT.addDependency( s.jacobianSOUT );
   errorSOUT.addDependency( s.errorSOUT );
-  if( s.withErrorDot() )
-    { errorTimeDerivativeSOUT.addDependency( s.getErrorDot() ); }
+  errorTimeDerivativeSOUT.addDependency (s.getErrorDot());
 }
 
 void Task::
@@ -132,8 +131,7 @@ clearFeatureList( void )
       FeatureAbstract & s = **iter;
       jacobianSOUT.removeDependency( s.jacobianSOUT );
       errorSOUT.removeDependency( s.errorSOUT );
-      if( s.withErrorDot() )
-	{ errorTimeDerivativeSOUT.removeDependency( s.getErrorDot() ); }
+      errorTimeDerivativeSOUT.removeDependency (s.getErrorDot());
     }
 
   featureList.clear();
@@ -237,17 +235,9 @@ computeErrorTimeDerivative( ml::Vector & res, int time)
       {
 	FeatureAbstract &feature = **iter;
 
-	if( feature.withErrorDot() )
-	  {
-	    const ml::Vector& partialErrorDot = feature.getErrorDot()(time);
-	    const int dim = partialErrorDot.size();
-	    for( int k=0;k<dim;++k ){ res(cursor++) = partialErrorDot(k); }
-	  }
-	else
-	  {
-	    const int dim = feature.errorSOUT(time).size();
-	    for( int k=0;k<dim;++k ){ res(cursor++) = 0; }
-	  }
+	const ml::Vector& partialErrorDot = feature.getErrorDot()(time);
+	const int dim = partialErrorDot.size();
+	for( int k=0;k<dim;++k ){ res(cursor++) = partialErrorDot(k); }
       }
 
   return res;