inverse-dynamics-formulation-acc-force.cpp 20.3 KB
Newer Older
1
//
2
// Copyright (c) 2017 CNRS, NYU, MPI Tübingen
3
//
jcarpent's avatar
jcarpent committed
4
5
// This file is part of tsid
// tsid is free software: you can redistribute it
6
7
8
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
jcarpent's avatar
jcarpent committed
9
// tsid is distributed in the hope that it will be
10
11
12
13
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
jcarpent's avatar
jcarpent committed
14
// tsid If not, see
15
16
17
// <http://www.gnu.org/licenses/>.
//

jcarpent's avatar
jcarpent committed
18
19
20
#include "tsid/formulations/inverse-dynamics-formulation-acc-force.hpp"
#include "tsid/math/constraint-bound.hpp"
#include "tsid/math/constraint-inequality.hpp"
21

jcarpent's avatar
jcarpent committed
22
using namespace tsid;
23
24
25
26
using namespace math;
using namespace tasks;
using namespace contacts;
using namespace solvers;
27

28
typedef pinocchio::Data Data;
29

30
TaskLevel::TaskLevel(tasks::TaskBase & task,
31
32
33
34
35
36
                     unsigned int priority):
  task(task),
  priority(priority)
{}


37
ContactLevel::ContactLevel(contacts::ContactBase & contact):
38
39
40
41
42
43
44
45
46
  contact(contact)
{}


InverseDynamicsFormulationAccForce::InverseDynamicsFormulationAccForce(const std::string & name,
                                                                       RobotWrapper & robot,
                                                                       bool verbose):
  InverseDynamicsFormulationBase(name, robot, verbose),
  m_data(robot.model()),
47
  m_baseDynamics("base-dynamics", robot.nv()-robot.na(), robot.nv()),
48
  m_solutionDecoded(false)
49
50
51
{
  m_t = 0.0;
  m_v = robot.nv();
52
  m_u = robot.nv() - robot.na();
53
  m_k = 0;
54
  m_eq = m_u;
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
  m_in = 0;
  m_hqpData.resize(2);
  m_Jc.setZero(m_k, m_v);
  m_hqpData[0].push_back(make_pair<double, ConstraintBase*>(1.0, &m_baseDynamics));
}


const Data & InverseDynamicsFormulationAccForce::data() const
{
  return m_data;
}

unsigned int InverseDynamicsFormulationAccForce::nVar() const
{
  return m_v+m_k;
}

unsigned int InverseDynamicsFormulationAccForce::nEq() const
{
  return m_eq;
}

unsigned int InverseDynamicsFormulationAccForce::nIn() const
{
  return m_in;
}


void InverseDynamicsFormulationAccForce::resizeHqpData()
{
  m_Jc.setZero(m_k, m_v);
86
  m_baseDynamics.resize(m_u, m_v+m_k);
87
  for(HQPData::iterator it=m_hqpData.begin(); it!=m_hqpData.end(); it++)
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
  {
    for(ConstraintLevel::iterator itt=it->begin(); itt!=it->end(); itt++)
    {
      itt->second->resize(itt->second->rows(), m_v+m_k);
    }
  }
}


void InverseDynamicsFormulationAccForce::addTask(TaskLevel* tl,
                                                 double weight,
                                                 unsigned int priorityLevel)
{
  if(priorityLevel > m_hqpData.size())
    m_hqpData.resize(priorityLevel);
  const ConstraintBase & c = tl->task.getConstraint();
  if(c.isEquality())
  {
    tl->constraint = new ConstraintEquality(c.name(), c.rows(), m_v+m_k);
    if(priorityLevel==0)
      m_eq += c.rows();
  }
  else if(c.isInequality())
  {
    tl->constraint = new ConstraintInequality(c.name(), c.rows(), m_v+m_k);
    if(priorityLevel==0)
      m_in += c.rows();
  }
  else
    tl->constraint = new ConstraintBound(c.name(), m_v+m_k);
  m_hqpData[priorityLevel].push_back(make_pair<double, ConstraintBase*>(weight, tl->constraint));
}


