GCC Code Coverage Report


Directory: ./
File: src/bi-rrt-planner.cc
Date: 2024-12-13 16:14:03
Exec Total Coverage
Lines: 0 75 0.0%
Branches: 0 224 0.0%

Line Branch Exec Source
1 //
2 // Copyright (c) 2014 CNRS
3 // Authors: Florent Lamiraux
4 //
5
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are
8 // met:
9 //
10 // 1. Redistributions of source code must retain the above copyright
11 // notice, this list of conditions and the following disclaimer.
12 //
13 // 2. Redistributions in binary form must reproduce the above copyright
14 // notice, this list of conditions and the following disclaimer in the
15 // documentation and/or other materials provided with the distribution.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
18 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
19 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
20 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
21 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
22 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
23 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
24 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
25 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
26 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
28 // DAMAGE.
29
30 #include <hpp/core/bi-rrt-planner.hh>
31 #include <hpp/core/config-projector.hh>
32 #include <hpp/core/configuration-shooter.hh>
33 #include <hpp/core/edge.hh>
34 #include <hpp/core/node.hh>
35 #include <hpp/core/path-validation.hh>
36 #include <hpp/core/path.hh>
37 #include <hpp/core/problem.hh>
38 #include <hpp/core/roadmap.hh>
39 #include <hpp/core/steering-method.hh>
40 #include <hpp/pinocchio/configuration.hh>
41 #include <hpp/pinocchio/device.hh>
42 #include <hpp/util/debug.hh>
43
44 namespace hpp {
45 namespace core {
46 using pinocchio::displayConfig;
47
48 BiRRTPlannerPtr_t BiRRTPlanner::createWithRoadmap(
49 const ProblemConstPtr_t& problem, const RoadmapPtr_t& roadmap) {
50 BiRRTPlanner* ptr = new BiRRTPlanner(problem, roadmap);
51 return BiRRTPlannerPtr_t(ptr);
52 }
53
54 BiRRTPlannerPtr_t BiRRTPlanner::create(const ProblemConstPtr_t& problem) {
55 BiRRTPlanner* ptr = new BiRRTPlanner(problem);
56 return BiRRTPlannerPtr_t(ptr);
57 }
58
59 BiRRTPlanner::BiRRTPlanner(const ProblemConstPtr_t& problem)
60 : PathPlanner(problem),
61 configurationShooter_(problem->configurationShooter()),
62 qProj_(problem->robot()->configSize()) {}
63
64 BiRRTPlanner::BiRRTPlanner(const ProblemConstPtr_t& problem,
65 const RoadmapPtr_t& roadmap)
66 : PathPlanner(problem, roadmap),
67 configurationShooter_(problem->configurationShooter()),
68 qProj_(problem->robot()->configSize()) {}
69
70 void BiRRTPlanner::init(const BiRRTPlannerWkPtr_t& weak) {
71 PathPlanner::init(weak);
72 weakPtr_ = weak;
73 }
74
75 PathPtr_t BiRRTPlanner::extendInternal(const SteeringMethodPtr_t& sm,
76 Configuration_t& qProj_,
77 const NodePtr_t& near,
78 const Configuration_t& target,
79 bool reverse) {
80 const ConstraintSetPtr_t& constraints(sm->constraints());
81 if (constraints) {
82 ConfigProjectorPtr_t configProjector(constraints->configProjector());
83 if (configProjector) {
84 configProjector->projectOnKernel(near->configuration(), target, qProj_);
85 } else {
86 qProj_ = target;
87 }
88 if (constraints->apply(qProj_)) {
89 return reverse ? (*sm)(qProj_, near->configuration())
90 : (*sm)(near->configuration(), qProj_);
91 } else {
92 return PathPtr_t();
93 }
94 }
95 return reverse ? (*sm)(target, near->configuration())
96 : (*sm)(near->configuration(), target);
97 }
98
99 /// One step of extension.
100 void BiRRTPlanner::startSolve() {
101 PathPlanner::startSolve();
102 startComponent_ = roadmap()->initNode()->connectedComponent();
103 for (NodeVector_t::const_iterator cit = roadmap()->goalNodes().begin();
104 cit != roadmap()->goalNodes().end(); ++cit) {
105 endComponents_.push_back((*cit)->connectedComponent());
106 }
107 }
108
109 void BiRRTPlanner::oneStep() {
110 PathPtr_t validPath, path;
111 PathValidationPtr_t pathValidation(problem()->pathValidation());
112 value_type distance;
113 NodePtr_t near, reachedNodeFromStart;
114 bool startComponentConnected(false), pathValidFromStart(false);
115 Configuration_t q_new;
116 // first try to connect to start component
117 Configuration_t q_rand;
118 configurationShooter_->shoot(q_rand);
119 near = roadmap()->nearestNode(q_rand, startComponent_, distance);
120 path = extendInternal(problem()->steeringMethod(), qProj_, near, q_rand);
121 if (path) {
122 PathValidationReportPtr_t report;
123 pathValidFromStart =
124 pathValidation->validate(path, false, validPath, report);
125 if (validPath) {
126 // Insert new path to q_near in roadmap
127 value_type t_final = validPath->timeRange().second;
128 if (t_final != path->timeRange().first) {
129 startComponentConnected = true;
130 q_new = validPath->end();
131 reachedNodeFromStart =
132 roadmap()->addNodeAndEdge(near, q_new, validPath);
133 }
134 }
135 }
136
137 // now try to connect to end components
138 for (std::vector<ConnectedComponentPtr_t>::const_iterator itcc =
139 endComponents_.begin();
140 itcc != endComponents_.end(); ++itcc) {
141 near = roadmap()->nearestNode(q_rand, *itcc, distance, true);
142 path =
143 extendInternal(problem()->steeringMethod(), qProj_, near, q_rand, true);
144 if (path) {
145 PathValidationReportPtr_t report;
146 if (pathValidation->validate(path, true, validPath, report) &&
147 pathValidFromStart) {
148 // we won, a path is found
149 roadmap()->addEdge(reachedNodeFromStart, near, validPath);
150 return;
151 } else if (validPath) {
152 value_type t_final = validPath->timeRange().second;
153 if (t_final != path->timeRange().first) {
154 Configuration_t q_newEnd = validPath->initial();
155 NodePtr_t newNode =
156 roadmap()->addNodeAndEdge(q_newEnd, near, validPath);
157 // now try to connect both nodes
158 if (startComponentConnected) {
159 path = (*(problem()->steeringMethod()))(q_new, q_newEnd);
160 if (path &&
161 pathValidation->validate(path, false, validPath, report)) {
162 roadmap()->addEdge(reachedNodeFromStart, newNode, path);
163 return;
164 }
165 }
166 }
167 }
168 }
169 }
170 }
171 } // namespace core
172 } // namespace hpp
173