Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/dynamic/dynamic-path-validation.cc	Lines:	0	74	0.0 %
Date:	2024-02-02 12:21:48	Branches:	0	108	0.0 %


//
// Copyright (c) 2017 CNRS
// Authors: Fernbach Pierre
//
// This file is part of hpp-rbprm
// hpp-core is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-core is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core  If not, see
// <http://www.gnu.org/licenses/>.

#include <hpp/core/collision-path-validation-report.hh>
#include <hpp/core/config-validation.hh>
#include <hpp/core/config-validations.hh>
#include <hpp/core/path-validation-report.hh>
#include <hpp/core/path.hh>
#include <hpp/core/validation-report.hh>
#include <hpp/pinocchio/configuration.hh>
#include <hpp/rbprm/dynamic/dynamic-path-validation.hh>
#include <hpp/rbprm/rbprm-validation.hh>
#include <hpp/util/debug.hh>
#include <hpp/util/timer.hh>

namespace hpp {
namespace rbprm {
using core::Configuration_t;
using core::value_type;

DynamicPathValidationPtr_t DynamicPathValidation::create(
    const core::DevicePtr_t& robot, const core::value_type& stepSize) {
  DynamicPathValidation* ptr(new DynamicPathValidation(robot, stepSize));
  DynamicPathValidationPtr_t shPtr(ptr);
  return shPtr;
}

DynamicPathValidation::DynamicPathValidation(const core::DevicePtr_t& robot,
                                             const core::value_type& stepSize)
    : RbPrmPathValidation(robot, stepSize) {}

/// validate with custom filter for the rom validation
bool DynamicPathValidation::validate(
    const core::PathPtr_t& path, bool reverse, core::PathPtr_t& validPart,
    core::PathValidationReportPtr_t& validationReport,
    const std::vector<std::string>& filter) {
  hppDout(notice, "dynamic path validation called with filters");
  hppStartBenchmark(PATH_VALIDATION);
  core::ValidationReportPtr_t configReport;
  Configuration_t q;
  if (reverse)
    (*path)(q, path->timeRange().second);
  else
    (*path)(q, path->timeRange().first);

  rbprmValidation_->validate(q, configReport);
  dynamicValidation_->setInitialReport(configReport);
  hppDout(notice, "dynamic validation set initial report OK");

  assert(path);
  bool valid = true;
  if (reverse) {
    value_type tmin = path->timeRange().first;
    value_type tmax = path->timeRange().second;
    value_type lastValidTime = tmax;
    value_type t = tmax;
    unsigned finished = 0;
    Configuration_t q(path->outputSize());
    while (finished < 2 && valid) {
      bool success = (*path)(q, t);
      if (!success || !rbprmValidation_->validate(q, configReport, filter) ||
          !dynamicValidation_->validate(q, configReport)) {
        validationReport = core::CollisionPathValidationReportPtr_t(
            new core::CollisionPathValidationReport(t, configReport));
        valid = false;
      } else {
        lastValidTime = t;
        t -= stepSize_;
      }
      if (t < tmin) {
        t = tmin;
        finished++;
      }
    }
    if (valid) {
      validPart = path;
      hppStopBenchmark(PATH_VALIDATION);
      hppDisplayBenchmark(PATH_VALIDATION);
      return true;
    } else {
      validPart = path->extract(std::make_pair(lastValidTime, tmax));
      hppStopBenchmark(PATH_VALIDATION);
      hppDisplayBenchmark(PATH_VALIDATION);
      return false;
    }
  } else {
    value_type tmin = path->timeRange().first;
    value_type tmax = path->timeRange().second;
    value_type lastValidTime = tmin;
    value_type t = tmin;
    unsigned finished = 0;
    Configuration_t q(path->outputSize());
    while (finished < 2 && valid) {
      bool success = (*path)(q, t);
      if (!success || !rbprmValidation_->validate(q, configReport, filter) ||
          !dynamicValidation_->validate(q, configReport)) {
        validationReport = core::CollisionPathValidationReportPtr_t(
            new core::CollisionPathValidationReport(t, configReport));
        valid = false;
      } else {
        lastValidTime = t;
        t += stepSize_;
      }
      if (t > tmax) {
        t = tmax;
        finished++;
      }
    }
    if (valid) {
      validPart = path;
      hppStopBenchmark(PATH_VALIDATION);
      hppDisplayBenchmark(PATH_VALIDATION);
      return true;
    } else {
      validPart = path->extract(std::make_pair(tmin, lastValidTime));
      hppStopBenchmark(PATH_VALIDATION);
      hppDisplayBenchmark(PATH_VALIDATION);
      return false;
    }
  }
  hppStopBenchmark(PATH_VALIDATION);
  hppDisplayBenchmark(PATH_VALIDATION);
}

bool DynamicPathValidation::validate(
    const core::PathPtr_t& path, bool reverse, core::PathPtr_t& validPart,
    core::PathValidationReportPtr_t& validationReport) {
  hppDout(info, "dynamic path validation called");
  hppDout(info, "path begin : " << path->timeRange().first);
  hppDout(info, "path end : " << path->timeRange().second);
  hppStartBenchmark(PATH_VALIDATION);
  core::ValidationReportPtr_t configReport;
  Configuration_t q(path->outputSize());
  if (reverse)
    (*path)(q, path->timeRange().second);
  else
    (*path)(q, path->timeRange().first);

  hppDout(info, "q = " << pinocchio::displayConfig(q));
  rbprmValidation_->validate(q, configReport);

  hppDout(info, "rbprmValidation called");
  dynamicValidation_->setInitialReport(configReport);
  hppDout(info, "dynamic validation set initial report OK");
  bool valid = core::pathValidation::Discretized::validate(
      path, reverse, validPart, validationReport);
  hppStopBenchmark(PATH_VALIDATION);
  hppDisplayBenchmark(PATH_VALIDATION);
  return valid;
}

}  // namespace rbprm
}  // namespace hpp


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2017 CNRS
3			// Authors: Fernbach Pierre
4			//
5			// This file is part of hpp-rbprm
6			// hpp-core is free software: you can redistribute it
7			// and/or modify it under the terms of the GNU Lesser General Public
8			// License as published by the Free Software Foundation, either version
9			// 3 of the License, or (at your option) any later version.
10			//
11			// hpp-core is distributed in the hope that it will be
12			// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
13			// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14			// General Lesser Public License for more details. You should have
15			// received a copy of the GNU Lesser General Public License along with
16			// hpp-core If not, see
17			// <http://www.gnu.org/licenses/>.
18
19			#include <hpp/core/collision-path-validation-report.hh>
20			#include <hpp/core/config-validation.hh>
21			#include <hpp/core/config-validations.hh>
22			#include <hpp/core/path-validation-report.hh>
23			#include <hpp/core/path.hh>
24			#include <hpp/core/validation-report.hh>
25			#include <hpp/pinocchio/configuration.hh>
26			#include <hpp/rbprm/dynamic/dynamic-path-validation.hh>
27			#include <hpp/rbprm/rbprm-validation.hh>
28			#include <hpp/util/debug.hh>
29			#include <hpp/util/timer.hh>
30
31			namespace hpp {
32			namespace rbprm {
33			using core::Configuration_t;
34			using core::value_type;
35
36			DynamicPathValidationPtr_t DynamicPathValidation::create(
37			const core::DevicePtr_t& robot, const core::value_type& stepSize) {
38			DynamicPathValidation* ptr(new DynamicPathValidation(robot, stepSize));
39			DynamicPathValidationPtr_t shPtr(ptr);
40			return shPtr;
41			}
42
43			DynamicPathValidation::DynamicPathValidation(const core::DevicePtr_t& robot,
44			const core::value_type& stepSize)
45			: RbPrmPathValidation(robot, stepSize) {}
46
47			/// validate with custom filter for the rom validation
48			bool DynamicPathValidation::validate(
49			const core::PathPtr_t& path, bool reverse, core::PathPtr_t& validPart,
50			core::PathValidationReportPtr_t& validationReport,
51			const std::vector<std::string>& filter) {
52			hppDout(notice, "dynamic path validation called with filters");
53			hppStartBenchmark(PATH_VALIDATION);
54			core::ValidationReportPtr_t configReport;
55			Configuration_t q;
56			if (reverse)
57			(*path)(q, path->timeRange().second);
58			else
59			(*path)(q, path->timeRange().first);
60
61			rbprmValidation_->validate(q, configReport);
62			dynamicValidation_->setInitialReport(configReport);
63			hppDout(notice, "dynamic validation set initial report OK");
64
65			assert(path);
66			bool valid = true;
67			if (reverse) {
68			value_type tmin = path->timeRange().first;
69			value_type tmax = path->timeRange().second;
70			value_type lastValidTime = tmax;
71			value_type t = tmax;
72			unsigned finished = 0;
73			Configuration_t q(path->outputSize());
74			while (finished < 2 && valid) {
75			bool success = (*path)(q, t);
76			if (!success \|\| !rbprmValidation_->validate(q, configReport, filter) \|\|
77			!dynamicValidation_->validate(q, configReport)) {
78			validationReport = core::CollisionPathValidationReportPtr_t(
79			new core::CollisionPathValidationReport(t, configReport));
80			valid = false;
81			} else {
82			lastValidTime = t;
83			t -= stepSize_;
84			}
85			if (t < tmin) {
86			t = tmin;
87			finished++;
88			}
89			}
90			if (valid) {
91			validPart = path;
92			hppStopBenchmark(PATH_VALIDATION);
93			hppDisplayBenchmark(PATH_VALIDATION);
94			return true;
95			} else {
96			validPart = path->extract(std::make_pair(lastValidTime, tmax));
97			hppStopBenchmark(PATH_VALIDATION);
98			hppDisplayBenchmark(PATH_VALIDATION);
99			return false;
100			}
101			} else {
102			value_type tmin = path->timeRange().first;
103			value_type tmax = path->timeRange().second;
104			value_type lastValidTime = tmin;
105			value_type t = tmin;
106			unsigned finished = 0;
107			Configuration_t q(path->outputSize());
108			while (finished < 2 && valid) {
109			bool success = (*path)(q, t);
110			if (!success \|\| !rbprmValidation_->validate(q, configReport, filter) \|\|
111			!dynamicValidation_->validate(q, configReport)) {
112			validationReport = core::CollisionPathValidationReportPtr_t(
113			new core::CollisionPathValidationReport(t, configReport));
114			valid = false;
115			} else {
116			lastValidTime = t;
117			t += stepSize_;
118			}
119			if (t > tmax) {
120			t = tmax;
121			finished++;
122			}
123			}
124			if (valid) {
125			validPart = path;
126			hppStopBenchmark(PATH_VALIDATION);
127			hppDisplayBenchmark(PATH_VALIDATION);
128			return true;
129			} else {
130			validPart = path->extract(std::make_pair(tmin, lastValidTime));
131			hppStopBenchmark(PATH_VALIDATION);
132			hppDisplayBenchmark(PATH_VALIDATION);
133			return false;
134			}
135			}
136			hppStopBenchmark(PATH_VALIDATION);
137			hppDisplayBenchmark(PATH_VALIDATION);
138			}
139
140			bool DynamicPathValidation::validate(
141			const core::PathPtr_t& path, bool reverse, core::PathPtr_t& validPart,
142			core::PathValidationReportPtr_t& validationReport) {
143			hppDout(info, "dynamic path validation called");
144			hppDout(info, "path begin : " << path->timeRange().first);
145			hppDout(info, "path end : " << path->timeRange().second);
146			hppStartBenchmark(PATH_VALIDATION);
147			core::ValidationReportPtr_t configReport;
148			Configuration_t q(path->outputSize());
149			if (reverse)
150			(*path)(q, path->timeRange().second);
151			else
152			(*path)(q, path->timeRange().first);
153
154			hppDout(info, "q = " << pinocchio::displayConfig(q));
155			rbprmValidation_->validate(q, configReport);
156
157			hppDout(info, "rbprmValidation called");
158			dynamicValidation_->setInitialReport(configReport);
159			hppDout(info, "dynamic validation set initial report OK");
160			bool valid = core::pathValidation::Discretized::validate(
161			path, reverse, validPart, validationReport);
162			hppStopBenchmark(PATH_VALIDATION);
163			hppDisplayBenchmark(PATH_VALIDATION);
164			return valid;
165			}
166
167			} // namespace rbprm
168			} // namespace hpp