small-steps.cc 29 KB
Newer Older
1
2
///
/// Copyright (c) 2015 CNRS
Joseph Mirabel's avatar
Joseph Mirabel committed
3
/// Authors: Florent Lamiraux, Joseph Mirabel
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
///
///
// This file is part of hpp-wholebody-step.
// hpp-wholebody-step-planner 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.
//
// hpp-wholebody-step-planner 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
// hpp-wholebody-step-planner. If not, see
// <http://www.gnu.org/licenses/>.

20
21
22
23
#include <hpp/wholebody-step/small-steps.hh>

#include <boost/assign/list_of.hpp>

24
25
#include <hpp/util/debug.hh>

26
#include <hpp/constraints/implicit.hh>
27
28
#include <hpp/constraints/symbolic-function.hh>

Joseph Mirabel's avatar
Joseph Mirabel committed
29
30
31
#include <hpp/pinocchio/humanoid-robot.hh>
#include <hpp/pinocchio/joint.hh>
#include <hpp/pinocchio/center-of-mass-computation.hh>
32
33

#include <hpp/core/problem.hh>
34
35
36
37
#include <hpp/core/path-vector.hh>
#include <hpp/core/straight-path.hh>
#include <hpp/core/constraint-set.hh>
#include <hpp/core/config-projector.hh>
38
39
#include <hpp/core/path-validation.hh>
#include <hpp/core/path-validation-report.hh>
40
41
42
43

#include <hpp/walkgen/bspline-based.hh>
#include <hpp/walkgen/foot-print.hh>

44
45
#include <hpp/wholebody-step/time-dependant.hh>
#include <hpp/wholebody-step/time-dependant-path.hh>
46
#include <hpp/wholebody-step/static-stability-constraint.hh> // STABILITY_CONTEXT
47

Joseph Mirabel's avatar
Joseph Mirabel committed
48
49
#include <small-steps/functors.hh>

50
51
namespace hpp {
  namespace wholebodyStep {
52

53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
    namespace {
      typedef std::map <value_type, value_type> TimeToParameterMap_t;
      void print_steps (const TimeToParameterMap_t& param,
                        const SmallSteps::FootPrints_t& footPrints,
                        const CubicBSplinePtr_t& com,
                        const std::size_t n) {
        hppDout (info, "## Start foot steps - Python ##");
        if (n == 0) {
          hppDout (info, "time_param = list ()");
          hppDout (info, "footPrint  = list ()");
          hppDout (info, "com  = list ()");
        }
        hppDout (info, "time_param.append(list ())");
        hppDout (info, "footPrint.append(list ())");
        hppDout (info, "com.append(list ())");
        for (TimeToParameterMap_t::const_iterator it = param.begin ();
            it != param.end (); ++it)
          hppDout (info, "time_param[" << n << "].append ([ "
              << it->first << ", " << it->second << " ])");
        for (SmallSteps::FootPrints_t::const_iterator it = footPrints.begin ();
            it != footPrints.end (); ++it)
          hppDout (info, "footPrint[" << n << "].append ([ "
              << it->position[0] << ", " << it->position[1] << ", "
              << it->orientation[0] << ", " << it->orientation[1] << " ])");
        vector_t res(2);
        const value_type step = (com->timeRange ().second - com->timeRange ().first) / 100;
        for (value_type t = com->timeRange ().first;
            t < com->timeRange ().second; t += step) {
          (*com) (res, t);
          hppDout (info, "com[" << n << "].append ([ "
              << t << ", " << res[0] << ", " << res[1] << " ])");
        }
        hppDout (info, "## Stop foot steps - Python ##");
      }
Joseph Mirabel's avatar
Joseph Mirabel committed
87
88
89
90
91
92
93
94
95
96
97

      template <typename Container>
      inline bool isStrictlyIncreasing (const Container& c)
      {
        typename Container::const_iterator b = c.begin (), e = c.end();
        return (std::adjacent_find (b, e, std::greater_equal<double>()) == e);
      }

      template <typename Scalar>
        inline bool isApprox (const Scalar& a, const Scalar& b, const Scalar& eps = Eigen::NumTraits<Scalar>::epsilon())
        { return std::abs (a - b) < eps; }
98
99
100
101

      struct HandConstraintData {
        SmallSteps::HandConstraint& model;

Joseph Mirabel's avatar
Joseph Mirabel committed
102
        DifferentiableFunctionPtr_t func;
103
        constraints::ImplicitPtr_t eq;
104
105
106
107
108
109
110
111
112
113
        TimeDependant TD;

        HandConstraintData (SmallSteps::HandConstraint& m) :
          model (m), TD (eq, RightHandSideFunctorPtr_t()) {}

