Merge pull request #273 from EtienneAr/fix_removeRigidContact

Fix removeRigidContact

Author: nim65s

CHANGELOG.md

@@ -7,6 +7,8 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ## [Unreleased]
 
+- Fix value of nEq when removeRigidContact for contact with priority > 0
+
 ## [1.8.0] - 2025-03-29
 
 - Fix missing `const` specifier in python bindings for methods `RobotInertia.set_rotor_inertia()`

include/tsid/formulations/inverse-dynamics-formulation-acc-force.hpp

@@ -105,6 +105,7 @@ class InverseDynamicsFormulationAccForce
   Vector getContactForces(const std::string& name, const HQPOutput& sol);
   bool getContactForces(const std::string& name, const HQPOutput& sol,
                         RefVector f) override;
+  unsigned int getTaskPriority(const std::string& name) override;
 
  public:
   template <class TaskLevelPointer>

include/tsid/formulations/inverse-dynamics-formulation-base.hpp

@@ -157,6 +157,8 @@ class InverseDynamicsFormulationBase {
   virtual bool getContactForces(const std::string& name, const HQPOutput& sol,
                                 RefVector f) = 0;
 
+  virtual unsigned int getTaskPriority(const std::string& name) = 0;
+
  protected:
   std::string m_name;
   RobotWrapper m_robot;

src/formulations/inverse-dynamics-formulation-acc-force.cpp

@@ -541,6 +541,9 @@ bool InverseDynamicsFormulationAccForce::removeRigidContact(
     return false;
   }
 
+  // Find motion task priority
+  bool is_motion_contraint =
+      (getTaskPriority(contactName + "_motion_task") == 0);
   bool first_constraint_found = removeFromHqpData(contactName + "_motion_task");
   assert(first_constraint_found);
 
@@ -556,7 +559,7 @@ bool InverseDynamicsFormulationAccForce::removeRigidContact(
   for (auto it = m_contacts.begin(); it != m_contacts.end(); it++) {
     if ((*it)->contact.name() == contactName) {
       m_k -= (*it)->contact.n_force();
-      m_eq -= (*it)->motionConstraint->rows();
+      if (is_motion_contraint) m_eq -= (*it)->motionConstraint->rows();
       m_in -= (*it)->forceConstraint->rows();
       m_contacts.erase(it);
       resizeHqpData();
@@ -600,3 +603,17 @@ bool InverseDynamicsFormulationAccForce::removeFromHqpData(
   }
   return false;
 }
+
+unsigned int InverseDynamicsFormulationAccForce::getTaskPriority(
+    const std::string &name) {
+  for (std::size_t i = 0; i < m_hqpData.size(); i++) {
+    const bool found = std::any_of(
+        m_hqpData[i].begin(), m_hqpData[i].end(),
+        [&name](const auto &c) { return c.second->name() == name; });
+    if (found) {
+      return i;
+    }
+  }
+  assert(false);  // Task name not found in formulation data
+  return -1;
+}

tests/tsid-formulation.cpp

@@ -322,6 +322,15 @@ BOOST_AUTO_TEST_CASE(test_invdyn_formulation_acc_force_remove_contact) {
     CHECK_LESS_THAN(v.norm(), 1e6);
   }
 
+  // Removing contact constraint should decrease nb of equality by 6
+  int nEq = tsid->nEq();
+  tsid->removeRigidContact("contact_rfoot");
+  nEq -= 6;
+  BOOST_CHECK_EQUAL(nEq, tsid->nEq());
+  tsid->removeRigidContact("contact_lfoot");
+  nEq -= 6;
+  BOOST_CHECK_EQUAL(nEq, tsid->nEq());
+
   delete solver;
   cout << "\n### TEST FINISHED ###\n";
   PRINT_VECTOR(v);
@@ -573,6 +582,7 @@ BOOST_AUTO_TEST_CASE(test_contact_point_invdyn_formulation_acc_force) {
   Vector3 contactNormal = Vector3::UnitZ();
   std::vector<std::shared_ptr<ContactPoint>> contacts(4);
 
+  int nEq = tsid->nEq();
   for (int i = 0; i < 4; i++) {
     auto cp = std::make_shared<ContactPoint>("contact_" + contactFrames[i],
                                              robot, contactFrames[i],
@@ -584,6 +594,9 @@ BOOST_AUTO_TEST_CASE(test_contact_point_invdyn_formulation_acc_force) {
     cp->useLocalFrame(false);
     tsid->addRigidContact(*cp, w_forceReg, 1.0, 1);
 
+    // Adding rigid contact as cost should not change nb of equalities
+    BOOST_CHECK_EQUAL(nEq, tsid->nEq());
+
     contacts[i] = cp;
   }
 
@@ -650,10 +663,15 @@ BOOST_AUTO_TEST_CASE(test_contact_point_invdyn_formulation_acc_force) {
     CHECK_LESS_THAN(v.norm(), 1e6);
   }
 
+  nEq = tsid->nEq();
   for (int i = 0; i < 4; i++) {
     Eigen::Matrix<double, 3, 1> f;
     tsid->getContactForces(contacts[i]->name(), *sol, f);
     cout << contacts[i]->name() << " force:" << f.transpose() << endl;
+
+    // Removing rigid contact (added as cost) should not change nb of equalities
+    tsid->removeRigidContact(contacts[i]->name());
+    BOOST_CHECK_EQUAL(nEq, tsid->nEq());
   }
 
   delete solver;