bool InverseDynamicsFormulationAccForce::addMotionTask(TaskMotion & task,
                                                       double weight,
                                                       unsigned int priorityLevel,
125
                                                       double transition_duration)
126
127
128
{
  assert(weight>=0.0);
  assert(transition_duration>=0.0);
129
130
  
  // This part is not used frequently so we can do some tests.
131
  if (weight<0.0)
132
133
134
135
    std::cerr << __FILE__ <<  " " << __LINE__ << " "
      << "weight should be positive" << std::endl;

  // This part is not used frequently so we can do some tests.
136
  if (transition_duration<0.0)
137
138
    std::cerr << "transition_duration should be positive" << std::endl;

139
  TaskLevel *tl = new TaskLevel(task, priorityLevel);
140
141
142
143
144
145
146
147
148
149
  m_taskMotions.push_back(tl);
  addTask(tl, weight, priorityLevel);

  return true;
}


bool InverseDynamicsFormulationAccForce::addForceTask(TaskContactForce & task,
                                                      double weight,
                                                      unsigned int priorityLevel,
150
                                                      double transition_duration)
151
152
153
{
  assert(weight>=0.0);
  assert(transition_duration>=0.0);
154
  // This part is not used frequently so we can do some tests.
155
  if (weight<0.0)
156
157
158
159
    std::cerr << __FILE__ <<  " " << __LINE__ << " "
      << "weight should be positive" << std::endl;

  // This part is not used frequently so we can do some tests.
160
  if (transition_duration<0.0)
161
    std::cerr << "transition_duration should be positive" << std::endl;
162
  TaskLevel *tl = new TaskLevel(task, priorityLevel);
163
164
165
166
167
168
  m_taskContactForces.push_back(tl);
  addTask(tl, weight, priorityLevel);
  return true;
}


169
170
171
172
bool InverseDynamicsFormulationAccForce::addActuationTask(TaskActuation & task,
                                                          double weight,
                                                          unsigned int priorityLevel,
                                                          double transition_duration)
173
174
175
{
  assert(weight>=0.0);
  assert(transition_duration>=0.0);
176
  if (weight<0.0)
177
178
179
180
    std::cerr << __FILE__ <<  " " << __LINE__ << " "
	      << "weight should be positive" << std::endl;

  // This part is not used frequently so we can do some tests.
181
  if (transition_duration<0.0)
182
183
    std::cerr << "transition_duration should be positive" << std::endl;

184
  TaskLevel *tl = new TaskLevel(task, priorityLevel);
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
  m_taskActuations.push_back(tl);

  if(priorityLevel > m_hqpData.size())
    m_hqpData.resize(priorityLevel);

  const ConstraintBase & c = tl->task.getConstraint();
  if(c.isEquality())
  {
    tl->constraint = new ConstraintEquality(c.name(), c.rows(), m_v+m_k);
    if(priorityLevel==0)
      m_eq += c.rows();
  }
  else  // an actuator bound becomes an inequality because actuator forces are not in the problem variables
  {
    tl->constraint = new ConstraintInequality(c.name(), c.rows(), m_v+m_k);
    if(priorityLevel==0)
      m_in += c.rows();
  }

  m_hqpData[priorityLevel].push_back(make_pair<double, ConstraintBase*>(weight, tl->constraint));

  return true;
}

209
210
211
bool InverseDynamicsFormulationAccForce::updateTaskWeight(const std::string & task_name,
                                                          double weight)
{
212
213
  ConstraintLevel::iterator it;
  // do not look into first priority level because weights do not matter there
Olivier Stasse's avatar
Olivier Stasse committed
214
  for(unsigned int i=1; i<m_hqpData.size(); i++)
215
  {
216
    for(it=m_hqpData[i].begin(); it!=m_hqpData[i].end(); it++)
217
    {
218
219
220
221
222
      if(it->second->name() == task_name)
      {
        it->first = weight;
        return true;
      }
223
224
225
226
227
    }
  }
  return false;
}

228

229
bool InverseDynamicsFormulationAccForce::addRigidContact(ContactBase & contact, 
230
231
232
                                                         double force_regularization_weight,
                                                         double motion_weight,
                                                         unsigned int motionPriorityLevel)
233
234
235
236
237
238
239
240
{
  ContactLevel *cl = new ContactLevel(contact);
  cl->index = m_k;
  m_k += contact.n_force();
  m_contacts.push_back(cl);
  resizeHqpData();

  const ConstraintBase & motionConstr = contact.getMotionConstraint();
241
  cl->motionConstraint = new ConstraintEquality(contact.name()+"_motion_task", motionConstr.rows(), m_v+m_k);
242
  m_hqpData[motionPriorityLevel].push_back(make_pair<double, ConstraintBase*>(motion_weight, cl->motionConstraint));
243
244

  const ConstraintInequality & forceConstr = contact.getForceConstraint();
245
  cl->forceConstraint = new ConstraintInequality(contact.name()+"_force_constraint", forceConstr.rows(), m_v+m_k);
246
247
248
  m_hqpData[0].push_back(make_pair<double, ConstraintBase*>(1.0, cl->forceConstraint));

  const ConstraintEquality & forceRegConstr = contact.getForceRegularizationTask();
249
  cl->forceRegTask = new ConstraintEquality(contact.name()+"_force_reg_task", forceRegConstr.rows(), m_v+m_k);
250
  m_hqpData[1].push_back(make_pair<double, ConstraintBase*>(force_regularization_weight, cl->forceRegTask));
251
252
253
254
255
256
257

  m_eq += motionConstr.rows();
  m_in += forceConstr.rows();

  return true;
}

258
259
260
261
262
263
bool InverseDynamicsFormulationAccForce::addRigidContact(ContactBase & contact)
{
    std::cout<<"[InverseDynamicsFormulationAccForce] Method addRigidContact(ContactBase) is deprecated. You should use addRigidContact(ContactBase, double) instead.\n";
    return addRigidContact(contact, 1e-5);
}

264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
bool InverseDynamicsFormulationAccForce::updateRigidContactWeights(const std::string & contact_name,
                                                                   double force_regularization_weight,
                                                                   double motion_weight)
{
    // update weight of force regularization task
    ConstraintLevel::iterator itt;
    bool force_reg_task_found = false;
    bool motion_task_found = false;
    for(unsigned int i=1; i<m_hqpData.size(); i++)
    {
      for(itt=m_hqpData[i].begin(); itt!=m_hqpData[i].end(); itt++)
      {
        if(itt->second->name() == contact_name+"_force_reg_task")
        {
          if(force_regularization_weight>=0.0)
            itt->first = force_regularization_weight;
          if(motion_task_found)
            return true;
          force_reg_task_found = true;
        }
        else if(itt->second->name() == contact_name+"_motion_task")
        {
          if(motion_weight>=0.0)
            itt->first = motion_weight;
          if(force_reg_task_found)
            return true;
          motion_task_found = true;
        }
      }
    }
294
    return false;
295
296
}

297