        void setup (const HumanoidRobotPtr_t& robot,
            const JointPtr_t joint, const PathPtr_t& path,
            const SmallSteps::PiecewiseAffine& param)
        {
          if (!model.active) return;
Joseph Mirabel's avatar
Joseph Mirabel committed
114
115
116
          // Position only
          func = PointInJointFunction::create
            ("point-hand-walkgen", robot, PointInJoint::create (joint, vector3_t (0,0,0)));
117
          eq = constraints::Implicit::create (func, 3 * constraints::Equality);
118
119
120
121
122
123
124
125
          TD = TimeDependant (eq, RightHandSideFunctorPtr_t
              (new ReproducePath (func, path, param))
              );
        }

        inline void add (const ConfigProjectorPtr_t& proj) const
        {
          if (!model.active) return;
Joseph Mirabel's avatar
Joseph Mirabel committed
126
          if (model.optional) {
Guilhem Saurel's avatar
Guilhem Saurel committed
127
            proj->add (eq, 1);
Joseph Mirabel's avatar
Joseph Mirabel committed
128
129
130
131
            proj->lastIsOptional (true);
          } else {
            proj->add (eq);
          }
132
133
134
135
136
137
138
139
140
141
142
143
144
        }

        inline void add (const TimeDependantPathPtr_t& p) const
        {
          if (!model.active) return;
          p->add (TD);
        }

        inline void updateAbscissa (const value_type& t) const
        {
          if (model.active) TD.rhsAbscissa (t);
        }
      };
145
146
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
147
    value_type SmallSteps::PiecewiseAffine::operator () (const value_type& t) const
148
    {
Joseph Mirabel's avatar
Joseph Mirabel committed
149
150
151
152
153
154
155
156
157
158
159
160
161
      Pairs_t::const_iterator it = pairs_.lower_bound (t);
      if (it == pairs_.end ()) {
        --it; return it->second;
      } else if (it == pairs_.begin ()) {
        return it->second;
      } else {
        value_type t1 = it->first;
        value_type p1 = it->second;
        --it;
        value_type t0 = it->first;
        value_type p0 = it->second;
        value_type alpha = (t-t0)/(t1-t0);
        return (1-alpha)*p0 + alpha*p1;
162
      }
Joseph Mirabel's avatar
Joseph Mirabel committed
163
    }
164

Joseph Mirabel's avatar
Joseph Mirabel committed
165
    inline void SmallSteps::PiecewiseAffine::addPair (const value_type& t, const value_type value)
Joseph Mirabel's avatar
Joseph Mirabel committed
166
167
168
    {
      pairs_ [t] = value;
    }
169

170
    SmallStepsPtr_t SmallSteps::create (const ProblemConstPtr_t& problem)
171
172
173
174
175
176
177
    {
      SmallSteps* ptr = new SmallSteps (problem);
      SmallStepsPtr_t shPtr (ptr);
      ptr->init (shPtr);
      return shPtr;
    }

178
    SmallSteps::SmallSteps (const ProblemConstPtr_t& problem) :
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
      core::PathOptimizer (problem), robot_ (), minStepLength_ (0.2),
      maxStepLength_ (0.2), defaultStepHeight_ (0.05),
      defaultDoubleSupportTime_ (0.1), defaultSingleSupportTime_ (0.6),
      defaultInitializationTime_ (0.6), pg_ ()
    {
    }

    void SmallSteps::init (const SmallStepsWkPtr_t& weak)
    {
      weakPtr_ = weak;
    }

