Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/rbprm-validation.cc	Lines:	87	118	73.7 %
Date:	2024-02-02 12:21:48	Branches:	78	160	48.8 %


// Copyright (c) 2014, LAAS-CNRS
// Authors: Steve Tonneau (steve.tonneau@laas.fr)
//
// This file is part of hpp-rbprm.
// hpp-rbprm 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-rbprm 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-rbprm. If not, see <http://www.gnu.org/licenses/>.

#include <hpp/core/collision-validation.hh>
#include <hpp/core/joint-bound-validation.hh>
#include <hpp/rbprm/rbprm-validation-report.hh>
#include <hpp/rbprm/rbprm-validation.hh>

namespace {
hpp::core::CollisionValidationPtr_t tuneFclValidation(
    const hpp::pinocchio::RbPrmDevicePtr_t& robot) {
  typedef hpp::core::CollisionRequests_t CollisionRequests_t;
  hpp::core::CollisionValidationPtr_t validation =
      hpp::core::CollisionValidation::create(robot);
  // enable contact for all collision pairs
  CollisionRequests_t& req(validation->requests());
  for (CollisionRequests_t::iterator it(req.begin()); it != req.end(); ++it) {
    it->enable_contact = true;
  }
  validation->defaultRequest().enable_contact = true;
  return validation;
}

hpp::rbprm::T_RomValidation createRomValidations(
    const hpp::pinocchio::RbPrmDevicePtr_t& robot,
    const std::map<std::string, std::vector<std::string> >& affFilters) {
  hpp::rbprm::T_RomValidation result;
  hppDout(notice, "Number of ROM : " << robot->robotRoms_.size());
  for (hpp::pinocchio::T_Rom::const_iterator cit = robot->robotRoms_.begin();
       cit != robot->robotRoms_.end(); ++cit) {
    hppDout(notice, "name = " << cit->first);
    std::map<std::string, std::vector<std::string> >::const_iterator cfit =
        affFilters.find(cit->first);
    if (cfit != affFilters.end()) {
      result.insert(std::make_pair(
          cit->first,
          hpp::rbprm::RbPrmRomValidation::create(cit->second, cfit->second)));
    } else {  // no affordance filter defined for this ROM, assume it can
              // collide with all the objects
      result.insert(std::make_pair(
          cit->first, hpp::rbprm::RbPrmRomValidation::create(cit->second)));
    }
  }
  return result;
}
}  // namespace

