From 6a0cf421769e58bb209129dd37452e97839ca1dc Mon Sep 17 00:00:00 2001
From: Mansard <nmansard@laas.fr>
Date: Mon, 24 Jan 2011 18:01:09 +0100
Subject: [PATCH] Added signals to estimate the ff velocity.
---
include/sot-dynamic/angle-estimator.h | 10 ++++-
src/angle-estimator.cpp | 60 ++++++++++++++++++++++++++-
2 files changed, 67 insertions(+), 3 deletions(-)
diff --git a/include/sot-dynamic/angle-estimator.h b/include/sot-dynamic/angle-estimator.h
index 24d20c7..4c41a43 100644
--- a/include/sot-dynamic/angle-estimator.h
+++ b/include/sot-dynamic/angle-estimator.h
@@ -86,6 +86,11 @@ class SOTANGLEESTIMATOR_EXPORT AngleEstimator
dg::SignalTimeDependent<MatrixHomogeneous,int> waistWorldPositionSOUT; // worldMwaist
dg::SignalTimeDependent<ml::Vector,int> waistWorldPoseRPYSOUT; // worldMwaist
+ dg::SignalPtr<ml::Matrix,int> jacobianSIN;
+ dg::SignalPtr<ml::Vector,int> qdotSIN;
+ dg::SignalTimeDependent<ml::Vector,int> xff_dotSOUT;
+ dg::SignalTimeDependent<ml::Vector,int> qdotSOUT;
+
public: /* --- FUNCTIONS --- */
ml::Vector& computeAngles( ml::Vector& res,
const int& time );
@@ -101,6 +106,10 @@ class SOTANGLEESTIMATOR_EXPORT AngleEstimator
const int& time );
ml::Vector& computeWaistWorldPoseRPY( ml::Vector& res,
const int& time );
+ ml::Vector& compute_xff_dotSOUT( ml::Vector& res,
+ const int& time );
+ ml::Vector& compute_qdotSOUT( ml::Vector& res,
+ const int& time );
public: /* --- PARAMS --- */
void fromSensor(const bool& fs) { fromSensor_ = fs; }
@@ -111,7 +120,6 @@ class SOTANGLEESTIMATOR_EXPORT AngleEstimator
private:
bool fromSensor_;
-
};
diff --git a/src/angle-estimator.cpp b/src/angle-estimator.cpp
index dfc74ec..b597064 100644
--- a/src/angle-estimator.cpp
+++ b/src/angle-estimator.cpp
@@ -76,6 +76,17 @@ AngleEstimator( const std::string & name )
"sotAngleEstimator("+name
+")::output(vectorRollPitchYaw)::waistWorldPoseRPY" )
+ ,jacobianSIN(NULL,"sotAngleEstimator("+name
+ +")::input()::jacobian")
+ ,qdotSIN(NULL,"sotAngleEstimator("+name
+ +")::input()::qdot")
+ ,xff_dotSOUT( boost::bind(&AngleEstimator::compute_xff_dotSOUT,this,_1,_2)
+ ,jacobianSIN<<qdotSIN
+ ,"sotAngleEstimator("+name+")::output(vector)::xff_dot" )
+ ,qdotSOUT( boost::bind(&AngleEstimator::compute_qdotSOUT,this,_1,_2)
+ ,xff_dotSOUT<<qdotSIN
+ ,"sotAngleEstimator("+name+")::output(vector)::qdotOUT" )
+
,fromSensor_(true)
{
sotDEBUGIN(5);
@@ -85,7 +96,8 @@ AngleEstimator( const std::string & name )
<< anglesSOUT << flexibilitySOUT
<< driftSOUT << sensorWorldRotationSOUT
<< waistWorldRotationSOUT
- << waistWorldPositionSOUT << waistWorldPoseRPYSOUT );
+ << waistWorldPositionSOUT << waistWorldPoseRPYSOUT
+ << jacobianSIN << qdotSIN << xff_dotSOUT << qdotSOUT );
/* Commands. */
{
@@ -100,7 +112,8 @@ AngleEstimator( const std::string & name )
new ::dynamicgraph::command::Setter<AngleEstimator, bool>
(*this, &AngleEstimator::fromSensor, docstring));
- " Get flag specifying whether angle is measured from sensors or simulated.\n"
+ docstring = " \n"
+ " Get flag specifying whether angle is measured from sensors or simulated.\n"
" \n"
" No input,\n"
" return a boolean value.\n"
@@ -323,6 +336,49 @@ computeWaistWorldPoseRPY( ml::Vector& res,
return res;
}
+/* --- VELOCITY SIGS -------------------------------------------------------- */
+
+ml::Vector& AngleEstimator::
+compute_xff_dotSOUT( ml::Vector& res,
+ const int& time )
+{
+ const ml::Matrix & J = jacobianSIN( time );
+ const ml::Vector & dq = qdotSIN( time );
+
+ const int nr=J.nbRows(), nc=J.nbCols()-6;
+ assert( nr==6 );
+ ml::Matrix Ja( nr,nc ); ml::Vector dqa(nc);
+ for( int j=0;j<nc;++j )
+ {
+ for( int i=0;i<nr;++i )
+ Ja(i,j) = J(i,j+6);
+ dqa(j) = dq(j+6);
+ }
+ ml::Matrix Jff( 6,6 );
+ for( int j=0;j<6;++j )
+ for( int i=0;i<6;++i )
+ Jff(i,j) = J(i,j);
+
+ res.resize(nr); res = -Jff.inverse()*Ja*dqa;
+ return res;
+}
+
+ml::Vector& AngleEstimator::
+compute_qdotSOUT( ml::Vector& res,
+ const int& time )
+{
+ const ml::Vector & dq = qdotSIN( time );
+ const ml::Vector & dx = xff_dotSOUT( time );
+
+ assert( dx.size()==6 );
+
+ const int nr=dq.size();
+ res.resize( nr ); res=dq;
+ for( int i=0;i<6;++i ) res(i)=dx(i);
+
+ return res;
+}
+
/* --- PARAMS --------------------------------------------------------------- */
/* --- PARAMS --------------------------------------------------------------- */
/* --- PARAMS --------------------------------------------------------------- */
--
GitLab