FCLContinuousMotionValidator.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_CONTINUOUS_MOTION_VALIDATOR_
14 #define OMPLAPP_GEOMETRY_DETAIL_FCL_CONTINUOUS_MOTION_VALIDATOR_
15 
16 #include <ompl/base/MotionValidator.h>
17 #include <ompl/base/SpaceInformation.h>
18 
19 #include "omplapp/geometry/detail/FCLMethodWrapper.h"
20 #include "omplapp/geometry/detail/FCLStateValidityChecker.h"
21 #include "omplapp/geometry/GeometrySpecification.h"
22 
23 namespace ob = ompl::base;
24 
25 namespace ompl
26 {
27  namespace app
28  {
31  {
32  public:
33 
36  {
37  defaultSettings(mm);
38  }
39 
42  {
43  defaultSettings(mm);
44  }
45 
47  ~FCLContinuousMotionValidator() override = default;
48 
50  bool checkMotion(const ob::State *s1, const ob::State *s2) const override
51  {
52  double unused;
53 
54  // assume motion starts in a valid configuration so s1 is valid
55  // Must check validity of s2 before performing collision check between s1 and s2
56  bool valid = si_->isValid(s2) && fclWrapper_->isValid (s1, s2, unused);
57 
58  // Increment valid/invalid motion counters
59  valid ? valid_++ : invalid_++;
60 
61  return valid;
62  }
63 
67  bool checkMotion(const ob::State *s1, const ob::State *s2, std::pair<ob::State*, double> &lastValid) const override
68  {
69  bool valid = false;
70 
71  // if there is a collision, collisionTime will contain the time to collision,
72  // parameterized from [0,1), where s1 is 0 and s2 is 1.
73  double collisionTime;
74  valid = fclWrapper_->isValid (s1, s2, collisionTime);
75 
76  // Find the last valid state before collision...
77  // NOTE: This should probably be refactored so that the continuous checker
78  // returns the last valid time. Last valid transformation may also be
79  // possible, but that introduces a dependency on the geometry.
80  if (!valid)
81  {
82  ob::State *lastValidState;
83  if (lastValid.first)
84  lastValidState = lastValid.first;
85  else
86  lastValidState = si_->allocState ();
87 
88  collisionTime -= 0.01;
89  stateSpace_->interpolate (s1, s2, collisionTime, lastValidState);
90 
91  while (!si_->isValid (lastValidState) && collisionTime > 0)
92  {
93  collisionTime -= 0.01;
94  stateSpace_->interpolate (s1, s2, collisionTime, lastValidState);
95  }
96 
97  // ensure that collisionTime is greater than zero
98  if (collisionTime < 0.01)
99  {
100  collisionTime = 0.0;
101  si_->copyState (lastValidState, s1);
102  }
103 
104  lastValid.second = collisionTime;
105 
106  if (!lastValid.first)
107  si_->freeState (lastValidState);
108  }
109 
110  // Increment valid/invalid motion counters
111  valid ? valid_++ : invalid_++;
112 
113  return valid;
114  }
115 
116  protected:
117 
120  {
121  stateSpace_ = si_->getStateSpace().get();
122  if (!stateSpace_)
123  throw Exception("No state space for motion validator");
124 
125  // Extract FCLWrapper from FCLStateValidityChecker.
126  switch (mm)
127  {
128  case app::Motion_2D:
129  const app::FCLStateValidityChecker<app::Motion_2D> *fcl_2d_state_checker;
130  fcl_2d_state_checker = dynamic_cast <const app::FCLStateValidityChecker<app::Motion_2D>* > (si_->getStateValidityChecker ().get ());
131 
132  if (!fcl_2d_state_checker)
133  {
134  // Be extra verbose in this fatal error
135  OMPL_ERROR("Unable to cast state validity checker to FCLStateValidityChecker.");
136  assert (fcl_2d_state_checker != 0);
137  }
138 
139  fclWrapper_ = fcl_2d_state_checker->getFCLWrapper ();
140  break;
141 
142  case app::Motion_3D:
143  const app::FCLStateValidityChecker<app::Motion_3D> *fcl_3d_state_checker;
144  fcl_3d_state_checker = dynamic_cast <const app::FCLStateValidityChecker<app::Motion_3D>* > (si_->getStateValidityChecker ().get ());
145 
146  if (!fcl_3d_state_checker)
147  {
148  // Be extra verbose in this fatal error
149  OMPL_ERROR("Unable to cast state validity checker to FCLStateValidityChecker.");
150  assert (fcl_3d_state_checker != 0);
151  }
152 
153  fclWrapper_ = fcl_3d_state_checker->getFCLWrapper ();
154  break;
155 
156  default:
157  OMPL_WARN("Unknown motion model specified: %u", mm);
158  break;
159  }
160 
161  if (!fclWrapper_)
162  {
163  // Be extra verbose in this fatal error
164  OMPL_ERROR("FCLWrapper object is not valid.");
165  assert (fclWrapper_ != 0);
166  }
167  }
168 
170  FCLMethodWrapperPtr fclWrapper_;
171 
174  };
175  }
176 }
177 
178 #endif
unsigned int invalid_
Number of invalid segments.
const StateSpacePtr & getStateSpace() const
Return the instance of the used state space.
A motion validator that utilizes FCL&#39;s continuous collision checking.
void freeState(State *state) const
Free the memory of a state.
bool isValid(const State *state) const
Check if a given state is valid or not.
ob::StateSpace * stateSpace_
Handle to the statespace that this motion validator operates in.
State * allocState() const
Allocate memory for a state.
FCLContinuousMotionValidator(const ob::SpaceInformationPtr &si, MotionModel mm)
Constructor.
FCLMethodWrapperPtr fclWrapper_
Wrapper for FCL collision and distance methods.
void defaultSettings(MotionModel mm)
Restore settings to default values.
SpaceInformation * si_
The instance of space information this state validity checker operates on.
unsigned int valid_
Number of valid segments.
Abstract definition for a class checking the validity of motions – path segments between states...
~FCLContinuousMotionValidator() override=default
Destructor.
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
bool checkMotion(const ob::State *s1, const ob::State *s2, std::pair< ob::State *, double > &lastValid) const override
Checks the motion between s1 and s2. If the motion is invalid, lastValid contains the last valid stat...
void copyState(State *destination, const State *source) const
Copy a state to another.
const StateValidityCheckerPtr & getStateValidityChecker() const
Return the instance of the used state validity checker.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
A shared pointer wrapper for ompl::base::SpaceInformation.
MotionModel
Specify whether bodies are moving in 2D or bodies moving in 3D.
The base class for space information. This contains all the information about the space planning is d...
Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.
Definition: StateSpace.h:70
bool checkMotion(const ob::State *s1, const ob::State *s2) const override
Returns true if motion between s1 and s2 is collision free.
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
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
MotionValidator(SpaceInformation *si)
Constructor.
The exception type for ompl.
Definition: Exception.h:46
Wrapper for FCL collision and distance checking.
virtual void interpolate(const State *from, const State *to, const double t, State *state) const =0
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state...
FCLContinuousMotionValidator(ob::SpaceInformation *si, MotionModel mm)
Constructor.