path-projectors.cc 12.8 KB
Newer Older
Joseph Mirabel's avatar
Joseph Mirabel committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
// Copyright (c) 2016, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// This file is part of hpp-core.
// hpp-core 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-core 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-core. If not, see <http://www.gnu.org/licenses/>.

#define BOOST_TEST_MODULE pathProjector
18
#include <pinocchio/fwd.hpp>
Joseph Mirabel's avatar
Joseph Mirabel committed
19
20
21
22
23
#include <boost/test/included/unit_test.hpp>
#include <boost/test/test_case_template.hpp>
#include <boost/mpl/list.hpp>

#include <boost/shared_ptr.hpp>
24

Joseph Mirabel's avatar
Joseph Mirabel committed
25
26
27
28
29
30
31
32
33
34
35
// Boost version 1.54
// Cannot include boost CPU timers
// #include <boost/timer/timer.hpp>
// because the original timers are already included by
// the unit test framework
// #include <boost/timer.hh>

// Force benchmark output
#define HPP_ENABLE_BENCHMARK 1
#include <hpp/util/timer.hh>

36
#include <hpp/pinocchio/device.hh>
37
#include <hpp/pinocchio/urdf/util.hh>
Joseph Mirabel's avatar
Joseph Mirabel committed
38
39

#include <hpp/constraints/differentiable-function.hh>
40
41
#include <hpp/constraints/implicit.hh>

Joseph Mirabel's avatar
Joseph Mirabel committed
42
43
44
45
46
#include <hpp/core/straight-path.hh>
#include <hpp/core/config-projector.hh>
#include <hpp/core/constraint-set.hh>
#include <hpp/core/problem.hh>
#include <hpp/core/interpolated-path.hh>
47
#include <hpp/core/path/hermite.hh>
48

49
#include <hpp/core/steering-method/straight.hh>
50
#include <hpp/core/steering-method/hermite.hh>
Joseph Mirabel's avatar
Joseph Mirabel committed
51
52
53

#include <hpp/core/path-projector/global.hh>
#include <hpp/core/path-projector/progressive.hh>
54

55
#include <hpp/core/path-projector/recursive-hermite.hh>
56

57
using hpp::constraints::Implicit;
58
using hpp::constraints::EqualToZero;
59

Joseph Mirabel's avatar
Joseph Mirabel committed
60
using namespace hpp::core;
61
62
using namespace hpp::pinocchio;

Joseph Mirabel's avatar
Joseph Mirabel committed
63
64
DevicePtr_t createRobot ()
{
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
  std::string urdf ("<robot name='test'>"
      "<link name='link1'/>"
      "<link name='link2'/>"
      "<link name='link3'/>"
      "<joint name='tx' type='prismatic'>"
        "<parent link='link1'/>"
        "<child  link='link2'/>"
        "<limit effort='30' velocity='1.0' lower='-4' upper='4'/>"
      "</joint>"
      "<joint name='ty' type='prismatic'>"
        "<axis xyz='0 1 0'/>"
        "<parent link='link2'/>"
        "<child  link='link3'/>"
        "<limit effort='30' velocity='1.0' lower='-4' upper='4'/>"
      "</joint>"
      "</robot>"
      );
82

83
84
  DevicePtr_t robot = Device::create ("test");
  urdf::loadModelFromString (robot, 0, "", "anchor", urdf, "");
Joseph Mirabel's avatar
Joseph Mirabel committed
85
  return robot;
86
}
Joseph Mirabel's avatar
Joseph Mirabel committed
87
88
89
90
91
92
93
94
95
96
97
98
99

ConstraintSetPtr_t createConstraints (DevicePtr_t r)
{
  ConfigProjectorPtr_t proj =
    ConfigProjector::create (r, "Polynomial projector", 1e-4, 20);
  ConstraintSetPtr_t set = ConstraintSet::create (r, "Set");
  set->addConstraint (proj);
  return set;
}

class Polynomial : public DifferentiableFunction {
  public:
    Polynomial (DevicePtr_t robot) :
100
101
      DifferentiableFunction (robot->configSize(), robot->numberDof (),
                              LiegroupSpace::R1 (), "Polynomial"),
Joseph Mirabel's avatar
Joseph Mirabel committed
102
103
104
105
106
107
      coefs_ (vector_t::Ones (robot->configSize()))
    {}

    vector_t coefs_;

