StepOverPlanner.cpp 56.3 KB
Newer Older
Thomas Moulard's avatar
Thomas Moulard committed
1
/*
2
 * Copyright 2005, 2006, 2007, 2008, 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
 *
 * Bjorn Verrelst
 * Francois Keith
 * 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/>.
 *
23
 *  Research carried out within the scope of the
Thomas Moulard's avatar
Thomas Moulard committed
24
25
26
 *  Joint Japanese-French Robotics Laboratory (JRL)
 */
/** This object generate all the values for the foot trajectories,
27
28
    and the desired ZMP based on a sequence of steps for a robot
    to step over obstacles.
Thomas Moulard's avatar
Thomas Moulard committed
29
30
31
32
*/

#include <fstream>

33
#include "Debug.hh"
Thomas Moulard's avatar
Thomas Moulard committed
34

35
#include <MotionGeneration/StepOverPlanner.hh>
Thomas Moulard's avatar
Thomas Moulard committed
36

37
using namespace ::PatternGeneratorJRL;
Thomas Moulard's avatar
Thomas Moulard committed
38
39

StepOverPlanner::StepOverPlanner(ObstaclePar &ObstacleParameters,
40
                                 PinocchioRobot *aPR) {
Thomas Moulard's avatar
Thomas Moulard committed
41

42
  m_PR = aPR;
Thomas Moulard's avatar
Thomas Moulard committed
43
  // Get information specific to the humanoid.
Olivier Stasse's avatar
Olivier Stasse committed
44
  double lWidth;
Olivier Stasse's avatar
Olivier Stasse committed
45
  Eigen::Vector3d AnklePosition;
Thomas Moulard's avatar
Thomas Moulard committed
46

47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
  if (m_PR != 0) {
    PRFoot *leftFoot = m_PR->leftFoot();
    lWidth = leftFoot->soleWidth;
    AnklePosition = leftFoot->anklePosition;
    m_AnkleSoilDistance = AnklePosition[2];
    m_tipToAnkle = lWidth - AnklePosition[0];
    m_heelToAnkle = m_AnkleSoilDistance;

  } else {
    lWidth = 0.2;
    cerr << "WARNING: no object with humanoid specificities properly defined."
         << endl;
    m_AnkleSoilDistance = 0.1;
    m_tipToAnkle = 0.1;
    m_heelToAnkle = 0.1;
  }
Thomas Moulard's avatar
Thomas Moulard committed
63

64
  m_CollDet = 0;
Thomas Moulard's avatar
Thomas Moulard committed
65
66
67
68

  SetObstacleInformation(ObstacleParameters);

  m_soleToAnkle = m_AnkleSoilDistance;
69

Thomas Moulard's avatar
Thomas Moulard committed
70
71
  m_heelDistAfter = 0.0;
  m_tipDistBefore = 0.0;
72

Thomas Moulard's avatar
Thomas Moulard committed
73
74
75
  m_nominalStepLenght = 0.2;
  m_nominalStepWidth = 0.19;

76
  // this angle is used to limit the position during feasibility in
Olivier Stasse's avatar
Olivier Stasse committed
77
  // double support over the obstacle
78
  m_KneeAngleBound = 15.0 * M_PI / 180.0;
79
80

  m_Tsingle = 0.78;
Thomas Moulard's avatar
Thomas Moulard committed
81
82
83
84
85
  m_Tdble = 0.02;

  m_TsingleStepOver = 1.5;
  m_TdbleStepOver = 0.04;

86
87
  m_TsingleStepOverBeforeAfter = m_TsingleStepOver * 1.0;
  m_TdbleStepOverBeforeAfter = m_TdbleStepOver * 1.0;
88

Thomas Moulard's avatar
Thomas Moulard committed
89
  m_WaistRotationStepOver = 25.0;
90

Thomas Moulard's avatar
Thomas Moulard committed
91
92
  m_PolynomeStepOverHipStep2 = new StepOverPolynomeHip4();

93
94
  m_PolynomeStepOverHipRotation = new StepOverPolynomeHip4();

95
96
97
98
99
  m_ClampedCubicSplineStepOverFootX = new StepOverClampedCubicSpline;
  m_ClampedCubicSplineStepOverFootY = new StepOverClampedCubicSpline;
  m_ClampedCubicSplineStepOverFootZ = new StepOverClampedCubicSpline;
  m_ClampedCubicSplineStepOverFootOmega = new StepOverClampedCubicSpline;
  m_ClampedCubicSplineStepOverFootOmegaImpact = new StepOverClampedCubicSpline;
100

Thomas Moulard's avatar
Thomas Moulard committed
101
102
  m_PC = 0;
  m_ZMPDiscr = 0;
103

Thomas Moulard's avatar
Thomas Moulard committed
104
105
106
107
108
  // Displacement between the hip and RLINK2
  // WARNING : Hardcoded values !
  m_Dt(0) = 0.0;
  m_Dt(1) = 0.05;
  m_Dt(2) = 0.0;
109

Thomas Moulard's avatar
Thomas Moulard committed
110
111
112
  // Displacement between the COM and the waist
  // WARNING : Hardcoded values !
  m_DiffBetweenComAndWaist = -0.15;
113

Thomas Moulard's avatar
Thomas Moulard committed
114
115
116
  m_StaticToTheLeftHip(0) = 0.0;
  m_StaticToTheLeftHip(1) = 0.1;
  m_StaticToTheLeftHip(2) = m_DiffBetweenComAndWaist;
117

Thomas Moulard's avatar
Thomas Moulard committed
118
119
120
121
  m_StaticToTheRightHip(0) = 0.0;
  m_StaticToTheRightHip(1) = -0.1;
  m_StaticToTheRightHip(2) = m_DiffBetweenComAndWaist;

Olivier Stasse's avatar
Olivier Stasse committed
122
123
  // defining the points on the shank to set the boundary lines
  // of the leg layout
124
  // for the values of the coordinates see paper guan san IROS 2004
Thomas Moulard's avatar
Thomas Moulard committed
125
126
127
  // 'feasibility of humanoid robots stepping over obstacles'

  double RadiusKnee;
128
  double Angle1, Angle2;
129

130
131
132
  RadiusKnee = 0.118;
  Angle1 = 60.0 / 180.0 * M_PI;
  Angle2 = 30.0 / 180.0 * M_PI;
133

134
  m_LegLayoutPoint.resize(3, 7);
135

136
137
138
139
  // point1
  m_LegLayoutPoint(0, 0) = RadiusKnee * cos(Angle1);
  m_LegLayoutPoint(1, 0) = 0.0;
  m_LegLayoutPoint(2, 0) = RadiusKnee * sin(Angle1);
Thomas Moulard's avatar
Thomas Moulard committed
140

141
142
143
144
  // point2
  m_LegLayoutPoint(0, 1) = RadiusKnee * cos(Angle2);
  m_LegLayoutPoint(1, 1) = 0.0;
  m_LegLayoutPoint(2, 1) = RadiusKnee * sin(Angle2);
Thomas Moulard's avatar
Thomas Moulard committed
145

146
147
148
149
  // point3
  m_LegLayoutPoint(0, 2) = RadiusKnee;
  m_LegLayoutPoint(1, 2) = 0.0;
  m_LegLayoutPoint(2, 2) = 0.0;
150

151
152
153
154
  // point4
  m_LegLayoutPoint(0, 3) = 0.0772;
  m_LegLayoutPoint(1, 3) = 0.0;
  m_LegLayoutPoint(2, 3) = -0.2253;
Thomas Moulard's avatar
Thomas Moulard committed
155

156
157
158
159
  // point5
  m_LegLayoutPoint(0, 4) = -0.0772;
  m_LegLayoutPoint(1, 4) = 0.0;
  m_LegLayoutPoint(2, 4) = -0.2492;
Thomas Moulard's avatar
Thomas Moulard committed
160

161
162
163
164
  // point6
  m_LegLayoutPoint(0, 5) = -0.0163;
  m_LegLayoutPoint(1, 5) = 0.0;
  m_LegLayoutPoint(2, 5) = -0.0939;
Thomas Moulard's avatar
Thomas Moulard committed
165

166
167
168
169
  // point7
  m_LegLayoutPoint(0, 6) = -0.0322;
  m_LegLayoutPoint(1, 6) = 0.0;
  m_LegLayoutPoint(2, 6) = -0.0183;
Thomas Moulard's avatar
Thomas Moulard committed
170
171

  m_TimeDistrFactor.resize(4);
172

173
174
175
176
  m_TimeDistrFactor[0] = 2.0;
  m_TimeDistrFactor[1] = 3.7;
  m_TimeDistrFactor[2] = 1.0;
  m_TimeDistrFactor[3] = 3.0;
177
178

  m_DeltaStepOverCOMHeightMax = 0.0;
Thomas Moulard's avatar
Thomas Moulard committed
179
180
}

