diff --git a/src/task-dyn-pd.cpp b/src/task-dyn-pd.cpp
index 9062a234c11a5d8aaa1b6c1cb2c565a7212b3a69..f209c2e428243918292cf7345a51fe58761f8be9 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;