1 |
|
|
// Copyright (c) 2014, LAAS-CNRS |
2 |
|
|
// Authors: Steve Tonneau (steve.tonneau@laas.fr) |
3 |
|
|
// |
4 |
|
|
// This file is part of hpp-rbprm. |
5 |
|
|
// hpp-rbprm is free software: you can redistribute it |
6 |
|
|
// and/or modify it under the terms of the GNU Lesser General Public |
7 |
|
|
// License as published by the Free Software Foundation, either version |
8 |
|
|
// 3 of the License, or (at your option) any later version. |
9 |
|
|
// |
10 |
|
|
// hpp-rbprm is distributed in the hope that it will be |
11 |
|
|
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty |
12 |
|
|
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU |
13 |
|
|
// General Lesser Public License for more details. You should have |
14 |
|
|
// received a copy of the GNU Lesser General Public License along with |
15 |
|
|
// hpp-rbprm. If not, see <http://www.gnu.org/licenses/>. |
16 |
|
|
|
17 |
|
|
#include <hpp/core/collision-path-validation-report.hh> |
18 |
|
|
#include <hpp/core/config-validation.hh> |
19 |
|
|
#include <hpp/core/config-validations.hh> |
20 |
|
|
#include <hpp/core/path-validation-report.hh> |
21 |
|
|
#include <hpp/core/path.hh> |
22 |
|
|
#include <hpp/core/validation-report.hh> |
23 |
|
|
#include <hpp/rbprm/rbprm-path-validation.hh> |
24 |
|
|
#include <hpp/rbprm/rbprm-validation.hh> |
25 |
|
|
|
26 |
|
|
namespace hpp { |
27 |
|
|
namespace rbprm { |
28 |
|
|
|
29 |
|
|
using core::Configuration_t; |
30 |
|
|
using core::value_type; |
31 |
|
|
|
32 |
|
13 |
RbPrmPathValidationPtr_t RbPrmPathValidation::create( |
33 |
|
|
const core::DevicePtr_t& robot, const core::value_type& stepSize) { |
34 |
✓✗ |
13 |
RbPrmPathValidation* ptr(new RbPrmPathValidation(robot, stepSize)); |
35 |
|
13 |
RbPrmPathValidationPtr_t shPtr(ptr); |
36 |
|
13 |
return shPtr; |
37 |
|
|
} |
38 |
|
|
|
39 |
|
13 |
RbPrmPathValidation::RbPrmPathValidation(const core::DevicePtr_t& /*robot*/, |
40 |
|
13 |
const core::value_type& stepSize) |
41 |
|
13 |
: core::pathValidation::Discretized(stepSize) {} |
42 |
|
|
|
43 |
|
13 |
void RbPrmPathValidation::add( |
44 |
|
|
const core::ConfigValidationPtr_t& configValidation) { |
45 |
|
13 |
core::pathValidation::Discretized::add(configValidation); |
46 |
|
13 |
rbprmValidation_ = dynamic_pointer_cast<RbPrmValidation>(configValidation); |
47 |
|
13 |
} |
48 |
|
|
|
49 |
|
|
bool RbPrmPathValidation::validate( |
50 |
|
|
const core::PathPtr_t& path, bool reverse, core::PathPtr_t& validPart, |
51 |
|
|
core::PathValidationReportPtr_t& validationReport, |
52 |
|
|
const std::vector<std::string>& filter) { |
53 |
|
|
core::ValidationReportPtr_t configReport; |
54 |
|
|
assert(path); |
55 |
|
|
bool valid = true; |
56 |
|
|
if (reverse) { |
57 |
|
|
value_type tmin = path->timeRange().first; |
58 |
|
|
value_type tmax = path->timeRange().second; |
59 |
|
|
value_type lastValidTime = tmax; |
60 |
|
|
value_type t = tmax; |
61 |
|
|
unsigned finished = 0; |
62 |
|
|
Configuration_t q(path->outputSize()); |
63 |
|
|
while (finished < 2 && valid) { |
64 |
|
|
bool success = (*path)(q, t); |
65 |
|
|
if (!success || !rbprmValidation_->validate(q, configReport, filter)) { |
66 |
|
|
validationReport = core::CollisionPathValidationReportPtr_t( |
67 |
|
|
new core::CollisionPathValidationReport(t, configReport)); |
68 |
|
|
valid = false; |
69 |
|
|
} else { |
70 |
|
|
lastValidTime = t; |
71 |
|
|
t -= stepSize_; |
72 |
|
|
} |
73 |
|
|
if (t < tmin) { |
74 |
|
|
t = tmin; |
75 |
|
|
finished++; |
76 |
|
|
} |
77 |
|
|
} |
78 |
|
|
if (valid) { |
79 |
|
|
validPart = path; |
80 |
|
|
return true; |
81 |
|
|
} else { |
82 |
|
|
validPart = path->extract(std::make_pair(lastValidTime, tmax)); |
83 |
|
|
return false; |
84 |
|
|
} |
85 |
|
|
} else { |
86 |
|
|
value_type tmin = path->timeRange().first; |
87 |
|
|
value_type tmax = path->timeRange().second; |
88 |
|
|
value_type lastValidTime = tmin; |
89 |
|
|
value_type t = tmin; |
90 |
|
|
unsigned finished = 0; |
91 |
|
|
Configuration_t q(path->outputSize()); |
92 |
|
|
while (finished < 2 && valid) { |
93 |
|
|
bool success = (*path)(q, t); |
94 |
|
|
if (!success || !rbprmValidation_->validate(q, configReport, filter)) { |
95 |
|
|
validationReport = core::CollisionPathValidationReportPtr_t( |
96 |
|
|
new core::CollisionPathValidationReport(t, configReport)); |
97 |
|
|
valid = false; |
98 |
|
|
} else { |
99 |
|
|
lastValidTime = t; |
100 |
|
|
t += stepSize_; |
101 |
|
|
} |
102 |
|
|
if (t > tmax) { |
103 |
|
|
t = tmax; |
104 |
|
|
finished++; |
105 |
|
|
} |
106 |
|
|
} |
107 |
|
|
if (valid) { |
108 |
|
|
validPart = path; |
109 |
|
|
return true; |
110 |
|
|
} else { |
111 |
|
|
validPart = path->extract(std::make_pair(tmin, lastValidTime)); |
112 |
|
|
return false; |
113 |
|
|
} |
114 |
|
|
} |
115 |
|
|
} |
116 |
|
|
|
117 |
|
|
} // namespace rbprm |
118 |
|
|
} // namespace hpp |