181
StepOverPlanner::~StepOverPlanner() {
182

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

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

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

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

195
  if (m_ClampedCubicSplineStepOverFootZ != 0)
Thomas Moulard's avatar
Thomas Moulard committed
196
    delete m_ClampedCubicSplineStepOverFootZ;
197

198
  if (m_ClampedCubicSplineStepOverFootOmega != 0)
Thomas Moulard's avatar
Thomas Moulard committed
199
200
    delete m_ClampedCubicSplineStepOverFootOmega;

201
  if (m_ClampedCubicSplineStepOverFootOmegaImpact != 0)
Thomas Moulard's avatar
Thomas Moulard committed
202
203
    delete m_ClampedCubicSplineStepOverFootOmegaImpact;

204
  if (m_CollDet != 0)
Thomas Moulard's avatar
Thomas Moulard committed
205
206
207
    delete m_CollDet;
}

208
209
void StepOverPlanner::CalculateFootHolds(
    deque<RelativeFootPosition> &aFootHolds) {
210

Thomas Moulard's avatar
Thomas Moulard committed
211
212
  m_FootHolds.clear();

213
214
  m_Tsingle = m_ZMPDiscr->GetTSingleSupport();
  m_Tdble = m_ZMPDiscr->GetTDoubleSupport();
Thomas Moulard's avatar
Thomas Moulard committed
215
216

  /// Returns the double support time.
217
218
  float GetTDoubleSupport();

Olivier Stasse's avatar
Olivier Stasse committed
219
  DoubleSupportFeasibility();
220
  // perform this function to set m_StepOverStepLenght and  m_StepOverHipHeight;
Thomas Moulard's avatar
Thomas Moulard committed
221
222
223

  double ankleDistToObstacle;

224
225
  ankleDistToObstacle = m_StepOverStepLenght - m_heelToAnkle - m_heelDistAfter -
                        m_ObstacleParameters.d;
Thomas Moulard's avatar
Thomas Moulard committed
226

227
  double footDistLeftToMove;
228

229
  footDistLeftToMove = m_ObstacleParameters.x - ankleDistToObstacle;
Thomas Moulard's avatar
Thomas Moulard committed
230
231

  double numberOfSteps;
232

233
  numberOfSteps = floor(footDistLeftToMove / m_nominalStepLenght);
234

Thomas Moulard's avatar
Thomas Moulard committed
235
236
  double walkStepLenght;

237
238
239
  walkStepLenght = m_nominalStepLenght +
                   (footDistLeftToMove - m_nominalStepLenght * numberOfSteps) /
                       numberOfSteps;
240

241
242
243
244
  cout << "Obstacle height with safety boundary:" << m_ObstacleParameters.h
       << endl;
  cout << "Obstacle thickness with safety boundary:" << m_ObstacleParameters.d
       << endl;
Thomas Moulard's avatar
Thomas Moulard committed
245
246
247
  cout << "Distance to Obstacle:" << m_ObstacleParameters.x << endl;
  cout << "StepOver steplenght:" << m_StepOverStepLenght << endl;
  cout << "StepOver COMHeight:" << m_StepOverHipHeight << endl;
248
249
  cout << "Ankle distance in front of the obstacle:" << ankleDistToObstacle
       << endl;
Thomas Moulard's avatar
Thomas Moulard committed
250
251
252
253
  cout << "number of Steps before the obstacle:" << numberOfSteps << endl;
  cout << "Steplenght during walking:" << walkStepLenght << endl;

  RelativeFootPosition tempPos;
254

255
256
257
258
259
  tempPos.sx = 0.0;
  tempPos.sy = -m_nominalStepWidth / 2.0;
  tempPos.theta = 0.0;
  tempPos.SStime = 0.0;
  tempPos.DStime = m_Tdble / 2.0;
260

261
  tempPos.stepType = 1;
Thomas Moulard's avatar
Thomas Moulard committed
262
263
264

  m_FootHolds.push_back(tempPos);

265
266
267
268
269
270
271
272
273
274
  for (int i = 0; i < numberOfSteps - 1; i++) {
    tempPos.sx = walkStepLenght;
    tempPos.sy = (-1.0) * (tempPos.sy) / std::fabs((double)tempPos.sy) *
                 m_nominalStepWidth;
    tempPos.theta = 0.0;
    tempPos.SStime = m_Tsingle;
    tempPos.DStime = m_Tdble;
    tempPos.stepType = 1;
    m_FootHolds.push_back(tempPos);
  };
Thomas Moulard's avatar
Thomas Moulard committed
275
276
277

  // one step before stepover obstacle

278
279
280
281
282
283
284
  tempPos.sx = walkStepLenght;
  tempPos.sy = (-1.0) * (tempPos.sy) / std::fabs((double)tempPos.sy) *
               m_nominalStepWidth;
  tempPos.theta = 0.0;
  tempPos.SStime = m_TsingleStepOverBeforeAfter;
  tempPos.DStime = m_TdbleStepOverBeforeAfter;
  tempPos.stepType = 2;
Thomas Moulard's avatar
Thomas Moulard committed
285
  m_FootHolds.push_back(tempPos);
286

Thomas Moulard's avatar
Thomas Moulard committed
287
288
  // first leg over the obsacle

289
290
291
  tempPos.sx = m_StepOverStepLenght;
  tempPos.sy = (-1.0) * (tempPos.sy) / std::fabs((double)tempPos.sy) *
               m_nominalStepWidth;
Thomas Moulard's avatar
Thomas Moulard committed
292
  //*cos(m_WaistRotationStepOver*M_PI/180.0);
293
294
295
296
  tempPos.theta = 0.0;
  tempPos.SStime = m_TsingleStepOver;
  tempPos.DStime = m_Tdble;
  tempPos.stepType = 3;
Thomas Moulard's avatar
Thomas Moulard committed
297
298
299
300
  m_FootHolds.push_back(tempPos);

  // second leg over the obsacle

301
302
303
  tempPos.sx = m_nominalStepLenght;
  tempPos.sy = (-1.0) * (tempPos.sy) / std::fabs((double)tempPos.sy) *
               m_nominalStepWidth;
Thomas Moulard's avatar
Thomas Moulard committed
304
  //*cos(m_WaistRotationStepOver*M_PI/180.0);
305
306
307
308
  tempPos.theta = 0.0;
  tempPos.SStime = m_TsingleStepOver;
  tempPos.DStime = m_TdbleStepOver;
  tempPos.stepType = 4;
Thomas Moulard's avatar
Thomas Moulard committed
309
  m_FootHolds.push_back(tempPos);
310

311
312
313
314
315
316
317
318
  // one step after the obstacle stepping over
  tempPos.sx = m_nominalStepLenght;
  tempPos.sy = (-1.0) * (tempPos.sy) / std::fabs((double)tempPos.sy) *
               m_nominalStepWidth;
  tempPos.theta = 0.0;
  tempPos.SStime = m_TsingleStepOverBeforeAfter;
  tempPos.DStime = m_TdbleStepOverBeforeAfter;
  tempPos.stepType = 5;
Thomas Moulard's avatar
Thomas Moulard committed
319
320
  m_FootHolds.push_back(tempPos);

321
322
323
324
325
326
327
  // one extra regular step
  tempPos.sx = m_nominalStepLenght;
  tempPos.sy = (-1.0) * (tempPos.sy) / std::fabs((double)tempPos.sy) *
               m_nominalStepWidth;
  tempPos.theta = 0.0;
  tempPos.SStime = m_Tsingle;
  tempPos.DStime = m_Tdble;
328

329
  tempPos.stepType = 1;
Thomas Moulard's avatar
Thomas Moulard committed
330
  m_FootHolds.push_back(tempPos);
331

332
333
334
335
336
337
338
339
  // last step
  tempPos.sx = 0;
  tempPos.sy = (-1.0) * (tempPos.sy) / std::fabs((double)tempPos.sy) *
               m_nominalStepWidth;
  tempPos.theta = 0.0;
  tempPos.SStime = m_Tsingle;
  tempPos.DStime = m_Tdble;
  tempPos.stepType = 1;
340
341
  m_FootHolds.push_back(tempPos);

342
  aFootHolds = m_FootHolds;
Thomas Moulard's avatar
Thomas Moulard committed
343
344
}

