GCC Code Coverage Report

Directory: ./
File: src/path-projector.cc
Date: 2025-03-10 11:18:21
Exec Total Coverage
Lines: 27 27 100.0%
Branches: 22 44 50.0%
Line Branch Exec Source
1 // Copyright (c) 2014, LAAS-CNRS
2 // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
3 //
4
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // 1. Redistributions of source code must retain the above copyright
10 // notice, this list of conditions and the following disclaimer.
11 //
12 // 2. Redistributions in binary form must reproduce the above copyright
13 // notice, this list of conditions and the following disclaimer in the
14 // documentation and/or other materials provided with the distribution.
15 //
16 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
22 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
26 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
27 // DAMAGE.
28
29 #include "hpp/core/path-projector.hh"
30
31 #include <hpp/core/distance.hh>
32 #include <hpp/core/path-vector.hh>
33 #include <hpp/core/problem.hh>
34 #include <hpp/core/steering-method.hh>
35 #include <hpp/util/pointer.hh>
36 #include <hpp/util/timer.hh>
37
38 namespace hpp {
39 namespace core {
40 namespace {
41 HPP_DEFINE_TIMECOUNTER(PathProjection);
42 }
43
44 12 PathProjector::PathProjector(const DistancePtr_t& distance,
45 const SteeringMethodPtr_t& steeringMethod,
46 12 bool keepSteeringMethodConstraints)
47 12 : steeringMethod_(steeringMethod->copy()), distance_(distance) {
48
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 12 times.
 12 assert(distance_ != NULL);
49
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 12 times.
 12 assert(steeringMethod_ != NULL);
50
1/2
✓ Branch 0 taken 12 times.
✗ Branch 1 not taken.
 12 if (!keepSteeringMethodConstraints) {
51 12 steeringMethod_->constraints(ConstraintSetPtr_t());
52 }
53 12 }
54
55 24 PathProjector::~PathProjector() {
56 HPP_DISPLAY_TIMECOUNTER(PathProjection);
57 HPP_RESET_TIMECOUNTER(PathProjection);
58 }
59
60 11338 value_type PathProjector::d(ConfigurationIn_t q1, ConfigurationIn_t q2) const {
61
2/4
✓ Branch 3 taken 11338 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 11338 times.
✗ Branch 7 not taken.
 11338 return (*distance_)(q1, q2);
62 }
63
64 1856360 PathPtr_t PathProjector::steer(ConfigurationIn_t q1,
65 ConfigurationIn_t q2) const {
66
2/4
✓ Branch 3 taken 1856360 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1856360 times.
✗ Branch 7 not taken.
 3712720 PathPtr_t result((*steeringMethod_)(q1, q2));
67 // In the case of hermite path, we want the paths to be constrained.
68 // assert (!result->constraints ());
69 1856360 return result;
70 }
71
72 192 bool PathProjector::apply(const PathPtr_t& path, PathPtr_t& proj) const {
73 HPP_START_TIMECOUNTER(PathProjection);
74 192 bool ret = impl_apply(path, proj);
75 HPP_STOP_TIMECOUNTER(PathProjection);
76 192 return ret;
77 }
78
79 // ----------- Declare parameters ------------------------------------- //
80
81 18 HPP_START_PARAMETER_DECLARATION(pathProjection)
82
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 18 Problem::declareParameter(
83
3/6
✓ 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.
 36 ParameterDescription(Parameter::FLOAT, "PathProjection/HessianBound",
84 "A bound on the norm of the hessian of the "
85 "constraints. Not considered if negative.",
86
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 36 Parameter(-1.)));
87
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 18 Problem::declareParameter(
88
3/6
✓ 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.
 36 ParameterDescription(Parameter::FLOAT, "PathProjection/MinimalDist",
89 "The threshold which stops the projection (distance "
90 "between consecutive interpolation points.)",
91
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 36 Parameter(1e-3)));
92
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(
93 Parameter::FLOAT, "PathProjection/RecursiveHermite/Beta",
94 "See \"Fast Interpolation and Time-Optimization on Implicit Contact "
95 "Submanifolds\" from Kris Hauser.",
96
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 36 Parameter(0.9)));
97 18 HPP_END_PARAMETER_DECLARATION(pathProjection)
98 } // namespace core
99 } // namespace hpp
100