  protected:
108
      void impl_compute (LiegroupElementRef result, vectorIn_t argument) const {
109
        result.vector ()[0] = argument.cwiseProduct (argument).dot (coefs_) - 1;
Joseph Mirabel's avatar
Joseph Mirabel committed
110
111
112
113
114
115
      }
      void impl_jacobian (matrixOut_t jacobian, vectorIn_t arg) const {
        jacobian.row(0) = 2 * arg.cwiseProduct (coefs_);
      }
};

Guilhem Saurel's avatar
Guilhem Saurel committed
116
typedef std::shared_ptr<Polynomial> PolynomialPtr_t;
Joseph Mirabel's avatar
Joseph Mirabel committed
117

118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
/*
bool checkContinuity (PathPtr_t path) {
  const value_type stepPath = path->length () / (100 - 1);
  Configuration_t q = path->initial(), qq = path->initial();
  vector_t v1 (func->outputSize()), v2(func->outputSize());
  std::cerr << std::fixed << std::showpos << std::setprecision (4);
  const char* sep = "\t| ";
  for (std::size_t i = 0; i < 100; ++i) {
    if (!(*path) (q, (value_type)i * stepPath))
      std::cerr << "Could not project path at " << (value_type)i*stepPath
		<< "\n";
    if (!(*projection) (qq, (value_type) i * stepProj))
      std::cerr << "Could not project projection at "
		<< (value_type) i*stepProj << "\n";
    (*func) (v1, q);
    (*func) (v2, qq);
    std::cerr << q.transpose () << sep << v1
      << sep << qq.transpose () << sep << v2 << "\n";
  }

  // Analyse projection
  if (!HPP_DYNAMIC_PTR_CAST (InterpolatedPath, projection)) {
    std::cout << "Path is not an InterpolatedPath\n";
    std::cerr
      << projection->timeRange().first << sep << projection->initial ().transpose() << '\n'
      << projection->timeRange().first + projection->timeRange().second << sep << projection->end ().transpose() << '\n';
    return;
  }
  InterpolatedPath& p = *(HPP_DYNAMIC_PTR_CAST (InterpolatedPath, projection));
  typedef InterpolatedPath::InterpolationPoints_t InterpolationPoints_t;
  const InterpolationPoints_t& points = p.interpolationPoints ();
  std::cout << "InterpolatedPath: " << points.size() << " interpolation points.\n";
  for (InterpolationPoints_t::const_iterator it = points.begin();
      it != points.end(); ++it) {
    std::cerr << it->first << sep << it->second.transpose() << '\n';
  }
}
// */
Joseph Mirabel's avatar
Joseph Mirabel committed
156
157
158
159
void displayPaths (PathPtr_t path, PathPtr_t projection, DifferentiableFunctionPtr_t func) {
  const value_type stepPath = path->length () / (100 - 1);
  const value_type stepProj = projection->length () / (100 - 1);
  Configuration_t q = path->initial(), qq = path->initial();
160
  LiegroupElement v1 (func->outputSpace ()), v2(func->outputSpace());
Joseph Mirabel's avatar
Joseph Mirabel committed
161
162
163
  std::cerr << std::fixed << std::showpos << std::setprecision (4);
  const char* sep = "\t| ";
  for (std::size_t i = 0; i < 100; ++i) {
164
165
166
167
168
169
    if (!(*path) (q, (value_type)i * stepPath))
      std::cerr << "Could not project path at " << (value_type)i*stepPath
		<< "\n";
    if (!(*projection) (qq, (value_type) i * stepProj))
      std::cerr << "Could not project projection at "
		<< (value_type) i*stepProj << "\n";
170
171
    func->value (v1, q);
    func->value (v2, qq);
Joseph Mirabel's avatar
Joseph Mirabel committed
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
    std::cerr << q.transpose () << sep << v1
      << sep << qq.transpose () << sep << v2 << "\n";
  }

  // Analyse projection
  if (!HPP_DYNAMIC_PTR_CAST (InterpolatedPath, projection)) {
    std::cout << "Path is not an InterpolatedPath\n";
    std::cerr
      << projection->timeRange().first << sep << projection->initial ().transpose() << '\n'
      << projection->timeRange().first + projection->timeRange().second << sep << projection->end ().transpose() << '\n';
    return;
  }
  InterpolatedPath& p = *(HPP_DYNAMIC_PTR_CAST (InterpolatedPath, projection));
  typedef InterpolatedPath::InterpolationPoints_t InterpolationPoints_t;
  const InterpolationPoints_t& points = p.interpolationPoints ();
  std::cout << "InterpolatedPath: " << points.size() << " interpolation points.\n";
  for (InterpolationPoints_t::const_iterator it = points.begin();
      it != points.end(); ++it) {
    std::cerr << it->first << sep << it->second.transpose() << '\n';
  }
}

