diff --git a/rotational_recovery/src/rotational_recovery.cpp b/rotational_recovery/src/rotational_recovery.cpp
index c2aacee1aa8121d0bc0c61c61b4bb414a62eeaf1..23aab7d28f739b070bb4262434dd65b91f24d530 100644
--- a/rotational_recovery/src/rotational_recovery.cpp
+++ b/rotational_recovery/src/rotational_recovery.cpp
@@ -171,7 +171,7 @@ namespace rotational_recovery
         }
 
       //if (next_cost > cost) {
-      if (t>0.7 && (next_cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE || next_cost == costmap_2d::LETHAL_OBSTACLE)) {
+      if (t>0.4 && (next_cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE || next_cost == costmap_2d::LETHAL_OBSTACLE)) {
         ROS_DEBUG_STREAM_NAMED ("cost", "Cost at " << t << " and pose " << forwardSimulate(current, twist, t)
                                 << " is " << next_cost << " which is greater than previous cost " << cost);
         break;
diff --git a/twist_recovery/src/twist_recovery.cpp b/twist_recovery/src/twist_recovery.cpp
index f2888d21b774d8b7d9436765aa934b2e6268f8fc..be56b0472a2f9915bd1b7e057de51cb25bee8963 100644
--- a/twist_recovery/src/twist_recovery.cpp
+++ b/twist_recovery/src/twist_recovery.cpp
@@ -157,7 +157,7 @@ namespace twist_recovery
     cost = cost_after_min_time;
     for (t=min_time+simulation_inc_; t<=duration_; t+=simulation_inc_) {
       const double next_cost = normalizedPoseCost(forwardSimulate(current_position, twist, t));
-      if (next_cost > cost) {
+      if ((next_cost > cost) || next_cost >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE) {
         ROS_DEBUG_STREAM_NAMED ("cost", "Cost after " << t << "seconds and pose " << forwardSimulate(current_position, twist, t)
         << " [" << next_cost << "] is greater than previous cost [" << cost << "]");
         break;