centroidal_dynamics.cpp 24.2 KB
Newer Older
1
2
3
4
5
/*
 * Copyright 2015, LAAS-CNRS
 * Author: Andrea Del Prete
 */

Steve Tonneau's avatar
Steve Tonneau committed
6
#include <centroidal-dynamics-lib/centroidal_dynamics.hh>
7
8
#include <centroidal-dynamics-lib/logger.hh>
#include <centroidal-dynamics-lib/stop-watch.hh>
9
10
#include <iostream>
#include <vector>
11
#include <ctime>
12
13

using namespace std;
14

Steve Tonneau's avatar
Steve Tonneau committed
15
namespace centroidal_dynamics
16
17
{

Steve Tonneau's avatar
Steve Tonneau committed
18
bool Equilibrium::m_is_cdd_initialized = false;
19

Steve Tonneau's avatar
Steve Tonneau committed
20
21
Equilibrium::Equilibrium(const string& name, const double mass, const unsigned int generatorsPerContact,
                                     const SolverLP solver_type, const bool useWarmStart,
22
                                     const unsigned int max_num_cdd_trials, const bool canonicalize_cdd_matrix)
t steve's avatar
t steve committed
23
24
25
    : m_mass(mass)
    , m_gravity(0.,0.,-9.81)
    , m_is_cdd_stable(true)
26
    , max_num_cdd_trials(max_num_cdd_trials)
27
    , canonicalize_cdd_matrix(canonicalize_cdd_matrix)
28
{
29
30
31
32
  if(!m_is_cdd_initialized)
  {
    init_cdd_library();
    m_is_cdd_initialized = true;
33
    //srand ( (unsigned int) (time(NULL)) );
34
35
  }

36
  m_name = name;
37
  m_solver_type = solver_type;
38
  m_solver = Solver_LP_abstract::getNewSolver(solver_type);
39
  m_solver->setUseWarmStart(useWarmStart);
40

41
  m_generatorsPerContact = generatorsPerContact;
Steve Tonneau's avatar
Steve Tonneau committed
42
43
44
45
46
47
  if(generatorsPerContact<3)
  {
    SEND_WARNING_MSG("Algorithm cannot work with less than 3 generators per contact!");
    m_generatorsPerContact = 3;
  }

t steve's avatar
t steve committed
48
49
  /*m_gravity.setZero();
  m_gravity(2) = -9.81;*/
50

51
52
53
  m_d.setZero();
  m_d.head<3>() = m_mass*m_gravity;
  m_D.setZero();
54
  m_D.block<3,3>(3,0) = crossMatrix(-m_mass*m_gravity);
55
56
}

Steve Tonneau's avatar
Steve Tonneau committed
57
bool Equilibrium::computeGenerators(Cref_matrixX3 contactPoints, Cref_matrixX3 contactNormals,
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
                                          double frictionCoefficient, const bool perturbate)
{
    long int c = contactPoints.rows();
    unsigned int &cg = m_generatorsPerContact;
    double theta, delta_theta=2*M_PI/cg;
    const value_type pi_4 = value_type(M_PI_4);
    // perturbation for libcdd
    const value_type epsilon = 2*1e-3;
    if(perturbate)
        frictionCoefficient = frictionCoefficient +(rand()/ value_type(RAND_MAX))*epsilon;
    // Tangent directions
    Vector3 T1, T2, N;
    // contact point
    Vector3 P;
    // Matrix mapping a 3d contact force to gravito-inertial wrench (6 X 3)
    Matrix63 A;
    A.topRows<3>() = -Matrix3::Identity();
    Matrix3X G(3, cg);
    for(long int i=0; i<c; i++)
    {
      // check that contact normals have norm 1
79
      if(fabs(contactNormals.row(i).norm()-1.0)>1e-4)
80
81
82
83
84
85
86
87
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
125
126
127
128
129
130
131
132
      {
        SEND_ERROR_MSG("Contact normals should have norm 1, this has norm %f"+toString(contactNormals.row(i).norm()));
        return false;
      }
      // compute tangent directions
      N = contactNormals.row(i);
      P = contactPoints.row(i);
      if(perturbate)
      {
          for(int k =0; k<3; ++k)
          {
              N(k) +=(rand()/ value_type(RAND_MAX))*epsilon;
              P(k) +=(rand()/ value_type(RAND_MAX))*epsilon;
          }
          N.normalize();
      }
      T1 = N.cross(Vector3::UnitY());
      if(T1.norm()<1e-5)
        T1 = N.cross(Vector3::UnitX());
      T2 = N.transpose().cross(T1);
      T1.normalize();
      T2.normalize();

      // compute matrix mapping contact forces to gravito-inertial wrench
      A.bottomRows<3>() = crossMatrix(-1.0*P);

      // compute generators
      theta = pi_4;
      for(int j=0; j<cg; j++)
      {
        G.col(j) = frictionCoefficient*sin(theta)*T1
                  + frictionCoefficient*cos(theta)*T2
                  + contactNormals.row(i).transpose();
        G.col(j).normalize();
  //      SEND_DEBUG_MSG("Contact "+toString(i)+" generator "+toString(j)+" = "+toString(G.col(j).transpose()));
        theta += delta_theta;
      }

      // project generators in 6d centroidal space
      m_G_centr.block(0,cg*i,6,cg) = A * G;
    }
    // Compute the coefficient to convert b0 to e_max
    Vector3 f0 = Vector3::Zero();
    for(int j=0; j<cg; j++)
      f0 += G.col(j); // sum of the contact generators
    // Compute the distance between the friction cone boundaries and
    // the sum of the contact generators, which is e_max when b0=1.
    // When b0!=1 we just multiply b0 times this value.
    // This value depends only on the number of generators and the friction coefficient
    m_b0_to_emax_coefficient = (f0.cross(G.col(0))).norm();
    return true;
}

Pierre Fernbach's avatar
Pierre Fernbach committed
133
134
135
136
137
138
139
void Equilibrium::setAlgorithm(EquilibriumAlgorithm algorithm){
    if(algorithm == EQUILIBRIUM_ALGORITHM_PP && m_G_centr.rows() > 0)
      SEND_DEBUG_MSG("Cannot set algorithm to PP after setting contacts, you should call again setNewContact with PP algorithm");
    else
      m_algorithm = algorithm;
}

140
141
142
143
144
145
146
147
148
149
bool Equilibrium::setNewContacts(const MatrixX3ColMajor& contactPoints, const MatrixX3ColMajor& contactNormals,
                                       const double frictionCoefficient, const EquilibriumAlgorithm alg)
{
    MatrixX3 _contactPoints  = contactPoints;
    MatrixX3 _contactNormals = contactNormals;
    return setNewContacts(_contactPoints,_contactNormals, frictionCoefficient, alg);
}

bool Equilibrium::setNewContacts(const MatrixX3& contactPoints, const MatrixX3& contactNormals,
                                 const double frictionCoefficient, const EquilibriumAlgorithm alg)
150
{
151
152
  assert(contactPoints.rows()==contactNormals.rows());

Steve Tonneau's avatar
Steve Tonneau committed
153
  if(alg==EQUILIBRIUM_ALGORITHM_IP)
154
155
156
157
  {
    SEND_ERROR_MSG("Algorithm IP not implemented yet");
    return false;
  }
Steve Tonneau's avatar
Steve Tonneau committed
158
  if(alg==EQUILIBRIUM_ALGORITHM_DIP)
159
160
161
162
163
164
165
  {
    SEND_ERROR_MSG("Algorithm DIP not implemented yet");
    return false;
  }

  m_algorithm = alg;

166
  // Lists of contact generators (3 X generatorsPerContact)
167
  m_G_centr.resize(6,contactPoints.rows()*m_generatorsPerContact);
168

169
  if (!computeGenerators(contactPoints,contactNormals,frictionCoefficient,false))
170
  {
171
    return false;
172
173
  }

Steve Tonneau's avatar
Steve Tonneau committed
174
  if(m_algorithm==EQUILIBRIUM_ALGORITHM_PP)
175
  {
176
177
178
179
180
181
    unsigned int attempts = max_num_cdd_trials;
    while(!computePolytopeProjection(m_G_centr) && attempts>0)
    {
      computeGenerators(contactPoints,contactNormals,frictionCoefficient,true);
      attempts--;
    }
182
183
    // resetting random because obviously libcdd always resets to 0
    srand(time(NULL));
184
185
    if(!m_is_cdd_stable)
    {
186
      return false;
187
    }
188
189
190
    m_HD = m_H * m_D;
    m_Hd = m_H * m_d;
  }
191

192
193
194
  return true;
}

195
static const Vector3 zero_acc = Vector3::Zero();
196

Steve Tonneau's avatar
Steve Tonneau committed
197
LP_status Equilibrium::computeEquilibriumRobustness(Cref_vector3 com, double &robustness)
198
{
199
200
201
202
203
204
205
206
207
    return computeEquilibriumRobustness(com, zero_acc, robustness);
}

LP_status Equilibrium::computeEquilibriumRobustness(Cref_vector3 com, Cref_vector3 acc, double &robustness)
{
  // Take the acceleration in account in D and d :
  m_D.block<3,3>(3,0) = crossMatrix(-m_mass * (m_gravity - acc));
  m_d.head<3>()= m_mass * (m_gravity - acc);

208
  const long m = m_G_centr.cols(); // number of gravito-inertial wrench generators
209
210
  if(m==0)
    return LP_STATUS_INFEASIBLE;
211

Steve Tonneau's avatar
Steve Tonneau committed
212
  if(m_algorithm==EQUILIBRIUM_ALGORITHM_LP)
213
  {
214
215
    /* Compute the robustness measure of the equilibrium of a specified CoM position
     * by solving the following LP:
216
217
218
219
220
221
222
223
224
225
          find          b, b0
          minimize      -b0
          subject to    D c + d <= G b    <= D c + d
                        0       <= b - b0 <= Inf
        where
          b         are the coefficient of the contact force generators (f = V b)
          b0        is the robustness measure
          c         is the CoM position
          G         is the matrix whose columns are the gravito-inertial wrench generators
    */
226
    VectorX b_b0(m+1);
227
228
    VectorX c = VectorX::Zero(m+1);
    c(m) = -1.0;
229
    VectorX lb = -VectorX::Ones(m+1)*1e5;
230
231
232
233
234
235
236
237
238
239
    VectorX ub = VectorX::Ones(m+1)*1e10;
    VectorX Alb = VectorX::Zero(6+m);
    VectorX Aub = VectorX::Ones(6+m)*1e100;
    MatrixXX A = MatrixXX::Zero(6+m, m+1);
    Alb.head<6>() = m_D * com + m_d;
    Aub.head<6>() = Alb.head<6>();
    A.topLeftCorner(6,m)      = m_G_centr;
    A.bottomLeftCorner(m,m)   = MatrixXX::Identity(m,m);
    A.bottomRightCorner(m,1)  = -VectorX::Ones(m);

240
241
    LP_status lpStatus = m_solver->solve(c, lb, ub, A, Alb, Aub, b_b0);
    if(lpStatus==LP_STATUS_OPTIMAL)
242
    {
243
      robustness = convert_b0_to_emax(-1.0*m_solver->getObjectiveValue());
244
      return lpStatus;
245
    }
246

247
248
    SEND_DEBUG_MSG("Primal LP problem could not be solved: "+toString(lpStatus));
    return lpStatus;
249
250
  }

Steve Tonneau's avatar
Steve Tonneau committed
251
  if(m_algorithm==EQUILIBRIUM_ALGORITHM_LP2)
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
  {
    /* Compute the robustness measure of the equilibrium of a specified CoM position
     * by solving the following LP:
            find          b, b0
            minimize      -b0
            subject to    D c + d <= G (b + 1*b0) <= D c + d
                          0       <= b            <= Inf
          where
            b         are the (delta) coefficient of the contact force generators (f = G (b+b0))
            b0        is the robustness measure
            c         is the CoM position
            G         is the matrix whose columns are the gravito-inertial wrench generators
      */
    VectorX b_b0(m+1);
    VectorX c = VectorX::Zero(m+1);
    c(m) = -1.0;
    VectorX lb = VectorX::Zero(m+1);
    lb(m) = -1e10;
    VectorX ub = VectorX::Ones(m+1)*1e10;
    MatrixXX A = MatrixXX::Zero(6, m+1);
    Vector6 Alb = m_D * com + m_d;
    Vector6 Aub = Alb;
    A.leftCols(m)  = m_G_centr;
    A.rightCols(1) = m_G_centr * VectorX::Ones(m);

    LP_status lpStatus_primal = m_solver->solve(c, lb, ub, A, Alb, Aub, b_b0);
278
    if(lpStatus_primal==LP_STATUS_OPTIMAL)
279
    {
280
      robustness = convert_b0_to_emax(-1.0*m_solver->getObjectiveValue());
281
      return lpStatus_primal;
282
    }
283

284
285
    SEND_DEBUG_MSG("Primal LP problem could not be solved: "+toString(lpStatus_primal));
    return lpStatus_primal;
286
287
  }

Steve Tonneau's avatar
Steve Tonneau committed
288
  if(m_algorithm==EQUILIBRIUM_ALGORITHM_DLP)
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
  {
    /*Compute the robustness measure of the equilibrium of a specified CoM position
      by solving the following dual LP:
        find          v
        minimize      (d+D*com)' v
        subject to    G' v >= 0
                      1' G' v = 1
      where
        -(d+D c)' v   is the robustness measure
        c             is the CoM position
        G             is the matrix whose columns are the gravito-inertial wrench generators
     */
    Vector6 v;
    Vector6 c = m_D*com + m_d;
    Vector6 lb = Vector6::Ones()*-1e100;
    Vector6 ub = Vector6::Ones()*1e100;
    VectorX Alb = VectorX::Zero(m+1);
    Alb(m) = 1.0;
    VectorX Aub = VectorX::Ones(m+1)*1e100;
    Aub(m) = 1.0;
    MatrixX6 A(m+1,6);
    A.topRows(m) = m_G_centr.transpose();
    A.bottomRows<1>() = (m_G_centr*VectorX::Ones(m)).transpose();

    LP_status lpStatus_dual = m_solver->solve(c, lb, ub, A, Alb, Aub, v);
    if(lpStatus_dual==LP_STATUS_OPTIMAL)
315
    {
316
      robustness = convert_b0_to_emax(m_solver->getObjectiveValue());
317
      return lpStatus_dual;
318
    }
319
    SEND_DEBUG_MSG("Dual LP problem for com position "+toString(com.transpose())+" could not be solved: "+toString(lpStatus_dual));
320
321
322
323
324
325
326

    // switch UNFEASIBLE and UNBOUNDED flags because we are solving dual problem
    if(lpStatus_dual==LP_STATUS_INFEASIBLE)
      lpStatus_dual = LP_STATUS_UNBOUNDED;
    else if(lpStatus_dual==LP_STATUS_UNBOUNDED)
      lpStatus_dual = LP_STATUS_INFEASIBLE;

327
    return lpStatus_dual;
328
  }
329

330
  SEND_ERROR_MSG("checkRobustEquilibrium is not implemented for the specified algorithm");
331
  return LP_STATUS_ERROR;
332
333
}

334

Steve Tonneau's avatar
Steve Tonneau committed
335
LP_status Equilibrium::checkRobustEquilibrium(Cref_vector3 com, bool &equilibrium, double e_max)
336
{
337
338
339
340
341
  if(m_G_centr.cols()==0)
  {
    equilibrium=false;
    return LP_STATUS_OPTIMAL;
  }
342
343
  if(e_max!=0.0)
  {
344
    SEND_ERROR_MSG("checkRobustEquilibrium with e_max!=0 not implemented yet");
345
    return LP_STATUS_ERROR;
346
  }
Steve Tonneau's avatar
Steve Tonneau committed
347
  if(m_algorithm!=EQUILIBRIUM_ALGORITHM_PP)
348
349
  {
    SEND_ERROR_MSG("checkRobustEquilibrium is only implemented for the PP algorithm");
350
    return LP_STATUS_ERROR;
351
352
353
354
355
  }

  VectorX res = m_HD * com + m_Hd;
  for(long i=0; i<res.size(); i++)
    if(res(i)>0.0)
356
357
    {
      equilibrium = false;
358
      return LP_STATUS_OPTIMAL;
359
360
361
    }

  equilibrium = true;
362
  return LP_STATUS_OPTIMAL;
363
364
}

Steve Tonneau's avatar
Steve Tonneau committed
365

Steve Tonneau's avatar
Steve Tonneau committed
366
LP_status Equilibrium::getPolytopeInequalities(MatrixXX& H, VectorX& h) const
Steve Tonneau's avatar
Steve Tonneau committed
367
{
Steve Tonneau's avatar
Steve Tonneau committed
368
    if(m_algorithm!=EQUILIBRIUM_ALGORITHM_PP)
Steve Tonneau's avatar
Steve Tonneau committed
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
    {
      SEND_ERROR_MSG("getPolytopeInequalities is only implemented for the PP algorithm");
      return LP_STATUS_ERROR;
    }
    if(!m_is_cdd_stable)
    {
      SEND_ERROR_MSG("numerical instability in cddlib");
      return LP_STATUS_ERROR;
    }
    if(m_G_centr.cols()==0)
    {
      return LP_STATUS_INFEASIBLE;
    }
    H = m_H;
    h = m_h;
    return LP_STATUS_OPTIMAL;
}

Steve Tonneau's avatar
Steve Tonneau committed
387
LP_status Equilibrium::findExtremumOverLine(Cref_vector3 a, Cref_vector3 a0, double e_max, Ref_vector3 com)
388
{
389
  const long m = m_G_centr.cols(); // number of gravito-inertial wrench generators
390
391
392
  if(m_G_centr.cols()==0)
    return LP_STATUS_INFEASIBLE;

393
  double b0 = convert_emax_to_b0(e_max);
394

Steve Tonneau's avatar
Steve Tonneau committed
395
  if(m_algorithm==EQUILIBRIUM_ALGORITHM_LP)
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
  {
    /* Compute the extremum CoM position over the line a*p + a0 that is in robust equilibrium
     * by solving the following LP:
          find          b, p
          minimize      -p
          subject to    D (a p + a0) + d <= G (b + b0) <= D (a p + a0) + d
                        0       <= b <= Inf
        where
          b0+b      are the coefficient of the contact force generators (f = G (b0+b))
          b0        is the robustness measure
          p         is the line parameter
          G         is the matrix whose columns are the gravito-inertial wrench generators
    */
    VectorX b_p(m+1);
    VectorX c = VectorX::Zero(m+1);
    c(m) = -1.0;
    VectorX lb = -VectorX::Zero(m+1);
    lb(m) = -1e5;
    VectorX ub = VectorX::Ones(m+1)*1e10;
415
    Vector6 Alb = m_D*a0 + m_d - m_G_centr*VectorX::Ones(m)*b0;
416
417
418
419
420
421
422
423
424
    Vector6 Aub = Alb;
    Matrix6X A = Matrix6X::Zero(6, m+1);
    A.leftCols(m)     = m_G_centr;
    A.rightCols(1)    = -m_D*a;

    LP_status lpStatus_primal = m_solver->solve(c, lb, ub, A, Alb, Aub, b_p);
    if(lpStatus_primal==LP_STATUS_OPTIMAL)
    {
      com = a0 + a*b_p(m);
425

426
//#define WRITE_LPS_TO_FILE
427
428
429
430
431
432
433
434
435
436
#ifdef WRITE_LPS_TO_FILE
      string date_time = getDateAndTimeAsString();
      string filename = "LP_findExtremumOverLine"+date_time+".dat";
      if(!m_solver->writeLpToFile(filename.c_str(), c, lb, ub, A, Alb, Aub))
        SEND_ERROR_MSG("Error while writing LP to file "+filename);
      filename = "LP_findExtremumOverLine"+date_time+"_solution.dat";
      if(!writeMatrixToFile(filename.c_str(), b_p))
        SEND_ERROR_MSG("Error while writing LP solution to file "+filename);
#endif

437
      return lpStatus_primal;
438
439
    }

440
441
442
443
    com = a0;
    SEND_DEBUG_MSG("Primal LP problem could not be solved suggesting that no equilibrium position with robustness "+
                     toString(e_max)+" exists over the line starting from "+toString(a0.transpose())+
                     " in direction "+toString(a.transpose())+", solver error code: "+toString(lpStatus_primal));
444
    return lpStatus_primal;
445
446
  }

Steve Tonneau's avatar
Steve Tonneau committed
447
  if(m_algorithm==EQUILIBRIUM_ALGORITHM_DLP)
448
449
450
451
452
453
454
455
456
457
458
  {
    /* Compute the extremum CoM position over the line a*x + a0 that is in robust equilibrium
     * by solving the following dual LP:
          find          v
          minimize      (D a0 + d -G b0)' v
          subject to    0  <= G' v    <= Inf
                        -1 <= a' D' v <= -1
        where
          G         is the matrix whose columns are the gravito-inertial wrench generators
    */
    Vector6 v;
459
    Vector6 c = m_D*a0 + m_d - m_G_centr*VectorX::Ones(m)*b0;
460
461
    Vector6 lb = Vector6::Ones()*-1e10;
    Vector6 ub = Vector6::Ones()*1e10;
462
463
    VectorX Alb = VectorX::Zero(m+1);
    Alb(m) = -1.0;
464
    VectorX Aub = VectorX::Ones(m+1)*1e10;
465
466
467
468
469
470
471
472
473
474
    Aub(m) = -1.0;
    MatrixX6 A(m+1,6);
    A.topRows(m) = m_G_centr.transpose();
    A.bottomRows<1>() = (m_D*a).transpose();

    LP_status lpStatus_dual = m_solver->solve(c, lb, ub, A, Alb, Aub, v);
    if(lpStatus_dual==LP_STATUS_OPTIMAL)
    {
      double p = m_solver->getObjectiveValue();
      com = a0 + a*p;
475

476
477
478
479
480
481
482
483
484
485
#ifdef WRITE_LPS_TO_FILE
      string date_time = getDateAndTimeAsString();
      string filename = "DLP_findExtremumOverLine"+date_time+".dat";
      if(!m_solver->writeLpToFile(filename.c_str(), c, lb, ub, A, Alb, Aub))
        SEND_ERROR_MSG("Error while writing LP to file "+filename);
      filename = "DLP_findExtremumOverLine"+date_time+"_solution.dat";
      if(!writeMatrixToFile(filename.c_str(), v))
        SEND_ERROR_MSG("Error while writing LP solution to file "+filename);
#endif

486
487
488
489
      // since QP oases cannot detect unboundedness we check here whether the objective value is a large negative value
      if(m_solver_type==SOLVER_LP_QPOASES && p<-1e7)
      {
        SEND_DEBUG_MSG("Dual LP problem with robustness "+toString(e_max)+
490
                       " over the line starting from "+toString(a0.transpose())+
491
                       " in direction "+toString(a.transpose())+" has large negative objective value: "+toString(p)+
492
                       " suggesting it is probably unbounded.");
493
        lpStatus_dual = LP_STATUS_UNBOUNDED;
494
      }
495

496
      return lpStatus_dual;
497
498
    }

499
    com = a0;
500
    SEND_DEBUG_MSG("Dual LP problem could not be solved suggesting that no equilibrium position with robustness "+
501
502
                     toString(e_max)+" exists over the line starting from "+toString(a0.transpose())+
                     " in direction "+toString(a.transpose())+", solver error code: "+toString(lpStatus_dual));
503
504
505
506
507
508
509

    // switch UNFEASIBLE and UNBOUNDED flags because we are solving dual problem
    if(lpStatus_dual==LP_STATUS_INFEASIBLE)
      lpStatus_dual = LP_STATUS_UNBOUNDED;
    else if(lpStatus_dual==LP_STATUS_UNBOUNDED)
      lpStatus_dual = LP_STATUS_INFEASIBLE;

510
    return lpStatus_dual;
511
512
  }

513
  SEND_ERROR_MSG("findExtremumOverLine is not implemented for the specified algorithm");
514
  return LP_STATUS_ERROR;
515
516
}

Steve Tonneau's avatar
Steve Tonneau committed
517
LP_status Equilibrium::findExtremumInDirection(Cref_vector3 direction, Ref_vector3 com, double e_max)
518
{
519
520
  if(m_G_centr.cols()==0)
    return LP_STATUS_INFEASIBLE;
521
  SEND_ERROR_MSG("findExtremumInDirection not implemented yet");
522
  return LP_STATUS_ERROR;
523
}
Steve Tonneau's avatar
Steve Tonneau committed
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
///
/// \brief Computes factorial of a number
///
int fact(const int n)
{
    assert(n>=0);
    int res = 1;
    for (int i=2 ; i <= n ; ++i)
       res *= i;
    return res;
}

///
/// \brief Computes a binomal coefficient
///
int choosenk(const int n, const int k)
{
    return fact(n) / (fact(k) * fact(n - k));
}
543

Steve Tonneau's avatar
ongoing    
Steve Tonneau committed
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
VectorX computeCombVector(const int order)
{
    VectorX res = VectorX::Zero(order);
    for (int i = 0; i< order; ++i)
        res[i] = i;
    return res;
}
#include <algorithm>
#include <set>
#include <iterator>

std::set<int> vec_to_set(Cref_vectorX A)
{
    std::set<int> res;
    for(int i = 0; i < A.size(); ++i)
        res.insert(A[i]);
    return res;
}

//elements of A not in B
std::set<int> setDiff(Cref_vectorX A, Cref_vectorX B)
{
    std::set<int> s1 = vec_to_set(A), s2 = vec_to_set(B);
    // Fill in s1 and s2 with values
    std::set<int> result;
    std::set_difference(s1.begin(), s1.end(), s2.begin(), s2.end(),
        std::inserter(result, result.end()));
}
Steve Tonneau's avatar
Steve Tonneau committed
572
bool Equilibrium::computePolytopeProjection(Cref_matrix6X v)
573
{
574

Steve Tonneau's avatar
Steve Tonneau committed
575
576
577
578
579
580
581
582
    // todo: for the moment using ad hoc upper bound = 500 N
    int n = (int)v.rows();
    int m = (int)v.cols();
    if (n>m)
    {
        SEND_ERROR_MSG("V has more lines that columns, this case is not handled!");
        return false;
    }
Steve Tonneau's avatar
ongoing    
Steve Tonneau committed
583
584
585
586
587
588
589
590
591
592
593
594
    MatrixXX I;
    VectorX comb = computeCombVector(m);
    nchoosek(comb,n-1,I);
    int nbcomb = I.rows();
    int sizemat = (int)(2*nchoosek(m,n-1));
    MatrixXX C = MatrixXX::Zero(sizemat,n);
    VectorX d = VectorX::Zero(sizemat);
    MatrixXX V = MatrixXX::Zero(n,m);
    for(int i = 0; i< nbcomb; ++i)
    {
        const Eigen::Ref<VectorX>& chosen_comb = I.row(i);
        for(int j = 0; j < chosen_comb.size(); ++j)
Steve Tonneau's avatar
ongoing    
Steve Tonneau committed
595
            V.col(j) = v.col((int)(chosen_comb[j]));
Steve Tonneau's avatar
ongoing    
Steve Tonneau committed
596
597
        Eigen::FullPivLU<MatrixXX> lu(V);
        MatrixXX c = lu.kernel();
Steve Tonneau's avatar
ongoing    
Steve Tonneau committed
598
599
        std::cout << "c " << c.cols() << std::endl;
        std::cout << "V " << V.cols() << std::endl;
Steve Tonneau's avatar
ongoing    
Steve Tonneau committed
600
601
602
603
604
605
606
607
608
        if(c.cols()==1) // The n-1 lines of V are lineraly independent
        {
            c.normalize();
            C.row(2*i-1) = c;
            C.row(2*i) = c;
            std::set<int> J = setDiff(comb,chosen_comb);
            std::set<int>::const_iterator setit = J.begin();
            MatrixXX VV = MatrixXX::Zero(n,J.size());
            for(int j = 0; j < chosen_comb.size(); ++j, ++setit)
Steve Tonneau's avatar
ongoing    
Steve Tonneau committed
609
                VV.col(j) = v.col(*setit);
Steve Tonneau's avatar
ongoing    
Steve Tonneau committed
610
611
612
613
614
            MatrixXX scalar=VV*c(0,0);
            std::cout << "scalar " << scalar << std::endl;
        }
    }

Steve Tonneau's avatar
Steve Tonneau committed
615
    //int I = I=nchoosek(1:m,n-1);*/
616
//  getProfiler().start("eigen_to_cdd");
Steve Tonneau's avatar
ongoing    
Steve Tonneau committed
617
 /* dd_MatrixPtr V = cone_span_eigen_to_cdd(v.transpose(),canonicalize_cdd_matrix);
618
619
//  getProfiler().stop("eigen_to_cdd");

620
  dd_ErrorType error = dd_NoError;
Steve Tonneau's avatar
Steve Tonneau committed
621
  m_is_cdd_stable = true;
622
623

//  getProfiler().start("dd_DDMatrix2Poly");
624
  dd_PolyhedraPtr H_= dd_DDMatrix2Poly(V, &error);
625
626
//  getProfiler().stop("dd_DDMatrix2Poly");

627
628
  if(error != dd_NoError)
  {
629
    SEND_ERROR_MSG("numerical instability in cddlib. ill formed polytope");
Steve Tonneau's avatar
Steve Tonneau committed
630
    m_is_cdd_stable = false;
631
632
633
    return false;
  }

634
//  getProfiler().start("cdd to eigen");
635
  dd_MatrixPtr b_A = dd_CopyInequalities(H_);
636
637
638
639
640
641
642
643
644
645
  if(canonicalize_cdd_matrix)
  {
    dd_ErrorType error = dd_NoError;
    dd_rowset redset,impl_linset;
    dd_rowindex newpos;
    dd_MatrixCanonicalize(&b_A, &impl_linset, &redset, &newpos, &error);
    set_free(redset);
    set_free(impl_linset);
    free(newpos);
  }
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
  // get equalities and add them as complementary inequality constraints
  std::vector<long> eq_rows;
  for(long elem=1;elem<=(long)(b_A->linset[0]);++elem)
  {
    if (set_member(elem,b_A->linset))
      eq_rows.push_back(elem);
  }
  int rowsize = (int)b_A->rowsize;
  m_H.resize(rowsize + eq_rows.size(), (int)b_A->colsize-1);
  m_h.resize(rowsize + eq_rows.size());
  for(int i=0; i < rowsize; ++i)
  {
    m_h(i) = (value_type)(*(b_A->matrix[i][0]));
    for(int j=1; j < b_A->colsize; ++j)
      m_H(i, j-1) = -(value_type)(*(b_A->matrix[i][j]));
  }
  int i = 0;
  for(std::vector<long int>::const_iterator cit = eq_rows.begin(); cit != eq_rows.end(); ++cit, ++i)
  {
    m_h(rowsize + i) = -m_h((int)(*cit));
    m_H(rowsize + i) = -m_H((int)(*cit));
  }
Steve Tonneau's avatar
ongoing    
Steve Tonneau committed
668
<<<<<<< HEAD
669
//  getProfiler().stop("cdd to eigen");
670
671
672
673
674
675
676
  if(m_h.rows() < n )
    {
        SEND_ERROR_MSG("numerical instability in cddlib. ill formed polytope");
        m_is_cdd_stable = false;
        return false;
    }

Steve Tonneau's avatar
ongoing    
Steve Tonneau committed
677
678
679
=======
//  getProfiler().stop("cdd to eigen");*/
>>>>>>> ongoing
680

681
682
  return true;
}
683

Steve Tonneau's avatar
Steve Tonneau committed
684
double Equilibrium::convert_b0_to_emax(double b0)
685
686
687
688
{
  return (b0*m_b0_to_emax_coefficient);
}

Steve Tonneau's avatar
Steve Tonneau committed
689
double Equilibrium::convert_emax_to_b0(double emax)
690
691
692
693
{
  return (emax/m_b0_to_emax_coefficient);
}

694

695

Pierre Fernbach's avatar
Pierre Fernbach committed
696
LP_status Equilibrium::findMaximumAcceleration(Cref_matrix63 H, Cref_vector6 h,Cref_vector3 v, double& alpha0){
697
698
699
700
  int m = (int)m_G_centr.cols(); // 4* number of contacts
  VectorX b_a0(m);
  VectorX c = VectorX::Zero(m);
  MatrixXX A = MatrixXX::Zero(6, m);
701
702
  A.topLeftCorner(6,m) = - m_G_centr;
  A.topRightCorner(6,1) = H * v;
703
704
705
  c(m-1) = -1.0;  // because we search max alpha0, and c = [ B alpha ]^t
  VectorX lb = VectorX::Zero(m);
  VectorX ub = VectorX::Ones(m)*1e10; // Inf
706
707
708
  VectorX Alb = -h;
  VectorX Aub = -h;

709
710
711
712
713
714
715
  int iter = 0;
  LP_status lpStatus;
  do{
    lpStatus = m_solver->solve(c, lb, ub, A, Alb, Aub, b_a0);
    iter ++;
  }while(lpStatus == LP_STATUS_ERROR && iter < 3);

716

717
  if(lpStatus==LP_STATUS_UNBOUNDED){
Pierre Fernbach's avatar
Pierre Fernbach committed
718
    //SEND_DEBUG_MSG("Primal LP problem is unbounded : "+toString(lpStatus));
719
720
721
    alpha0 = std::numeric_limits<double>::infinity();
    return lpStatus;
  }
722
723
724
725
726
727
  if(lpStatus==LP_STATUS_OPTIMAL)
  {
    alpha0 = -1.0 * m_solver->getObjectiveValue();
    return lpStatus;
  }
  alpha0 = 0.0;
Pierre Fernbach's avatar
Pierre Fernbach committed
728
  //SEND_DEBUG_MSG("Primal LP problem could not be solved: "+toString(lpStatus));
729
730
731
732
  return lpStatus;

}

733

Pierre Fernbach's avatar
Pierre Fernbach committed
734
bool Equilibrium::checkAdmissibleAcceleration(Cref_matrix63 H, Cref_vector6 h, Cref_vector3 a ){
735
  int m = (int)m_G_centr.cols(); // number of contact * generator per contacts
736
737
738
739
740
741
  VectorX b(m);
  VectorX c = VectorX::Zero(m);
  VectorX lb = VectorX::Zero(m);
  VectorX ub = VectorX::Ones(m)*1e10; // Inf
  VectorX Alb = H*a + h;
  VectorX Aub = H*a + h;
742
743
744
  int iter = 0;
  LP_status lpStatus;
  do{
745
    lpStatus = m_solver->solve(c, lb, ub, m_G_centr, Alb, Aub, b);
746
    iter ++;
747
  }while(lpStatus == LP_STATUS_ERROR && iter < 3);
748

749
  if(lpStatus==LP_STATUS_OPTIMAL || lpStatus==LP_STATUS_UNBOUNDED)
750
751
752
753
  {
    return true;
  }
  else{
754
    //SEND_DEBUG_MSG("Primal LP problem could not be solved: "+toString(lpStatus));
755
756
757
    return false;
  }
}
758
759


Steve Tonneau's avatar
Steve Tonneau committed
760
} // end namespace centroidal_dynamics