From 40173bbee6c6c9722bfabdfa09e28417755fa133 Mon Sep 17 00:00:00 2001 From: dmronga Date: Fri, 3 May 2024 10:09:33 +0200 Subject: [PATCH] Fix task weighting (Issue #64). Task weights should be multiplied by task matrix and task reference --- src/scenes/acceleration/AccelerationScene.cpp | 4 +++- .../AccelerationSceneReducedTSID.cpp | 4 +++- src/scenes/acceleration_tsid/AccelerationSceneTSID.cpp | 4 +++- src/scenes/velocity_qp/VelocitySceneQP.cpp | 4 +++- 4 files changed, 12 insertions(+), 4 deletions(-) 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);