PLDPSolver.cpp 26.7 KB
Newer Older
Thomas Moulard's avatar
Thomas Moulard committed
1
/*
Thomas Moulard's avatar
Thomas Moulard committed
2
 * Copyright 2009, 2010,
Thomas Moulard's avatar
Thomas Moulard committed
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
 *
 * Francois   Keith
 * Nicolas    Mansard
 * Olivier    Stasse
 *
 * JRL, CNRS/AIST
 *
 * This file is part of walkGenJrl.
 * walkGenJrl is free software: you can redistribute it 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.
 *
 * walkGenJrl is distributed in the hope that it will be 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 walkGenJrl.  If not, see <http://www.gnu.org/licenses/>.
 *
Thomas Moulard's avatar
Thomas Moulard committed
23
 *  Research carried out within the scope of the
Thomas Moulard's avatar
Thomas Moulard committed
24
25
26
27
28
 *  Joint Japanese-French Robotics Laboratory (JRL)
 */
/*! \file PLDPSolver.cpp
  \brief This file implements the optimized QP solver proposed by Dimitar 2009.
*/
Thomas Moulard's avatar
Thomas Moulard committed
29
#include <cstring>
30
31
#include <fstream>
#include <iostream>
Thomas Moulard's avatar
Thomas Moulard committed
32

33
#include <math.h>
34
#include <stdlib.h>
35

Thomas Moulard's avatar
Thomas Moulard committed
36
#include "portability/gettimeofday.hh"
Thomas Moulard's avatar
Thomas Moulard committed
37
38

#ifdef WIN32
39
#include <Windows.h>
Thomas Moulard's avatar
Thomas Moulard committed
40
41
#endif /* WIN32 */

42
#include <Mathematics/PLDPSolver.hh>
Thomas Moulard's avatar
Thomas Moulard committed
43
44
45
46
47
48

// Isnan and Isinf
#ifdef WIN32
#include <float.h>
#define isnan _isnan

49
50
51
52
// definition of isinf for win32
// src:  http://www.gnu.org/software/libtool/manual/autoconf/
// Function-Portability.html
inline int isinf(double x) { return isnan(x - x); }
Thomas Moulard's avatar
Thomas Moulard committed
53
54
#endif /* WIN32 */

55
#include <Debug.hh>
Thomas Moulard's avatar
Thomas Moulard committed
56
57
58
59
60

using namespace PatternGeneratorJRL;
using namespace Optimization::Solver;
using namespace std;

61
62
PLDPSolver::PLDPSolver(unsigned int CardU, double *iPu, double *Px, double *Pu,
                       double *iLQ) {
Thomas Moulard's avatar
Thomas Moulard committed
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
  m_DebugMode = 0;
  m_HotStart = true;
  m_InternalTime = 0.0;
  m_tol = 1e-8;
  m_LimitedComputationTime = true;
  m_AmountOfLimitedComputationTime = 0.0013;
  /* Initialize pointers */
  m_CardV = CardU;

  m_Pu = Pu;
  m_iPu = iPu;
  m_Px = Px;
  m_iPuPx = 0;
  m_Vk = 0;
  m_CstPartOfCostFunction = 0;
78
79
  m_UnconstrainedDescentDirection = 0;
  m_L = 0;
Thomas Moulard's avatar
Thomas Moulard committed
80
81
82
83
  m_iLQ = iLQ;
  m_d = 0;
  m_v1 = m_v2 = m_y = 0;
  m_tmp1 = m_tmp2 = 0;
84
  m_A = m_b = 0;
Thomas Moulard's avatar
Thomas Moulard committed
85
86
87

  m_ConstraintsValueComputed = 0;

88
  m_NbMaxOfConstraints = 8 * m_CardV;
Thomas Moulard's avatar
Thomas Moulard committed
89
90
  m_NbOfConstraints = 0;

91
92
  m_OptCholesky = new PatternGeneratorJRL::OptCholesky(
      m_NbMaxOfConstraints, 2 * m_CardV, OptCholesky::MODE_FORTRAN);
Thomas Moulard's avatar
Thomas Moulard committed
93
94
95

  string Buffer("InfosPLDP");
  if (m_HotStart)
96
    Buffer += "HS";
Thomas Moulard's avatar
Thomas Moulard committed
97
  if (m_LimitedComputationTime)
98
99
100
    Buffer += "LT";
  Buffer += ".dat";
  RESETDEBUG6((char *)Buffer.c_str());
Thomas Moulard's avatar
Thomas Moulard committed
101
102
103
104
  RESETDEBUG6("ActivatedConstraints.dat");
  AllocateMemoryForSolver();
}

105
void PLDPSolver::AllocateMemoryForSolver() {
Thomas Moulard's avatar
Thomas Moulard committed
106
  PrecomputeiPuPx();
107
108
  m_Vk = new double[2 * m_CardV];
  memset(m_Vk, 0, 2 * m_CardV * sizeof(double));
Thomas Moulard's avatar
Thomas Moulard committed
109

110
111
  m_PreviousZMPSolution = new double[2 * m_CardV];
  memset(m_PreviousZMPSolution, 0, 2 * m_CardV * sizeof(double));
Thomas Moulard's avatar
Thomas Moulard committed
112

113
114
  m_UnconstrainedDescentDirection = new double[2 * m_CardV];
  memset(m_UnconstrainedDescentDirection, 0, 2 * m_CardV * sizeof(double));
Thomas Moulard's avatar
Thomas Moulard committed
115

116
117
  m_L = new double[m_NbMaxOfConstraints * m_NbMaxOfConstraints];
  m_iL = new double[m_NbMaxOfConstraints * m_NbMaxOfConstraints];
Thomas Moulard's avatar
Thomas Moulard committed
118
119
120
121
122
123
124
125

  m_v1 = new double[m_NbMaxOfConstraints];
  m_v2 = new double[m_NbMaxOfConstraints];
  m_tmp1 = new double[m_NbMaxOfConstraints];
  m_tmp2 = new double[m_NbMaxOfConstraints];

  m_y = new double[m_NbMaxOfConstraints];

126
  m_ConstraintsValueComputed = new bool[m_NbMaxOfConstraints * 2];
Thomas Moulard's avatar
Thomas Moulard committed
127

128
  m_d = new double[2 * m_CardV];
Thomas Moulard's avatar
Thomas Moulard committed
129
130
131
132
  m_OptCholesky->SetL(m_L);
  m_OptCholesky->SetiL(m_iL);
}

