GCC Code Coverage Report


Directory: ./
File: src/visibility-prm-planner.cc
Date: 2024-12-13 16:14:03
Exec Total Coverage
Lines: 0 84 0.0%
Branches: 0 154 0.0%

Line Branch Exec Source
1 //
2 // Copyright (c) 2014 CNRS
3 // Authors: Mylene Campana
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 <stdio.h>
31 #include <time.h>
32
33 #include <hpp/core/config-projector.hh>
34 #include <hpp/core/config-validations.hh>
35 #include <hpp/core/configuration-shooter.hh>
36 #include <hpp/core/connected-component.hh>
37 #include <hpp/core/path-validation.hh>
38 #include <hpp/core/problem.hh>
39 #include <hpp/core/roadmap.hh>
40 #include <hpp/core/visibility-prm-planner.hh>
41 #include <hpp/pinocchio/configuration.hh>
42 #include <hpp/pinocchio/device.hh>
43 #include <hpp/util/debug.hh>
44
45 namespace hpp {
46 namespace core {
47 using pinocchio::displayConfig;
48
49 VisibilityPrmPlannerPtr_t VisibilityPrmPlanner::createWithRoadmap(
50 const ProblemConstPtr_t& problem, const RoadmapPtr_t& roadmap) {
51 VisibilityPrmPlanner* ptr = new VisibilityPrmPlanner(problem, roadmap);
52 return VisibilityPrmPlannerPtr_t(ptr);
53 }
54
55 VisibilityPrmPlannerPtr_t VisibilityPrmPlanner::create(
56 const ProblemConstPtr_t& problem) {
57 VisibilityPrmPlanner* ptr = new VisibilityPrmPlanner(problem);
58 return VisibilityPrmPlannerPtr_t(ptr);
59 }
60
61 VisibilityPrmPlanner::VisibilityPrmPlanner(const ProblemConstPtr_t& problem)
62 : PathPlanner(problem) {}
63
64 VisibilityPrmPlanner::VisibilityPrmPlanner(const ProblemConstPtr_t& problem,
65 const RoadmapPtr_t& roadmap)
66 : PathPlanner(problem, roadmap) {}
67
68 void VisibilityPrmPlanner::init(const VisibilityPrmPlannerWkPtr_t& weak) {
69 PathPlanner::init(weak);
70 weakPtr_ = weak;
71 }
72
73 bool VisibilityPrmPlanner::visibleFromCC(const Configuration_t q,
74 const ConnectedComponentPtr_t cc) {
75 PathPtr_t validPart;
76 bool found = false;
77 value_type length = std::numeric_limits<value_type>::infinity();
78 PathValidationPtr_t pathValidation(problem()->pathValidation());
79 SteeringMethodPtr_t sm(problem()->steeringMethod());
80 RoadmapPtr_t r(roadmap());
81 DelayedEdge_t delayedEdge;
82
83 for (NodeVector_t::const_iterator n_it = cc->nodes().begin();
84 n_it != cc->nodes().end(); ++n_it) {
85 if (nodeStatus_[*n_it]) { // only iterate on guard nodes
86 Configuration_t qCC = (*n_it)->configuration();
87 PathPtr_t path = (*sm)(q, qCC);
88 PathValidationReportPtr_t report;
89 if (path && pathValidation->validate(path, false, validPart, report)) {
90 // q and qCC see each other
91 if (path->length() < length) {
92 length = path->length();
93 // Save shortest edge
94 delayedEdge = DelayedEdge_t(*n_it, q, path->reverse());
95 }
96 found = true;
97 }
98 }
99 }
100 if (found) {
101 // Store shortest delayed edge in list
102 delayedEdges_.push_back(delayedEdge);
103 return true;
104 } else
105 return false;
106 }
107
108 void VisibilityPrmPlanner::applyConstraints(const Configuration_t& qFrom,
109 const Configuration_t& qTo,
110 Configuration_t& qout) {
111 ConstraintSetPtr_t constraints(problem()->constraints());
112 if (constraints) {
113 ConfigProjectorPtr_t configProjector(constraints->configProjector());
114 if (configProjector) {
115 constrApply_ = false; // while apply has not successed
116 configProjector->projectOnKernel(qFrom, qTo, qout);
117 if (constraints->apply(qout)) {
118 constrApply_ = true;
119 return;
120 }
121 }
122 }
123 qout = qTo;
124 }
125
126 void VisibilityPrmPlanner::oneStep() {
127 DevicePtr_t robot(problem()->robot());
128 ConfigurationShooterPtr_t configurationShooter(
129 problem()->configurationShooter());
130 ConfigValidationsPtr_t configValidations(problem()->configValidations());
131 RoadmapPtr_t r(roadmap());
132 value_type count; // number of times q has been seen
133 constrApply_ = true; // stay true if no constraint in Problem
134 Configuration_t q_init(r->initNode()->configuration()),
135 q_proj(robot->configSize()), q_rand(robot->configSize());
136
137 /* Initialization of guard status */
138 nodeStatus_[r->initNode()] = true; // init node is guard
139 for (NodeVector_t::const_iterator itg = r->goalNodes().begin();
140 itg != r->goalNodes().end(); ++itg) {
141 nodeStatus_[*itg] = true; // goal nodes are guards
142 }
143
144 // Shoot random config as long as not collision-free
145 ValidationReportPtr_t report;
146 do {
147 configurationShooter->shoot(q_rand);
148 applyConstraints(q_init, q_rand, q_proj);
149 // Joseph: I don't think this is necessary.
150 // robot->currentConfiguration(q_proj);
151 // robot->computeForwardKinematics();
152 } while (!configValidations->validate(q_proj, report) || !constrApply_);
153 count = 0;
154
155 for (ConnectedComponents_t::const_iterator itcc =
156 r->connectedComponents().begin();
157 itcc != r->connectedComponents().end(); ++itcc) {
158 ConnectedComponentPtr_t cc = *itcc;
159 if (visibleFromCC(q_proj, cc)) {
160 // delayedEdges_ will completed if visible
161 count++; // count how many times q has been seen
162 }
163 }
164
165 if (count == 0) { // q not visible from anywhere
166 NodePtr_t newNode = r->addNode(q_proj); // add q as a guard node
167 nodeStatus_[newNode] = true;
168 hppDout(info, "q is a guard node: " << displayConfig(q_proj));
169 }
170 if (count > 1) { // q visible several times
171 // Insert delayed edges from list and add q as a connection node
172 for (const auto& edge : delayedEdges_) {
173 const NodePtr_t& near = std::get<0>(edge);
174 const Configuration_t& q_new = std::get<1>(edge);
175 const PathPtr_t& validPath = std::get<2>(edge);
176 NodePtr_t newNode = r->addNode(q_new);
177 nodeStatus_[newNode] = false;
178 r->addEdge(near, newNode, validPath);
179 r->addEdge(newNode, near, validPath->reverse());
180 hppDout(info,
181 "connection between q1: " << displayConfig(near->configuration())
182 << "and q2: " << displayConfig(q_new));
183 }
184 }
185 delayedEdges_.clear();
186 }
187
188 } // namespace core
189 } // namespace hpp
190