From 1716ac2969003a1b49b11a21ddff10c263cb8f98 Mon Sep 17 00:00:00 2001
From: Mansard <nmansard@laas.fr>
Date: Thu, 27 Jan 2011 19:16:39 +0100
Subject: [PATCH] Added a signal for the dyn drift.
---
include/sot-dynamic/dynamic.h | 2 ++
src/dynamic.cpp | 24 +++++++++++++++++++++---
2 files changed, 23 insertions(+), 3 deletions(-)
diff --git a/include/sot-dynamic/dynamic.h b/include/sot-dynamic/dynamic.h
index a0dd0ca..c8ed58d 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 1eff557..91b94a0 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 ------------------------------------------------------------- */
--
GitLab