Directory: | ./ |
---|---|
File: | src/path-optimization/rs-time-parameterization.cc |
Date: | 2024-12-13 16:14:03 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 12 | 64 | 18.8% |
Branches: | 25 | 138 | 18.1% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2019 - 2024 CNRS | ||
3 | // | ||
4 | // Author: Florent Lamiraux | ||
5 | // | ||
6 | |||
7 | // Redistribution and use in source and binary forms, with or without | ||
8 | // modification, are permitted provided that the following conditions are | ||
9 | // met: | ||
10 | // | ||
11 | // 1. Redistributions of source code must retain the above copyright | ||
12 | // notice, this list of conditions and the following disclaimer. | ||
13 | // | ||
14 | // 2. Redistributions in binary form must reproduce the above copyright | ||
15 | // notice, this list of conditions and the following disclaimer in the | ||
16 | // documentation and/or other materials provided with the distribution. | ||
17 | // | ||
18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
19 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
20 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR | ||
21 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT | ||
22 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, | ||
23 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT | ||
24 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, | ||
25 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY | ||
26 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT | ||
27 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE | ||
28 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH | ||
29 | // DAMAGE. | ||
30 | |||
31 | #include <hpp/core/path-optimization/rs-time-parameterization.hh> | ||
32 | #include <hpp/core/path-vector.hh> | ||
33 | #include <hpp/core/problem.hh> | ||
34 | #include <path-optimization/reeds-shepp/piecewise-quadratic.hh> | ||
35 | |||
36 | namespace hpp { | ||
37 | namespace core { | ||
38 | namespace pathOptimization { | ||
39 | |||
40 | ✗ | RSTimeParameterizationPtr_t RSTimeParameterization::create( | |
41 | const ProblemConstPtr_t& problem) { | ||
42 | ✗ | RSTimeParameterization* pointer(new RSTimeParameterization(problem)); | |
43 | ✗ | return RSTimeParameterizationPtr_t(pointer); | |
44 | } | ||
45 | |||
46 | ✗ | PathVectorPtr_t RSTimeParameterization::optimize(const PathVectorPtr_t& path) { | |
47 | using reedsShepp::PiecewiseQuadratic; | ||
48 | using reedsShepp::PiecewiseQuadraticPtr_t; | ||
49 | value_type minLinVel_( | ||
50 | ✗ | problem() | |
51 | ✗ | ->getParameter("RSTimeParameterization/MinLinearVelocity") | |
52 | ✗ | .floatValue()); | |
53 | value_type maxLinVel_( | ||
54 | ✗ | problem() | |
55 | ✗ | ->getParameter("RSTimeParameterization/MaxLinearVelocity") | |
56 | ✗ | .floatValue()); | |
57 | value_type maxAngVel_( | ||
58 | ✗ | problem() | |
59 | ✗ | ->getParameter("RSTimeParameterization/MaxAngularVelocity") | |
60 | ✗ | .floatValue()); | |
61 | value_type linearAcceleration_( | ||
62 | ✗ | problem() | |
63 | ✗ | ->getParameter("RSTimeParameterization/LinearAcceleration") | |
64 | ✗ | .floatValue()); | |
65 | value_type linearDeceleration_( | ||
66 | ✗ | problem() | |
67 | ✗ | ->getParameter("RSTimeParameterization/LinearDeceleration") | |
68 | ✗ | .floatValue()); | |
69 | |||
70 | // Retrieve parameters from problem | ||
71 | |||
72 | ✗ | value_type eps(sqrt(std::numeric_limits<value_type>::epsilon())); | |
73 | PathVectorPtr_t flattenedPath( | ||
74 | ✗ | PathVector::create(path->outputSize(), path->outputDerivativeSize())); | |
75 | // Flatten input path in case it contains PathVector instances with | ||
76 | // ConstantCurvature instances. | ||
77 | ✗ | path->flatten(flattenedPath); | |
78 | PathVectorPtr_t result( | ||
79 | ✗ | PathVector::create(path->outputSize(), path->outputDerivativeSize())); | |
80 | |||
81 | // Start with zero velocity and end first segment with the | ||
82 | // minimal velocity | ||
83 | ✗ | value_type initVel(0), targetVel(minLinVel_), maxLinVel; | |
84 | value_type linearAcceleration, linearDeceleration; | ||
85 | // Loop over each constant curvature segment | ||
86 | ✗ | for (std::size_t i = 0; i < flattenedPath->numberPaths(); ++i) { | |
87 | // if last segment, end with 0 velocity | ||
88 | ✗ | targetVel = 0; | |
89 | ✗ | PathPtr_t p(flattenedPath->pathAtRank(i)->copy()); | |
90 | ✗ | if (p->length() > eps) { | |
91 | ✗ | value_type t0(p->timeRange().second); | |
92 | ✗ | vector_t v0(p->outputDerivativeSize()); | |
93 | ✗ | p->derivative(v0, t0, 1); | |
94 | ✗ | if (i != flattenedPath->numberPaths() - 1) { | |
95 | // If two consecutive segments are in the same direction (i.e. | ||
96 | // forward or backward), decelerate to minLinVel_ | ||
97 | ✗ | PathPtr_t next(flattenedPath->pathAtRank(i + 1)); | |
98 | ✗ | value_type t1(next->timeRange().first); | |
99 | ✗ | vector_t v1(next->outputDerivativeSize()); | |
100 | ✗ | next->derivative(v1, t1, 1); | |
101 | ✗ | if ((v1 - v0).head<2>().norm() < 1e-3) { | |
102 | ✗ | targetVel = minLinVel_; | |
103 | } | ||
104 | } | ||
105 | // Compute maximal linear velocity | ||
106 | ✗ | value_type curvature(fabs(v0[2])); | |
107 | hppDout(info, "curvature=" << curvature); | ||
108 | ✗ | if (curvature * maxLinVel_ < maxAngVel_) { | |
109 | // Sature linear velocity | ||
110 | ✗ | maxLinVel = maxLinVel_; | |
111 | ✗ | linearAcceleration = linearAcceleration_; | |
112 | ✗ | linearDeceleration = linearDeceleration_; | |
113 | } else { | ||
114 | // Saturate angular velocity | ||
115 | ✗ | maxLinVel = maxAngVel_ / curvature; | |
116 | // reduce accelerations by the same ratio | ||
117 | // We could also add maximal angular acceleration parameter | ||
118 | ✗ | linearAcceleration = linearAcceleration_ * maxLinVel / maxLinVel_; | |
119 | ✗ | linearDeceleration = linearDeceleration_ * maxLinVel / maxLinVel_; | |
120 | } | ||
121 | // Create one parameterization per segment | ||
122 | ✗ | PiecewiseQuadraticPtr_t param(PiecewiseQuadratic::create(initVel)); | |
123 | ✗ | param->addSegments(p->length(), linearAcceleration, linearDeceleration, | |
124 | maxLinVel, targetVel); | ||
125 | ✗ | p->timeParameterization(param, param->definitionInterval()); | |
126 | ✗ | result->appendPath(p); | |
127 | // From second segment, start at minimal velocity | ||
128 | ✗ | initVel = minLinVel_; | |
129 | } | ||
130 | } | ||
131 | ✗ | return result; | |
132 | } | ||
133 | // ----------- Declare parameters ------------------------------------- // | ||
134 | |||
135 | 18 | HPP_START_PARAMETER_DECLARATION(RSTimeParameterization) | |
136 |
4/8✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 18 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 18 times.
✗ Branch 13 not taken.
|
18 | Problem::declareParameter(ParameterDescription( |
137 | Parameter::FLOAT, "RSTimeParameterization/MinLinearVelocity", | ||
138 | "Maximal linear velocity allowed when crossing a curvature discontinuity", | ||
139 |
1/2✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
|
36 | Parameter(.1))); |
140 |
4/8✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 18 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 18 times.
✗ Branch 13 not taken.
|
18 | Problem::declareParameter(ParameterDescription( |
141 | Parameter::FLOAT, "RSTimeParameterization/MaxLinearVelocity", | ||
142 |
1/2✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
|
36 | "Maximal linear velocity", Parameter(1.))); |
143 |
4/8✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 18 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 18 times.
✗ Branch 13 not taken.
|
18 | Problem::declareParameter(ParameterDescription( |
144 | Parameter::FLOAT, "RSTimeParameterization/MaxAngularVelocity", | ||
145 |
1/2✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
|
36 | "Maximal angular velocity", Parameter(1.))); |
146 |
4/8✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 18 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 18 times.
✗ Branch 13 not taken.
|
18 | Problem::declareParameter(ParameterDescription( |
147 | Parameter::FLOAT, "RSTimeParameterization/LinearAcceleration", | ||
148 |
1/2✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
|
36 | "Linear acceleration", Parameter(1.))); |
149 |
4/8✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 18 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 18 times.
✗ Branch 13 not taken.
|
18 | Problem::declareParameter(ParameterDescription( |
150 | Parameter::FLOAT, "RSTimeParameterization/LinearDeceleration", | ||
151 |
1/2✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
|
36 | "Linear deceleration", Parameter(1.))); |
152 | 18 | HPP_END_PARAMETER_DECLARATION(RSTimeParameterization) | |
153 | } // namespace pathOptimization | ||
154 | } // namespace core | ||
155 | } // namespace hpp | ||
156 |