133
void PLDPSolver::InitializeSolver() {
Thomas Moulard's avatar
Thomas Moulard committed
134
135
#if 0
  // Allocation max:
Thomas Moulard's avatar
Thomas Moulard committed
136
  // We assume that we might at max. 8 actives constraintes
Thomas Moulard's avatar
Thomas Moulard committed
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
  // per control time.
  memset(m_v1,0,m_NbMaxOfConstraints*sizeof(double));
  // Same than v1.
  memset(m_v2,0,m_NbMaxOfConstraints*sizeof(double));
  // Same than y.
  memset(m_y,0,m_NbMaxOfConstraints*sizeof(double));

  // Same than L.
  memset(m_L,0,8*m_CardV*sizeof(double));
  // Same than iL.
  memset(m_iL,0,8*m_CardV*sizeof(double));

  // Same for the descent.
  memset(m_d,0,2*m_CardV*sizeof(double));
#endif
  m_ActivatedConstraints.clear();
}

155
156
157
PLDPSolver::~PLDPSolver() {
  if (m_iPuPx != 0)
    delete[] m_iPuPx;
Thomas Moulard's avatar
Thomas Moulard committed
158

159
160
  if (m_Vk != 0)
    delete[] m_Vk;
Thomas Moulard's avatar
Thomas Moulard committed
161

162
163
  if (m_PreviousZMPSolution != 0)
    delete[] m_PreviousZMPSolution;
Thomas Moulard's avatar
Thomas Moulard committed
164

165
166
  if (m_UnconstrainedDescentDirection != 0)
    delete[] m_UnconstrainedDescentDirection;
Thomas Moulard's avatar
Thomas Moulard committed
167

168
  if (m_OptCholesky != 0)
Thomas Moulard's avatar
Thomas Moulard committed
169
170
    delete m_OptCholesky;

171
172
  if (m_L != 0)
    delete[] m_L;
Thomas Moulard's avatar
Thomas Moulard committed
173

174
175
  if (m_iL != 0)
    delete[] m_iL;
Thomas Moulard's avatar
Thomas Moulard committed
176

177
178
  if (m_v1 != 0)
    delete[] m_v1;
Thomas Moulard's avatar
Thomas Moulard committed
179

180
181
  if (m_v2 != 0)
    delete[] m_v2;
Thomas Moulard's avatar
Thomas Moulard committed
182

183
184
  if (m_tmp1 != 0)
    delete[] m_tmp1;
Thomas Moulard's avatar
Thomas Moulard committed
185

186
187
  if (m_tmp2 != 0)
    delete[] m_tmp2;
Thomas Moulard's avatar
Thomas Moulard committed
188

189
190
  if (m_d != 0)
    delete[] m_d;
Thomas Moulard's avatar
Thomas Moulard committed
191

192
193
  if (m_y != 0)
    delete[] m_y;
Thomas Moulard's avatar
Thomas Moulard committed
194

195
196
  if (m_ConstraintsValueComputed != 0)
    delete[] m_ConstraintsValueComputed;
Thomas Moulard's avatar
Thomas Moulard committed
197
198
}

199
200
int PLDPSolver::PrecomputeiPuPx() {
  m_iPuPx = new double[2 * m_CardV * 6];
Thomas Moulard's avatar
Thomas Moulard committed
201

202
  memset(m_iPuPx, 0, 2 * m_CardV * 6);
Thomas Moulard's avatar
Thomas Moulard committed
203

204
205
206
207
208
209
210
211
212
213
  if (m_DebugMode > 1) {
    ofstream aof;
    aof.open("iPu.dat", ofstream::out);
    for (unsigned int i = 0; i < m_CardV; i++) {
      for (unsigned int j = 0; j < m_CardV; j++) {
        aof << m_iPu[i * m_CardV + j] << " ";
      }
      aof << endl;
    }
    aof.close();
Thomas Moulard's avatar
Thomas Moulard committed
214

215
216
217
218
219
220
221
222
    aof.open("Pu.dat", ofstream::out);
    for (unsigned int i = 0; i < m_CardV; i++) {
      for (unsigned int j = 0; j < m_CardV; j++) {
        aof << m_Pu[i * m_CardV + j] << " ";
      }
      aof << endl;
    }
    aof.close();
Thomas Moulard's avatar
Thomas Moulard committed
223

224
225
226
227
228
229
230
231
    aof.open("Px.dat", ofstream::out);
    for (unsigned int i = 0; i < m_CardV; i++) {
      for (unsigned int j = 0; j < 3; j++) {
        aof << m_Px[i * 3 + j] << " ";
      }
      aof << endl;
    }
    aof.close();
Thomas Moulard's avatar
Thomas Moulard committed
232

233
234
235
236
237
238
    aof.open("isLQ.dat", ofstream::out);
    for (unsigned int i = 0; i < m_CardV; i++) {
      for (unsigned int j = 0; j < m_CardV; j++) {
        aof << m_iLQ[i * 2 * m_CardV + j] << " ";
      }
      aof << endl;
Thomas Moulard's avatar
Thomas Moulard committed
239
    }
240
241
    aof.close();
  }
Thomas Moulard's avatar
Thomas Moulard committed
242

Thomas Moulard's avatar
Thomas Moulard committed
243
244
  // This should be reimplemented with a proper matrix library.
  // will be done in a second time.
245
246
247
248
249
250
251
252
253
254
255
256
257
258
  for (unsigned int i = 0; i < m_CardV; i++) {
    for (unsigned int j = 0; j < 3; j++) {
      m_iPuPx[i * 6 + j] = 0.0;
      m_iPuPx[i * 6 + j + 3] = 0.0;
      m_iPuPx[(i + m_CardV) * 6 + j] = 0.0;
      m_iPuPx[(i + m_CardV) * 6 + j + 3] = 0.0;
      for (unsigned int k = 0; k < m_CardV; k++) {
        double tmp = m_iPu[k * m_CardV + i] * m_Px[k * 3 + j];

        m_iPuPx[i * 6 + j] += tmp;
        //            m_iPuPx[i*6+j+3]+= tmp;
        //            m_iPuPx[(i+m_CardV)*6+j]+= tmp;
        m_iPuPx[(i + m_CardV) * 6 + j + 3] += tmp;
      }
Thomas Moulard's avatar
Thomas Moulard committed
259
    }
260
  }
Thomas Moulard's avatar
Thomas Moulard committed
261
262
263
  return 0;
}