345
void StepOverPlanner::DoubleSupportFeasibility() {
Thomas Moulard's avatar
Thomas Moulard committed
346
  double StepOverStepWidth;
347
348
  double StepOverStepLenght, StepOverStepLenghtMin, StepOverStepLenghtMax;
  double StepOverCOMHeight, StepOverCOMHeightMin, StepOverCOMHeightMax;
Olivier Stasse's avatar
Olivier Stasse committed
349
  double OrientationFeetToObstacle = 0.0, OmegaAngleFeet = 0.0;
350
  // this is the factor determining
Olivier Stasse's avatar
Olivier Stasse committed
351
  // aproximately the COM position due to preview control during double support
352
  double DoubleSupportCOMPosFactor;
Thomas Moulard's avatar
Thomas Moulard committed
353
354
355
356

  int EvaluationNumber = 10;
  double IncrementStepLenght, IncrementCOMHeight;

Olivier Stasse's avatar
Olivier Stasse committed
357
358
  Eigen::Matrix3d Body_R;
  Eigen::Vector3d Body_P;
359
  Eigen::Matrix3d Foot_R;
Olivier Stasse's avatar
Olivier Stasse committed
360
  Eigen::Vector3d Foot_P;
361
  double c, s, co, so;
Olivier Stasse's avatar
Olivier Stasse committed
362
  Eigen::Vector3d ToTheHip;
363
364
  Eigen::Matrix<double, 6, 1> LeftLegAngles;
  Eigen::Matrix<double, 6, 1> RightLegAngles;
Olivier Stasse's avatar
Olivier Stasse committed
365
366
  LeftLegAngles.Zero();
  RightLegAngles.Zero();
Olivier Stasse's avatar
Olivier Stasse committed
367
368
369
  Eigen::Vector3d AnkleBeforeObst;
  Eigen::Vector3d AnkleAfterObst;
  Eigen::Vector3d TempCOMState;
370
371
  Eigen::Matrix<double, 3, 3> Temp;
  ;
372

Thomas Moulard's avatar
Thomas Moulard committed
373
374
  COMState aCOMState;

Olivier Stasse's avatar
Olivier Stasse committed
375
376
377
378
  Eigen::Vector3d PointOnLeg;
  Eigen::Vector3d AbsCoord;
  Eigen::Vector3d AbsCoord1;
  Eigen::Vector3d AbsCoord2;
379
380
  Eigen::Matrix<double, 6, 1> LegAngles;
  ;
Olivier Stasse's avatar
Olivier Stasse committed
381
382
383
384
385
  Eigen::Matrix3d WaistRot;
  Eigen::Vector3d WaistPos;
  Eigen::Vector3d ObstFrameCoord;
  Eigen::Vector3d ObstFrameCoord1;
  Eigen::Vector3d ObstFrameCoord2;
386

Thomas Moulard's avatar
Thomas Moulard committed
387
  bool CollisionStatus, FinalCollisionStatus;
388

Olivier Stasse's avatar
Olivier Stasse committed
389
  StepOverStepLenghtMin = m_ObstacleParameters.d + m_heelToAnkle +
390
                          m_tipToAnkle + m_heelDistAfter + m_tipDistBefore;
Thomas Moulard's avatar
Thomas Moulard committed
391
392
  StepOverStepLenghtMax = 0.6;

393
394
395
  // 0.4 - m_DiffBetweenComAndWaist + m_soleToAnkle;0.6
  // *cos(90.0*M_PI/180.0/2.0)
  StepOverCOMHeightMin = 0.4 - m_DiffBetweenComAndWaist + m_soleToAnkle;
Thomas Moulard's avatar
Thomas Moulard committed
396

397
  // m_NominalCOMStepHeight;//0.6 * cos(m_KneeAngleBound/2.0) -
Olivier Stasse's avatar
Olivier Stasse committed
398
  // m_DiffBetweenComAndWaist + m_soleToAnkle;
399
  StepOverCOMHeightMax = 0.75 - m_DeltaStepOverCOMHeightMax;
Thomas Moulard's avatar
Thomas Moulard committed
400

401
402
403
404
  IncrementStepLenght = double((StepOverStepLenghtMax - StepOverStepLenghtMin) /
                               ((EvaluationNumber)));
  IncrementCOMHeight = double((StepOverCOMHeightMax - StepOverCOMHeightMin) /
                              ((EvaluationNumber)));
405

406
  /*! this angle can be used to extend the steplength
Olivier Stasse's avatar
Olivier Stasse committed
407
    during stepover but currently it is set to 0 convinience*/
408

409
  /*! this parameter should be evaluated and checked and in the end
Olivier Stasse's avatar
Olivier Stasse committed
410
    to be retreieved
Thomas Moulard's avatar
Thomas Moulard committed
411
412
    from a table containing these values for different step situations ...
    for which a first round of preview control has been performed */
413
  DoubleSupportCOMPosFactor = 0.50;
Thomas Moulard's avatar
Thomas Moulard committed
414
415
416
  CollisionStatus = 1;
  FinalCollisionStatus = 1;

417
  /*! we suppose that both feet have the same orentation with respect
Olivier Stasse's avatar
Olivier Stasse committed
418
    to the obstacle */
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
  for (int i = 0; i < EvaluationNumber + 1; i++) {
    for (int j = 0; j < EvaluationNumber + 1; j++) {

      StepOverStepLenght = StepOverStepLenghtMin + i * IncrementStepLenght;
      StepOverCOMHeight =
          StepOverCOMHeightMax - (double(j * IncrementCOMHeight));
      StepOverStepWidth = m_nominalStepWidth;

      // cout << "StepOverStepcd ../Lenght: " << StepOverStepLenght <<
      //" StepOverStepWidth: " << StepOverStepWidth
      // << " StepOverCOMHeight: " << StepOverCOMHeight << endl;

      // coordinates ankles in obstacle frame
      // assuming the left foot is in front of the obstacle
      // and that in the Y direction of the obstacle the feet
      // are symmetrical with respect to the obstacle origin

      AnkleBeforeObst(0) = -(StepOverStepLenght - m_heelToAnkle -
                             m_heelDistAfter - m_ObstacleParameters.d);
      AnkleBeforeObst(1) = StepOverStepWidth / 2.0;
      AnkleBeforeObst(2) = m_soleToAnkle;

      AnkleAfterObst(0) = AnkleBeforeObst(0) + StepOverStepLenght;
      AnkleAfterObst(1) = -StepOverStepWidth / 2.0;
      AnkleAfterObst(2) = m_soleToAnkle;

      // position left foot in front of the obstacle
      // to world frame coordinates
      Foot_P = m_ObstaclePosition + m_ObstacleRot * AnkleBeforeObst;

      TempCOMState(0) =
          AnkleBeforeObst(0) + DoubleSupportCOMPosFactor * StepOverStepLenght;
      TempCOMState(1) = 0.0;
      // suppose the preview control sets Y coordinate
      // in the middle of the double support
      TempCOMState(2) = StepOverCOMHeight;

      // to worldframe
      TempCOMState = m_ObstaclePosition + m_ObstacleRot * TempCOMState;

      aCOMState.x[0] = TempCOMState(0);
      aCOMState.y[0] = TempCOMState(1);
      aCOMState.z[0] = TempCOMState(2);

      aCOMState.yaw[0] = -m_WaistRotationStepOver;
      // m_ObstacleParameters.theta + OrientationHipToObstacle;

      c = cos(aCOMState.yaw[0] * M_PI / 180.0);
      s = sin(aCOMState.yaw[0] * M_PI / 180.0);

      // COM Orientation
      Body_R(0, 0) = c;
      Body_R(0, 1) = -s;
      Body_R(0, 2) = 0;
      Body_R(1, 0) = s;
      Body_R(1, 1) = c;
      Body_R(1, 2) = 0;
      Body_R(2, 0) = 0;
      Body_R(2, 1) = 0;
      Body_R(2, 2) = 1;

      // COM position
      ToTheHip = Body_R * m_StaticToTheLeftHip;
      Body_P(0) = aCOMState.x[0] + ToTheHip(0);
      Body_P(1) = aCOMState.y[0] + ToTheHip(1);
      Body_P(2) = aCOMState.z[0] + ToTheHip(2);

      // Left Foot.

      c = cos(m_ObstacleParameters.theta * M_PI / 180.0);
      s = sin(m_ObstacleParameters.theta * M_PI / 180.0);
      co = cos(0.0 * M_PI / 180.0);
      // at the moment the feet stand flat on
      // the ground when in double support phase
      so = sin(0.0 * M_PI / 180.0);

      Foot_R(0, 0) = c * co;
      Foot_R(0, 1) = -s;
      Foot_R(0, 2) = c * so;
      Foot_R(1, 0) = s * co;
      Foot_R(1, 1) = c;
      Foot_R(1, 2) = s * so;
      Foot_R(2, 0) = -so;
      Foot_R(2, 1) = 0;
      Foot_R(2, 2) = co;

      // Compute the inverse kinematics.
      cout << __FUNCTION__ << ":" << __LINE__
           << ": You should implement something here" << endl;
      /* To implement:
         m_HDR->ComputeInverseKinematics2ForLegs(Body_R,
         Body_P,
         m_Dt,
         Foot_R,
         Foot_P,
         LeftLegAngles);
      */

      // RIGHT FOOT //
      m_Dt(1) = -m_Dt(1);

      // Right Foot.
      c = cos(OrientationFeetToObstacle * M_PI / 180.0);
      s = sin(OrientationFeetToObstacle * M_PI / 180.0);
      co = cos(OmegaAngleFeet * M_PI / 180.0);
      so = sin(OmegaAngleFeet * M_PI / 180.0);

      // Orientation
      Foot_R(0, 0) = c * co;
      Foot_R(0, 1) = -s;
      Foot_R(0, 2) = c * so;
      Foot_R(1, 0) = s * co;
      Foot_R(1, 1) = c;
      Foot_R(1, 2) = s * so;
      Foot_R(2, 0) = -so;
      Foot_R(2, 1) = 0;
      Foot_R(2, 2) = co;

      // position
      Foot_P = m_ObstaclePosition + m_ObstacleRot * AnkleAfterObst;

      // COM position
      ToTheHip = Body_R * m_StaticToTheRightHip;
      Body_P(0) = aCOMState.x[0] + ToTheHip(0);
      Body_P(1) = aCOMState.y[0] + ToTheHip(1);
      Body_P(2) = aCOMState.z[0] + ToTheHip(2);

      /*To implement:
        m_IK->ComputeInverseKinematics2ForLegs(Body_R,
        Body_P,
        m_Dt,
        Foot_R,
        Foot_P,
        RightLegAngles);
      */
      m_Dt(1) = -m_Dt(1);

      /// TO DO a check on all the maximum values for the angles
      // after the inverse kinematics....or implement a check in the
      // inverskinematics claas itself...at this moments there is only a
      // protection against knee overstretch built in
      if (!((LeftLegAngles(3) < m_KneeAngleBound) ||
            (RightLegAngles(3) < m_KneeAngleBound))) {

        WaistPos(0) = aCOMState.x[0];
        WaistPos(1) = aCOMState.y[0];
        WaistPos(2) = aCOMState.z[0] + m_DiffBetweenComAndWaist;

        WaistRot = Body_R;

        // check collision : for the leg in front of the obstacle
        // only lines (points 1, 2, 3, 4) on the shin
        // for the leg behind the obstacle only lines
        // (point 5, 6, 7) on the calf
        CollisionStatus = 0;
        FinalCollisionStatus = 0;

        // leg in front of the obstacle (for now always left leg)
        for (unsigned int k = 0; k < 3; k++) {
          // PointOnLeg = m_LegLayoutPoint.GetNColumns(k,1);
          PointOnLeg[0] = m_LegLayoutPoint(0, k);
          PointOnLeg[1] = m_LegLayoutPoint(1, k);
          PointOnLeg[2] = m_LegLayoutPoint(2, k);
          //            MAL_MATRIX_C_eq_EXTRACT_A(PointOnLeg,
          // m_LegLayoutPoint,double,0,k,3,1);
          ODEBUG("PointOnLeg : " << k << endl << PointOnLeg << endl);
          m_CollDet->CalcCoordShankLowerLegPoint(
              PointOnLeg, AbsCoord, LeftLegAngles, WaistRot, WaistPos, 1);
          m_CollDet->WorldFrameToObstacleFrame(AbsCoord, ObstFrameCoord1);
          // PointOnLeg = m_LegLayoutPoint.GetNColumns(k+1,1);
          PointOnLeg[0] = m_LegLayoutPoint(0, k + 1);
          PointOnLeg[1] = m_LegLayoutPoint(1, k + 1);
          PointOnLeg[2] = m_LegLayoutPoint(2, k + 1);

          //            MAL_MATRIX_C_eq_EXTRACT_A(PointOnLeg,
          // m_LegLayoutPoint,double,0,k+1,3,1);
          ODEBUG("PointOnLeg : " << k + 1 << endl << PointOnLeg << endl);
          m_CollDet->CalcCoordShankLowerLegPoint(
              PointOnLeg, AbsCoord, LeftLegAngles, WaistRot, WaistPos, 1);
          m_CollDet->WorldFrameToObstacleFrame(AbsCoord, ObstFrameCoord2);
          CollisionStatus = m_CollDet->CollisionLineObstacleComplete(
              ObstFrameCoord1, ObstFrameCoord2);
          // cout << "collision status for line with starting point "
          // << k+1 << " is : " << CollisionStatus << endl;
          FinalCollisionStatus = FinalCollisionStatus || CollisionStatus;
604
        }
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
        for (unsigned int k = 4; k < 6; k++) {
          ODEBUG("PointOnLeg : " << k << endl << PointOnLeg << endl);
          // PointOnLeg = m_LegLayoutPoint.GetNColumns(k,1);
          PointOnLeg[0] = m_LegLayoutPoint(0, k);
          PointOnLeg[1] = m_LegLayoutPoint(1, k);
          PointOnLeg[2] = m_LegLayoutPoint(2, k);
          // MAL_MATRIX_C_eq_EXTRACT_A(PointOnLeg,
          //                  m_LegLayoutPoint,double,0,k,3,1);
          m_CollDet->CalcCoordShankLowerLegPoint(
              PointOnLeg, AbsCoord, LeftLegAngles, WaistRot, WaistPos, 1);
          m_CollDet->WorldFrameToObstacleFrame(AbsCoord, ObstFrameCoord1);
          // PointOnLeg = m_LegLayoutPoint.GetNColumns(k+1,1);
          PointOnLeg[0] = m_LegLayoutPoint(0, k + 1);
          PointOnLeg[1] = m_LegLayoutPoint(1, k + 1);
          PointOnLeg[2] = m_LegLayoutPoint(2, k + 1);

          // MAL_MATRIX_C_eq_EXTRACT_A(PointOnLeg,m_LegLayoutPoint,
          // double,0,k+1,3,1);
          ODEBUG("PointOnLeg : " << k + 1 << endl << PointOnLeg << endl);
          m_CollDet->CalcCoordShankLowerLegPoint(
              PointOnLeg, AbsCoord, LeftLegAngles, WaistRot, WaistPos, 1);
          m_CollDet->WorldFrameToObstacleFrame(AbsCoord, ObstFrameCoord2);
          CollisionStatus = m_CollDet->CollisionLineObstacleComplete(
              ObstFrameCoord1, ObstFrameCoord2);
          // cout << "collision status for line with starting point "
          // << k+1 << " is : " << CollisionStatus << endl;
          FinalCollisionStatus = FinalCollisionStatus || CollisionStatus;
632
        }
633
634
635
636
      }
      // cout << "FinalCollisionStatus is " << FinalCollisionStatus << endl;
      if (!FinalCollisionStatus)
        break;
Thomas Moulard's avatar
Thomas Moulard committed
637
    }
638
639
640
641
642
643
644
645
646
647
648
649
    if (!FinalCollisionStatus) {
      m_StepOverStepLenght = StepOverStepLenght;
      m_StepOverHipHeight = StepOverCOMHeight;
      // cout << "feasibility selected StepOverStepLenght : " <<
      // StepOverStepLenght << " and StepOverCOMHeight : "
      // << StepOverCOMHeight << endl;
      //   cout << "while the nominal steplength is : "
      // << m_nominalStepLenght << " and the nominal COMHeight is "
      // << m_NominalCOMStepHeight << endl;;
      break;
    }
  }
Thomas Moulard's avatar
Thomas Moulard committed
650
651
}

