Commits

Mark Moll  committed c1643dc

print helpful warnings if SO(2) or SO(3) states don't satisfy bounds (i.e., they don't represent proper rotations)

  • Participants
  • Parent commits 75dbc12

Comments (0)

Files changed (2)

File src/ompl/base/spaces/src/SO2StateSpace.cpp

 #include "ompl/tools/config/MagicConstants.h"
 #include <boost/math/constants/constants.hpp>
 
+// Define for boost version < 1.47
+#ifndef BOOST_ASSERT_MSG
+#define BOOST_ASSERT_MSG(expr, msg) assert(expr)
+#endif
+
 void ompl::base::SO2StateSampler::sampleUniform(State *state)
 {
     state->as<SO2StateSpace::StateType>()->value =
 {
     // assuming the states 1 & 2 are within bounds
     double d = fabs(state1->as<StateType>()->value - state2->as<StateType>()->value);
+    BOOST_ASSERT_MSG(d > -std::numeric_limits<float>::epsilon(),
+        "The SO(2) distance is negative. This means the corresponding states "
+        "are not within bounds. Call SO2StateSpace::enforceBounds() in, e.g., "
+        "ompl::control::ODESolver::PostPropagationEvent, "
+        "ompl::control::StatePropagator, or ompl::base::StateValidityChecker");
     return (d > boost::math::constants::pi<double>()) ? 2.0 * boost::math::constants::pi<double>() - d : d;
 }
 

File src/ompl/base/spaces/src/SO3StateSpace.cpp

 #include "ompl/tools/config/MagicConstants.h"
 #include <boost/math/constants/constants.hpp>
 
+// Define for boost version < 1.47
+#ifndef BOOST_ASSERT_MSG
+#define BOOST_ASSERT_MSG(expr, msg) assert(expr)
+#endif
+
 static const double MAX_QUATERNION_NORM_ERROR = 1e-9;
 
 /// @cond IGNORE
             const SO3StateSpace::StateType *qs1 = static_cast<const SO3StateSpace::StateType*>(state1);
             const SO3StateSpace::StateType *qs2 = static_cast<const SO3StateSpace::StateType*>(state2);
             double dq = fabs(qs1->x * qs2->x + qs1->y * qs2->y + qs1->z * qs2->z + qs1->w * qs2->w);
+            BOOST_ASSERT_MSG(dq < 1 + MAX_QUATERNION_NORM_ERROR,
+                "The SO(3) distance is larger than 1. This means the corresponding states "
+                "are not within bounds. Call SO3StateSpace::enforceBounds() in, e.g., "
+                "ompl::control::ODESolver::PostPropagationEvent, "
+                "ompl::control::StatePropagator, or ompl::base::StateValidityChecker");
             if (dq > 1.0 - MAX_QUATERNION_NORM_ERROR)
                 return 0.0;
             else