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 
41  FCLContinuousMotionValidator(const ob::SpaceInformationPtr &si, MotionModel mm) : ob::MotionValidator(si)
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 != nullptr)
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 == nullptr)
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_ == nullptr)
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 == nullptr)
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 == nullptr)
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
The base class for space information. This contains all the information about the space planning is d...
Wrapper for FCL collision and distance checking.
FCLContinuousMotionValidator(ob::SpaceInformation *si, MotionModel mm)
Constructor.
FCLContinuousMotionValidator(const ob::SpaceInformationPtr &si, MotionModel mm)
Constructor.
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition: StateSpace.h:71
MotionModel
Specify whether bodies are moving in 2D or bodies moving in 3D.
Definition of an abstract state.
Definition: State.h:50
This namespace contains sampling based planning routines shared by both planning under geometric cons...
bool checkMotion(const ob::State *s1, const ob::State *s2) const override
Returns true if motion between s1 and s2 is collision free.
virtual void interpolate(const State *from, const State *to, 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....
ob::StateSpace * stateSpace_
Handle to the statespace that this motion validator operates in.
void freeState(State *state) const
Free the memory of a state.
void defaultSettings(MotionModel mm)
Restore settings to default values.
void copyState(State *destination, const State *source) const
Copy a state to another.
A motion validator that utilizes FCL's continuous collision checking.
MotionValidator(SpaceInformation *si)
Constructor.
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...
Abstract definition for a class checking the validity of motions – path segments between states....
const StateValidityCheckerPtr & getStateValidityChecker() const
Return the instance of the used state validity checker.
State * allocState() const
Allocate memory for a state.
unsigned int invalid_
Number of invalid segments.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
SpaceInformation * si_
The instance of space information this state validity checker operates on.
FCLMethodWrapperPtr fclWrapper_
Wrapper for FCL collision and distance methods.
const StateSpacePtr & getStateSpace() const
Return the instance of the used state space.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
The exception type for ompl.
Definition: Exception.h:47
unsigned int valid_
Number of valid segments.
bool isValid(const State *state) const
Check if a given state is valid or not.
Main namespace. Contains everything in this library.
Definition: AppBase.h:22
~FCLContinuousMotionValidator() override=default
Destructor.