652
653
654
655
void StepOverPlanner::PolyPlanner(deque<COMState> &aCOMBuffer,
                                  deque<FootAbsolutePosition> &aLeftFootBuffer,
                                  deque<FootAbsolutePosition> &aRightFootBuffer,
                                  deque<ZMPPosition> &aZMPPositions) {
Thomas Moulard's avatar
Thomas Moulard committed
656
  m_RightFootBuffer = aRightFootBuffer;
657
  m_LeftFootBuffer = aLeftFootBuffer;
Thomas Moulard's avatar
Thomas Moulard committed
658
  m_COMBuffer = aCOMBuffer;
659
660
  m_ZMPPositions = aZMPPositions;

661
  m_ModulationSupportCoefficient =
662
      0.9; // m_ZMPDiscr->GetModulationSupportCoefficient();
663

Thomas Moulard's avatar
Thomas Moulard committed
664
665
666
667
668
669
670
671
672
673
674
675
  m_StartStepOver = 0;
  m_StartDoubleSupp = 0;
  m_StartSecondStep = 0;
  m_EndStepOver = 0;

  m_WhileSpecialSteps = false;
  m_StartPrevStepOver = 0;
  m_EndPrevStepOver = 0;

  m_StartAfterStepOver = 0;
  m_EndAfterStepOver = 0;

676
677
678
679
680
  for (unsigned int u = 0; u < m_LeftFootBuffer.size(); u++) {
    if ((std::fabs((double)m_LeftFootBuffer[u].stepType) == 2) &
        (m_StartPrevStepOver == 0)) {
      m_StartPrevStepOver = u;
      m_WhileSpecialSteps = true;
Thomas Moulard's avatar
Thomas Moulard committed
681
    }
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
    if ((std::fabs((double)m_LeftFootBuffer[u].stepType) == 13) &
        (m_EndPrevStepOver == 0))
      m_EndPrevStepOver = u;
    if ((std::fabs((double)m_LeftFootBuffer[u].stepType) == 3) &
        (m_StartStepOver == 0))
      m_StartStepOver = u;
    if ((m_LeftFootBuffer[u].stepType == 14) & (m_StartDoubleSupp == 0))
      m_StartDoubleSupp = u;
    if ((std::fabs((double)m_LeftFootBuffer[u].stepType) == 4) &
        (m_StartSecondStep == 0))
      m_StartSecondStep = u;
    if ((m_LeftFootBuffer[u].stepType == 15) & (m_EndStepOver == 0))
      m_EndStepOver = u;
    if ((std::fabs((double)m_LeftFootBuffer[u].stepType) == 5) &
        (m_StartAfterStepOver == 0))
      m_StartAfterStepOver = u;
    if ((std::fabs((double)m_LeftFootBuffer[u].stepType) == 11) &
        (m_EndAfterStepOver == 0) & (m_WhileSpecialSteps == true)) {
      m_EndAfterStepOver = u;

      m_WhileSpecialSteps = false;
      break;
Thomas Moulard's avatar
Thomas Moulard committed
704
    }
705
706
707
708
709
  }

  if (m_LeftFootBuffer[m_StartStepOver].stepType > 0) {
    m_WaistRotationStepOver = -m_WaistRotationStepOver;
  }
710

Thomas Moulard's avatar
Thomas Moulard committed
711
712
  PolyPlannerHip();

713
714
715
716
717
718
719
720
721
  if (m_LeftFootBuffer[m_StartStepOver].stepType > 0) {
    m_WhoIsFirst = -1;
    PolyPlannerFirstStep(m_LeftFootBuffer);
    PolyPlannerSecondStep(m_RightFootBuffer);
  } else {
    m_WhoIsFirst = +1;
    PolyPlannerFirstStep(m_RightFootBuffer);
    PolyPlannerSecondStep(m_LeftFootBuffer);
  }
722

Thomas Moulard's avatar
Thomas Moulard committed
723
724
725
  aRightFootBuffer = m_RightFootBuffer;
  aLeftFootBuffer = m_LeftFootBuffer;
  aCOMBuffer = m_COMBuffer;
726
  aZMPPositions = m_ZMPPositions;
Thomas Moulard's avatar
Thomas Moulard committed
727
728
729

#ifdef _DEBUG_

730
  // cout << "dumping foot data in StepOverBuffers_1.csv" << endl;
Thomas Moulard's avatar
Thomas Moulard committed
731
  ofstream aof_StepOverBuffers;
732
733
734
735
736
737
  static unsigned char FirstCall = 1;
  if (FirstCall) {
    aof_StepOverBuffers.open("StepOverBuffers_1.csv", ofstream::out);
  } else {
    aof_StepOverBuffers.open("StepOverBuffers_1.csv", ofstream::app);
  }
738

Thomas Moulard's avatar
Thomas Moulard committed
739
740
  if (FirstCall)
    FirstCall = 0;
741

742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
  for (unsigned int i = 0; i < m_LeftFootBuffer.size(); i++) {
    if (aof_StepOverBuffers.is_open()) {
      aof_StepOverBuffers << m_LeftFootBuffer[i].time << " " <<
          // m_ZMPBuffer[i].px << " "<<
          // m_ZMPBuffer[i].py<< " " <<
          m_COMBuffer[i].x[0] << " " << m_COMBuffer[i].y[0] << " "
                          << m_COMBuffer[i].z[0] << " "
                          << m_LeftFootBuffer[i].stepType << " "
                          << m_LeftFootBuffer[i].x << " "
                          << m_LeftFootBuffer[i].y << " "
                          << m_LeftFootBuffer[i].z << " "
                          << m_LeftFootBuffer[i].omega << " "
                          << m_RightFootBuffer[i].stepType << " "
                          << m_RightFootBuffer[i].x << " "
                          << m_RightFootBuffer[i].y << " "
                          << m_RightFootBuffer[i].z << " "
                          << m_RightFootBuffer[i].omega << " " << endl;
Thomas Moulard's avatar
Thomas Moulard committed
759
    }
760
  }
761

762
763
764
  if (aof_StepOverBuffers.is_open()) {
    aof_StepOverBuffers.close();
  }
765
766
#endif

767
  // return 1;*/
olivier-stasse's avatar
olivier-stasse committed
768
}
Thomas Moulard's avatar
Thomas Moulard committed
769

