Commit f66a8459 authored by andreadelprete's avatar andreadelprete
Browse files

[inv-dyn-form-acc-force] Fix bug: weight of force regularization task was not updated

parent 64e6efa0
......@@ -278,6 +278,20 @@ const HQPData & InverseDynamicsFormulationAccForce::computeProblemData(double ti
const ConstraintEquality & fr = cl->contact.computeForceRegularizationTask(time, q, v, m_data);
cl->forceRegTask->matrix().middleCols(m_v+cl->index, m) = fr.matrix();
cl->forceRegTask->vector() = fr.vector();
// update weight of force regularization task
ConstraintLevel::iterator itt;
for(int i=1; i<m_hqpData.size(); i++)
{
for(itt=m_hqpData[i].begin(); itt!=m_hqpData[i].end(); itt++)
{
if(itt->second->name() == cl->contact.name())
{
itt->first = cl->contact.getForceRegularizationWeight();
break;
}
}
}
}
const Matrix & M_a = m_robot.mass(m_data).bottomRows(m_v-6);
......
Supports Markdown
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