    void SmallSteps::getStepParameters (const PathVectorPtr_t& path)
    {
      stepParameters_.clear ();
      footPrints_.clear ();
      value_type eps = 1e-3;
      value_type s = 0, s_next;
      stepParameters_.push_back (s);
      hppDout (info, "s = " << s);
      value_type length = path->length ();
      bool finished = false;
Florent Lamiraux's avatar
Florent Lamiraux committed
201
      bool stepLeft (true);
202
203
      FootPrint rfp = footPrintAtParam (path, s, true);
      FootPrint lfp = footPrintAtParam (path, s, false);
204
      value_type halfStepLength = .5*maxStepLength_;
205
206
207
      value_type distBetweenFeetAtDoubleSupport =
        sqrt (halfStepLength*halfStepLength +
            (rfp.position - lfp.position).squaredNorm());
208
      while (!finished) {
Joseph Mirabel's avatar
Joseph Mirabel committed
209
210
        FootPrint rfp = footPrintAtParam (path, s, true);
        FootPrint lfp = footPrintAtParam (path, s, false);
211
212
213
	// Store foot prints
	if (s==0) {
	  // left foot first
Joseph Mirabel's avatar
Joseph Mirabel committed
214
          footPrints_.push_back (lfp);
215
          footPrintsIsRight_.push_back (false);
Joseph Mirabel's avatar
Joseph Mirabel committed
216
          footPrints_.push_back (rfp);
217
          footPrintsIsRight_.push_back (true);
218
219
220
	  stepLeft = true;
	} else {
	  if (stepLeft) {
Joseph Mirabel's avatar
Joseph Mirabel committed
221
            footPrints_.push_back (lfp);
222
            footPrintsIsRight_.push_back (false);
223
224
	    stepLeft = false;
	  } else {
Joseph Mirabel's avatar
Joseph Mirabel committed
225
            footPrints_.push_back (rfp);
226
            footPrintsIsRight_.push_back (true);
227
228
229
230
231
232
233
	    stepLeft = true;
	  }
	}
	s_next = std::min (length, s + halfStepLength);
	bool found = false;
	while (!found) {
	  // Compute position of both feet at s_next
Joseph Mirabel's avatar
Joseph Mirabel committed
234
235
          FootPrint rfp_next = footPrintAtParam (path, s_next, true);
          FootPrint lfp_next = footPrintAtParam (path, s_next, false);
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260

          value_type err;
          if (stepLeft) {
            /// The right foot is on the ground and we are looking for the next
            /// left foot position.
            /// Find a position not too far from the last right foot position.
            err = (rfp.position - lfp_next.position).norm ()
              - distBetweenFeetAtDoubleSupport;
          } else {
            err = (lfp.position - rfp_next.position).norm ()
              - distBetweenFeetAtDoubleSupport;
          }

          if (std::abs (err) < eps) {
            found = true; /// s_next is accepted
          } else {
            if (err < 0 && s_next == length)
              found = true;
            else {
              s_next = std::min (length, s_next - err);
              if (s_next < s)
                throw std::runtime_error ("Step parameter cannot decrease.");
            }
          }

Joseph Mirabel's avatar
Joseph Mirabel committed
261
262
263
264
          if (found) {
            rfp = rfp_next;
            lfp = lfp_next;
          }
265
266
267
268
269
	} // while (!found)
	s = s_next;
	stepParameters_.push_back (s);
	hppDout (info, "s = " << s);
	if (s == length) {
270
	  finished = true;
271
	  if (stepLeft) {
Joseph Mirabel's avatar
Joseph Mirabel committed
272
	    footPrints_.push_back (lfp);
273
            footPrintsIsRight_.push_back (false);
Joseph Mirabel's avatar
Joseph Mirabel committed
274
            footPrints_.push_back (rfp);
275
            footPrintsIsRight_.push_back (true);
276
	  } else {
Joseph Mirabel's avatar
Joseph Mirabel committed
277
            footPrints_.push_back (rfp);
278
            footPrintsIsRight_.push_back (true);
Joseph Mirabel's avatar
Joseph Mirabel committed
279
            footPrints_.push_back (lfp);
280
            footPrintsIsRight_.push_back (false);
281
282
283
284
285
286
287
288
	  }
	}
      } // while (!finished)
    }

