From dd56f3e8eb7139de59111d1979eb9a47be2fdd30 Mon Sep 17 00:00:00 2001
From: dmronga <dennis.mronga@dfki.de>
Date: Wed, 29 Nov 2023 16:36:42 +0100
Subject: [PATCH] serious fix to avoid sudden jump in control output when
 deactiving, moving the robot with some other controller and activating again

---
 src/whole_body_controller.cpp | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/src/whole_body_controller.cpp b/src/whole_body_controller.cpp
index 1337f05..4bdfa3d 100644
--- a/src/whole_body_controller.cpp
+++ b/src/whole_body_controller.cpp
@@ -356,6 +356,14 @@ controller_interface::CallbackReturn WholeBodyController::on_activate(const rclc
                 command_indices[iface_name].push_back(get_command_idx(joint_name, iface_name));
         }
     }
+
+    //Clear all task references, weights etc. to have to secure initial state
+    for(const auto &cfg : task_config)
+       scene->getTask(cfg.name)->reset();
+
+    // Also reinit the integrate as it will store the last joint position as a starting point
+    joint_integrator.reinit();
+
     return CallbackReturn::SUCCESS;
 }