hpp-core  4.12.0
Implement basic classes for canonical path planning for kinematic chains.
collision-pair.hh
Go to the documentation of this file.
1 //
2 // Copyright (c) 2021 CNRS
3 // Authors: Joseph Mirabel
4 //
5 // This file is part of hpp-core
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 #ifndef HPP_CORE_COLLISION_PAIR_HH
20 # define HPP_CORE_COLLISION_PAIR_HH
21 
22 #include <hpp/fcl/collision.h>
23 #include <pinocchio/spatial/fcl-pinocchio-conversions.hpp>
24 
25 #include <hpp/pinocchio/device-data.hh>
26 #include <hpp/pinocchio/collision-object.hh>
27 #include <hpp/core/fwd.hh>
28 
29 namespace hpp {
30 namespace core {
31 
32 typedef std::vector<fcl::CollisionRequest> CollisionRequests_t;
33 
34 struct CollisionPair {
37  fcl::ComputeCollision computeCollision;
38 
40  : first(f), second(s), computeCollision(f->geometry().get(), s->geometry().get())
41  {}
42 
43  inline auto collide(fcl::CollisionRequest& request, fcl::CollisionResult& result) const
44  //decltype(computeCollision(tf1,tf2,request,result))
45  {
46  assert(!first ->getTransform().translation().hasNaN());
47  assert(!first ->getTransform().rotation ().hasNaN());
48  assert(!second->getTransform().translation().hasNaN());
49  assert(!second->getTransform().rotation ().hasNaN());
50  return computeCollision(
51  first ->getFclTransform(),
52  second->getFclTransform(),
53  request,result);
54  }
55 
56  inline auto collide(const pinocchio::DeviceData& d,
57  fcl::CollisionRequest& request, fcl::CollisionResult& result) const
58  //decltype(computeCollision(tf1,tf2,request,result))
59  {
60  using ::pinocchio::toFclTransform3f;
61  assert(!first ->getTransform(d).translation().hasNaN());
62  assert(!first ->getTransform(d).rotation ().hasNaN());
63  assert(!second->getTransform(d).translation().hasNaN());
64  assert(!second->getTransform(d).rotation ().hasNaN());
65  return computeCollision(
66  toFclTransform3f(first ->getTransform (d)),
67  toFclTransform3f(second->getTransform (d)),
68  request,result);
69  }
70 };
71 
72 } // namespace core
73 } // namespace hpp
74 #endif // HPP_CORE_COLLISION_PAIR_HH
Definition: bi-rrt-planner.hh:24
std::vector< fcl::CollisionRequest > CollisionRequests_t
Definition: collision-pair.hh:32
Definition: collision-pair.hh:34
CollisionObjectConstPtr_t first
Definition: collision-pair.hh:35
fcl::ComputeCollision computeCollision
Definition: collision-pair.hh:37
CollisionObjectConstPtr_t second
Definition: collision-pair.hh:36
CollisionPair(CollisionObjectConstPtr_t f, CollisionObjectConstPtr_t s)
Definition: collision-pair.hh:39
auto collide(fcl::CollisionRequest &request, fcl::CollisionResult &result) const
Definition: collision-pair.hh:43
pinocchio::CollisionObjectConstPtr_t CollisionObjectConstPtr_t
Definition: fwd.hh:90
auto collide(const pinocchio::DeviceData &d, fcl::CollisionRequest &request, fcl::CollisionResult &result) const
Definition: collision-pair.hh:56