simple-time-parameterization.cc 11.1 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
// Copyright (c) 2017, 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/>.

#include <hpp/core/path-optimization/simple-time-parameterization.hh>

19
20
#include <limits>

Joseph Mirabel's avatar
Joseph Mirabel committed
21
22
#include <pinocchio/multibody/model.hpp>

23
24
25
26
27
28
29
#include <hpp/pinocchio/configuration.hh>
#include <hpp/pinocchio/device.hh>
#include <hpp/pinocchio/liegroup.hh>
#include <hpp/core/interpolated-path.hh>
#include <hpp/core/path-vector.hh>
#include <hpp/core/problem.hh>
#include <hpp/core/straight-path.hh>
30
#include <hpp/core/time-parameterization/polynomial.hh>
31
32
33
34

namespace hpp {
  namespace core {
    namespace pathOptimization {
35
      using timeParameterization::Polynomial;
Guilhem Saurel's avatar
Guilhem Saurel committed
36
      using std::isfinite;
37
38
39
40

      namespace {
        TimeParameterizationPtr_t computeTimeParameterizationFirstOrder (
            const value_type& s0, const value_type& s1, const value_type& B,
41
            value_type& T)
42
43
44
45
46
        {
          vector_t a(2);
          T = (s1 - s0) / B;
          a[0] = s0;
          a[1] = B;
47
48
          hppDout (info, "Time parametrization returned " << a.transpose()
              << ", " << T);
49
50
51
52
          if (!isfinite(T) || !a.allFinite() || a.hasNaN()) {
            HPP_THROW(std::logic_error, "Invalid time parameterization "
                "coefficients: " << a.transpose());
          }
53
54
55
56
57
          return TimeParameterizationPtr_t (new Polynomial (a));
        }