298
const HQPData & InverseDynamicsFormulationAccForce::computeProblemData(double time,
299
300
301
302
                                                                       ConstRefVector q,
                                                                       ConstRefVector v)
{
  m_t = time;
303

304
  std::vector<ContactTransitionInfo*>::iterator it_ct;
305
306
307
308
  for(it_ct=m_contactTransitions.begin(); it_ct!=m_contactTransitions.end(); it_ct++)
  {
    const ContactTransitionInfo * c = *it_ct;
    assert(c->time_start <= m_t);
andreadelprete's avatar
andreadelprete committed
309
    if(m_t <= c->time_end)
310
311
312
313
314
315
316
    {
      const double alpha = (m_t - c->time_start) / (c->time_end - c->time_start);
      const double fMax = c->fMax_start + alpha*(c->fMax_end - c->fMax_start);
      c->contactLevel->contact.setMaxNormalForce(fMax);
    }
    else
    {
andreadelprete's avatar
andreadelprete committed
317
318
      std::cout<<"[InverseDynamicsFormulationAccForce] Remove contact "<<
                 c->contactLevel->contact.name()<<" at time "<<time<<std::endl;
319
320
321
322
323
324
325
326
      removeRigidContact(c->contactLevel->contact.name());
      // FIXME: this won't work if multiple contact transitions occur at the same time
      // because after erasing an element the iterator is invalid
      m_contactTransitions.erase(it_ct);
      break;
    }
  }

327
328
  m_robot.computeAllTerms(m_data, q, v);

329
  for(std::vector<ContactLevel*>::iterator it=m_contacts.begin(); it!=m_contacts.end(); it++)
330
331
332
333
334
335
336
337
  {
    ContactLevel* cl = *it;
    unsigned int m = cl->contact.n_force();

    const ConstraintBase & mc = cl->contact.computeMotionTask(time, q, v, m_data);
    cl->motionConstraint->matrix().leftCols(m_v) = mc.matrix();
    cl->motionConstraint->vector() = mc.vector();

338
    const Matrix & T = cl->contact.getForceGeneratorMatrix(); // e.g., 6x12 for a 6d contact
339
    m_Jc.middleRows(cl->index, m).noalias() = T.transpose()*mc.matrix();
340
341
342
343
344
345
346
347
348
349
350

    const ConstraintInequality & fc = cl->contact.computeForceTask(time, q, v, m_data);
    cl->forceConstraint->matrix().middleCols(m_v+cl->index, m) = fc.matrix();
    cl->forceConstraint->lowerBound() = fc.lowerBound();
    cl->forceConstraint->upperBound() = fc.upperBound();

    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();
  }

351
352
353
354
355
356
  const Matrix & M_a = m_robot.mass(m_data).bottomRows(m_v-m_u);
  const Vector & h_a = m_robot.nonLinearEffects(m_data).tail(m_v-m_u);
  const Matrix & J_a = m_Jc.rightCols(m_v-m_u);
  const Matrix & M_u = m_robot.mass(m_data).topRows(m_u);
  const Vector & h_u = m_robot.nonLinearEffects(m_data).head(m_u);
  const Matrix & J_u = m_Jc.leftCols(m_u);
357
358
359
360
361

  m_baseDynamics.matrix().leftCols(m_v) = M_u;
  m_baseDynamics.matrix().rightCols(m_k) = -J_u.transpose();
  m_baseDynamics.vector() = -h_u;

362
  std::vector<TaskLevel*>::iterator it;
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
  for(it=m_taskMotions.begin(); it!=m_taskMotions.end(); it++)
  {
    const ConstraintBase & c = (*it)->task.compute(time, q, v, m_data);
    if(c.isEquality())
    {
      (*it)->constraint->matrix().leftCols(m_v) = c.matrix();
      (*it)->constraint->vector() = c.vector();
    }
    else if(c.isInequality())
    {
      (*it)->constraint->matrix().leftCols(m_v) = c.matrix();
      (*it)->constraint->lowerBound() = c.lowerBound();
      (*it)->constraint->upperBound() = c.upperBound();
    }
    else
    {
      (*it)->constraint->lowerBound().head(m_v) = c.lowerBound();
      (*it)->constraint->upperBound().head(m_v) = c.upperBound();
    }
  }

  for(it=m_taskContactForces.begin(); it!=m_taskContactForces.end(); it++)
  {
    assert(false);
  }

  for(it=m_taskActuations.begin(); it!=m_taskActuations.end(); it++)
  {
    const ConstraintBase & c = (*it)->task.compute(time, q, v, m_data);
    if(c.isEquality())
    {
394
395
396
397
      (*it)->constraint->matrix().leftCols(m_v).noalias() = c.matrix() * M_a;
      (*it)->constraint->matrix().rightCols(m_k).noalias() = - c.matrix() * J_a.transpose();
      (*it)->constraint->vector() = c.vector();
      (*it)->constraint->vector().noalias() -= c.matrix() * h_a;
398
399
400
    }
    else if(c.isInequality())
    {
401
402
403
404
405
406
      (*it)->constraint->matrix().leftCols(m_v).noalias() = c.matrix() * M_a;
      (*it)->constraint->matrix().rightCols(m_k).noalias() = - c.matrix() * J_a.transpose();
      (*it)->constraint->lowerBound() = c.lowerBound();
      (*it)->constraint->lowerBound().noalias() -= c.matrix() * h_a;
      (*it)->constraint->upperBound() = c.upperBound();
      (*it)->constraint->upperBound().noalias() -= c.matrix() * h_a;
407
408
409
410
411
412
413
414
415
416
417
    }
    else
    {
      // NB: An actuator bound becomes an inequality
      (*it)->constraint->matrix().leftCols(m_v) = M_a;
      (*it)->constraint->matrix().rightCols(m_k) = - J_a.transpose();
      (*it)->constraint->lowerBound() = c.lowerBound() - h_a;
      (*it)->constraint->upperBound() = c.upperBound() - h_a;
    }
  }

418
419
  m_solutionDecoded = false;

420
421
422
  return m_hqpData;
}

