Expand sampling region

Author: rrwwee

path.py

@@ -93,7 +93,7 @@ def computepath(qinit,
 
         def plane_sampler():
             while True:
-                s = np.random.uniform(0.0, 1.0)
+                s = np.random.uniform(-1.0, 2.0)
                 t = np.random.uniform(-half_width, half_width)
                 xy = (1.0 - s) * p0_xy + s * p1_xy + perp * t
                 z = np.random.uniform(zmin, zmax)

rrt.py

@@ -13,7 +13,7 @@
 from pinocchio.utils import rotate
 
 from constants import EDGE_DISTANCE_TOL, DEFAULT_DISCRETISATION_STEPS, DEFAULT_CHECK_EDGE_STEPS
-from tools import setcubeplacement
+from tools import setcubeplacement, distanceToObstacle
 from constants import DEFAULT_VIZ_DELAY
 import time
 import sampling
@@ -192,7 +192,7 @@ def get_q_new(self,
                 # update seed for the next interpolation step if IK succeeded
                 if found and grasping_q is not None:
                     seed_grasping_q = grasping_q
-                if not found:
+                if not found or distanceToObstacle(self.robot, grasping_q) < 0.001:
                     # cannot progress past the previous valid step
                     if i - 1 == 0:
                         q_new_found = False