770
771
void StepOverPlanner::PolyPlannerFirstStep(
    deque<FootAbsolutePosition> &aStepOverFootBuffer) {
Thomas Moulard's avatar
Thomas Moulard committed
772

773
774
775
776
  Eigen::Matrix<double, 8, 1> aBoundCondZ;
  Eigen::Matrix<double, 8, 1> aBoundCondY;
  Eigen::Matrix<double, 8, 1> aBoundCondX;
  Eigen::Matrix<double, 8, 1> aBoundCondOmega;
777

Thomas Moulard's avatar
Thomas Moulard committed
778
779
  double StepTime;
  double StepLenght;
780
  double Omega1, Omega2, OmegaImpact;
Olivier Stasse's avatar
Olivier Stasse committed
781
  double xOffset;
Olivier Stasse's avatar
Olivier Stasse committed
782
783
  double Point1X, Point1Z;
  double Point2X, Point2Z;
Thomas Moulard's avatar
Thomas Moulard committed
784
  double Point3Z;
785

786
787
788
789
  StepTime = aStepOverFootBuffer[m_StartDoubleSupp].time -
             aStepOverFootBuffer[m_StartStepOver].time;
  StepLenght = aStepOverFootBuffer[m_StartDoubleSupp].x -
               aStepOverFootBuffer[m_StartStepOver].x;
790

791
  xOffset = 0.00;
Thomas Moulard's avatar
Thomas Moulard committed
792

793
794
795
  Omega1 = 0.0;
  Omega2 = 0.0;
  OmegaImpact = -2.0;
Thomas Moulard's avatar
Thomas Moulard committed
796

797
798
  // m_ModulationSupportCoefficient=0.8;// MOET ERGENS ANDERS GEDEFINIEERD
  // WORDEN
Thomas Moulard's avatar
Thomas Moulard committed
799

800
801
  // for now it is only in the 2D and with the obstacle
  // perpendicular to absolute x direction
802

803
804
805
  Point1X = StepLenght - m_heelToAnkle - m_ObstacleParameters.d - xOffset -
            m_tipToAnkle * cos(Omega1 * M_PI / 180.0);
  Point1Z = m_ObstacleParameters.h - m_tipToAnkle * sin(Omega1 * M_PI / 180.0);
806

807
808
809
  Point2X = StepLenght - m_heelToAnkle + xOffset +
            m_heelToAnkle * cos(Omega2 * M_PI / 180.0);
  Point2Z = m_ObstacleParameters.h - m_tipToAnkle * sin(Omega2 * M_PI / 180.0);
810

811
  Point3Z = Point1Z + 0.04;
Olivier Stasse's avatar
Olivier Stasse committed
812
  // m_ObstacleParameters.h+zOffset+0.04+m_tipToAnkle*sin(Omega2*M_PI/180.0);
Thomas Moulard's avatar
Thomas Moulard committed
813

814
  vector<double> aTimeDistr, aTimeDistrModulated;
Thomas Moulard's avatar
Thomas Moulard committed
815
  double ModulatedStepTime = StepTime * m_ModulationSupportCoefficient;
816
817
  double LiftOffTime = (StepTime - ModulatedStepTime) * 0.5;
  double TouchDownTime = StepTime - (StepTime - ModulatedStepTime) * 0.5;
Thomas Moulard's avatar
Thomas Moulard committed
818
819

  aTimeDistr.resize(3);
820

821
822
823
  aTimeDistr[0] = m_TimeDistrFactor[0] * StepTime / 5.0;
  aTimeDistr[1] = m_TimeDistrFactor[1] * StepTime / 5.0;
  aTimeDistr[2] = StepTime;
Thomas Moulard's avatar
Thomas Moulard committed
824

Olivier Stasse's avatar
Olivier Stasse committed
825
826
827
  // this time schedule is used for the X and Y coordinate of the foot in order
  // to make sure the foot lifts the ground (Z) before moving
  // the X and Y direction
828

Thomas Moulard's avatar
Thomas Moulard committed
829
  aTimeDistrModulated.resize(3);
830

831
832
833
  aTimeDistrModulated[0] = aTimeDistr[0] - LiftOffTime;
  aTimeDistrModulated[1] = aTimeDistr[1] - LiftOffTime;
  aTimeDistrModulated[2] = aTimeDistr[2] - 2.0 * LiftOffTime;
Thomas Moulard's avatar
Thomas Moulard committed
834

835
836
837
838
  Eigen::Matrix<double, 5, 1> ZfootPos;
  Eigen::Matrix<double, 5, 1> TimeIntervalsZ;
  Eigen::Matrix<double, 2, 1> ZfootSpeedBound;
  double PreviousSpeedZ, EndSpeedZ, SpeedAccZ, IntermediateZAcc;
Thomas Moulard's avatar
Thomas Moulard committed
839
840
  vector<double> SpeedWeightZ;

841
842
  ZfootSpeedBound(0) = 0.0;
  ZfootSpeedBound(1) = 0.0;
843

Guilhem Saurel's avatar
Guilhem Saurel committed
844
  int NumberIntermediate = 0, Counter = 0, CounterTemp = 0;
Thomas Moulard's avatar
Thomas Moulard committed
845
846
847
  double IntermediateTimeStep;

  NumberIntermediate = 10;
848

849
850
  ZfootPos.resize(2 + 3 * NumberIntermediate);
  TimeIntervalsZ.resize(2 + 3 * NumberIntermediate);
Thomas Moulard's avatar
Thomas Moulard committed
851
  SpeedWeightZ.resize(NumberIntermediate);
852

Thomas Moulard's avatar
Thomas Moulard committed
853
854
  ZfootPos(0) = 0.0;
  ZfootPos(1) = Point1Z;
855

Thomas Moulard's avatar
Thomas Moulard committed
856
857
858
  TimeIntervalsZ(0) = 0.0;
  TimeIntervalsZ(1) = aTimeDistr[0];

859
  // from point1Z going up to top
860

861
862
  IntermediateTimeStep =
      (aTimeDistr[1] - aTimeDistr[0]) / 2.0 / (NumberIntermediate);
863

Thomas Moulard's avatar
Thomas Moulard committed
864
865
  SpeedAccZ = 0.0;
  IntermediateZAcc = 0.0;
866
  PreviousSpeedZ = (Point1Z) / (aTimeDistr[0]);
Thomas Moulard's avatar
Thomas Moulard committed
867
868
869
  EndSpeedZ = 0.0;

  Counter = 1;
870

871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
  for (int i = 1; i <= NumberIntermediate; i++) {
    SpeedWeightZ[i - 1] = (EndSpeedZ - PreviousSpeedZ) *
                              ((double(i - 1) / (double(NumberIntermediate)))) +
                          PreviousSpeedZ;
    //      cout << "SpeedWeightZ[i-1]" << SpeedWeightZ[i-1] << endl;
    SpeedAccZ = SpeedAccZ + SpeedWeightZ[i - 1];
  }
  for (int i = 1; i <= NumberIntermediate; i++) {
    IntermediateZAcc = IntermediateZAcc +
                       (Point3Z - Point1Z) * SpeedWeightZ[i - 1] / SpeedAccZ;
    ZfootPos(Counter + i) = IntermediateZAcc + ZfootPos(Counter);

    TimeIntervalsZ(Counter + i) =
        TimeIntervalsZ(Counter) + i * IntermediateTimeStep;
    CounterTemp = i;
  }
887

888
  // from top going down to point2Z
Thomas Moulard's avatar
Thomas Moulard committed
889
890
  SpeedAccZ = 0.0;
  IntermediateZAcc = 0.0;
891
892
  PreviousSpeedZ = 0.0;
  EndSpeedZ = (-Point2Z) / (aTimeDistr[2] - aTimeDistr[1]);
Thomas Moulard's avatar
Thomas Moulard committed
893
894
  Counter = CounterTemp + Counter;

895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
  for (int i = 1; i <= NumberIntermediate; i++) {
    SpeedWeightZ[i - 1] = (EndSpeedZ - PreviousSpeedZ) *
                              ((double(i - 1) / (double(NumberIntermediate)))) +
                          PreviousSpeedZ;
    SpeedAccZ = SpeedAccZ + SpeedWeightZ[i - 1];
  }
  for (int i = 1; i <= NumberIntermediate; i++) {
    IntermediateZAcc = IntermediateZAcc +
                       (Point2Z - Point3Z) * SpeedWeightZ[i - 1] / SpeedAccZ;
    ZfootPos(Counter + i) = IntermediateZAcc + ZfootPos(Counter);

    TimeIntervalsZ(Counter + i) =
        TimeIntervalsZ(Counter) + i * IntermediateTimeStep;
    CounterTemp = i;
  }
910

911
912
  // going down from point2Z to the ground with smooth velocity profile
  // at touch down
913

914
  IntermediateTimeStep = (aTimeDistr[2] - aTimeDistr[1]) / (NumberIntermediate);
915

Thomas Moulard's avatar
Thomas Moulard committed
916
917
  SpeedAccZ = 0.0;
  IntermediateZAcc = 0.0;
918
  PreviousSpeedZ = (-Point2Z) / (aTimeDistr[2] - aTimeDistr[1]);
Thomas Moulard's avatar
Thomas Moulard committed
919
920
921
  EndSpeedZ = ZfootSpeedBound(1);
  Counter = CounterTemp + Counter;

922
923
924
925
926
927
928
  for (int i = 1; i <= NumberIntermediate; i++) {
    SpeedWeightZ[i - 1] =
        (EndSpeedZ - PreviousSpeedZ) *
            pow((double(i - 1) / (double(NumberIntermediate))), 1) +
        PreviousSpeedZ;
    SpeedAccZ = SpeedAccZ + SpeedWeightZ[i - 1];
  }
Thomas Moulard's avatar
Thomas Moulard committed
929

930
931
932
933
  for (int i = 1; i <= NumberIntermediate; i++) {
    IntermediateZAcc =
        IntermediateZAcc + (-Point2Z) * SpeedWeightZ[i - 1] / SpeedAccZ;
    ZfootPos(Counter + i) = IntermediateZAcc + ZfootPos(Counter);
Thomas Moulard's avatar
Thomas Moulard committed
934

935
936
937
    TimeIntervalsZ(Counter + i) =
        TimeIntervalsZ(Counter) + i * IntermediateTimeStep;
  }
Thomas Moulard's avatar
Thomas Moulard committed
938

939
  m_ClampedCubicSplineStepOverFootZ->SetParameters(ZfootPos, TimeIntervalsZ,
940
                                                   ZfootSpeedBound);
941

942
943
944
  Eigen::Matrix<double, 4, 1> XfootPos;
  Eigen::Matrix<double, 4, 1> TimeIntervalsX;
  Eigen::Matrix<double, 2, 1> XfootSpeedBound;
945

Thomas Moulard's avatar
Thomas Moulard committed
946
947
948
949
950
951
952
953
954
955
  XfootPos(0) = 0.0;
  XfootPos(1) = Point1X;
  XfootPos(2) = Point2X;
  XfootPos(3) = StepLenght;

  TimeIntervalsX(0) = 0.0;
  TimeIntervalsX(1) = aTimeDistrModulated[0];
  TimeIntervalsX(2) = aTimeDistrModulated[1];
  TimeIntervalsX(3) = aTimeDistrModulated[2];

956
957
  XfootSpeedBound(0) = 0.0;
  XfootSpeedBound(1) = 0.0;
Thomas Moulard's avatar
Thomas Moulard committed
958

959
  m_ClampedCubicSplineStepOverFootX->SetParameters(XfootPos, TimeIntervalsX,
960
                                                   XfootSpeedBound);
961

962
963
964
  Eigen::Matrix<double, 4, 1> OmegafootPos;
  Eigen::Matrix<double, 4, 1> TimeIntervalsOmega;
  Eigen::Matrix<double, 2, 1> OmegafootSpeedBound;
Thomas Moulard's avatar
Thomas Moulard committed
965
966

  OmegafootPos(0) = 0.0;
967
968
  OmegafootPos(1) = OmegaImpact * 1.0 / 3.0;
  OmegafootPos(2) = OmegaImpact * 2.0 / 3.0;
Thomas Moulard's avatar
Thomas Moulard committed
969
970
971
972
973
974
975
  OmegafootPos(3) = OmegaImpact;

  TimeIntervalsOmega(0) = 0.0;
  TimeIntervalsOmega(1) = aTimeDistrModulated[0];
  TimeIntervalsOmega(2) = aTimeDistrModulated[1];
  TimeIntervalsOmega(3) = aTimeDistrModulated[2];

976
977
  OmegafootSpeedBound(0) = 0.0;
  OmegafootSpeedBound(1) = 0.0;
Thomas Moulard's avatar
Thomas Moulard committed
978

979
980
  m_ClampedCubicSplineStepOverFootOmega->SetParameters(
      OmegafootPos, TimeIntervalsOmega, OmegafootSpeedBound);
981

982
983
984
  Eigen::Matrix<double, 2, 1> OmegaImpactfootPos;
  Eigen::Matrix<double, 2, 1> TimeIntervalsOmegaImpact;
  Eigen::Matrix<double, 2, 1> OmegaImpactfootSpeedBound;
Thomas Moulard's avatar
Thomas Moulard committed
985
986
987
988
989
990
991

  OmegaImpactfootPos(0) = OmegaImpact;
  OmegaImpactfootPos(1) = 0.0;

  TimeIntervalsOmegaImpact(0) = 0.0;
  TimeIntervalsOmegaImpact(1) = LiftOffTime;

992
993
  OmegaImpactfootSpeedBound(0) = 0.0;
  OmegaImpactfootSpeedBound(1) = 0.0;
Thomas Moulard's avatar
Thomas Moulard committed
994

995
996
  m_ClampedCubicSplineStepOverFootOmegaImpact->SetParameters(
      OmegaImpactfootPos, TimeIntervalsOmegaImpact, OmegaImpactfootSpeedBound);
Thomas Moulard's avatar
Thomas Moulard committed
997
998

  vector<double> aTimeDistrModulatedYSide;
999
  Eigen::Matrix<double, 5, 1> aBoundCondYSide;
1000

Thomas Moulard's avatar
Thomas Moulard committed
1001
1002
  aTimeDistrModulatedYSide.resize(2);

1003
1004
  aTimeDistrModulatedYSide[0] = 3.0 * (StepTime - 2.0 * LiftOffTime) / 5.0;
  aTimeDistrModulatedYSide[1] = StepTime - 2.0 * LiftOffTime;
Thomas Moulard's avatar
Thomas Moulard committed
1005

1006
1007
  /* this time schedule is used for the X and Y coordinate of the foot
     in order to make sure the foot lifts the ground (Z)
Thomas Moulard's avatar
Thomas Moulard committed
1008
     before moving the X and Y direction */
1009

1010
1011
1012
  Eigen::Matrix<double, 4, 1> YfootPos;
  Eigen::Matrix<double, 4, 1> TimeIntervalsY;
  Eigen::Matrix<double, 2, 1> YfootSpeedBound;
Thomas Moulard's avatar
Thomas Moulard committed
1013
1014

  {
1015
1016
1017
1018
1019
1020
1021
1022
1023
1024
1025
1026
1027
1028
1029
    if (m_ObstacleParameters.h > 0.20) {
      // when the obstacle is to high an auto collision occurs at the hip joint
      // so the foot is turned a little inwaqrds to avoid this
      YfootPos(0) = 0.0;
      YfootPos(1) =
          m_WhoIsFirst * 0.01 * (m_ObstacleParameters.h - 0.20) / 0.05;
      YfootPos(2) =
          m_WhoIsFirst * 0.07 * (m_ObstacleParameters.h - 0.20) / 0.05;
      YfootPos(3) = 0.0;
    } else {
      YfootPos(0) = 0.0;
      YfootPos(1) = 0.0;
      YfootPos(2) = 0.0;
      YfootPos(3) = 0.0;
    }
1030

Thomas Moulard's avatar
Thomas Moulard committed
1031
1032
1033
1034
    TimeIntervalsY(0) = 0.0;
    TimeIntervalsY(1) = aTimeDistrModulated[0];
    TimeIntervalsY(2) = aTimeDistrModulated[1];
    TimeIntervalsY(3) = aTimeDistrModulated[2];
1035

1036
1037
    YfootSpeedBound(0) = 0.0;
    YfootSpeedBound(1) = 0.0;
1038

1039
    m_ClampedCubicSplineStepOverFootY->SetParameters(YfootPos, TimeIntervalsY,
1040
                                                     YfootSpeedBound);
Thomas Moulard's avatar
Thomas Moulard committed
1041
1042
  }

1043
1044
1045
  // update the footbuffers with the new calculated polynomials for stepping
  // over
  unsigned int diff = m_StartDoubleSupp - m_StartStepOver;
Thomas Moulard's avatar
Thomas Moulard committed
1046
1047
  double LocalTime;
  int aStart = m_StartStepOver;
1048
1049
1050
1051
1052
1053
1054
1055
1056
1057
1058
1059
1060
1061
1062
1063
1064
1065
1066
1067
1068
1069
1070
1071
1072
1073
1074
1075
1076
1077
1078
1079
1080
  // double temp;

  for (unsigned int i = 0; i <= diff; i++) {
    LocalTime = (i)*m_SamplingPeriod;

    if (LocalTime < LiftOffTime) {
      aStepOverFootBuffer[i + aStart].x = aStepOverFootBuffer[aStart].x;
      aStepOverFootBuffer[i + aStart].y = aStepOverFootBuffer[aStart].y;
      aStepOverFootBuffer[i + aStart].theta = aStepOverFootBuffer[aStart].theta;
      aStepOverFootBuffer[i + aStart].omega = aStepOverFootBuffer[aStart].omega;
    } else if (LocalTime >= TouchDownTime) {
      aStepOverFootBuffer[i + aStart].x = aStepOverFootBuffer[i + aStart - 1].x;
      aStepOverFootBuffer[i + aStart].y = aStepOverFootBuffer[i + aStart - 1].y;
      aStepOverFootBuffer[i + aStart].theta =
          aStepOverFootBuffer[i + aStart - 1].theta;
      aStepOverFootBuffer[i + aStart].omega =
          m_ClampedCubicSplineStepOverFootOmegaImpact->GetValueSpline(
              TimeIntervalsOmegaImpact, LocalTime - TouchDownTime) +
          aStepOverFootBuffer[aStart].omega;
    } else {
      aStepOverFootBuffer[i + aStart].x =
          m_ClampedCubicSplineStepOverFootX->GetValueSpline(
              TimeIntervalsX, LocalTime - LiftOffTime) +
          aStepOverFootBuffer[aStart].x;
      aStepOverFootBuffer[i + aStart].y =
          m_ClampedCubicSplineStepOverFootY->GetValueSpline(
              TimeIntervalsY, LocalTime - LiftOffTime) +
          aStepOverFootBuffer[aStart].y;
      aStepOverFootBuffer[i + aStart].omega =
          m_ClampedCubicSplineStepOverFootOmega->GetValueSpline(
              TimeIntervalsOmega, LocalTime - LiftOffTime) +
          aStepOverFootBuffer[aStart].omega;
      aStepOverFootBuffer[i + aStart].theta = aStepOverFootBuffer[aStart].theta;
Thomas Moulard's avatar
Thomas Moulard committed
1081
    }
1082
1083
1084
1085
1086
    aStepOverFootBuffer[i + aStart].z =
        m_ClampedCubicSplineStepOverFootZ->GetValueSpline(TimeIntervalsZ,
                                                          LocalTime) +
        aStepOverFootBuffer[aStart].z;
  }
olivier-stasse's avatar
olivier-stasse committed
1087
}
Thomas Moulard's avatar
Thomas Moulard committed
1088