423
424


425
bool InverseDynamicsFormulationAccForce::decodeSolution(const HQPOutput & sol)
426
{
427
428
  if(m_solutionDecoded)
    return true;
429
430
431
  const Matrix & M_a = m_robot.mass(m_data).bottomRows(m_v-m_u);
  const Vector & h_a = m_robot.nonLinearEffects(m_data).tail(m_v-m_u);
  const Matrix & J_a = m_Jc.rightCols(m_v-m_u);
432
433
  m_dv = sol.x.head(m_v);
  m_f = sol.x.tail(m_k);
434
435
436
  m_tau = h_a;
  m_tau.noalias() += M_a*m_dv;
  m_tau.noalias() -= J_a.transpose()*m_f;
437
438
439
440
  m_solutionDecoded = true;
  return true;
}

441
const Vector & InverseDynamicsFormulationAccForce::getActuatorForces(const HQPOutput & sol)
442
443
{
  decodeSolution(sol);
444
445
446
  return m_tau;
}

447
const Vector & InverseDynamicsFormulationAccForce::getAccelerations(const HQPOutput & sol)
448
449
450
451
452
{
  decodeSolution(sol);
  return m_dv;
}

453
const Vector & InverseDynamicsFormulationAccForce::getContactForces(const HQPOutput & sol)
454
455
456
457
458
{
  decodeSolution(sol);
  return m_f;
}

459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
Vector InverseDynamicsFormulationAccForce::getContactForces(const std::string & name,
                                                            const HQPOutput & sol)
{
  decodeSolution(sol);
  for(std::vector<ContactLevel*>::iterator it=m_contacts.begin(); it!=m_contacts.end(); it++)
  {
    if((*it)->contact.name()==name)
    {
      const int k = (*it)->contact.n_force();
      return m_f.segment((*it)->index, k);
    }
  }
  return Vector::Zero(0);
}

474
bool InverseDynamicsFormulationAccForce::getContactForces(const std::string & name,
475
                                                          const HQPOutput & sol,
476
477
478
                                                          RefVector f)
{
  decodeSolution(sol);
479
  for(std::vector<ContactLevel*>::iterator it=m_contacts.begin(); it!=m_contacts.end(); it++)
480
481
482
483
484
485
486
487
488
489
490
  {
    if((*it)->contact.name()==name)
    {
      const int k = (*it)->contact.n_force();
      assert(f.size()==k);
      f = m_f.segment((*it)->index, k);
      return true;
    }
  }
  return false;
}
491
492

bool InverseDynamicsFormulationAccForce::removeTask(const std::string & taskName,
Olivier Stasse's avatar
Olivier Stasse committed
493
                                                    double )