264
265
int PLDPSolver::ComputeInitialSolution(double *ZMPRef, double *XkYk,
                                       bool StartingSequence) {
Thomas Moulard's avatar
Thomas Moulard committed
266
267
  /*! The initial solution of the problem is given by
    eq(14) Dimitar ICRA 2008
Thomas Moulard's avatar
Thomas Moulard committed
268
    U0 = iPu * Px [Xkt Ykt]t + iPu * ZMPRef
Thomas Moulard's avatar
Thomas Moulard committed
269
    The only part which can not be precomputed is ZMPRef.
270
  */
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
  if ((m_HotStart) && (!StartingSequence)) {
    for (unsigned int i = 0; i < m_CardV; i++) {
      m_Vk[i] = 0.0;
      m_Vk[i + m_CardV] = 0.0;
      for (unsigned int j = 0; j < 3; j++)
        m_Vk[i] -= m_iPuPx[i * 6 + j] * XkYk[j];

      for (unsigned int j = 3; j < 6; j++)
        m_Vk[i + m_CardV] -= m_iPuPx[(i + m_CardV) * 6 + j] * XkYk[j];

      for (unsigned int j = 0; j < m_CardV - 1; j++)
        m_Vk[i] += m_iPu[j * m_CardV + i] * m_PreviousZMPSolution[j + 1];
      m_Vk[i] += m_iPu[(m_CardV - 1) * m_CardV + i] * ZMPRef[m_CardV - 1];

      for (unsigned int j = 0; j < m_CardV - 1; j++)
        m_Vk[i + m_CardV] +=
            m_iPu[j * m_CardV + i] * m_PreviousZMPSolution[j + m_CardV + 1];
      m_Vk[i + m_CardV] +=
          m_iPu[(m_CardV - 1) * m_CardV + i] * ZMPRef[m_CardV - 1 + m_CardV];
Thomas Moulard's avatar
Thomas Moulard committed
290
    }
Thomas Moulard's avatar
Thomas Moulard committed
291

292
293
294
295
296
297
  } else {
    for (unsigned int i = 0; i < m_CardV; i++) {
      m_Vk[i] = 0.0;
      m_Vk[i + m_CardV] = 0.0;
      for (unsigned int j = 0; j < 3; j++)
        m_Vk[i] -= m_iPuPx[i * 6 + j] * XkYk[j];
Thomas Moulard's avatar
Thomas Moulard committed
298

299
300
      for (unsigned int j = 3; j < 6; j++)
        m_Vk[i + m_CardV] -= m_iPuPx[(i + m_CardV) * 6 + j] * XkYk[j];
Thomas Moulard's avatar
Thomas Moulard committed
301

302
303
      for (unsigned int j = 0; j < m_CardV; j++)
        m_Vk[i] += m_iPu[j * m_CardV + i] * ZMPRef[j];
Thomas Moulard's avatar
Thomas Moulard committed
304

305
306
      for (unsigned int j = 0; j < m_CardV; j++)
        m_Vk[i + m_CardV] += m_iPu[j * m_CardV + i] * ZMPRef[j + m_CardV];
Thomas Moulard's avatar
Thomas Moulard committed
307
    }
308
  }
Thomas Moulard's avatar
Thomas Moulard committed
309
310
311
  return 0;
}

312
int PLDPSolver::ForwardSubstitution() {
Thomas Moulard's avatar
Thomas Moulard committed
313
314
315
316
  // Compute v2 q (14b) in Dimitrov 2009.
  // First Phase
  // EE^t v2 = v1 <-> LL^t v2 = v1
  // Now solving
Thomas Moulard's avatar
Thomas Moulard committed
317
  // L y = v1
318
  unsigned int startIndex = 0;
Thomas Moulard's avatar
Thomas Moulard committed
319
  /*
320
321
    if ((m_HotStart==false) ||
    (m_ItNb>0))
Thomas Moulard's avatar
Thomas Moulard committed
322
323
    startIndex = m_ActivatedConstraints.size()-1;
  */
324
325
326
327
328
329
330
331
  for (unsigned int i = startIndex; i < m_ActivatedConstraints.size(); i++) {
    m_y[i] = m_v1[i];
    for (unsigned int k = 0; k < i; k++)
      m_y[i] += -m_L[i * m_NbMaxOfConstraints + k] * m_y[k];

    if (m_L[i * m_NbMaxOfConstraints + i] != 0.0)
      m_y[i] /= m_L[i * m_NbMaxOfConstraints + i];
  }
Thomas Moulard's avatar
Thomas Moulard committed
332
333
334
  return 0;
}

