Merge pull request #91 from ARC-OPT/fix_task_weighting

Fix task weighting (Issue #64). Task weights should be multiplied
ARC-OPT · May 3, 2024 · 4beec62 · 4beec62
2 parents dc398db + 40173bb
commit 4beec62
Show file tree

Hide file tree

Showing 4 changed files with 12 additions and 4 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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);