struct traits_circle {
  static DifferentiableFunctionPtr_t func (DevicePtr_t dev) {
    return PolynomialPtr_t (new Polynomial (dev));
  }
  static void make_conf (ConfigurationOut_t q1, ConfigurationOut_t q2,
      const int index) {
    switch (index) {
      case 0: q1 << 0, 1; q2 <<  1, 0; break;
      case 1: q1 << 1, 0; q2 << -1, 0; break;
      case 2: {
                double c = -0.99;
                q1 << 1, 0; q2 << c, sqrt(1 - c*c);
                break;
              }
      case 3: {
                double c = -0.9;
                q1 << 1, 0; q2 << c, sqrt(1 - c*c);
                break;
              }
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
      case 4: {
                double c = -0.85;
                q1 << 1, 0; q2 << c, sqrt(1 - c*c);
                break;
              }
      case 5: {
                double c = -0.8;
                q1 << 1, 0; q2 << c, sqrt(1 - c*c);
                break;
              }
      case 6: {
                double c = -0.75;
                q1 << 1, 0; q2 << c, sqrt(1 - c*c);
                break;
              }
      case 7: {
                double c = -0.7;
                q1 << 1, 0; q2 << c, sqrt(1 - c*c);
                break;
              }
Joseph Mirabel's avatar
Joseph Mirabel committed
233
234
235
    }
  }
  static const int NB_CONFS;
236
  static const value_type K;
Joseph Mirabel's avatar
Joseph Mirabel committed
237
238
239
240
241
242
243
244
245
246
  static const char* _func;
};
struct traits_parabola {
  static DifferentiableFunctionPtr_t func (DevicePtr_t dev) {
    PolynomialPtr_t parabola (new Polynomial (dev));
    parabola->coefs_(1) = 0;
    return parabola;
  }
  static void make_conf (ConfigurationOut_t q1, ConfigurationOut_t q2,
      const int index) {
247
248
249
250
251
    q1 <<  1,0; q2 << -1,(value_type)(2*index) / (NB_CONFS - 1);
    // switch (index) {
      // case 0: q1 <<  1,0; q2 << -1,0; break; // Should be really fast
      // case 1: q1 <<  1,0; q2 << -1,1; break; // Should be slower
    // }
Joseph Mirabel's avatar
Joseph Mirabel committed
252
253
  }
  static const int NB_CONFS;
254
  static const value_type K;
Joseph Mirabel's avatar
Joseph Mirabel committed
255
256
  static const char* _func;
};
257
const int traits_circle::NB_CONFS = 8;
258
const value_type traits_circle::K = 2.001;
Joseph Mirabel's avatar
Joseph Mirabel committed
259
const char* traits_circle::_func = "circle";
260
const int traits_parabola::NB_CONFS = 8;
261
const value_type traits_parabola::K = 2 * sqrt(2);
Joseph Mirabel's avatar
Joseph Mirabel committed
262
263
264
265
266
const char* traits_parabola::_func = "parabola";

struct traits_progressive {
  typedef pathProjector::Progressive Proj_t;
  typedef pathProjector::ProgressivePtr_t ProjPtr_t;
267
  typedef steeringMethod::Straight SM_t;
Joseph Mirabel's avatar
Joseph Mirabel committed
268
269
270
271
272
273
  static const value_type projection_step;
  static const char* _proj;
};
struct traits_global {
  typedef pathProjector::Global Proj_t;
  typedef pathProjector::GlobalPtr_t ProjPtr_t;
274
  typedef steeringMethod::Straight SM_t;
275
276
277
278
279
280
281
  static const value_type projection_step;
  static const char* _proj;
};
struct traits_hermite {
  typedef pathProjector::RecursiveHermite Proj_t;
  typedef pathProjector::RecursiveHermitePtr_t ProjPtr_t;
  typedef steeringMethod::Hermite SM_t;
Joseph Mirabel's avatar
Joseph Mirabel committed
282
283
284
285
  static const value_type projection_step;
  static const char* _proj;
};
const value_type traits_progressive::projection_step = 0.1;
286
287
const value_type traits_global     ::projection_step = 0.1;
const value_type traits_hermite    ::projection_step = 2;
Joseph Mirabel's avatar
Joseph Mirabel committed
288
const char* traits_progressive::_proj = "progressive";
289
290
const char* traits_global     ::_proj = "global";
const char* traits_hermite    ::_proj = "hermite";
Joseph Mirabel's avatar
Joseph Mirabel committed
291