        TimeParameterizationPtr_t computeTimeParameterizationThirdOrder (
            const value_type& s0, const value_type& s1, const value_type& B,
58
            value_type& T)
59
60
        {
          vector_t a(4);
61
          T = 3 * (s1 - s0) / (2 * B);
62
63
64
65
          a[0] = s0;
          a[1] = 0;
          a[2] = 3 * (s1 - s0) / (T * T);
          a[3] = - 2 * a[2] / (3 * T);
66
67
          hppDout (info, "Time parametrization returned " << a.transpose()
              << ", " << T);
68
69
70
71
          if (!isfinite(T) || !a.allFinite() || a.hasNaN()) {
            HPP_THROW(std::logic_error, "Invalid time parameterization "
                "coefficients: " << a.transpose());
          }
72
73
          return TimeParameterizationPtr_t (new Polynomial (a));
        }
74

75
76
        TimeParameterizationPtr_t computeTimeParameterizationFifthOrder (
            const value_type& s0, const value_type& s1, const value_type& B,
77
            const value_type& C, value_type& T)
78
79
        {
          vector_t a(6);
80
81
82
83
84
85
86
87
88
          if (C > 0) {
            T = std::max(
                15 * (s1 - s0) / (8 * B)                     , // Velocity limit
                sqrt((10./std::sqrt(3))) * sqrt((s1 - s0) / C) // Acceleration limit
                );
          } else {
            T = 15 * (s1 - s0) / (8 * B);
          }

89
90
91
92
93
94
95
96
97
98
99
          value_type Tpow = T*T*T;
          a[0] = s0;
          a[1] = 0;
          a[2] = 0;
          a[3] =  10 * (s1 - s0) / Tpow;
          Tpow *= T;
          a[4] = -15 * (s1 - s0) / Tpow;
          Tpow *= T;
          a[5] =   6 * (s1 - s0) / Tpow;
          hppDout (info, "Time parametrization returned " << a.transpose()
              << ", " << T);
100
101
102
103
          if (!isfinite(T) || !a.allFinite() || a.hasNaN()) {
            HPP_THROW(std::logic_error, "Invalid time parameterization "
                "coefficients: " << a.transpose());
          }
104
105
106
          return TimeParameterizationPtr_t (new Polynomial (a));
        }

107
        void checkTimeParameterization (const TimeParameterizationPtr_t tp,
108
            const size_type order, const interval_t sr, const value_type B, const value_type& C, const value_type T)
109
110
        {
          using std::fabs;
111
          const value_type thr = std::sqrt(Eigen::NumTraits<value_type>::dummy_precision());
112
113
          if (   fabs(tp->value(0) - sr.first) >= thr
              || fabs(tp->value(T) - sr.second) >= thr) {
114
115
116
            HPP_THROW(std::logic_error, "Interval of TimeParameterization result"
                " is not correct. Expected " << sr.first << ", " << sr.second <<
                ". Got " << tp->value(0) << ", " << tp->value(T));
117
          }
118
          if (order >= 1
119
120
              && (fabs(tp->derivative(0, 1)) > thr
              ||  fabs(tp->derivative(T, 1)) > thr
121
              ||  ((C <= 0) && fabs(tp->derivative(T/2, 1) - B) > thr))
122
             ) {
123
124
            HPP_THROW(std::logic_error,
                "Derivative of TimeParameterization are not correct:"
125
126
127
                << "\ntp->derivative(0, 1) = " << tp->derivative(0, 1)
                << "\ntp->derivative(T, 1) = " << tp->derivative(T, 1)
                << "\ntp->derivative(T/2, 1) - B = " << tp->derivative(T/2, 1) - B
128
129
                << "\nT = " << T
                << "\nB = " << B
130
                << "\nC = " << C
131
                );
132
          }
133
134
135
136
137
138
139
140
141
142
143
          if (order >= 2
              && (fabs(tp->derivative(0, 2)) > thr
              ||  fabs(tp->derivative(T, 2)) > thr)
             ) {
            HPP_THROW(std::logic_error,
                "Derivative of TimeParameterization are not correct:"
                << "\ntp->derivative(0, 2) = " << tp->derivative(0, 2)
                << "\ntp->derivative(T, 2) = " << tp->derivative(T, 2)
                << "\nT = " << T
                );
          }
144
        }
145
146
      }

147
148
149
150
151
152
153
154
      SimpleTimeParameterizationPtr_t SimpleTimeParameterization::create (const Problem& problem)
      {
        SimpleTimeParameterizationPtr_t ptr (new SimpleTimeParameterization(problem));
        return ptr;
      }

