Commit e654d54b authored by Nicolas Mansard's avatar Nicolas Mansard Committed by Nicolas Mansard
Browse files

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

This reverts commit 9fd2003a.
parent 9fd2003a
......@@ -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,
......
......@@ -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 ------------------------------------------------------------- */
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment