centroidal_dynamics.cpp 21.8 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
524
}

Steve Tonneau's avatar
Steve Tonneau committed
525
bool Equilibrium::computePolytopeProjection(Cref_matrix6X v)
526
{
527
528
529
    int n = (int)(v.rows());
    int m = (int)(v.cols());

530
//  getProfiler().start("eigen_to_cdd");
531
  dd_MatrixPtr V = cone_span_eigen_to_cdd(v.transpose(),canonicalize_cdd_matrix);
532
533
//  getProfiler().stop("eigen_to_cdd");

534
  dd_ErrorType error = dd_NoError;
Steve Tonneau's avatar
Steve Tonneau committed
535
  m_is_cdd_stable = true;
536
537

//  getProfiler().start("dd_DDMatrix2Poly");
538
  dd_PolyhedraPtr H_= dd_DDMatrix2Poly(V, &error);
539
540
//  getProfiler().stop("dd_DDMatrix2Poly");

541
542
  if(error != dd_NoError)
  {
543
    SEND_ERROR_MSG("numerical instability in cddlib. ill formed polytope");
Steve Tonneau's avatar
Steve Tonneau committed
544
    m_is_cdd_stable = false;
545
546
547
    return false;
  }

548
//  getProfiler().start("cdd to eigen");
549
  dd_MatrixPtr b_A = dd_CopyInequalities(H_);
550
551
552
553
554
555
556
557
558
559
  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);
  }
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
  // 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));
  }
582
//  getProfiler().stop("cdd to eigen");
583

584
585
586
587
588
589
590
591
592
  std::cout<<" inequalities : m = "<<m<<std::endl;
  if(m_h.rows() < n )
    {
        SEND_ERROR_MSG("numerical instability in cddlib. ill formed polytope");
        m_is_cdd_stable = false;
        return false;
    }


593
594
  return true;
}
595

Steve Tonneau's avatar
Steve Tonneau committed
596
double Equilibrium::convert_b0_to_emax(double b0)
597
598
599
600
{
  return (b0*m_b0_to_emax_coefficient);
}

Steve Tonneau's avatar
Steve Tonneau committed
601
double Equilibrium::convert_emax_to_b0(double emax)
602
603
604
605
{
  return (emax/m_b0_to_emax_coefficient);
}

606

607

Pierre Fernbach's avatar
Pierre Fernbach committed
608
LP_status Equilibrium::findMaximumAcceleration(Cref_matrix63 H, Cref_vector6 h,Cref_vector3 v, double& alpha0){
609
610
611
612
  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);
613
614
  A.topLeftCorner(6,m) = - m_G_centr;
  A.topRightCorner(6,1) = H * v;
615
616
617
  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
618
619
620
  VectorX Alb = -h;
  VectorX Aub = -h;

621
622
623
624
625
626
627
  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);

628

629
  if(lpStatus==LP_STATUS_UNBOUNDED){
Pierre Fernbach's avatar
Pierre Fernbach committed
630
    //SEND_DEBUG_MSG("Primal LP problem is unbounded : "+toString(lpStatus));
631
632
633
    alpha0 = std::numeric_limits<double>::infinity();
    return lpStatus;
  }
634
635
636
637
638
639
  if(lpStatus==LP_STATUS_OPTIMAL)
  {
    alpha0 = -1.0 * m_solver->getObjectiveValue();
    return lpStatus;
  }
  alpha0 = 0.0;
Pierre Fernbach's avatar
Pierre Fernbach committed
640
  //SEND_DEBUG_MSG("Primal LP problem could not be solved: "+toString(lpStatus));
641
642
643
644
  return lpStatus;

}

645

Pierre Fernbach's avatar
Pierre Fernbach committed
646
bool Equilibrium::checkAdmissibleAcceleration(Cref_matrix63 H, Cref_vector6 h, Cref_vector3 a ){
647
  int m = (int)m_G_centr.cols(); // number of contact * generator per contacts
648
649
650
651
652
653
  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;
654
655
656
  int iter = 0;
  LP_status lpStatus;
  do{
657
    lpStatus = m_solver->solve(c, lb, ub, m_G_centr, Alb, Aub, b);
658
    iter ++;
659
  }while(lpStatus == LP_STATUS_ERROR && iter < 3);
660

661
  if(lpStatus==LP_STATUS_OPTIMAL || lpStatus==LP_STATUS_UNBOUNDED)
662
663
664
665
  {
    return true;
  }
  else{
666
    //SEND_DEBUG_MSG("Primal LP problem could not be solved: "+toString(lpStatus));
667
668
669
    return false;
  }
}
670
671


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