[exotica_ompl_solver] Add warning for unbounded rotation (#629), mino…

…r memory fix
ipab-slmc · Jul 11, 2019 · 5064c1f · 5064c1f
1 parent ebee64d
commit 5064c1f
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 13 deletions.
diff --git a/exotations/solvers/exotica_ompl_solver/src/ompl_exo.cpp b/exotations/solvers/exotica_ompl_solver/src/ompl_exo.cpp
@@ -134,31 +134,34 @@ void OMPLSE3RNStateSpace::SetBounds(SamplingProblemPtr &prob)
         addSubspace(ompl::base::StateSpacePtr(new ompl::base::RealVectorStateSpace(dim_ - 6)), 1.0);
     }
 
-    if (prob->GetBounds().size() == 2 * dim_)
+    auto bounds = prob->GetBounds();
+    if (bounds.size() == 2 * dim_)
     {
+        // NB: We can only bound the xyz, rotation is unbounded (!)
         ompl::base::RealVectorBounds SE3bounds(3);
         for (int i = 0; i < 3; ++i)
         {
-            SE3bounds.setHigh(i, prob->GetBounds()[i + dim_]);
-            SE3bounds.setLow(i, prob->GetBounds()[i]);
+            SE3bounds.setHigh(i, bounds[i + dim_]);
+            SE3bounds.setLow(i, bounds[i]);
         }
         getSubspace(0)->as<ompl::base::SE3StateSpace>()->setBounds(SE3bounds);
+        WARNING_NAMED("OMPLSE3RNStateSpace::SetBounds", "Orientation bounds on SE(3) component ignored.");
 
         if (dim_ > 6)
         {
             ompl::base::RealVectorBounds RNbounds(dim_ - 6);
             for (int i = 6; i < dim_; ++i)
             {
-                RNbounds.setHigh(i - 6, prob->GetBounds()[i + dim_]);
-                RNbounds.setLow(i - 6, prob->GetBounds()[i]);
+                RNbounds.setHigh(i - 6, bounds[i + dim_]);
+                RNbounds.setLow(i - 6, bounds[i]);
             }
             getSubspace(1)->as<ompl::base::RealVectorStateSpace>()->setBounds(RNbounds);
         }
     }
     else
     {
         ERROR("State space bounds were not specified!\n"
-              << prob->GetBounds().size() << " " << dim_);
+              << bounds.size() << " " << dim_);
     }
     setLongestValidSegmentFraction(init_.LongestValidSegmentFraction);
     lock();
@@ -217,15 +220,17 @@ void OMPLSE2RNStateSpace::SetBounds(SamplingProblemPtr &prob)
         addSubspace(ompl::base::StateSpacePtr(new ompl::base::RealVectorStateSpace(dim_ - 3)), 1.0);
     }
 
-    if (prob->GetBounds().size() == 2 * dim_)
+    auto bounds = prob->GetBounds();
+    if (bounds.size() == 2 * dim_)
     {
         ompl::base::RealVectorBounds SE2bounds(2);
         for (int i = 0; i < 3; ++i)
         {
-            SE2bounds.setHigh(i, prob->GetBounds()[i + dim_]);
-            SE2bounds.setLow(i, prob->GetBounds()[i]);
+            SE2bounds.setHigh(i, bounds[i + dim_]);
+            SE2bounds.setLow(i, bounds[i]);
         }
-        getSubspace(0)->as<ompl::base::SE3StateSpace>()->setBounds(SE2bounds);
+        getSubspace(0)->as<ompl::base::SE2StateSpace>()->setBounds(SE2bounds);
+        WARNING_NAMED("OMPLSE3RNStateSpace::SetBounds", "Yaw bounds on SE(2) component ignored.");
 
         if (dim_ > 3)
         {
@@ -241,7 +246,7 @@ void OMPLSE2RNStateSpace::SetBounds(SamplingProblemPtr &prob)
     else
     {
         ERROR("State space bounds were not specified!" << std::endl
-                                                       << prob->GetBounds().size() << " " << dim_);
+                                                       << bounds.size() << " " << dim_);
     }
     setLongestValidSegmentFraction(init_.LongestValidSegmentFraction);
     lock();

diff --git a/exotations/solvers/exotica_ompl_solver/src/ompl_solver.cpp b/exotations/solvers/exotica_ompl_solver/src/ompl_solver.cpp
@@ -122,10 +122,11 @@ void OMPLSolver<ProblemType>::SetGoalState(Eigen::VectorXdRefConst qT, const dou
         state_space_->as<OMPLStateSpace>()->StateDebug(qT);
 
         // Debug state and bounds
+        auto bounds = prob_->GetBounds();
         std::string out_of_bounds_joint_ids = "";
         for (int i = 0; i < qT.rows(); ++i)
-            if (qT(i) < prob_->GetBounds()[i] || qT(i) > prob_->GetBounds()[i + qT.rows()])
-                out_of_bounds_joint_ids += "[j" + std::to_string(i) + "=" + std::to_string(qT(i)) + ", ll=" + std::to_string(prob_->GetBounds()[i]) + ", ul=" + std::to_string(prob_->GetBounds()[i + qT.rows()]) + "]\n";
+            if (qT(i) < bounds[i] || qT(i) > bounds[i + qT.rows()])
+                out_of_bounds_joint_ids += "[j" + std::to_string(i) + "=" + std::to_string(qT(i)) + ", ll=" + std::to_string(bounds[i]) + ", ul=" + std::to_string(bounds[i + qT.rows()]) + "]\n";
 
         ThrowNamed("Invalid goal state [Invalid joint bounds for joint indices: \n"
                    << out_of_bounds_joint_ids << "]");