namespace hpp {
using namespace core;
namespace rbprm {

RbPrmValidationPtr_t RbPrmValidation::create(
    const pinocchio::RbPrmDevicePtr_t& robot,
    const std::vector<std::string>& filter,
    const std::map<std::string, std::vector<std::string> >& affFilters,
    const hpp::rbprm::affMap_t& affordances,
    const core::ObjectStdVector_t& geometries) {
  RbPrmValidation* ptr =
      new RbPrmValidation(robot, filter, affFilters, affordances, geometries);
  return RbPrmValidationPtr_t(ptr);
}

RbPrmValidation::RbPrmValidation(
    const pinocchio::RbPrmDevicePtr_t& robot,
    const std::vector<std::string>& filter,
    const std::map<std::string, std::vector<std::string> >& affFilters,
    const hpp::rbprm::affMap_t& affordances,
    const hpp::core::ObjectStdVector_t& geometries)
    : CollisionValidation(robot),
      trunkValidation_(tuneFclValidation(robot)),
      boundValidation_(core::JointBoundValidation::create(robot)),
      romValidations_(createRomValidations(robot, affFilters)),
      unusedReport_(new CollisionValidationReport) {
  for (std::vector<std::string>::const_iterator cit = filter.begin();
       cit != filter.end(); ++cit) {
    if (romValidations_.find(*cit) == romValidations_.end()) {
      std::cout << "warning: default filter impossible to match in rbprmshooter"
                << std::endl;
    }
  }

  for (hpp::core::ObjectStdVector_t::const_iterator cit = geometries.begin();
       cit != geometries.end(); ++cit) {
    addObstacle(*cit);
  }
  // Add obstacles corresponding to affordances of given rom
  hppDout(notice, "add obstacle, filter size = " << filter.size());
  for (hpp::pinocchio::T_Rom::const_iterator cit = robot->robotRoms_.begin();
       cit != robot->robotRoms_.end(); ++cit) {
    std::map<std::string, std::vector<std::string> >::const_iterator
        affFilterIt = affFilters.find(cit->first);
    // Check if all rom filters have been given an affordance filter.
    // If not, an error is thrown.
    if (affFilterIt == affFilters.end()) {
      hppDout(notice, "Filter " + cit->first +
                          " has no affordance filter settings! Please add them "
                          "to continue.");
    } else {
      defaultFilter_.push_back(cit->first);

      T_RomValidation::const_iterator romIt =
          romValidations_.find(affFilterIt->first);
      if (romIt == romValidations_.end()) {
        // TODO: Throw error?
        std::runtime_error("No romValidator object found for filter " +
                           affFilterIt->first + "!");
      }
      for (unsigned int fIdx = 0; fIdx < affFilterIt->second.size(); fIdx++) {
        if (!affordances.has(std::string(affFilterIt->second[fIdx]))) {
          std::cout << "Filter " << affFilterIt->first
                    << " has invalid affordance filter setting "
                    << affFilterIt->second[fIdx]
                    << ". Ignoring such filter setting." << std::endl;
        } else {
          const hpp::core::AffordanceObjects_t& affObjs =
              affordances.get(affFilterIt->second[fIdx]);
          for (std::size_t affIdx = 0; affIdx < affObjs.size(); affIdx++) {
            romIt->second->addObstacle(affObjs[affIdx].second);
          }
        }
      }
      if (std::find(filter.begin(), filter.end(), romIt->first) ==
          filter.end()) {
        romIt->second->setOptional(true);
      }
    }
  }
}

bool RbPrmValidation::validateRoms(const core::Configuration_t& config,
                                   const std::vector<std::string>& filter,
                                   RbprmValidationReportPtr_t& rbprmReport)

{
  ValidationReportPtr_t rbprmReportCast = rbprmReport;
  unsigned int filterMatch(0);
  for (T_RomValidation::const_iterator cit = romValidations_.begin();
       cit != romValidations_.end() &&
       (filterMatch < 1 || filterMatch < filter.size());
       ++cit) {
    if ((filter.empty() ||
         std::find(filter.begin(), filter.end(), cit->first) != filter.end()) &&
        cit->second->validate(config, rbprmReportCast)) {
      ++filterMatch;
    }
  }

  rbprmReport->romsValid = filterMatch >= filter.size();
  return rbprmReport->romsValid;
}

bool RbPrmValidation::validateRoms(
    const core::Configuration_t& config,
    hpp::core::RbprmValidationReportPtr_t& validationReport) {
  return validateRoms(config, defaultFilter_, validationReport);
}

bool RbPrmValidation::validateRoms(const core::Configuration_t& config,
                                   const std::vector<std::string>& filter) {
  RbprmValidationReportPtr_t unusedReport;
  return validateRoms(config, filter, unusedReport);
}

bool RbPrmValidation::validateRoms(const core::Configuration_t& config) {
  RbprmValidationReportPtr_t unusedReport;
  return validateRoms(config, defaultFilter_, unusedReport);
}

bool RbPrmValidation::validate(const Configuration_t& config) {
  return validate(config, unusedReport_, defaultFilter_);
}

bool RbPrmValidation::validate(const Configuration_t& config,
                               ValidationReportPtr_t& validationReport) {
  return validate(config, validationReport, defaultFilter_);
}

bool RbPrmValidation::validate(const Configuration_t& config,
                               const std::vector<std::string>& filter) {
  return validate(config, unusedReport_, filter);
}

bool RbPrmValidation::validate(
    const Configuration_t& config,
    hpp::core::ValidationReportPtr_t& validationReport,
    const std::vector<std::string>& filter) {
  CollisionValidationReportPtr_t colReport =
      dynamic_pointer_cast<CollisionValidationReport>(validationReport);
  if (colReport) colReport->result.clear();

  RbprmValidationReportPtr_t rbprmReport(new RbprmValidationReport);

  bool success = trunkValidation_->validate(config, validationReport);
  if (success) {
    rbprmReport->trunkInCollision = false;
  } else {
    colReport =
        dynamic_pointer_cast<CollisionValidationReport>(validationReport);
    rbprmReport->object1 = colReport->object1;
    rbprmReport->object2 = colReport->object2;
    rbprmReport->result = colReport->result;
    rbprmReport->trunkInCollision = true;
  }
  // always compute full report
  success = validateRoms(config, filter, rbprmReport) && success &&
            boundValidation_->validate(config, validationReport);
  validationReport = rbprmReport;
  return success;
}

bool RbPrmValidation::validateTrunk(
    const Configuration_t& config,
    hpp::core::ValidationReportPtr_t& validationReport) {
  return trunkValidation_->validate(
      config,
      validationReport);  // &&
                          // boundValidation_->validate(config,validationReport);
}

void RbPrmValidation::addObstacle(const CollisionObjectConstPtr_t& object) {
  trunkValidation_->addObstacle(object);
}

void RbPrmValidation::removeObstacleFromJoint(
    const JointPtr_t& joint, const CollisionObjectPtr_t& obstacle) {
  trunkValidation_->removeObstacleFromJoint(joint, obstacle);
  for (T_RomValidation::const_iterator cit = romValidations_.begin();
       cit != romValidations_.end(); ++cit) {
    cit->second->removeObstacleFromJoint(joint, obstacle);
  }
}

void RbPrmValidation::randomnizeCollisionPairs() {
  for (T_RomValidation::const_iterator cit = romValidations_.begin();
       cit != romValidations_.end(); ++cit) {
    cit->second->randomnizeCollisionPairs();
  }
}

void RbPrmValidation::computeAllContacts(bool computeAllContacts) {
  for (T_RomValidation::const_iterator cit = romValidations_.begin();
       cit != romValidations_.end(); ++cit) {
    cit->second->computeAllContacts(computeAllContacts);
  }
}

}  // namespace rbprm
}  // namespace hpp


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
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-validation.hh>
18			#include <hpp/core/joint-bound-validation.hh>
19			#include <hpp/rbprm/rbprm-validation-report.hh>
20			#include <hpp/rbprm/rbprm-validation.hh>
21
22			namespace {
23		50	hpp::core::CollisionValidationPtr_t tuneFclValidation(
24			const hpp::pinocchio::RbPrmDevicePtr_t& robot) {
25			typedef hpp::core::CollisionRequests_t CollisionRequests_t;
26			hpp::core::CollisionValidationPtr_t validation =
27	✓✗	50	hpp::core::CollisionValidation::create(robot);
28			// enable contact for all collision pairs
29		50	CollisionRequests_t& req(validation->requests());
30	✗✓	50	for (CollisionRequests_t::iterator it(req.begin()); it != req.end(); ++it) {
31			it->enable_contact = true;
32			}
33		50	validation->defaultRequest().enable_contact = true;
34		50	return validation;
35			}
36
37		50	hpp::rbprm::T_RomValidation createRomValidations(
38			const hpp::pinocchio::RbPrmDevicePtr_t& robot,
39			const std::map<std::string, std::vector<std::string> >& affFilters) {
40		50	hpp::rbprm::T_RomValidation result;
41			hppDout(notice, "Number of ROM : " << robot->robotRoms_.size());
42		162	for (hpp::pinocchio::T_Rom::const_iterator cit = robot->robotRoms_.begin();
43	✓✓	274	cit != robot->robotRoms_.end(); ++cit) {
44			hppDout(notice, "name = " << cit->first);
45			std::map<std::string, std::vector<std::string> >::const_iterator cfit =
46	✓✗	112	affFilters.find(cit->first);
47	✓✗	112	if (cfit != affFilters.end()) {
48		112	result.insert(std::make_pair(
49	✓✗	112	cit->first,
50	✓✗✓✗	336	hpp::rbprm::RbPrmRomValidation::create(cit->second, cfit->second)));
51			} else { // no affordance filter defined for this ROM, assume it can
52			// collide with all the objects
53			result.insert(std::make_pair(
54			cit->first, hpp::rbprm::RbPrmRomValidation::create(cit->second)));
55			}
56			}
57		50	return result;
58			}
59			} // namespace
60
61			namespace hpp {
62			using namespace core;
63			namespace rbprm {
64
65		50	RbPrmValidationPtr_t RbPrmValidation::create(
66			const pinocchio::RbPrmDevicePtr_t& robot,
67			const std::vector<std::string>& filter,
68			const std::map<std::string, std::vector<std::string> >& affFilters,
69			const hpp::rbprm::affMap_t& affordances,
70			const core::ObjectStdVector_t& geometries) {
71			RbPrmValidation* ptr =
72	✓✗	50	new RbPrmValidation(robot, filter, affFilters, affordances, geometries);
73		50	return RbPrmValidationPtr_t(ptr);
74			}
75
76		50	RbPrmValidation::RbPrmValidation(
77			const pinocchio::RbPrmDevicePtr_t& robot,
78			const std::vector<std::string>& filter,
79			const std::map<std::string, std::vector<std::string> >& affFilters,
80			const hpp::rbprm::affMap_t& affordances,
81		50	const hpp::core::ObjectStdVector_t& geometries)
82			: CollisionValidation(robot),
83			trunkValidation_(tuneFclValidation(robot)),
84			boundValidation_(core::JointBoundValidation::create(robot)),
85			romValidations_(createRomValidations(robot, affFilters)),
86	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	50	unusedReport_(new CollisionValidationReport) {
87		162	for (std::vector<std::string>::const_iterator cit = filter.begin();
88	✓✓	274	cit != filter.end(); ++cit) {
89	✓✗✗✓	112	if (romValidations_.find(*cit) == romValidations_.end()) {
90			std::cout << "warning: default filter impossible to match in rbprmshooter"
91			<< std::endl;
92			}
93			}
94
95		482	for (hpp::core::ObjectStdVector_t::const_iterator cit = geometries.begin();
96	✓✓	482	cit != geometries.end(); ++cit) {
97	✓✗	432	addObstacle(*cit);
98			}
99			// Add obstacles corresponding to affordances of given rom
100			hppDout(notice, "add obstacle, filter size = " << filter.size());
101		162	for (hpp::pinocchio::T_Rom::const_iterator cit = robot->robotRoms_.begin();
102	✓✓	274	cit != robot->robotRoms_.end(); ++cit) {
103			std::map<std::string, std::vector<std::string> >::const_iterator
104	✓✗	112	affFilterIt = affFilters.find(cit->first);
105			// Check if all rom filters have been given an affordance filter.
106			// If not, an error is thrown.
107	✓✗	112	if (affFilterIt == affFilters.end()) {
108			hppDout(notice, "Filter " + cit->first +
109			" has no affordance filter settings! Please add them "
110			"to continue.");
111			} else {
112	✓✗	112	defaultFilter_.push_back(cit->first);
113
114			T_RomValidation::const_iterator romIt =
115	✓✗	112	romValidations_.find(affFilterIt->first);
116	✗✓	112	if (romIt == romValidations_.end()) {
117			// TODO: Throw error?
118			std::runtime_error("No romValidator object found for filter " +
119			affFilterIt->first + "!");
120			}
121	✓✓	224	for (unsigned int fIdx = 0; fIdx < affFilterIt->second.size(); fIdx++) {
122	✓✗✓✗ ✗✓	112	if (!affordances.has(std::string(affFilterIt->second[fIdx]))) {
123			std::cout << "Filter " << affFilterIt->first
124			<< " has invalid affordance filter setting "
125			<< affFilterIt->second[fIdx]
126			<< ". Ignoring such filter setting." << std::endl;
127			} else {
128			const hpp::core::AffordanceObjects_t& affObjs =
129	✓✗	112	affordances.get(affFilterIt->second[fIdx]);
130	✓✓	434	for (std::size_t affIdx = 0; affIdx < affObjs.size(); affIdx++) {
131	✓✗	322	romIt->second->addObstacle(affObjs[affIdx].second);
132			}
133			}
134			}
135	✓✗	112	if (std::find(filter.begin(), filter.end(), romIt->first) ==
136	✗✓	224	filter.end()) {
137			romIt->second->setOptional(true);
138			}
139			}
140			}
141		50	}
142
143		347420	bool RbPrmValidation::validateRoms(const core::Configuration_t& config,
144			const std::vector<std::string>& filter,
145			RbprmValidationReportPtr_t& rbprmReport)
146
147			{
148		347420	ValidationReportPtr_t rbprmReportCast = rbprmReport;
149		347420	unsigned int filterMatch(0);
150		1598564	for (T_RomValidation::const_iterator cit = romValidations_.begin();
151	✓✓✓✓ ✓✓	2502084	cit != romValidations_.end() &&
152	✓✗	903520	(filterMatch < 1 \|\| filterMatch < filter.size());
153		1251144	++cit) {
154	✓✗	2502288	if ((filter.empty() \|\|
155	✓✗✓✗ ✓✓	2502288	std::find(filter.begin(), filter.end(), cit->first) != filter.end()) &&
156	✓✗✓✓	1251144	cit->second->validate(config, rbprmReportCast)) {
157		1250710	++filterMatch;
158			}
159			}
160
161		347420	rbprmReport->romsValid = filterMatch >= filter.size();
162		694840	return rbprmReport->romsValid;
163			}
164
165			bool RbPrmValidation::validateRoms(
166			const core::Configuration_t& config,
167			hpp::core::RbprmValidationReportPtr_t& validationReport) {
168			return validateRoms(config, defaultFilter_, validationReport);
169			}
170
171			bool RbPrmValidation::validateRoms(const core::Configuration_t& config,
172			const std::vector<std::string>& filter) {
173			RbprmValidationReportPtr_t unusedReport;
174			return validateRoms(config, filter, unusedReport);
175			}
176
177			bool RbPrmValidation::validateRoms(const core::Configuration_t& config) {
178			RbprmValidationReportPtr_t unusedReport;
179			return validateRoms(config, defaultFilter_, unusedReport);
180			}
181
182			bool RbPrmValidation::validate(const Configuration_t& config) {
183			return validate(config, unusedReport_, defaultFilter_);
184			}
185
186		299473	bool RbPrmValidation::validate(const Configuration_t& config,
187			ValidationReportPtr_t& validationReport) {
188		299473	return validate(config, validationReport, defaultFilter_);
189			}
190
191			bool RbPrmValidation::validate(const Configuration_t& config,
192			const std::vector<std::string>& filter) {
193			return validate(config, unusedReport_, filter);
194			}
195
196		347420	bool RbPrmValidation::validate(
197			const Configuration_t& config,
198			hpp::core::ValidationReportPtr_t& validationReport,
199			const std::vector<std::string>& filter) {
200			CollisionValidationReportPtr_t colReport =
201		694840	dynamic_pointer_cast<CollisionValidationReport>(validationReport);
202	✓✓	347420	if (colReport) colReport->result.clear();
203
204	✓✗✓✗ ✓✗	347420	RbprmValidationReportPtr_t rbprmReport(new RbprmValidationReport);
205
206	✓✗	347420	bool success = trunkValidation_->validate(config, validationReport);
207	✓✓	347420	if (success) {
208		297059	rbprmReport->trunkInCollision = false;
209			} else {
210			colReport =
211		50361	dynamic_pointer_cast<CollisionValidationReport>(validationReport);
212		50361	rbprmReport->object1 = colReport->object1;
213		50361	rbprmReport->object2 = colReport->object2;
214	✓✗	50361	rbprmReport->result = colReport->result;
215		50361	rbprmReport->trunkInCollision = true;
216			}
217			// always compute full report
218	✓✗✓✓ ✓✓	644256	success = validateRoms(config, filter, rbprmReport) && success &&
219	✓✗✓✗	296836	boundValidation_->validate(config, validationReport);
220		347420	validationReport = rbprmReport;
221		694840	return success;
222			}
223
224			bool RbPrmValidation::validateTrunk(
225			const Configuration_t& config,
226			hpp::core::ValidationReportPtr_t& validationReport) {
227			return trunkValidation_->validate(
228			config,
229			validationReport); // &&
230			// boundValidation_->validate(config,validationReport);
231			}
232
233		1047	void RbPrmValidation::addObstacle(const CollisionObjectConstPtr_t& object) {
234		1047	trunkValidation_->addObstacle(object);
235		1047	}
236
237			void RbPrmValidation::removeObstacleFromJoint(
238			const JointPtr_t& joint, const CollisionObjectPtr_t& obstacle) {
239			trunkValidation_->removeObstacleFromJoint(joint, obstacle);
240			for (T_RomValidation::const_iterator cit = romValidations_.begin();
241			cit != romValidations_.end(); ++cit) {
242			cit->second->removeObstacleFromJoint(joint, obstacle);
243			}
244			}
245
246		548	void RbPrmValidation::randomnizeCollisionPairs() {
247		2652	for (T_RomValidation::const_iterator cit = romValidations_.begin();
248	✓✓	4756	cit != romValidations_.end(); ++cit) {
249	✓✗	2104	cit->second->randomnizeCollisionPairs();
250			}
251		548	}
252
253		27040	void RbPrmValidation::computeAllContacts(bool computeAllContacts) {
254		113796	for (T_RomValidation::const_iterator cit = romValidations_.begin();
255	✓✓	200552	cit != romValidations_.end(); ++cit) {
256		86756	cit->second->computeAllContacts(computeAllContacts);
257			}
258		27040	}
259
260			} // namespace rbprm
261			} // namespace hpp