From e936ce5d47ae5dab435bc5fc391d2cf6b74d26f1 Mon Sep 17 00:00:00 2001
From: dmronga <dennis.mronga@dfki.de>
Date: Fri, 6 Sep 2024 23:13:16 +0200
Subject: [PATCH] Bug fix in AccelerationSceneTSID

---
 .../acceleration_tsid/AccelerationSceneTSID.cpp       |  4 +++-
 .../acceleration_tsid/AccelerationSceneTSID.hpp       | 11 +++++++++++
 2 files changed, 14 insertions(+), 1 deletion(-)

diff --git a/src/scenes/acceleration_tsid/AccelerationSceneTSID.cpp b/src/scenes/acceleration_tsid/AccelerationSceneTSID.cpp
index 2325e189..fda65856 100644
--- a/src/scenes/acceleration_tsid/AccelerationSceneTSID.cpp
+++ b/src/scenes/acceleration_tsid/AccelerationSceneTSID.cpp
@@ -141,7 +141,9 @@ const types::JointCommand& AccelerationSceneTSID::solve(const HierarchicalQP& hq
 
     // solve
     solver_output.resize(hqp[0].nq);
-    solver->solve(hqp, solver_output);
+    bool allow_warm_start = !contactsHaveChanged(contacts, robot_model->getContacts());
+    contacts = robot_model->getContacts();
+    solver->solve(hqp, solver_output, allow_warm_start);
 
     // Convert solver output: Acceleration and torque
     uint nj = robot_model->nj();
diff --git a/src/scenes/acceleration_tsid/AccelerationSceneTSID.hpp b/src/scenes/acceleration_tsid/AccelerationSceneTSID.hpp
index 5eaf2aae..f4c84d03 100644
--- a/src/scenes/acceleration_tsid/AccelerationSceneTSID.hpp
+++ b/src/scenes/acceleration_tsid/AccelerationSceneTSID.hpp
@@ -41,10 +41,21 @@ class AccelerationSceneTSID : public Scene{
     static SceneRegistry<AccelerationSceneTSID> reg;
 
     // Helper variables
+    std::vector<Contact> contacts;
     Eigen::VectorXd robot_acc, solver_output_acc;
     std::vector<types::Wrench> contact_wrenches;
     double hessian_regularizer;
 
+    bool contactsHaveChanged(const std::vector<Contact>& old_contacts, const std::vector<Contact>& new_contacts){
+        if(old_contacts.size() != new_contacts.size())
+            return true;
+        for(uint i = 0; i < old_contacts.size(); i++){
+            if(old_contacts[i].active != new_contacts[i].active)
+                return true;
+        }
+        return false;
+    }
+
 public:
     AccelerationSceneTSID(RobotModelPtr robot_model, QPSolverPtr solver, const double dt);
     virtual ~AccelerationSceneTSID(){