335
int PLDPSolver::BackwardSubstitution() {
Thomas Moulard's avatar
Thomas Moulard committed
336
  // Compute v2 q (14b) in Dimitrov 2009.
Thomas Moulard's avatar
Thomas Moulard committed
337
  // Second phase
Thomas Moulard's avatar
Thomas Moulard committed
338
339
340
341
  // Now solving
  // LL^t v2 = v1 <-> L y = v1 with L^t v2 = y
  // y solved with first phase.
  // So now we are looking for v2.
Olivier Stasse's avatar
Olivier Stasse committed
342
  std::vector<unsigned int>::size_type SizeOfL = m_ActivatedConstraints.size();
343
  if (SizeOfL == 0)
Thomas Moulard's avatar
Thomas Moulard committed
344
    return 0;
Thomas Moulard's avatar
Thomas Moulard committed
345

Thomas Moulard's avatar
Thomas Moulard committed
346
  ODEBUG("BackwardSubstitution " << m_ItNb);
Olivier Stasse's avatar
Olivier Stasse committed
347
348

  for (std::vector<unsigned int>::size_type i = SizeOfL - 1;; i--) {
349
350
    double tmp = 0.0;
    m_v2[i] = m_y[i];
Olivier Stasse's avatar
Olivier Stasse committed
351
352
    for (std::vector<unsigned int>::size_type k = i + 1;
         k < SizeOfL; k++) {
Olivier Stasse's avatar
Olivier Stasse committed
353
      if (k == SizeOfL - 1)
354
355
356
        tmp = m_v2[i];

      m_v2[i] -= m_L[k * m_NbMaxOfConstraints + i] * m_v2[k];
Thomas Moulard's avatar
Thomas Moulard committed
357
    }
358
359
360
361
362
363
    m_v2[i] = m_v2[i] / m_L[i * m_NbMaxOfConstraints + i];

    tmp = tmp / m_L[i * m_NbMaxOfConstraints + i];
    ODEBUG("BS: m_L[i*m_NbMaxOfConstraints+i]:"
           << m_L[i * m_NbMaxOfConstraints + i] << " " << m_y[i]);
    ODEBUG("m_v2[" << i << " ] = " << m_v2[i] << " " << tmp);
Olivier Stasse's avatar
Olivier Stasse committed
364
    if (i==0) break;
365
  }
Thomas Moulard's avatar
Thomas Moulard committed
366
367
368
  return 0;
}

369
int PLDPSolver::ComputeProjectedDescentDirection() {
Thomas Moulard's avatar
Thomas Moulard committed
370

371
372
  if (m_DebugMode > 1) {
    ofstream aof;
Thomas Moulard's avatar
Thomas Moulard committed
373

374
375
376
377
378
379
380
    char Buffer[1024];
    sprintf(Buffer, "AC_%02d.dat", m_ItNb);
    aof.open(Buffer, ofstream::out);
    for (unsigned int li = 0; li < m_ActivatedConstraints.size(); li++)
      aof << m_ActivatedConstraints[li] << " ";
    aof << endl;
    aof.close();
Thomas Moulard's avatar
Thomas Moulard committed
381

382
383
384
385
386
387
388
    sprintf(Buffer, "E_%02d.dat", m_ItNb);
    aof.open(Buffer, ofstream::out);
    for (unsigned int li = 0; li < m_ActivatedConstraints.size(); li++) {
      unsigned int RowCstMatrix = m_ActivatedConstraints[li];
      for (unsigned int lj = 0; lj < 2 * m_CardV; lj++) {
        aof << m_A[RowCstMatrix + lj * (m_NbOfConstraints + 1)] << " ";
      }
Thomas Moulard's avatar
Thomas Moulard committed
389
390
      aof << endl;
    }
391
392
393
394
395
396
397
398
399
400
    aof.close();

    sprintf(Buffer, "c_%02d.dat", m_ItNb);
    aof.open(Buffer, ofstream::out);
    for (unsigned int li = 0; li < 2 * m_CardV; li++) {
      aof << m_UnconstrainedDescentDirection[li] << " ";
    }
    aof << endl;
    aof.close();
  }
Thomas Moulard's avatar
Thomas Moulard committed
401
  // Compute v1 eq (14a) in Dimitrov 2009
402
403
  ODEBUG_NENDL("m_v1(" << m_ItNb << ")= [");
  unsigned int startIndex = 0;
Thomas Moulard's avatar
Thomas Moulard committed
404
  /*
405
406
    if ((m_HotStart==false) ||
    (m_ItNb>0))
Thomas Moulard's avatar
Thomas Moulard committed
407
408
    startIndex = m_ActivatedConstraints.size()-1;
  */
409
410
411
412
413
414
415
  for (unsigned int li = startIndex; li < m_ActivatedConstraints.size(); li++) {
    m_v1[li] = 0.0;
    unsigned int RowCstMatrix = m_ActivatedConstraints[li];
    ODEBUG("RowCstMatrix:" << RowCstMatrix);
    for (unsigned int lj = 0; lj < 2 * m_CardV; lj++) {
      m_v1[li] += m_A[RowCstMatrix + lj * (m_NbOfConstraints + 1)] *
                  m_UnconstrainedDescentDirection[lj];
Thomas Moulard's avatar
Thomas Moulard committed
416
    }
417
418
    ODEBUG_NENDL(m_v1[li] << " ");
  }
Thomas Moulard's avatar
Thomas Moulard committed
419
420
  ODEBUG_NENDL("]" << std::endl);

421
422
423
424
425
426
427
428
429
  if (m_DebugMode > 1) {
    ofstream aof;
    char Buffer[1024];
    sprintf(Buffer, "v1_%02d.dat", m_ItNb);
    aof.open(Buffer, ofstream::out);
    for (unsigned int lj = 0; lj < m_ActivatedConstraints.size(); lj++)
      aof << m_v1[lj] << endl;
    aof.close();
  }
Thomas Moulard's avatar
Thomas Moulard committed
430
431

  // Compute v2 by Forward and backward substitution.
Thomas Moulard's avatar
Thomas Moulard committed
432
  // v2 = iLt iL E c
Thomas Moulard's avatar
Thomas Moulard committed
433
434
435
  // LtL v2 = c
  // Lt y =c
  ForwardSubstitution();
436
437
438
439
440
441
442
443
444
  if (m_DebugMode > 1) {
    ofstream aof;
    char Buffer[1024];
    sprintf(Buffer, "y_%02d.dat", m_ItNb);
    aof.open(Buffer, ofstream::out);
    for (unsigned int lj = 0; lj < m_ActivatedConstraints.size(); lj++)
      aof << m_y[lj] << endl;
    aof.close();
  }
Thomas Moulard's avatar
Thomas Moulard committed
445

Thomas Moulard's avatar
Thomas Moulard committed
446
447
448
  // Solve L v2 = y
  BackwardSubstitution();

449
450
451
452
453
454
455
456
457
  if (m_DebugMode > 1) {
    ofstream aof;
    char Buffer[1024];
    sprintf(Buffer, "v2_%02d.dat", m_ItNb);
    aof.open(Buffer, ofstream::out);
    for (unsigned int lj = 0; lj < m_ActivatedConstraints.size(); lj++)
      aof << m_v2[lj] << endl;
    aof.close();
  }
Thomas Moulard's avatar
Thomas Moulard committed
458
459
460

  // Compute d
  // d = c - Et v2
461
462
463
464
465
466
  ODEBUG("Size of ActivatedConstraints: " << m_ActivatedConstraints.size());
  for (unsigned int li = 0; li < 2 * m_CardV; li++) {
    m_d[li] = m_UnconstrainedDescentDirection[li];
    for (unsigned int lj = 0; lj < m_ActivatedConstraints.size(); lj++) {
      unsigned int RowCstMatrix = m_ActivatedConstraints[lj];
      m_d[li] -= m_A[RowCstMatrix + li * (m_NbOfConstraints + 1)] * m_v2[lj];
Thomas Moulard's avatar
Thomas Moulard committed
467
    }
468
469
470
471
472
473
474
475
476
477
478
479
  }

  if (m_DebugMode > 1) {
    ofstream aof;
    char Buffer[1024];
    sprintf(Buffer, "UDD_%02d.dat", m_ItNb);
    aof.open(Buffer, ofstream::out);
    for (unsigned int lj = 0; lj < 2 * m_CardV; lj++)
      aof << m_d[lj] << " ";
    aof << endl;
    aof.close();
  }
Thomas Moulard's avatar
Thomas Moulard committed
480
481
482
  return 0;
}

483
484
485
double PLDPSolver::ComputeAlpha(vector<unsigned int> &NewActivatedConstraints,
                                vector<int> &SimilarConstraint) {
  double Alpha = 10000000.0;
Thomas Moulard's avatar
Thomas Moulard committed
486
  double *ptA = 0;
487
488
  bool TheConstraintIsToBeAdded = false;
  unsigned int TheConstraintToActivate = 0;
Thomas Moulard's avatar
Thomas Moulard committed
489

490
  for (unsigned li = 0; li < m_NbOfConstraints; li++) {
Thomas Moulard's avatar
Thomas Moulard committed
491

492
    bool ConstraintFound = false;
Thomas Moulard's avatar
Thomas Moulard committed
493

494
495
496
497
498
499
500
501
502
503
504
505
506
507
    // Register that this constraint has yet been computed.
    m_ConstraintsValueComputed[li] = false;
    m_ConstraintsValueComputed[li + m_NbOfConstraints] = false;

    // Make sure that this constraint is not already activated.
    for (unsigned int ConstraintIndex = 0;
         ConstraintIndex < m_ActivatedConstraints.size(); ConstraintIndex++) {
      if (m_ActivatedConstraints[ConstraintIndex] == li) {
        ConstraintFound = true;
        break;
      }
    }
    if (ConstraintFound)
      continue;
Thomas Moulard's avatar
Thomas Moulard committed
508

509
    ptA = m_A + li;
Thomas Moulard's avatar
Thomas Moulard committed
510

511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
    m_tmp1[li] = 0.0;

    // Check if we can not reuse an already computed result
    {
      bool ToBeComputed = true;
      if (SimilarConstraint[li] != 0) {
        int lindex = li + SimilarConstraint[li];
        if (m_ConstraintsValueComputed[lindex]) {
          m_tmp1[li] = -m_tmp1[lindex];
          ToBeComputed = false;
        }
      }

      if (ToBeComputed)
        for (unsigned lj = 0; lj < 2 * m_CardV; lj++) {
          m_tmp1[li] += *ptA * m_d[lj];
          ptA += (m_NbOfConstraints + 1);
528
        }
529
    }
Thomas Moulard's avatar
Thomas Moulard committed
530

531
    m_ConstraintsValueComputed[li] = true;
Thomas Moulard's avatar
Thomas Moulard committed
532

533
534
535
536
    if (m_tmp1[li] < 0.0) {
      double lalpha = 0.0;
      double *pt2A = m_A + li;
      m_tmp2[li] = -m_b[li];
Thomas Moulard's avatar
Thomas Moulard committed
537

Thomas Moulard's avatar
Thomas Moulard committed
538
      // Check if we can not reuse an already computed result
Thomas Moulard's avatar
Thomas Moulard committed
539
      {
540
541
542
543
544
545
        bool ToBeComputed = true;
        if (SimilarConstraint[li] != 0) {
          int lindex = li + SimilarConstraint[li];
          if (m_ConstraintsValueComputed[lindex + m_NbOfConstraints]) {
            m_tmp2[li] += -m_tmp2[lindex] - m_b[lindex];
            ToBeComputed = false;
546
          }
547
        }
548

549
550
551
552
        if (ToBeComputed)
          for (unsigned lj = 0; lj < 2 * m_CardV; lj++) {
            m_tmp2[li] -= *pt2A * m_Vk[lj];
            pt2A += (m_NbOfConstraints + 1);
553
          }
554
      }
555

556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
      if (m_tmp2[li] > m_tol) {
        std::cerr << "PB ON constraint " << li << " at time " << m_InternalTime
                  << endl;
        std::cerr << " Check current V k=" << m_ItNb << endl;
        std::cerr << " should be faisable : " << m_tmp2[li] << " " << -m_v2[li]
                  << endl;
      } else if (m_tmp2[li] > 0.0)
        m_tmp2[li] = -m_tol;

      lalpha = m_tmp2[li] / m_tmp1[li];

      if (Alpha > lalpha) {
        ODEBUG("m_v2[li] : " << m_v2[li] << " m_v1[li] : " << m_v1[li]
                             << " "
                                " lalpha: "
                             << lalpha
                             << " "
                                " Constrainte "
                             << li << " on " << m_NbOfConstraints
                             << " constraints.");

        Alpha = lalpha;
        if (Alpha < 1) {
          TheConstraintIsToBeAdded = true;
          TheConstraintToActivate = li;
581
        }
582
      }
Thomas Moulard's avatar
Thomas Moulard committed
583
    }
584
  }
Thomas Moulard's avatar
Thomas Moulard committed
585
586
587
588
589
590

  if (TheConstraintIsToBeAdded)
    NewActivatedConstraints.push_back(TheConstraintToActivate);

  return Alpha;
}
591
592
593
594
595
int PLDPSolver::SolveProblem(
    double *CstPartOfTheCostFunction, unsigned int NbOfConstraints,
    double *LinearPartOfConstraints, double *CstPartOfConstraints,
    double *ZMPRef, double *XkYk, double *X, vector<int> &SimilarConstraints,
    unsigned int NumberOfRemovedConstraints, bool StartingSequence) {
Thomas Moulard's avatar
Thomas Moulard committed
596
597
598
  vector<unsigned int> NewActivatedConstraints;
  if (StartingSequence)
    m_InternalTime = 0.0;
Thomas Moulard's avatar
Thomas Moulard committed
599

Thomas Moulard's avatar
Thomas Moulard committed
600
601
602
603
604
605
606
607
608
  InitializeSolver();

  m_A = LinearPartOfConstraints;
  m_b = CstPartOfConstraints;
  m_NbOfConstraints = NbOfConstraints;

  /* Step zero : Algorithm initialization. */
  m_CstPartOfCostFunction = CstPartOfTheCostFunction;

609
610
  ODEBUG("State: " << XkYk[0] << " " << XkYk[3] << " " << XkYk[1] << " "
                   << XkYk[4] << " " << XkYk[2] << " " << XkYk[5] << " ");
Thomas Moulard's avatar
Thomas Moulard committed
611

612
  ComputeInitialSolution(ZMPRef, XkYk, StartingSequence);
Thomas Moulard's avatar
Thomas Moulard committed
613

614
  ODEBUG("DebugMode:" << m_DebugMode);
Thomas Moulard's avatar
Thomas Moulard committed
615

616
617
618
  if (m_DebugMode > 1) {
    string DebugVkFileName("VkInit.dat");
    WriteCurrentZMPSolution("VkInit.dat", XkYk);
Thomas Moulard's avatar
Thomas Moulard committed
619

620
621
622
623
624
625
    ofstream aof;
    aof.open("InitialSolution.dat", ofstream::out);
    for (unsigned int i = 0; i < 2 * m_CardV; i++)
      aof << m_Vk[i] << " ";
    aof << endl;
    aof.close();
Thomas Moulard's avatar
Thomas Moulard committed
626

627
628
629
630
    aof.open("iPuPx.dat", ofstream::out);
    for (unsigned int i = 0; i < 2 * m_CardV; i++) {
      for (unsigned int j = 0; j < 6; j++)
        aof << m_iPuPx[i * 6 + j] << " ";
Thomas Moulard's avatar
Thomas Moulard committed
631
      aof << endl;
632
633
634
    }
    aof << endl;
    aof.close();
Thomas Moulard's avatar
Thomas Moulard committed
635

636
637
638
639
    aof.open("A.dat", ofstream::out);
    for (unsigned int i = 0; i < m_NbOfConstraints; i++) {
      for (unsigned int j = 0; j < 2 * m_CardV; j++)
        aof << m_A[j * (m_NbOfConstraints + 1) + i] << " ";
Thomas Moulard's avatar
Thomas Moulard committed
640
      aof << endl;
641
642
643
    }
    aof << endl;
    aof.close();
Thomas Moulard's avatar
Thomas Moulard committed
644

645
646
647
    aof.open("b.dat", ofstream::out);
    for (unsigned int i = 0; i < m_NbOfConstraints; i++) {
      aof << m_b[i] << " ";
Thomas Moulard's avatar
Thomas Moulard committed
648
    }
649
650
    aof << endl;
    aof.close();
Thomas Moulard's avatar
Thomas Moulard committed
651

652
653
654
655
656
657
    aof.open("ZMPRef.dat", ofstream::out);
    for (unsigned int i = 0; i < 2 * m_CardV; i++) {
      aof << ZMPRef[i] << " ";
    }
    aof << endl;
    aof.close();
Thomas Moulard's avatar
Thomas Moulard committed
658

659
660
661
662
663
664
665
666
667
668
669
670
671
672
    aof.open("XkYk.dat", ofstream::out);
    for (unsigned int i = 0; i < 6; i++) {
      aof << XkYk[i] << " ";
    }
    aof << endl;
    aof.close();
  }
  if (m_DebugMode > 1) {
    ofstream aof;
    aof.open("A.dat", ofstream::out);
    for (unsigned int i = 0; i < m_NbOfConstraints; i++) {
      for (unsigned int j = 0; j < 2 * m_CardV; j++) {
        aof << m_A[i + j * (NbOfConstraints + 1)] << " ";
      }
Thomas Moulard's avatar
Thomas Moulard committed
673
      aof << endl;
674
675
676
    }
    aof << endl;
    aof.close();
Thomas Moulard's avatar
Thomas Moulard committed
677

678
    aof.open("b.dat", ofstream::out);
Thomas Moulard's avatar
Thomas Moulard committed
679

680
681
    for (unsigned int j = 0; j < m_NbOfConstraints; j++) {
      aof << m_b[j] << " ";
Thomas Moulard's avatar
Thomas Moulard committed
682
    }
683
    aof << endl;
Thomas Moulard's avatar
Thomas Moulard committed
684

685
686
    aof.close();
  }
Thomas Moulard's avatar
Thomas Moulard committed
687

688
  bool ContinueAlgo = true;
Thomas Moulard's avatar
Thomas Moulard committed
689
690

  /*! Initialization de cholesky. */
691
  m_OptCholesky->SetA(LinearPartOfConstraints, m_NbOfConstraints);
Thomas Moulard's avatar
Thomas Moulard committed
692
693
694
695
  m_OptCholesky->SetToZero();

  struct timeval begin;

696
  gettimeofday(&begin, 0);
Thomas Moulard's avatar
Thomas Moulard committed
697

Thomas Moulard's avatar
Thomas Moulard committed
698
  // Hot Start
699
700
701
702
703
704
705
  if (m_HotStart) {
    for (unsigned int i = 0; i < m_PreviouslyActivatedConstraints.size(); i++) {
      int lindex =
          m_PreviouslyActivatedConstraints[i] - NumberOfRemovedConstraints;
      if (lindex >= 0) {
        m_ActivatedConstraints.push_back(lindex);
      }
Thomas Moulard's avatar
Thomas Moulard committed
706
    }
707
708
709
    if (m_ActivatedConstraints.size() > 0)
      m_OptCholesky->AddActiveConstraints(m_ActivatedConstraints);
  }
Thomas Moulard's avatar
Thomas Moulard committed
710

Thomas Moulard's avatar
Thomas Moulard committed
711
  m_PreviouslyActivatedConstraints.clear();
Thomas Moulard's avatar
Thomas Moulard committed
712

713
714
715
716
717
718
719
720
  double alpha = 0.0;
  m_ItNb = 0;
  while (ContinueAlgo) {
    ODEBUG("Iteration Number:" << m_ItNb);
    /* Step one : Compute descent direction. */
    for (unsigned int i = 0; i < 2 * m_CardV; i++)
      m_UnconstrainedDescentDirection[i] =
          -m_CstPartOfCostFunction[i] - m_Vk[i];
Thomas Moulard's avatar
Thomas Moulard committed
721

722
723
    /*! Step two: Compute the projected descent direction. */
    ComputeProjectedDescentDirection();
Thomas Moulard's avatar
Thomas Moulard committed
724

725
726
    /*! Step three : Compute alpha */
    alpha = ComputeAlpha(NewActivatedConstraints, SimilarConstraints);
Thomas Moulard's avatar
Thomas Moulard committed
727

728
729
730
731
732
733
734
735
736
737
    if (alpha >= 1.0) {
      alpha = 1.0;
      ContinueAlgo = false;
    }
    if (alpha < 0.0) {
      std::cerr << "Problem with alpha: should be positive" << std::endl;
      std::cerr << "The initial solution is incorrect: " << m_ItNb << " "
                << m_InternalTime << std::endl;
      exit(0);
    }
Thomas Moulard's avatar
Thomas Moulard committed
738

739
740
741
    /*! Compute new solution. */
    for (unsigned int i = 0; i < 2 * m_CardV; i++) {
      m_Vk[i] = m_Vk[i] + alpha * m_d[i];
Thomas Moulard's avatar
Thomas Moulard committed
742
743
    }

744
    if (m_DebugMode > 1) {
Thomas Moulard's avatar
Thomas Moulard committed
745

746
      ODEBUG("Alpha:" << alpha);
Thomas Moulard's avatar
Thomas Moulard committed
747

748
749
750
751
752
753
754
755
756
      ofstream aof;
      char Buffer[1024];
      sprintf(Buffer, "U_%.3f_%02d.dat", m_InternalTime, m_ItNb);
      aof.open(Buffer, ofstream::out);
      for (unsigned int i = 0; i < 2 * m_CardV; i++) {
        aof << m_Vk[i] << " ";
      }
      aof.close();
      ;
Thomas Moulard's avatar
Thomas Moulard committed
757

758
759
      sprintf(Buffer, "Zk_%02d.dat", m_ItNb);
      WriteCurrentZMPSolution(Buffer, XkYk);
Thomas Moulard's avatar
Thomas Moulard committed
760
761
    }

762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
    if (ContinueAlgo) {
      ODEBUG("Nb of activated constraints: " << NewActivatedConstraints.size());
      m_OptCholesky->AddActiveConstraints(NewActivatedConstraints);
      for (unsigned int i = 0; i < NewActivatedConstraints.size(); i++)
        m_ActivatedConstraints.push_back(NewActivatedConstraints[i]);

      NewActivatedConstraints.clear();

      if (m_DebugMode > 1) {
        ofstream aof;
        char Buffer[1024];
        sprintf(Buffer, "LE_%02d.dat", m_ItNb);
        aof.open(Buffer, ofstream::out);
        for (unsigned int i = 0; i < m_ActivatedConstraints.size(); i++) {
          for (unsigned int j = 0; j < m_ActivatedConstraints.size(); j++) {
            aof << m_L[i * m_NbMaxOfConstraints + j] << " ";
          }
          aof << endl;
780
        }
781
782
783
784
785
786
787
        aof.close();
        ODEBUG("m_L(0,0)= " << m_L[0]);
        sprintf(Buffer, "alpha_%02d.dat", m_ItNb);
        aof.open(Buffer, ofstream::out);
        aof << alpha << endl;
        aof.close();
      }
Thomas Moulard's avatar
Thomas Moulard committed
788
    }
Thomas Moulard's avatar
Thomas Moulard committed
789

790
791
792
793
794
    // If limited computation time stop the algorithm.
    if (m_LimitedComputationTime) {
      struct timeval current;
      gettimeofday(&current, 0);
      double r = (double)(current.tv_sec - begin.tv_sec) +
Olivier Stasse's avatar
Olivier Stasse committed
795
                 0.000001 * (double)(current.tv_usec - begin.tv_usec);
796
797
798
      if (r > m_AmountOfLimitedComputationTime) {
        ContinueAlgo = false;
      }
Thomas Moulard's avatar
Thomas Moulard committed
799
800
    }

801
802
    m_ItNb++;
  }
Thomas Moulard's avatar
Thomas Moulard committed
803

804
805
  for (unsigned int i = 0; i < 2 * m_CardV; i++)
    X[i] = m_Vk[i];
Thomas Moulard's avatar
Thomas Moulard committed
806

807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
  if (m_HotStart) {
    ODEBUG("AR (" << lTime << ") :");
    for (unsigned int i = 0; i < m_ActivatedConstraints.size(); i++) {
      ODEBUG("( " << m_ActivatedConstraints[i] << " , " << m_v2[i] << " ) ");
      if (m_v2[i] < 0.0) {
        m_PreviouslyActivatedConstraints.push_back(m_ActivatedConstraints[i]);
        ODEBUG3(m_ActivatedConstraints[i] << " ");
      }
    }
    ODEBUG((int)m_ActivatedConstraints.size() -
               (int)m_PreviouslyActivatedConstraints.size()
           << " " << m_PreviouslyActivatedConstraints.size());
    StoreCurrentZMPSolution(XkYk);
  }

  if (0) {
    ofstream aof;
    aof.open("ActivatedConstraints.dat", ofstream::app);
    for (unsigned int i = 0; i < 320; i++) {
      bool FoundConstraint = false;

      if (i < m_NbOfConstraints) {
        for (unsigned int j = 0; j < m_ActivatedConstraints.size(); j++)
          if (m_ActivatedConstraints[j] == i) {
            aof << "1 ";
            FoundConstraint = true;
            break;
          }
      }
      if (!FoundConstraint)
        aof << "0 ";
Thomas Moulard's avatar
Thomas Moulard committed
838
    }
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
    aof << endl;
    aof.close();
  }

  if ((isnan(X[0])) || (isnan(X[m_CardV])) || (isinf(X[0])) ||
      (isinf(X[m_CardV]))) {
    std::cerr << "Nan or inf value " << X[0] << " " << X[m_CardV]
              << " at iteration " << m_ItNb - 1 << endl;
    return -1;
  }

  if (m_DebugMode > 1) {
    ofstream aof;
    aof.open("NbIt.dat", ofstream::out);
    aof << m_ItNb;
    aof.close();

    aof.open("Pb.dat", ofstream::out);
    aof << " Number of iterations " << m_ItNb << endl;
    aof.close();

    aof.open("UFinal.dat", ofstream::out);
    for (unsigned int i = 0; i < 2 * m_CardV; i++) {
      aof << X[i] << " ";
    }
    aof << endl;
    aof.close();
Thomas Moulard's avatar
Thomas Moulard committed
866

867
868
869
870
871
    WriteCurrentZMPSolution("FinalSolution.dat", XkYk);
    aof.open("Pb.dat", ofstream::out);
    aof << " Number of iterations " << m_ItNb << endl;
    aof.close();
  }
Thomas Moulard's avatar
Thomas Moulard committed
872
873
874

  string Buffer("InfosPLDP");
  if (m_HotStart)
875
    Buffer += "HS";
Thomas Moulard's avatar
Thomas Moulard committed
876
  if (m_LimitedComputationTime)
877
878
    Buffer += "LT";
  Buffer += ".dat";
Thomas Moulard's avatar
Thomas Moulard committed
879

880
881
882
883
884
885
  ODEBUG6(m_ActivatedConstraints.size()
              << " " << NbOfConstraints << " "
              << m_ActivatedConstraints.size() -
                     m_PreviouslyActivatedConstraints.size()
              << " " << m_ItNb,
          (char *)Buffer.c_str());
Thomas Moulard's avatar
Thomas Moulard committed
886
887
888
889
890
891

  m_InternalTime += 0.02;
  return 0;
}