      PathVectorPtr_t SimpleTimeParameterization::optimize (const PathVectorPtr_t& path)
      {
155
156
157
        if (path->length() == 0) {
          return path;
        }
158
159
        const value_type infinity = std::numeric_limits<value_type>::infinity();

160
161
        const value_type safety = problem().getParameter("SimpleTimeParameterization/safety").floatValue();
        const size_type order = problem().getParameter("SimpleTimeParameterization/order").intValue();
162
        const value_type maxAcc = problem().getParameter("SimpleTimeParameterization/maxAcceleration").floatValue();
163
        if (order <= 1 && maxAcc > 0) {
164
165
          throw std::invalid_argument ("Maximum acceleration cannot be set when order is <= to 1. Please set parameter SimpleTimeParameterization/maxAcceleration to a negative value.");
        }
166
167
168
169
170
171

        // Retrieve velocity limits
        const DevicePtr_t& robot = problem().robot();
        vector_t ub ( robot->model().velocityLimit),
                 lb (-robot->model().velocityLimit),
                 cb ((ub + lb) / 2);
172
173
        for (size_type i=0; i < cb.size(); ++i) {
          if (std::isnan (cb [i])) {
174
175
176
177
178
            HPP_THROW(std::runtime_error,
              "in SimpleTimeParameterization::optimize:\n"
              << "  the velocities of the input device should be bounded\n"
              << "  velocity bounds at rank " << i << " are [" << lb[i]
              << ", " << ub[i] << "].");
179
180
          }
        }
Joseph Mirabel's avatar
Joseph Mirabel committed
181
182
        assert (cb.size() + robot->extraConfigSpace().dimension()
            == robot->numberDof());
183
184
185
186
187

        // The velocity must be in [lb, ub]
        ub = cb + safety * (ub - cb);
        lb = cb + safety * (lb - cb);

188
        // When ub or lb are NaN, set them to infinity.
189
190
        ub = (ub.array() == ub.array()).select(ub,  infinity);
        lb = (lb.array() == lb.array()).select(lb, -infinity);
191

192
193
194
195
196
197
198
199
200
201
202
203
204
        hppDout (info, "Lower velocity bound :" << lb.transpose());
        hppDout (info, "Upper velocity bound :" << ub.transpose());

        if (   ( ub.array() <= 0 ).any()
            && ( lb.array() >= 0 ).any())
          throw std::invalid_argument ("The case where zero is not an admissible velocity is not implemented.");

        PathVectorPtr_t input = PathVector::create(
            path->outputSize(), path->outputDerivativeSize());
        PathVectorPtr_t output = PathVector::create(
            path->outputSize(), path->outputDerivativeSize());
        path->flatten(input);

205
206
        vector_t v (robot->numberDof());
        vector_t v_inv (robot->numberDof());
207
208
        for (std::size_t i = 0; i < input->numberPaths(); ++i) {
          PathPtr_t p = input->pathAtRank(i);
209
          interval_t paramRange = p->paramRange();
210
          p->timeParameterization (TimeParameterizationPtr_t(), paramRange);
211
212
213
214
215
216
217

          // Compute B
          p->velocityBound (v, paramRange.first, paramRange.second);
          v_inv = (v.array() == 0).select(infinity, v.cwiseInverse());
          const value_type B = std::min(
              ( ub.cwiseProduct(v_inv)).minCoeff(),
              (-lb.cwiseProduct(v_inv)).minCoeff());
218
219
220
221
          if (B <= 0 || B != B) {
            HPP_THROW(std::runtime_error,"Invalid parametrization derivative "
                "velocity bound: " << B);
          }
222
223
224
225

          // Compute the polynom and total time
          value_type T;
          TimeParameterizationPtr_t tp;
226
227
228
229
230
231
232
233
          switch (order) {
            case 0:
              tp = computeTimeParameterizationFirstOrder (paramRange.first, paramRange.second, B, T);
              break;
            case 1:
              tp = computeTimeParameterizationThirdOrder (paramRange.first, paramRange.second, B, T);
              break;
            case 2:
234
              tp = computeTimeParameterizationFifthOrder (paramRange.first, paramRange.second, B, maxAcc, T);
235
236
237
238
239
              break;
            default:
              throw std::invalid_argument ("Parameter SimpleTimeParameterization/order should be in { 0, 1, 2 }");
              break;
          }
240

241
          checkTimeParameterization (tp, order, paramRange, B, maxAcc, T);
242

243
244
          PathPtr_t pp = p->copy();
          pp->timeParameterization (tp, interval_t (0, T));
245
246

          output->appendPath (pp);
247
248
249
250
251
252
        }
        return output;
      }

      SimpleTimeParameterization::SimpleTimeParameterization (const Problem& problem):
        PathOptimizer(problem) {}
253
254
255
256
257
258
259
260
261
262
263
264

      // ----------- Declare parameters ------------------------------------- //

      HPP_START_PARAMETER_DECLARATION(SimpleTimeParameterization)
      Problem::declareParameter(ParameterDescription (Parameter::FLOAT,
            "SimpleTimeParameterization/safety",
            "A scaling factor for the joint bounds.",
            Parameter(1.)));
      Problem::declareParameter(ParameterDescription (Parameter::INT,
            "SimpleTimeParameterization/order",
            "The desired continuity order.",
            Parameter((size_type)0)));
265
266
267
268
      Problem::declareParameter(ParameterDescription (Parameter::FLOAT,
            "SimpleTimeParameterization/maxAcceleration",
            "The maximum acceleration for each degree of freedom. Not considered if negative.",
            Parameter((value_type)-1)));
269
      HPP_END_PARAMETER_DECLARATION(SimpleTimeParameterization)
270
271
272
    } // namespace pathOptimization
  } // namespace core
} // namespace hpp