494
{
Olivier Stasse's avatar
Olivier Stasse committed
495
#ifndef NDEBUG
496
497
  bool taskFound = removeFromHqpData(taskName);
  assert(taskFound);
Olivier Stasse's avatar
Olivier Stasse committed
498
499
500
501
#else
  removeFromHqpData(taskName);
#endif
  
502
  std::vector<TaskLevel*>::iterator it;
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
  for(it=m_taskMotions.begin(); it!=m_taskMotions.end(); it++)
  {
    if((*it)->task.name()==taskName)
    {
      if((*it)->priority==0)
      {
        if((*it)->constraint->isEquality())
          m_eq -= (*it)->constraint->rows();
        else if((*it)->constraint->isInequality())
          m_in -= (*it)->constraint->rows();
      }
      m_taskMotions.erase(it);
      return true;
    }
  }
  for(it=m_taskContactForces.begin(); it!=m_taskContactForces.end(); it++)
  {
    if((*it)->task.name()==taskName)
    {
      m_taskContactForces.erase(it);
      return true;
    }
  }
  for(it=m_taskActuations.begin(); it!=m_taskActuations.end(); it++)
  {
    if((*it)->task.name()==taskName)
    {
      if((*it)->priority==0)
      {
        if((*it)->constraint->isEquality())
          m_eq -= (*it)->constraint->rows();
        else
          m_in -= (*it)->constraint->rows();
      }
      m_taskActuations.erase(it);
      return true;
    }
  }
  return false;
}


bool InverseDynamicsFormulationAccForce::removeRigidContact(const std::string & contactName,
                                                            double transition_duration)
{
548
549
  if(transition_duration>0.0)
  {
550
    for(std::vector<ContactLevel*>::iterator it=m_contacts.begin(); it!=m_contacts.end(); it++)
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
    {
      if((*it)->contact.name()==contactName)
      {
        ContactTransitionInfo * transitionInfo = new ContactTransitionInfo();
        transitionInfo->contactLevel = (*it);
        transitionInfo->time_start = m_t;
        transitionInfo->time_end = m_t + transition_duration;
        const int k = (*it)->contact.n_force();
        if(m_f.size() >= (*it)->index+k)
        {
          const Vector f = m_f.segment((*it)->index, k);
          transitionInfo->fMax_start = (*it)->contact.getNormalForce(f);
        }
        else
        {
          transitionInfo->fMax_start = (*it)->contact.getMaxNormalForce();
        }
        transitionInfo->fMax_end = (*it)->contact.getMinNormalForce() + 1e-3;
        m_contactTransitions.push_back(transitionInfo);
        return true;
      }
    }
    return false;
  }

576
  bool first_constraint_found = removeFromHqpData(contactName+"_motion_task");
577
578
  assert(first_constraint_found);

579
  bool second_constraint_found = removeFromHqpData(contactName+"_force_constraint");
580
  assert(second_constraint_found);
581

582
  bool third_constraint_found = removeFromHqpData(contactName+"_force_reg_task");
583
584
  assert(third_constraint_found);

585
  bool contact_found = false;
586
  for(std::vector<ContactLevel*>::iterator it=m_contacts.begin(); it!=m_contacts.end(); it++)
587
588
589
590
591
592
593
594
  {
    if((*it)->contact.name()==contactName)
    {
      m_k -= (*it)->contact.n_force();
      m_eq -= (*it)->motionConstraint->rows();
      m_in -= (*it)->forceConstraint->rows();
      m_contacts.erase(it);
      resizeHqpData();
595
596
      contact_found = true;
      break;
597
598
    }
  }
599
600

  int k=0;
601
  for(std::vector<ContactLevel*>::iterator it=m_contacts.begin(); it!=m_contacts.end(); it++)
602
603
604
605
606
  {
    ContactLevel * cl = *it;
    cl->index = k;
    k += cl->contact.n_force();
  }
607
  return contact_found && first_constraint_found && second_constraint_found && third_constraint_found;
608
}
609
610
611
612

bool InverseDynamicsFormulationAccForce::removeFromHqpData(const std::string & name)
{
  bool found = false;
613
  for(HQPData::iterator it=m_hqpData.begin(); !found && it!=m_hqpData.end(); it++)
614
615
616
617
618
619
  {
    for(ConstraintLevel::iterator itt=it->begin(); !found && itt!=it->end(); itt++)
    {
      if(itt->second->name()==name)
      {
        it->erase(itt);
620
        return true;
621
622
623
      }
    }
  }
624
  return false;
625
}