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 reg; // Helper variables + std::vector contacts; Eigen::VectorXd robot_acc, solver_output_acc; std::vector contact_wrenches; double hessian_regularizer; + bool contactsHaveChanged(const std::vector& old_contacts, const std::vector& 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(){