    PathVectorPtr_t SmallSteps::optimize (const PathVectorPtr_t& path)
    {
      // Check that robot is a humanoid robot
289
      robot_ = HPP_DYNAMIC_PTR_CAST (HumanoidRobot, problem()->robot ());
290
291
292
293
294
      if (!robot_) {
	throw std::runtime_error ("Robot is not of type humanoid");
      }
      getStepParameters (path);
      // Create pattern generator with height of center of mass
Joseph Mirabel's avatar
Joseph Mirabel committed
295
      // and compute initial and final COM state
Joseph Mirabel's avatar
Joseph Mirabel committed
296
      pinocchio::CenterOfMassComputationPtr_t comComp = pinocchio::CenterOfMassComputation::
297
298
299
        create (robot_);
      comComp->add (robot_->waist()->parentJoint ());

Joseph Mirabel's avatar
Joseph Mirabel committed
300
301
      robot_->currentConfiguration (path->initial ());
      robot_->computeForwardKinematics ();
302
      comComp->compute (hpp::pinocchio::COM);
Joseph Mirabel's avatar
Joseph Mirabel committed
303
      vector_t comInitPos(2); comInitPos << comComp->com()[0], comComp->com()[1];
304
      value_type comH = comComp->com () [2];
Joseph Mirabel's avatar
Joseph Mirabel committed
305
306
      robot_->currentConfiguration (path->end ());
      robot_->computeForwardKinematics ();
307
      comComp->compute (hpp::pinocchio::COM);
Joseph Mirabel's avatar
Joseph Mirabel committed
308
309
310
      vector_t comEndPos(2); comEndPos << comComp->com()[0], comComp->com()[1];
      vector_t comVelocity (vector_t::Zero (2));

Joseph Mirabel's avatar
Joseph Mirabel committed
311
312
      value_type ankleShift = robot_->leftAnkle()->currentTransformation ().translation () [2];
      if (std::abs (ankleShift - robot_->rightAnkle()->currentTransformation ().translation () [2]) > 1e-6) {
313
        hppDout (error, "Left and right ankle are not at the same height. Difference is "
Joseph Mirabel's avatar
Joseph Mirabel committed
314
            << std::abs (ankleShift - robot_->rightAnkle()->currentTransformation ().translation () [2]));
315
      }
316
      pg_ = SplineBased::create (comH);
317
      pg_->defaultStepHeight (defaultStepHeight_);
318
319
      bool valid = false;
      std::size_t nbTries = 0;
Joseph Mirabel's avatar
Joseph Mirabel committed
320
      PathVectorPtr_t opted, lastCollidingOpted, lastProjFailed;
321

322
323
      // Build time sequence
      std::size_t p = footPrints_.size ();
324
      Times_t times = buildInitialTimes (p);
325

Joseph Mirabel's avatar
Joseph Mirabel committed
326
      while (!valid && nbTries < 20) {
327
328
        // Build time parameterization of initial path
        // There are two footprints per step parameter
329
330
        // times.size() := T = 2*p - 2
        // stepParameters_.size() := S = p - 2
331
        PiecewiseAffine param;
332
        // s0    s0          s1        s2
333
        // |     |           |         |
334
335
        // t0 - t1 - t2 - t3 - t4 - t5 - t6
        //       s[-3]           s[-2]             s[-1]   s[-1]
336
        //       |               |                 |       |
337
338
339
340
        // t[-7] - t[-6] - t[-5] - t[-4] - t[-3] - t[-2] - t[-1]
        param.addPair (times[0], stepParameters_ [0]);
        param.addPair (times[1], stepParameters_ [0]);
        for (std::size_t i=1; i<p-3; ++i) {
341
          param.addPair (.5*(times [2*i+1] + times [2*i+2]),
342
              stepParameters_ [i]);
343
        }
344
        param.addPair (times [2*p-4], stepParameters_ [p-3]);
345
346
347
348
        param.addPair (times [2*p-3], stepParameters_ [p-3]);

        pg_->timeSequence (times);
        pg_->footPrintSequence (footPrints_);
Joseph Mirabel's avatar
Joseph Mirabel committed
349
350
        pg_->setInitialComState (comInitPos, comVelocity);
        pg_->setEndComState (comEndPos, comVelocity);
351
352
        CubicBSplinePtr_t com = pg_->solve ();

353
        value_type failureP = -1;
354
        print_steps (param.pairs_, footPrints_, com, nbTries);
355
        opted = generateOptimizedPath (path, param, com, comH, ankleShift,
356
357
            failureP);
        valid = (failureP < 0);
Joseph Mirabel's avatar
Joseph Mirabel committed
358
359
360
        if (!valid) {
          lastProjFailed = opted;
        } else {
361
          core::PathValidationPtr_t pv = problem()->pathValidation ();
362
363
364
365
366
          PathPtr_t unused;
          core::PathValidationReportPtr_t report;
          valid = pv->validate (opted, false, unused, report);
          if (!valid) {
            assert (report);
Joseph Mirabel's avatar
Joseph Mirabel committed
367
            hppDout (info, *report);
368
            failureP = report->parameter;
Joseph Mirabel's avatar
Joseph Mirabel committed
369
            lastCollidingOpted = opted;
370
371
          }
        }
372
        if (!valid) {
Joseph Mirabel's avatar
Joseph Mirabel committed
373
374
375
          if (!narrowAtTime (failureP, path, param, times)) {
            hppDout(error, "Could not narrow trajectory at time " << failureP);
            if (lastCollidingOpted) return lastCollidingOpted;
Joseph Mirabel's avatar
Joseph Mirabel committed
376
377
            else if (lastProjFailed) return lastProjFailed;
            else return path;
Joseph Mirabel's avatar
Joseph Mirabel committed
378
          }
379
380
          // Prepare next iteration
          p = footPrints_.size ();
381
        }
382
        nbTries++; 
383
      }
Joseph Mirabel's avatar
Joseph Mirabel committed
384
385
      hppDout(info, "valid= " << valid); 
      hppDout(info, "nbTries= " << nbTries); 
386
      return opted;
387
    }
388
389
390
391
392
393
394
395
396
397
398
399

    FootPrint SmallSteps::footPrintAtParam (const PathPtr_t& p, value_type s, bool right) const
    {
      assert (robot_);
      Configuration_t q (robot_->configSize ());
      (*p) (q, s);
      robot_->currentConfiguration (q);
      robot_->computeForwardKinematics ();
      JointPtr_t a;
      if (right) a = robot_->rightAnkle ();
      else       a = robot_->leftAnkle ();
      value_type xf, yf, cf, sf;
Joseph Mirabel's avatar
Joseph Mirabel committed
400
401
402
403
      xf = a->currentTransformation ().translation () [0];
      yf = a->currentTransformation ().translation () [1];
      cf = a->currentTransformation ().rotation () (0,0);
      sf = a->currentTransformation ().rotation () (1,0);
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
      return FootPrint (xf, yf, cf, sf);
    }

