Revert "Revert "Added a signal for the dyn drift.""

This reverts commit 9fd2003a.

include/sot-dynamic/dynamic.h

@@ -213,6 +213,7 @@ class SOTDYNAMIC_EXPORT Dynamic
   dg::SignalTimeDependent<ml::Matrix,int> inertiaRealSOUT;
   dg::SignalTimeDependent<ml::Vector,int> MomentaSOUT;
   dg::SignalTimeDependent<ml::Vector,int> AngularMomentumSOUT;
+  dg::SignalTimeDependent<ml::Vector,int> dynamicDriftSOUT;
 
  protected:
   ml::Vector& computeZmp( ml::Vector& res,int time );
@@ -233,6 +234,7 @@ class SOTDYNAMIC_EXPORT Dynamic
   ml::Vector& getUpperJointLimits( ml::Vector& res,const int& time );
   ml::Vector& getLowerJointLimits( ml::Vector& res,const int& time );
 
+  ml::Vector& computeTorqueDrift( ml::Vector& res,const int& time );
 
  public: /* --- PARAMS --- */
   virtual void commandLine( const std::string& cmdLine,

src/dynamic.cpp

@@ -134,6 +134,9 @@ Dynamic( const std::string & name, bool build )
   ,AngularMomentumSOUT( boost::bind(&Dynamic::computeAngularMomentum,this,_1,_2),
 			newtonEulerSINTERN,
 			"sotDynamic("+name+")::output(vector)::angularmomentum" )
+  ,dynamicDriftSOUT( boost::bind(&Dynamic::computeTorqueDrift,this,_1,_2),
+		     newtonEulerSINTERN,
+		     "sotDynamic("+name+")::output(vector)::dynamicDrift" )
 {
   sotDEBUGIN(5);
 
@@ -146,9 +149,9 @@ Dynamic( const std::string & name, bool build )
 		      <<jointVelocitySIN<<freeFlyerVelocitySIN
 		      <<jointAccelerationSIN<<freeFlyerAccelerationSIN);
   signalRegistration( zmpSOUT<<comSOUT<<JcomSOUT<<footHeightSOUT);
-	signalRegistration(upperJlSOUT<<lowerJlSOUT<<inertiaSOUT
-			 <<inertiaRealSOUT << inertiaRotorSOUT << gearRatioSOUT );
-  signalRegistration( MomentaSOUT << AngularMomentumSOUT );
+  signalRegistration(upperJlSOUT<<lowerJlSOUT<<inertiaSOUT
+		     <<inertiaRealSOUT << inertiaRotorSOUT << gearRatioSOUT );
+  signalRegistration( MomentaSOUT << AngularMomentumSOUT << dynamicDriftSOUT);
 
   //
   // Commands
@@ -1213,6 +1216,21 @@ getLowerJointLimits(ml::Vector& res, const int&)
   return res;
 }
 
+ml::Vector& Dynamic::
+computeTorqueDrift( ml::Vector& tauDrift,const int  & iter )
+{
+  sotDEBUGIN(25);
+  newtonEulerSINTERN(iter);
+  const unsigned int NB_JOINTS = jointPositionSIN.accessCopy().size();
+
+  tauDrift.resize(NB_JOINTS);
+  const vectorN& Torques = m_HDR->currentJointTorques();
+  for( unsigned int i=0;i<NB_JOINTS; ++i ) tauDrift(i) = Torques(i);
+
+  sotDEBUGOUT(25);
+  return tauDrift;
+}
+
 /* --- COMMANDS ------------------------------------------------------------- */
 /* --- COMMANDS ------------------------------------------------------------- */
 /* --- COMMANDS ------------------------------------------------------------- */