/* Store the ZMP solution for hotstart. */
892
void PLDPSolver::StoreCurrentZMPSolution(double *XkYk) {
Thomas Moulard's avatar
Thomas Moulard committed
893
894
895
  // The current solution is Vk,
  // but its graphical representation is better understood
  // when transformed in ZMP ref trajectory.
Thomas Moulard's avatar
Thomas Moulard committed
896

897
898
899
900
901
902
903
904
905
906
907
908
909
  for (unsigned int i = 0; i < m_CardV; i++) {
    m_PreviousZMPSolution[i] = 0.0;           // X axis
    m_PreviousZMPSolution[i + m_CardV] = 0.0; // Y axis
    for (unsigned int j = 0; j < m_CardV; j++) {
      m_PreviousZMPSolution[i] += m_Pu[j * m_CardV + i] * m_Vk[j];
      m_PreviousZMPSolution[i + m_CardV] +=
          m_Pu[j * m_CardV + i] * m_Vk[j + m_CardV];
    }
    for (unsigned int j = 0; j < 3; j++) {
      m_PreviousZMPSolution[i] += m_Px[i * 3 + j] * XkYk[j];
      //     lZMP[i] += m_Px[i*3+j] * XkYk[j+3];
      //     lZMP[i+m_CardV] += m_Px[i*3+j] * XkYk[j];
      m_PreviousZMPSolution[i + m_CardV] += m_Px[i * 3 + j] * XkYk[j + 3];
Thomas Moulard's avatar
Thomas Moulard committed
910
    }
911
912
    //      aof << lZMP[i] << " " << lZMP[i+m_CardV] << endl;
  }
Thomas Moulard's avatar
Thomas Moulard committed
913
914
915
916
917

  //  aof.close();
}

