diff --git a/include/sot-dynamic/dynamic.h b/include/sot-dynamic/dynamic.h index a0dd0cae2ccdd34d6c294d26eb9d4503186ff90a..c8ed58dd1f621458ebbdb2f82d1856abe45fb085 100644 --- a/include/sot-dynamic/dynamic.h +++ b/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, diff --git a/src/dynamic.cpp b/src/dynamic.cpp index 1eff5573b22c1f98ad3493b95ae82a68a28f28c8..91b94a0bccc15d19f039c2386e22b5d00d5463f6 100644 --- a/src/dynamic.cpp +++ b/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 ------------------------------------------------------------- */