    SmallSteps::Times_t SmallSteps::buildInitialTimes (const std::size_t& p)
    {
        Times_t times (2*p - 2);
        times [0] = 0.;
        value_type t = defaultInitializationTime_;
        times [1] = t;
        for (std::size_t i=1; i < p-1; ++i) {
          t += defaultSingleSupportTime_;
          times [2*i] = t;
          t += defaultDoubleSupportTime_;
          times [2*i+1] = t;
        }
        times [2*p-3] = times [2*p-4] + defaultInitializationTime_;
        return times;
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
422
423

    PathVectorPtr_t SmallSteps::generateOptimizedPath (PathVectorPtr_t path,
424
        const SmallSteps::PiecewiseAffine& param, CubicBSplinePtr_t com,
425
426
        value_type comHeight, value_type ankleShift,
        value_type& failureParameter)
Joseph Mirabel's avatar
Joseph Mirabel committed
427
428
    {
      assert (robot_);
429
      const TimeToParameterMap_t& TTP = param.pairs_;
Joseph Mirabel's avatar
Joseph Mirabel committed
430
431

      // Create the time varying equation for COM
Joseph Mirabel's avatar
Joseph Mirabel committed
432
      pinocchio::CenterOfMassComputationPtr_t comComp = pinocchio::CenterOfMassComputation::
Joseph Mirabel's avatar
Joseph Mirabel committed
433
        create (robot_);
434
      comComp->add (robot_->waist()->parentJoint ());
Joseph Mirabel's avatar
Joseph Mirabel committed
435
      PointComFunctionPtr_t comFunc = PointComFunction::create ("COM-walkgen",
436
          robot_, PointCom::create (comComp));
437
      constraints::ImplicitPtr_t comEq = constraints::Implicit::create
438
        (comFunc, 3 * constraints::Equality);
Joseph Mirabel's avatar
Joseph Mirabel committed
439
440
441
442
      TimeDependant comEqTD (comEq, RightHandSideFunctorPtr_t (new CubicBSplineToCom (com, comHeight)));

      // Create an time varying equation for each foot.
      JointFrameFunctionPtr_t leftFunc = JointFrameFunction::create ("left-foot-walkgen",
443
          robot_, JointFrame::create (robot_->leftAnkle ()));
444
445
      constraints::ImplicitPtr_t leftEq = constraints::Implicit::create
        (leftFunc, 6 * constraints::Equality);
Joseph Mirabel's avatar
Joseph Mirabel committed
446
447
448
449
450
      TimeDependant leftEqTD (leftEq, RightHandSideFunctorPtr_t
          (new FootPathToFootPos (pg_->leftFoot (), pg_->leftFootTrajectory (), ankleShift))
          );

      JointFrameFunctionPtr_t rightFunc = JointFrameFunction::create ("right-foot-walkgen",
451
          robot_, JointFrame::create (robot_->rightAnkle ()));
452
453
      constraints::ImplicitPtr_t rightEq = constraints::Implicit::create
        (rightFunc, 6 * constraints::Equality);
Joseph Mirabel's avatar
Joseph Mirabel committed
454
455
456
457
      TimeDependant rightEqTD (rightEq, RightHandSideFunctorPtr_t
          (new FootPathToFootPos (pg_->rightFoot (), pg_->rightFootTrajectory (), ankleShift))
          );

458
459
460
461
      HandConstraintData leftHand(leftHand_), rightHand(rightHand_);
      leftHand .setup(robot_, robot_-> leftWrist(), path, param);
      rightHand.setup(robot_, robot_->rightWrist(), path, param);

462
463
464
465
      // TODO: handle the case where each subpath has its own constraints.
      ConstraintSetPtr_t constraints = copyNonStabilityConstraints
        (path->pathAtRank (0)->constraints());
      ConfigProjectorPtr_t proj = constraints->configProjector ();
Joseph Mirabel's avatar
Joseph Mirabel committed
466
467
468
      proj->add (comEq);
      proj->add (leftEq);
      proj->add (rightEq);
469
470
      leftHand .add(proj);
      rightHand.add(proj);
Joseph Mirabel's avatar
Joseph Mirabel committed
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485

      PathVectorPtr_t opted = PathVector::create (path->outputSize (),
          path->outputDerivativeSize ());

      Configuration_t qi (robot_->configSize()), qe (robot_->configSize());
      value_type lastT = -1;
      for (TimeToParameterMap_t::const_iterator it = TTP.begin ();
          it != TTP.end (); ++it) {
        value_type T = it->first;
        value_type S = it->second;
        (*path) (qe, S);
        {
          comEqTD.rhsAbscissa (T);
          leftEqTD.rhsAbscissa (T);
          rightEqTD.rhsAbscissa (T);
486
487
          leftHand .updateAbscissa (T);
          rightHand.updateAbscissa (T);
Joseph Mirabel's avatar
Joseph Mirabel committed
488
        }
489
        // TODO proj->updateRightHandSide ();
Joseph Mirabel's avatar
Joseph Mirabel committed
490
        bool success = constraints->apply (qe);
491
492
        if (!success) {
          failureParameter = T;
Joseph Mirabel's avatar
Joseph Mirabel committed
493
          hppDout (error, "Failed to project a configuration at time " << T);
Joseph Mirabel's avatar
Joseph Mirabel committed
494
          return opted;
495
        }
Joseph Mirabel's avatar
Joseph Mirabel committed
496
497
498
499
        if (lastT >= 0) {
          TimeDependantPathPtr_t p = TimeDependantPath::create
            (StraightPath::create (robot_,qi,qe,T - lastT), constraints);
          p->add (comEqTD); p->add (leftEqTD); p->add (rightEqTD);
500
          leftHand .add(p); rightHand.add(p);
Joseph Mirabel's avatar
Joseph Mirabel committed
501
502
503
504
505
506
507
508
509
510
511
512
513
          p->setAffineTransform (1, lastT);
          Configuration_t qtest = qi;
          assert ((*p) (qtest, 0));
          assert ((qtest - qi).isZero ());
          assert ((*p) (qtest, T - lastT));
          assert ((qtest - qe).isZero ());
          opted->appendPath (p);
        }
        lastT = T;
        qi = qe;
      }
      return opted;
    }
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529

    ConstraintSetPtr_t SmallSteps::copyNonStabilityConstraints (ConstraintSetPtr_t orig) const
    {
      ConstraintSetPtr_t ret =
        ConstraintSet::create (robot_, "stepper-walkgen-set");
      ConfigProjectorPtr_t oldProj = orig->configProjector ();
      assert (oldProj && "There is no ConfigProjector in the path so I do not "
          "know what error threshold and max iteration to use.");
      ConfigProjectorPtr_t newProj =
        ConfigProjector::create (robot_, "stepper-walkgen",
          oldProj->errorThreshold (), oldProj->maxIterations ());

      ret->addConstraint (newProj);

      /// Copy the numerical constraints
      core::NumericalConstraints_t nc = oldProj->numericalConstraints ();
530
      //TODO core::IntervalsContainer_t pd = oldProj->passiveDofs ();
531
      core::NumericalConstraints_t::const_iterator it;
532
      // TODO core::IntervalsContainer_t::const_iterator itPdofs = pd.begin ();
533
534
      for (it = nc.begin (); it != nc.end (); ++it) {
        if ((*it)->function().context () != STABILITY_CONTEXT) {
535
536
          // TODO newProj->add (*it, *itPdofs);
          newProj->add (*it);
537
        }
538
        // TODO itPdofs++;
539
      }
540
      hppDout (warning, "Passive dofs are not copied anymore.");
541
542
543
544
545
546
547
548
      /// Copy the other constraints
      for (core::Constraints_t::const_iterator it = orig->begin ();
          it != orig->end (); ++it) {
        if (*it != oldProj)
          ret->addConstraint (*it);
      }
      return ret;
    }
549

Joseph Mirabel's avatar
Joseph Mirabel committed
550
    bool SmallSteps::narrowAtTime (const value_type& invalid_time, const PathPtr_t& path,
Florent Lamiraux's avatar
Florent Lamiraux committed
551
        const PiecewiseAffine&, Times_t& times)
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
    {
      typedef std::vector <value_type> SPs_t; // Step parameters
      enum Phase {
        FIRST_INIT,
        FIRST_SINGLE_SUPPORT,
        MIDDLE_PHASE,
        LAST_SINGLE_SUPPORT,
        LAST_INIT
      };

      // Find in which phase we are
      Times_t::iterator _time =
        std::lower_bound (times.begin(), times.end(), invalid_time);
      // i_time >= 1
      const std::size_t i_time = std::distance (times.begin(), _time);
      const std::size_t nb_time = times.size();
      Phase phase = MIDDLE_PHASE;
Joseph Mirabel's avatar
Joseph Mirabel committed
569
570
571
572
      if (!(i_time > 0 && i_time <= nb_time)) {
        hppDout (error, "Malformed time sequence");
        return false;
      }
573
574
575
576
577
578
579
580
      if      (i_time == 0) throw std::runtime_error ("Error");
      else if (i_time == 1)             phase = FIRST_INIT;
      else if (i_time == 2)             phase = FIRST_SINGLE_SUPPORT;
      else if (i_time == nb_time - 1)   phase = LAST_SINGLE_SUPPORT;
      else if (i_time == nb_time)       phase = LAST_INIT;
      else                              phase = MIDDLE_PHASE;

      // Find step parameter before and after collision
581
582
      SPs_t::iterator _step_After  = stepParameters_.begin (),
                      _step_Before = stepParameters_.begin ();
583
584
585
586
587
588
589
      switch (phase) {
        case FIRST_INIT:
        case FIRST_SINGLE_SUPPORT:
          _step_Before = _step_After = stepParameters_.begin ();
          std::advance (_step_After, 1);
          break;
        case MIDDLE_PHASE:
590
591
          std::advance (_step_Before, (i_time - 3) / 2);
          std::advance (_step_After,  std::min((i_time + 1) / 2, stepParameters_.size() - 1));
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
          break;
        case LAST_SINGLE_SUPPORT:
        case LAST_INIT:
          // Untested
          _step_Before = _step_After = stepParameters_.end ();
          std::advance (_step_Before, -2);
          std::advance (_step_After , -1);
          break;
      }
      // First step parameter to update
      const std::size_t ib_sp = std::distance (stepParameters_.begin (), _step_Before) + 1;
      // First foot print to update
      const std::size_t ib_fp = ib_sp + 1;
      // Time between the unchanged steps
      const value_type stepUnchanged = (*_step_After - *_step_Before );
      assert (stepUnchanged > 0);

      // Update the step parameters and footprints
Joseph Mirabel's avatar
Joseph Mirabel committed
610
      FootPrints_t::iterator _FP_Before = footPrints_.begin (); std::advance (_FP_Before, ib_sp);
611
612
613
614
615
616
617
618
619
      std::vector<bool>::iterator _FPiR_Before = footPrintsIsRight_.begin (); std::advance (_FPiR_Before, ib_fp);

      // Add two steps
      std::vector<bool> newFPiRs(2);
      SPs_t newSPs(2);
      footPrintsIsRight_.insert (_FPiR_Before + 1, newFPiRs.begin (), newFPiRs.end ());
      stepParameters_.insert (_step_Before + 1, newSPs.begin (), newSPs.end ());

      const std::size_t nbStep = std::distance(_step_Before, _step_After);
620
      const value_type step = stepUnchanged / (value_type) ( nbStep + 2);
621
622
623
624
625
626
      for (std::size_t i = 0; i < nbStep + 1; ++i)
        stepParameters_[ib_sp + i] = stepParameters_[ib_sp - 1 + i] + step;
      if (std::abs (stepParameters_[ib_sp + nbStep + 1] - stepParameters_[ib_sp + nbStep] - step) > 1e-6) {
        hppDout (error, "Step parameters are not consistent: "
            << std::abs (stepParameters_[ib_sp + nbStep + 1] - stepParameters_[ib_sp + nbStep] - step));
      }
Joseph Mirabel's avatar
Joseph Mirabel committed
627
      for (std::size_t i = 0; i < std::min(nbStep + 2, footPrintsIsRight_.size()); ++i)
628
        footPrintsIsRight_[ib_fp + i] = !footPrintsIsRight_[ib_fp + i - 1];
Joseph Mirabel's avatar
Joseph Mirabel committed
629
630
631
632
      for (std::size_t i = 1; i < footPrintsIsRight_.size(); ++i) {
        if (footPrintsIsRight_[i] == footPrintsIsRight_[i-1]) {
          hppDout (error, "Foot step should alternate between right and left.");
        }
Joseph Mirabel's avatar
Joseph Mirabel committed
633
      }
Joseph Mirabel's avatar
Joseph Mirabel committed
634
635
636
637
        if (!isStrictlyIncreasing(stepParameters_)) {
          hppDout(error, "Step parameter sequence should be strictly increasing.");
          return false;
        }
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666

      FootPrints_t newFPs (2, footPrints_ [ib_fp]);
      footPrints_.insert (_FP_Before + 1, newFPs.begin (), newFPs.end ());
      for (std::size_t i = 0; i < nbStep + 1; ++i) {
        footPrints_[ib_fp + i] = footPrintAtParam
          (path, stepParameters_[ib_sp + i], footPrintsIsRight_[ib_fp + i]);
      }

      // Update times
      // Linearly interpolate between the times corresponding to the two last steps.
      // ib_times is the first DD time that should decrease.
      Times_t newTimes (4);
      const value_type ratioSSDS = defaultSingleSupportTime_ / defaultDoubleSupportTime_;
      const value_type ratioIDS = defaultInitializationTime_ / defaultDoubleSupportTime_;

      switch (phase) {
        case FIRST_INIT:
        case FIRST_SINGLE_SUPPORT:
          {
            const value_type interval = times[4] - times[0];
            Times_t::iterator itTimesB = times.begin ();
            times.insert (itTimesB + 1, newTimes.begin(), newTimes.end ());
            // interval  = newIT + 4 * newSST + 3 * newDST
            // ratioSSDS = newSST / newDST
            // ratioIDS  = newIT / newDST
            const value_type newDST = interval / ( ratioIDS + 4 * ratioSSDS + 3 );
            const value_type newSST = newDST * ratioSSDS;
            const value_type newIT  = newDST * ratioIDS;
            times[1] = newIT; times[2] = newIT + newSST;
Joseph Mirabel's avatar
Joseph Mirabel committed
667
            const value_type last = times[2*3 + 2];
668
669
670
671
            for (std::size_t i = 1; i < 4; ++i) {
              times[2*i + 1] = times[2*i    ] + newDST;
              times[2*i + 2] = times[2*i + 1] + newSST; // for i == 3, this should do nothing.
            }
672
673
674
675
            if (!isApprox(last, times[2*3 + 2])) {
              hppDout(warning, "Inconsistent time rescaling. Error is " << (last - times[2*3 + 2]));
              hppDout(warning, "newSST= " << newSST << ", newDST= " << newDST << " and newIT= " << newIT);
            }
676
677
678
679
680
681
682
683
684
685
686
687
688
689
          }
          break;
        case MIDDLE_PHASE:
          {
            std::size_t ib_times = 2 * ib_sp;
            const value_type interval = times[ib_times+3] - times[ib_times-1];
            Times_t::iterator itTimesB = times.begin (); std::advance (itTimesB, ib_times);
            times.insert (itTimesB + 1, newTimes.begin(), newTimes.end ());
            // const value_type interval = times[ib_times+3] - times[ib_times-1];

            // interval = 4 * newSST + 4 * newDST
            // ratio    = newSST / newDST
            value_type newDST = ( interval / 4 ) / ( 1 + 1 * ratioSSDS );
            value_type newSST = ( interval / 4 ) - newDST;
Joseph Mirabel's avatar
Joseph Mirabel committed
690
            const value_type last = times[ib_times + 2*3 + 1];
691
692
693
694
            for (std::size_t i = 0; i < 4; ++i) {
              times[ib_times + 2*i    ] = times[ib_times + 2*i - 1] + newSST;
              times[ib_times + 2*i + 1] = times[ib_times + 2*i    ] + newDST; // for i == 3, this should do nothing.
            }
Joseph Mirabel's avatar
Joseph Mirabel committed
695
696
697
698
            if (!isApprox(last, times[ib_times + 2*3 + 1])) {
              hppDout(warning, "Inconsistent time rescaling. Error is " << (last - times[ib_times + 2*3 + 1]));
              hppDout(warning, "newSST= " << newSST << " and newDST= " << newDST);
            }
699
700
701
702
703
704
705
706
707
708
709
710
711
712
            // assert (times[ib_times + 1] == newTimes[3] + newSST)
          }
          break;
        case LAST_SINGLE_SUPPORT:
        case LAST_INIT:
          // Untested
          {
            const value_type interval = times[nb_time - 5] - times[nb_time - 1];
            times.insert (times.end () - 1, newTimes.begin(), newTimes.end ());
            // interval  = newIT + 4 * newSST + 3 * newDST
            // ratioSSDS = newSST / newDST
            // ratioIDS  = newIT / newDST
            const value_type newDST = interval / ( ratioIDS + 4 * ratioSSDS + 3 );
            const value_type newSST = newDST * ratioSSDS;
Florent Lamiraux's avatar
Florent Lamiraux committed
713
#ifndef NDEBUG
714
            const value_type newIT  = newDST * ratioIDS;
Florent Lamiraux's avatar
Florent Lamiraux committed
715
#endif
716
717
718
719
720
721
            const std::size_t i_start = nb_time - 9;
            for (std::size_t i = 0; i < 3; ++i) {
              times[i_start + 2*i + 1] = times[i_start + 2*i    ] + newSST;
              times[i_start + 2*i + 2] = times[i_start + 2*i + 1] + newDST; // for i == 3, this should do nothing.
            }
            times[nb_time - 2] = times[nb_time-3] + newSST;
Joseph Mirabel's avatar
Joseph Mirabel committed
722
723
724
725
726
            assert(
                isApprox(times[nb_time - 1],
                  times[nb_time-2] - newIT)
                );
            // times[nb_time - 1] = times[nb_time-2] + newIT; // Should be useless
727
728
729
          }
          break;
      }
Joseph Mirabel's avatar
Joseph Mirabel committed
730
731
732
733
734
735

      if (!isStrictlyIncreasing(times)) {
        hppDout(error, "Time sequence should be strictly increasing.");
        return false;
      }
      return true;
736
    }
737
738
  } // wholebodyStep
} // namespace hpp