Bug fix in AccelerationSceneTSID

ARC-OPT · Sep 6, 2024 · e936ce5 · e936ce5
1 parent 65e6485
commit e936ce5
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 1 deletion.
diff --git 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
@@ -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(){