FCLStateValidityChecker.h
1 /*********************************************************************
2 * Rice University Software Distribution License
3 *
4 * Copyright (c) 2011, Rice University
5 * All Rights Reserved.
6 *
7 * For a full description see the file named LICENSE.
8 *
9 *********************************************************************/
10 
11 /* Author: Ryan Luna */
12 
13 #ifndef OMPLAPP_GEOMETRY_DETAIL_FCL_COLLISION_CHECKER_
14 #define OMPLAPP_GEOMETRY_DETAIL_FCL_COLLISION_CHECKER_
15 
16 #include "omplapp/config.h"
17 
18 #include <ompl/base/SpaceInformation.h>
19 #include <ompl/base/StateValidityChecker.h>
20 #include <ompl/base/spaces/SE2StateSpace.h>
21 #include <ompl/base/spaces/SE3StateSpace.h>
22 
23 #include "omplapp/geometry/detail/FCLMethodWrapper.h"
24 #include "omplapp/geometry/GeometrySpecification.h"
25 
26 // Boost and STL headers
27 #include <memory>
28 
29 namespace ob = ompl::base;
30 
31 namespace ompl
32 {
33  namespace app
34  {
36  template<MotionModel T>
37  struct OMPL_FCL_StateType
38  {
39  using type = ob::SE3StateSpace::StateType;
40 
41  void FCLPoseFromState(FCLMethodWrapper::Transform &tf, const ob::State *state) const
42  {
43  const auto *derived = static_cast <const type*>(state);
44  const auto &q = derived->rotation();
45 #if FCL_MAJOR_VERSION==0 && FCL_MINOR_VERSION<6
46  tf = FCLMethodWrapper::Transform(
47  FCLMethodWrapper::Quaternion(q.w, q.x, q.y, q.z),
48  FCLMethodWrapper::Vector3(derived->getX(), derived->getY(), derived->getZ())
49  );
50 #else
51  tf = fcl::Translation3d(derived->getX(), derived->getY(), derived->getZ()) *
52  FCLMethodWrapper::Quaternion(q.w, q.x, q.y, q.z);
53 #endif
54  }
55  };
56 
57  template<>
58  struct OMPL_FCL_StateType<Motion_2D>
59  {
60  using type = ob::SE2StateSpace::StateType;
61 
62  void FCLPoseFromState(FCLMethodWrapper::Transform &tf, const ob::State *state) const
63  {
64  static const FCLMethodWrapper::Vector3 zaxis(0., 0., 1.);
65  const auto * derived = static_cast <const type*>(state);
66 #if FCL_MAJOR_VERSION==0 && FCL_MINOR_VERSION<6
67  FCLMethodWrapper::Quaternion q;
68  q.fromAxisAngle(zaxis, derived->getYaw());
69  tf = FCLMethodWrapper::Transform(q,
70  FCLMethodWrapper::Vector3(derived->getX(), derived->getY(), 0.)
71  );
72 #else
73  tf = fcl::Translation3d(derived->getX(), derived->getY(), 0.) *
74  fcl::AngleAxisd(derived->getYaw(), zaxis);
75 #endif
76  }
77  };
79 
81  template<MotionModel T>
82  class FCLStateValidityChecker : public ob::StateValidityChecker
83  {
84  public:
85  FCLStateValidityChecker(const ob::SpaceInformationPtr &si, const GeometrySpecification &geom,
86  const GeometricStateExtractor &se, bool selfCollision)
87  : ob::StateValidityChecker(si),
88  fclWrapper_(std::make_shared<FCLMethodWrapper>(geom, se, selfCollision,
89  [this](FCLMethodWrapper::Transform &tf, const ob::State *state)
90  {
91  stateConvertor_.FCLPoseFromState(tf, state);
92  }))
93  {
95  }
96 
97  ~FCLStateValidityChecker() override = default;
98 
101  bool isValid(const ob::State *state) const override
102  {
103  return si_->satisfiesBounds(state) && fclWrapper_->isValid(state);
104  }
105 
107  double clearance(const ob::State *state) const override
108  {
109  return fclWrapper_->clearance(state);
110  }
111 
112  const FCLMethodWrapperPtr& getFCLWrapper() const
113  {
114  return fclWrapper_;
115  }
116 
117  protected:
118 
120  OMPL_FCL_StateType<T> stateConvertor_;
121 
123  FCLMethodWrapperPtr fclWrapper_;
124 
125  };
126  }
127 }
128 
129 #endif
bool isValid(const ob::State *state) const override
Checks whether the given robot state collides with the environment or itself.
Definition of an abstract state.
Definition: State.h:113
@ EXACT
Exact clearance computation is available.
This namespace contains sampling based planning routines shared by both planning under geometric cons...
FCLMethodWrapperPtr fclWrapper_
Wrapper for FCL collision and distance methods.
ClearanceComputationType clearanceComputationType
Value indicating the kind of clearance computation this StateValidityChecker can compute (if any).
StateValidityCheckerSpecs specs_
The specifications of the state validity checker (its capabilities)
double clearance(const ob::State *state) const override
Returns the minimum distance from the given robot state and the environment.
StateValidityChecker(SpaceInformation *si)
Constructor.
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:641
bool satisfiesBounds(const State *state) const
Check if a state is inside the bounding box.
SpaceInformation * si_
The instance of space information this state validity checker operates on.
OMPL_FCL_StateType< T > stateConvertor_
Object to convert a configuration of the robot to a type desirable for FCL.
Abstract definition for a class checking the validity of states. The implementation of this class mus...
Main namespace. Contains everything in this library.
Definition: AppBase.h:21