292
293
294
struct traits_global_circle        : traits_global     , traits_circle   {};
struct traits_global_parabola      : traits_global     , traits_parabola {};
struct traits_progressive_circle   : traits_progressive, traits_circle   {};
Joseph Mirabel's avatar
Joseph Mirabel committed
295
struct traits_progressive_parabola : traits_progressive, traits_parabola {};
296
297
struct traits_hermite_circle       : traits_hermite    , traits_circle   {};
struct traits_hermite_parabola     : traits_hermite    , traits_parabola {};
Joseph Mirabel's avatar
Joseph Mirabel committed
298

299
300
typedef boost::mpl::list <  traits_global_circle
                          , traits_progressive_circle
301
                          , traits_hermite_circle
302
303
                          , traits_global_parabola
                          , traits_progressive_parabola
304
                          , traits_hermite_parabola
Joseph Mirabel's avatar
Joseph Mirabel committed
305
306
307
308
                          > test_types;

BOOST_AUTO_TEST_CASE_TEMPLATE (projectors, traits, test_types)
{
309
  DevicePtr_t dev = createRobot();
Joseph Mirabel's avatar
Joseph Mirabel committed
310
  BOOST_REQUIRE (dev);
Joseph Mirabel's avatar
Joseph Mirabel committed
311
  ProblemPtr_t problem = Problem::create(dev);
Joseph Mirabel's avatar
Joseph Mirabel committed
312
313
314

  ConstraintSetPtr_t c = createConstraints (dev);
  DifferentiableFunctionPtr_t func = traits::func (dev);
315
316
  c->configProjector ()->add (Implicit::create (func,
    ComparisonTypes_t(func->outputSpace()->nv(), EqualToZero)));
317
  problem->steeringMethod(traits::SM_t::create(problem));
Joseph Mirabel's avatar
Joseph Mirabel committed
318
  problem->steeringMethod ()->constraints (c);
Joseph Mirabel's avatar
Joseph Mirabel committed
319

320
321
  for (int c = 0; c < 2; ++c) {
    if (c == 0)
Joseph Mirabel's avatar
Joseph Mirabel committed
322
      problem->setParameter ("PathProjection/HessianBound", Parameter((value_type)-1));
323
    else
Joseph Mirabel's avatar
Joseph Mirabel committed
324
      problem->setParameter ("PathProjection/HessianBound", Parameter(traits::K));
325
326

    typename traits::ProjPtr_t projector =
327
      traits::Proj_t::create (problem, traits::projection_step);
328
329
330
331
332
333
334
335
336

    std::cout << "========================================\n";

    Configuration_t q1 (dev->configSize());
    Configuration_t q2 (dev->configSize());
    for (int i = 0; i < traits::NB_CONFS; ++i) {

      // HPP_DEFINE_TIMECOUNTER(projector);
      traits::make_conf (q1, q2, i);
Joseph Mirabel's avatar
Joseph Mirabel committed
337
      PathPtr_t path = (*problem->steeringMethod ()) (q1,q2);
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352

      PathPtr_t projection;
      // Averaging the projection
      bool success;
      for (int j = 0; j < 2; ++j) {
        // HPP_START_TIMECOUNTER (projector);
        success = projector->apply (path, projection);
        // HPP_STOP_TIMECOUNTER (projector);
        // HPP_DISPLAY_LAST_TIMECOUNTER (projector);
      }
      std::cout << traits::_proj << " " << traits::_func << ": projection of "
        << q1.transpose() << " -> " << q2.transpose() << " "
        << (success?"succeeded.":"failed.") << std::endl;
      // HPP_STREAM_TIMECOUNTER (std::cout, projector) << std::endl;
      // displayPaths (path, projection, func);
Joseph Mirabel's avatar
Joseph Mirabel committed
353
354
355
    }
  }
}