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(fcl::Vec3f &trans, fcl::Quaternion3f &quat, const ob::State *state) const
42  {
43  const type * derived = static_cast <const type*> (state);
44 
45  trans.setValue (derived->getX (), derived->getY (), derived->getZ ());
46  quat.getW () = derived->rotation ().w;
47  quat.getX () = derived->rotation ().x;
48  quat.getY () = derived->rotation ().y;
49  quat.getZ () = derived->rotation ().z;
50  }
51  };
52 
53  template<>
54  struct OMPL_FCL_StateType<Motion_2D>
55  {
56  using type = ob::SE2StateSpace::StateType;
57 
58  void FCLPoseFromState (fcl::Vec3f &trans, fcl::Quaternion3f &quat, const ob::State *state) const
59  {
60  static const fcl::Vec3f zaxis(0., 0., 1.);
61  const type * derived = static_cast <const type*> (state);
62 
63  trans.setValue (derived->getX (), derived->getY (), 0.0);
64  quat.fromAxisAngle(zaxis, derived->getYaw ());
65  }
66  };
68 
70  template<MotionModel T>
72  {
73  public:
75  const GeometricStateExtractor &se, bool selfCollision)
77  fclWrapper_(std::make_shared<FCLMethodWrapper>(geom, se, selfCollision,
78  [this](fcl::Vec3f &trans, fcl::Quaternion3f &quat, const ob::State *state)
79  {
80  stateConvertor_.FCLPoseFromState(trans, quat, state);
81  }))
82  {
83  specs_.clearanceComputationType = base::StateValidityCheckerSpecs::EXACT;
84  }
85 
86  ~FCLStateValidityChecker () override = default;
87 
90  bool isValid (const ob::State *state) const override
91  {
92  return si_->satisfiesBounds (state) && fclWrapper_->isValid (state);
93  }
94 
96  double clearance (const ob::State *state) const override
97  {
98  return fclWrapper_->clearance (state);
99  }
100 
101  const FCLMethodWrapperPtr& getFCLWrapper () const
102  {
103  return fclWrapper_;
104  }
105 
106  protected:
107 
109  OMPL_FCL_StateType<T> stateConvertor_;
110 
112  FCLMethodWrapperPtr fclWrapper_;
113 
114  };
115  }
116 }
117 
118 #endif
OMPL_FCL_StateType< T > stateConvertor_
Object to convert a configuration of the robot to a type desirable for FCL.
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:577
bool isValid(const ob::State *state) const override
Checks whether the given robot state collides with the environment or itself.
Abstract definition for a class checking the validity of states. The implementation of this class mus...
Exact clearance computation is available.
A shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition: State.h:49
This namespace contains sampling based planning routines shared by both planning under geometric cons...
Definition: Cost.h:44
Wrapper for FCL collision and distance checking.
double clearance(const ob::State *state) const override
Returns the minimum distance from the given robot state and the environment.
FCLMethodWrapperPtr fclWrapper_
Wrapper for FCL collision and distance methods.