Commit 0105d1c7 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Add CollisionPair.

parent 69b7bfaf
......@@ -47,6 +47,7 @@ FIND_PACKAGE(Boost REQUIRED COMPONENTS unit_test_framework)
# Declare Headers
SET(${PROJECT_NAME}_HEADERS
include/hpp/core/bi-rrt-planner.hh
include/hpp/core/collision-pair.hh
include/hpp/core/collision-path-validation-report.hh
include/hpp/core/collision-validation.hh
include/hpp/core/relative-motion.hh
......
//
// Copyright (c) 2021 CNRS
// Authors: Joseph Mirabel
//
// This file is part of hpp-core
// 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/>.
#ifndef HPP_CORE_COLLISION_PAIR_HH
# define HPP_CORE_COLLISION_PAIR_HH
#include <hpp/fcl/collision.h>
#include <pinocchio/spatial/fcl-pinocchio-conversions.hpp>
#include <hpp/pinocchio/device-data.hh>
#include <hpp/pinocchio/collision-object.hh>
#include <hpp/core/fwd.hh>
namespace hpp {
namespace core {
struct CollisionPair {
CollisionObjectConstPtr_t first;
CollisionObjectConstPtr_t second;
fcl::ComputeCollision computeCollision;
inline CollisionPair (CollisionObjectConstPtr_t f, CollisionObjectConstPtr_t s)
: first(f), second(s), computeCollision(f->geometry().get(), s->geometry().get())
{}
inline auto collide(fcl::CollisionRequest& request, fcl::CollisionResult& result) const
//decltype(computeCollision(tf1,tf2,request,result))
{
assert(!first ->getTransform(d).translation().hasNaN());
assert(!first ->getTransform(d).rotation ().hasNaN());
assert(!second->getTransform(d).translation().hasNaN());
assert(!second->getTransform(d).rotation ().hasNaN());
return computeCollision(
first ->getFclTransform(),
second->getFclTransform(),
request,result);
}
inline auto collide(const pinocchio::DeviceData& d,
fcl::CollisionRequest& request, fcl::CollisionResult& result) const
//decltype(computeCollision(tf1,tf2,request,result))
{
using ::pinocchio::toFclTransform3f;
assert(!first ->getTransform(d).translation().hasNaN());
assert(!first ->getTransform(d).rotation ().hasNaN());
assert(!second->getTransform(d).translation().hasNaN());
assert(!second->getTransform(d).rotation ().hasNaN());
return computeCollision(
toFclTransform3f(first ->getTransform (d)),
toFclTransform3f(second->getTransform (d)),
request,result);
}
};
} // namespace core
} // namespace hpp
#endif // HPP_CORE_COLLISION_PAIR_HH
......@@ -24,6 +24,7 @@
# include <hpp/pinocchio/collision-object.hh>
# include <hpp/core/validation-report.hh>
# include <hpp/fcl/collision_data.h>
# include <hpp/core/collision-pair.hh>
namespace hpp {
namespace core {
......
......@@ -24,6 +24,7 @@
# include <hpp/fcl/collision_data.h>
# include <hpp/core/collision-pair.hh>
# include <hpp/core/collision-validation-report.hh>
# include <hpp/core/continuous-validation/interval-validation.hh>
......@@ -51,9 +52,6 @@ namespace hpp {
class BodyPairCollision : public IntervalValidation
{
public:
typedef std::pair<CollisionObjectConstPtr_t, CollisionObjectConstPtr_t> CollisionPair_t;
typedef std::vector<CollisionPair_t> CollisionPairs_t;
/// Validate interval centered on a path parameter
/// \param t parameter value in the path interval of definition
/// \param[in,out] interval as input, interval over which
......
......@@ -216,8 +216,8 @@ namespace hpp {
CenterOfMassComputationMap_t;
// Collision pairs
typedef std::pair <CollisionObjectConstPtr_t, CollisionObjectConstPtr_t>
CollisionPair_t;
struct CollisionPair;
typedef CollisionPair CollisionPair_t; // For backward compatibility.
typedef std::vector <CollisionPair_t> CollisionPairs_t;
class ExtractedPath;
......
......@@ -23,6 +23,7 @@
# include <hpp/core/config.hh>
# include <hpp/core/fwd.hh>
# include <hpp/core/relative-motion.hh>
# include <hpp/core/collision-pair.hh>
namespace hpp {
namespace core {
......@@ -166,8 +167,6 @@ namespace hpp {
public:
virtual ~ObstacleUser () = default;
typedef std::pair<CollisionObjectConstPtr_t, CollisionObjectConstPtr_t> CollisionPair_t;
typedef std::vector<CollisionPair_t> CollisionPairs_t;
typedef std::vector<fcl::CollisionRequest> CollisionRequests_t;
static bool collide (const CollisionPairs_t& pairs,
......
......@@ -50,13 +50,16 @@ namespace hpp {
fcl::CollisionResult collisionResult;
std::size_t iPair = 0;
const ObstacleUser::CollisionPairs_t* pairs (&cPairs_);
const CollisionPairs_t* pairs (&cPairs_);
ObstacleUser::CollisionRequests_t* requests (&cRequests_);
bool collide = ObstacleUser::collide (cPairs_, cRequests_,
collisionResult, iPair, device.d());
if (!collide && checkParameterized_) {
collide = ObstacleUser::collide (pPairs_, pRequests_,
collisionResult, iPair, device.d());
pairs = &pPairs_;
requests = &pRequests_;
}
if (collide) {
CollisionValidationReportPtr_t report (
......@@ -69,12 +72,10 @@ namespace hpp {
allReport->collisionReports.push_back(report);
// then loop over all the remaining pairs :
++iPair;
for (;iPair < cPairs_.size(); ++iPair) {
for (;iPair < pairs->size(); ++iPair) {
collisionResult.clear();
if (fcl::collide (
cPairs_[iPair].first ->fcl (device.d()),
cPairs_[iPair].second->fcl (device.d()),
cRequests_[iPair], collisionResult) != 0) {
if ((*pairs)[iPair].collide(device.d(),
(*requests)[iPair], collisionResult) != 0) {
CollisionValidationReportPtr_t report (
new CollisionValidationReport ((*pairs)[iPair], collisionResult));
allReport->collisionReports.push_back(report);
......
......@@ -23,6 +23,7 @@
#include <hpp/fcl/collision_data.h>
#include <hpp/fcl/collision.h>
#include <pinocchio/spatial/fcl-pinocchio-conversions.hpp>
#include <hpp/pinocchio/body.hh>
#include <hpp/pinocchio/collision-object.hh>
......@@ -32,6 +33,7 @@
namespace hpp {
namespace core {
namespace continuousValidation {
using ::pinocchio::toFclTransform3f;
bool BodyPairCollision::validateConfiguration (const value_type& t, interval_t& interval,
ValidationReportPtr_t& report,
......@@ -150,15 +152,12 @@ namespace hpp {
distanceLowerBound = numeric_limits <value_type>::infinity ();
assert (collisionRequest_.enable_distance_lower_bound == true);
const CollisionPairs_t& prs (pairs());
for (CollisionPairs_t::const_iterator _pair = prs.begin();
_pair != prs.end(); ++_pair) {
pinocchio::FclConstCollisionObjectPtr_t object_a = _pair->first ->fcl (data);
pinocchio::FclConstCollisionObjectPtr_t object_b = _pair->second->fcl (data);
for (const auto& pair : prs) {
fcl::CollisionResult result;
fcl::collide (object_a, object_b, collisionRequest_, result);
pair.collide(data, collisionRequest_, result);
// Get result
if (result.isCollision ()) {
setReport(report, result, *_pair);
setReport(report, result, pair);
return false;
}
if (result.distance_lower_bound < distanceLowerBound) {
......
......@@ -25,6 +25,8 @@
#include <hpp/pinocchio/device.hh>
#include <hpp/pinocchio/joint.hh>
#include <hpp/core/collision-pair.hh>
namespace hpp {
namespace core {
void DistanceBetweenObjects::addObstacle
......@@ -36,7 +38,7 @@ namespace hpp {
BodyPtr_t body = joint->linkedBody ();
if (body) {
for (size_type j = 0; j < body->nbInnerObjects(); ++j) {
collisionPairs_.push_back (CollisionPair_t (body->innerObjectAt(j), object));
collisionPairs_.emplace_back(body->innerObjectAt(j), object);
}
}
}
......@@ -60,7 +62,8 @@ namespace hpp {
const CollisionObjectConstPtr_t& obj1 = itCol->first;
const CollisionObjectConstPtr_t& obj2 = itCol->second;
distanceResults_[rank].clear ();
fcl::distance (obj1->fcl (), obj2->fcl (),
fcl::distance (obj1->geometry().get(), obj1->getFclTransform(),
obj2->geometry().get(), obj2->getFclTransform(),
distanceRequest, distanceResults_[rank]);
++rank;
}
......
......@@ -38,16 +38,8 @@ namespace hpp {
{
for (i = 0; i < pairs.size(); ++i) {
res.clear();
const CollisionObjectConstPtr_t& a (pairs[i].first );
const CollisionObjectConstPtr_t& b (pairs[i].second);
assert(!a->getTransform(data).translation().hasNaN());
assert(!a->getTransform(data).rotation ().hasNaN());
assert(!b->getTransform(data).translation().hasNaN());
assert(!b->getTransform(data).rotation ().hasNaN());
if (fcl::collide (
a->geometry().get(), toFclTransform3f(a->getTransform (data)),
b->geometry().get(), toFclTransform3f(b->getTransform (data)),
reqs[i], res) != 0) return true;
if (pairs[i].collide(data, reqs[i], res) != 0)
return true;
}
return false;
}
......@@ -147,8 +139,7 @@ namespace hpp {
hppDout(info, "Disabling collision between "
<< pair.first ->name() << " and "
<< pair.second->name());
if (fcl::collide (pair.first ->fcl (), pair.second->fcl (),
cRequests_[i], unused) != 0) {
if (pair.collide(cRequests_[i], unused) != 0) {
hppDout(warning, "Disabling collision detection between two "
"bodies in collision.");
}
......
......@@ -125,7 +125,9 @@ namespace hpp {
fcl::CollisionResult result;
fcl::CollisionRequestFlag flag = enableContact? fcl::CONTACT : fcl::NO_REQUEST ;
fcl::CollisionRequest collisionRequest (flag, 1);
fcl::collide (object1_->fcl (), object2_->fcl (), collisionRequest, result);
fcl::collide (object1_->geometry().get(), object1_->getFclTransform(),
object2_->geometry().get(), object2_->getFclTransform(),
collisionRequest, result);
return result;
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment