GCC Code Coverage Report

Directory: ./
File: src/path-optimization/rs-time-parameterization.cc
Date: 2024-08-10 11:29:48
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