/* Write the current solution for debugging. */
918
void PLDPSolver::WriteCurrentZMPSolution(string filename, double *XkYk) {
Thomas Moulard's avatar
Thomas Moulard committed
919
920
921
  // The current solution is Vk,
  // but its graphical representation is better understood
  // when transformed in ZMP ref trajectory.
922
923
  int lZMP_size = 2 * m_CardV;
  double *lZMP = new double[lZMP_size];
Thomas Moulard's avatar
Thomas Moulard committed
924
925

  ofstream aof;
926
927
928
929
930
931
932
933
934
935
936
937
938
939
  aof.open(filename.c_str(), ofstream::out);

  for (unsigned int i = 0; i < m_CardV; i++) {
    lZMP[i] = 0.0;           // X axis
    lZMP[i + m_CardV] = 0.0; // Y axis
    for (unsigned int j = 0; j < m_CardV; j++) {
      lZMP[i] += m_Pu[j * m_CardV + i] * m_Vk[j];
      lZMP[i + m_CardV] += m_Pu[j * m_CardV + i] * m_Vk[j + m_CardV];
    }
    for (unsigned int j = 0; j < 3; j++) {
      lZMP[i] += m_Px[i * 3 + j] * XkYk[j];
      //     lZMP[i] += m_Px[i*3+j] * XkYk[j+3];
      //     lZMP[i+m_CardV] += m_Px[i*3+j] * XkYk[j];
      lZMP[i + m_CardV] += m_Px[i * 3 + j] * XkYk[j + 3];
Thomas Moulard's avatar
Thomas Moulard committed
940
    }
941
942
    aof << lZMP[i] << " " << lZMP[i + m_CardV] << endl;
  }
Thomas Moulard's avatar
Thomas Moulard committed
943
944
945
946

  aof.close();
  delete lZMP;
}