diff --git a/src/scenes/acceleration/AccelerationScene.cpp b/src/scenes/acceleration/AccelerationScene.cpp
index 9f80eb68..1499f1b0 100644
--- a/src/scenes/acceleration/AccelerationScene.cpp
+++ b/src/scenes/acceleration/AccelerationScene.cpp
@@ -72,8 +72,10 @@ const HierarchicalQP& AccelerationScene::update(){
            task->y_ref_root.setZero();
         }
 
-        for(int i = 0; i < task->A.rows(); i++)
+        for(int i = 0; i < task->A.rows(); i++){
             task->Aw.row(i) = task->weights_root(i) * task->A.row(i) * task->activation * (!task->timeout);
+            task->y_ref_root(i) = task->y_ref_root(i) * task->weights_root(i) * task->activation;
+        }
         for(int i = 0; i < task->A.cols(); i++)
             task->Aw.col(i) = joint_weights[i] * task->Aw.col(i);
 
diff --git a/src/scenes/acceleration_reduced_tsid/AccelerationSceneReducedTSID.cpp b/src/scenes/acceleration_reduced_tsid/AccelerationSceneReducedTSID.cpp
index 11e6f8b7..ab798fe1 100644
--- a/src/scenes/acceleration_reduced_tsid/AccelerationSceneReducedTSID.cpp
+++ b/src/scenes/acceleration_reduced_tsid/AccelerationSceneReducedTSID.cpp
@@ -126,8 +126,10 @@ const HierarchicalQP& AccelerationSceneReducedTSID::update(){
            task->y_ref_root.setZero();
         }
 
-        for(int i = 0; i < task->A.rows(); i++)
+        for(int i = 0; i < task->A.rows(); i++){
             task->Aw.row(i) = task->weights_root(i) * task->A.row(i) * task->activation * (!task->timeout);
+            task->y_ref_root(i) = task->y_ref_root(i) * task->weights_root(i) * task->activation;
+        }
         for(int i = 0; i < task->A.cols(); i++)
             task->Aw.col(i) = joint_weights[i] * task->Aw.col(i);
 
diff --git a/src/scenes/acceleration_tsid/AccelerationSceneTSID.cpp b/src/scenes/acceleration_tsid/AccelerationSceneTSID.cpp
index baad8886..c34cb349 100644
--- a/src/scenes/acceleration_tsid/AccelerationSceneTSID.cpp
+++ b/src/scenes/acceleration_tsid/AccelerationSceneTSID.cpp
@@ -116,8 +116,10 @@ const HierarchicalQP& AccelerationSceneTSID::update(){
            task->y_ref_root.setZero();
         }
 
-        for(int i = 0; i < task->A.rows(); i++)
+        for(int i = 0; i < task->A.rows(); i++){
             task->Aw.row(i) = task->weights_root(i) * task->A.row(i) * task->activation * (!task->timeout);
+            task->y_ref_root(i) = task->y_ref_root(i) * task->weights_root(i) * task->activation;
+        }
         for(int i = 0; i < task->A.cols(); i++)
             task->Aw.col(i) = joint_weights[i] * task->Aw.col(i);
 
diff --git a/src/scenes/velocity_qp/VelocitySceneQP.cpp b/src/scenes/velocity_qp/VelocitySceneQP.cpp
index caab0fea..9d56d96e 100644
--- a/src/scenes/velocity_qp/VelocitySceneQP.cpp
+++ b/src/scenes/velocity_qp/VelocitySceneQP.cpp
@@ -104,8 +104,10 @@ const HierarchicalQP& VelocitySceneQP::update(){
            task->y_ref_root.setZero();
         }
 
-        for(int i = 0; i < task->A.rows(); i++)
+        for(int i = 0; i < task->A.rows(); i++){
             task->Aw.row(i) = task->weights_root(i) * task->A.row(i) * task->activation * (!task->timeout);
+            task->y_ref_root(i) = task->y_ref_root(i) * task->weights_root(i) * task->activation;
+        }
         for(int i = 0; i < task->A.cols(); i++)
             task->Aw.col(i) = joint_weights[i] * task->Aw.col(i);