1089
1090
void StepOverPlanner::PolyPlannerSecondStep(
    deque<FootAbsolutePosition> &aStepOverFootBuffer) {
1091

1092
1093
1094
1095
  Eigen::Matrix<double, 8, 1> aBoundCondZ;
  Eigen::Matrix<double, 8, 1> aBoundCondY;
  Eigen::Matrix<double, 8, 1> aBoundCondX;
  Eigen::Matrix<double, 8, 1> aBoundCondOmega;
1096

Thomas Moulard's avatar
Thomas Moulard committed
1097
1098
  double StepTime;
  double StepLenght;
1099
  double Omega1, Omega2, OmegaImpact;
Olivier Stasse's avatar
Olivier Stasse committed
1100
  double xOffset;
Olivier Stasse's avatar
Olivier Stasse committed
1101
1102
  double Point1X, Point1Z;
  double Point2X, Point2Z;
Thomas Moulard's avatar
Thomas Moulard committed
1103
  double Point3Z;
1104

1105
1106
1107
1108
  StepTime = aStepOverFootBuffer[m_EndStepOver].time -
             aStepOverFootBuffer[m_StartSecondStep].time;
  StepLenght = aStepOverFootBuffer[m_EndStepOver].x -
               aStepOverFootBuffer[m_StartSecondStep].x;
1109

1110
  xOffset = 0.0;
Thomas Moulard's avatar
Thomas Moulard committed
1111

1112
1113
1114
  Omega1 = 120.0 * m_ObstacleParameters.h; // in degrees
  Omega2 = 120.0 * m_ObstacleParameters.h;
  OmegaImpact = -2.0;
Thomas Moulard's avatar
Thomas Moulard committed
1115

1116
1117
1118
  Point1X = m_StepOverStepLenght - m_heelToAnkle - m_ObstacleParameters.d -
            xOffset - m_tipToAnkle * cos(Omega1 * M_PI / 180.0);
  Point1Z = m_ObstacleParameters.h + m_tipToAnkle * sin(Omega1 * M_PI / 180.0);
1119

1120
1121
  Point2X = m_StepOverStepLenght - m_heelToAnkle + xOffset +
            m_heelToAnkle * cos(Omega2 * M_PI / 180.0);
Olivier Stasse's avatar
Olivier Stasse committed
1122
1123
  Point2Z = Point1Z;
  // m_ObstacleParameters.h+0.04;//-m_tipToAnkle*sin(Omega2*M_PI/180.0);
Thomas Moulard's avatar
Thomas Moulard committed
1124

1125
  vector<double> aTimeDistr, aTimeDistrModulated;
Thomas Moulard's avatar
Thomas Moulard committed
1126
  double ModulatedStepTime = StepTime * m_ModulationSupportCoefficient;
1127
1128
  double LiftOffTime = (StepTime - ModulatedStepTime) * 0.5;
  double TouchDownTime = StepTime - (StepTime - ModulatedStepTime) * 0.5;
Thomas Moulard's avatar
Thomas Moulard committed
1129
1130

  aTimeDistr.resize(4);
1131

1132
1133
1134
1135
  aTimeDistr[0] = m_TimeDistrFactor[2] * StepTime / 5.0;
  // aTimeDistr[1]=1.8*StepTime/5.0;
  aTimeDistr[1] = m_TimeDistrFactor[3] * StepTime / 5.0;
  aTimeDistr[2] = StepTime;
Thomas Moulard's avatar
Thomas Moulard committed
1136

Olivier Stasse's avatar
Olivier Stasse committed
1137
1138
1139
  // this time schedule is used for the X and Y coordinate of the foot in
  // order to make sure the foot lifts the ground (Z) before
  // moving the X and Y direction
1140

Thomas Moulard's avatar
Thomas Moulard committed
1141
  aTimeDistrModulated.resize(3);
1142

1143