Add signal FeaturePosture::postureDot

Author: jmirabel

include/sot/core/feature-posture.hh

@@ -79,9 +79,11 @@ namespace dynamicgraph {
       virtual dg::Vector& computeError( dg::Vector& res, int );
       virtual dg::Matrix& computeJacobian( dg::Matrix& res, int );
       virtual dg::Vector& computeActivation( dg::Vector& res, int );
+      virtual dg::Vector& computeErrorDot (dg::Vector& res,int time);
 
       signalIn_t state_;
       signalIn_t posture_;
+      signalIn_t postureDot_;
       signalOut_t error_;
       dg::Matrix jacobian_;
     private:

src/feature/feature-posture.cpp

@@ -53,11 +53,12 @@ namespace dynamicgraph {
       : FeatureAbstract(name),
 	state_(NULL, "FeaturePosture("+name+")::input(Vector)::state"),
 	posture_(0, "FeaturePosture("+name+")::input(Vector)::posture"),
+	postureDot_(0, "FeaturePosture("+name+")::input(Vector)::postureDot"),
 	jacobian_(),
 	activeDofs_ (),
 	nbActiveDofs_ (0)
     {
-      signalRegistration (state_ << posture_);
+      signalRegistration (state_ << posture_ << postureDot_);
 
       errorSOUT.addDependency (state_);
 
@@ -116,6 +117,21 @@ namespace dynamicgraph {
       return res;
     }
 
+    dg::Vector& FeaturePosture::computeErrorDot( dg::Vector& res, int t)
+    {
+      const Vector& postureDot = postureDot_.access (t);
+
+      res.resize (nbActiveDofs_);
+      std::size_t index=0;
+      for (std::size_t i=0; i<activeDofs_.size (); ++i) {
+	if (activeDofs_ [i]) {
+	  res (index) = postureDot (i);
+	  index ++;
+	}
+      }
+      return res;
+    }
+
     void
     FeaturePosture::selectDof (unsigned dofId, bool control)
     {