From 33f97af3d61ce2ce04a5a1767a3a5ba7840fe446 Mon Sep 17 00:00:00 2001
From: Mansard <nmansard@laas.fr>
Date: Thu, 8 Dec 2011 14:42:10 +0100
Subject: [PATCH] Account for the error dot in the task computation.

---
 src/task-dyn-pd.cpp | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/src/task-dyn-pd.cpp b/src/task-dyn-pd.cpp
index 9062a23..f209c2e 100644
--- a/src/task-dyn-pd.cpp
+++ b/src/task-dyn-pd.cpp
@@ -112,14 +112,15 @@ namespace dynamicgraph
 	sotDEBUGIN(15);
 
 	const ml::Vector & e = errorSOUT(time);
-	const ml::Vector & edot = errorDotSOUT(time);
+	const ml::Vector & edot_measured = errorDotSOUT(time);
+	const ml::Vector & edot_ref = errorTimeDerivativeSOUT(time);
 	const double& Kp = controlGainSIN(time);
 	const double& Kv = KvSIN(time);
 
-	assert( e.size() == edot.size() );
+	assert( e.size() == edot_measured.size()  && e.size() == edot_ref.size() );
 	task .resize( e.size() );
 	for( unsigned int i=0;i<task.size(); ++i )
-	  task[i] = - Kp*e(i) - Kv*edot(i);
+	  task[i] = - Kp*e(i) - Kv*(edot_measured(i)-edot_ref(i));
 
 	sotDEBUGOUT(15);
 	return task;
-- 
GitLab