Commit 61fcaae2 authored by andreadelprete's avatar andreadelprete
Browse files

[inv-dyn-form-acc-force] Fix potential bug in removal of contact constraint

parent c8600af6
......@@ -504,6 +504,9 @@ bool InverseDynamicsFormulationAccForce::removeRigidContact(const std::string &
bool second_constraint_found = removeFromHqpData(contactName);
assert(second_constraint_found);
bool third_constraint_found = removeFromHqpData(contactName);
assert(third_constraint_found);
bool contact_found = false;
for(std::vector<ContactLevel*>::iterator it=m_contacts.begin(); it!=m_contacts.end(); it++)
{
......@@ -526,7 +529,7 @@ bool InverseDynamicsFormulationAccForce::removeRigidContact(const std::string &
cl->index = k;
k += cl->contact.n_force();
}
return contact_found;
return contact_found && first_constraint_found && second_constraint_found && third_constraint_found;
}
bool InverseDynamicsFormulationAccForce::removeFromHqpData(const std::string & name)
......@@ -539,9 +542,9 @@ bool InverseDynamicsFormulationAccForce::removeFromHqpData(const std::string & n
if(itt->second->name()==name)
{
it->erase(itt);
found = true; // keep looking because there may be more constraints associated to the same task
return true;
}
}
}
return found;
return false;
}
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment