Updated submodules

examples/test_turtlebot_motion.py

@@ -22,13 +22,15 @@
     Pose, get_all_links, can_collide, aabb_overlap, set_collision_pair_mask, randomize, DEFAULT_RESOLUTION, \
     base_aligned_z, load_pybullet, get_collision_fn, get_custom_limits, get_limits_fn, \
     get_joint_velocities, control_joint, get_time_step, remove_handles, Interval, get_distance, \
-    get_duration_fn, velocity_control_joint, get_max_velocities, get_difference, plan_base_joint_motion
+    get_duration_fn, velocity_control_joint, get_max_velocities, get_difference, plan_base_joint_motion, UNBOUNDED_LIMITS
 
 from motion_planners.trajectory.smooth import smooth_curve
 from motion_planners.trajectory.linear import solve_multi_linear, quickest_inf_accel
 from motion_planners.trajectory.limits import check_spline
 from motion_planners.utils import waypoints_from_path, default_selector, irange
 from motion_planners.trajectory.discretize import time_discretize_curve
+#from motion_planners.tkinter.samplers import get_cost_fn
+#from motion_planners.lazy_prm import ROADMAPS # TODO: draw roadmap
 
 from pybullet_tools.retime import interpolate_path, sample_curve
 
@@ -421,7 +423,7 @@ def step_curve(robot, joints, path, step_size=None):
 
 ##################################################
 
-def iterate_path(robot, joints, path, simulate=True, step_size=None, resolutions=None, smooth=True, **kwargs): # 1e-2 | None
+def iterate_path(robot, joints, path, simulate=False, step_size=None, resolutions=None, smooth=False, **kwargs): # 1e-2 | None
     if path is None:
         return
     path = adjust_path(robot, joints, path)
@@ -445,7 +447,9 @@ def iterate_path(robot, joints, path, simulate=True, step_size=None, resolutions
         curve_collision_fn = get_curve_collision_fn(robot, joints, resolutions=resolutions, **kwargs)
         with LockRenderer():
             with BodySaver(robot):
-                curve = smooth_curve(curve, MAX_VELOCITIES, MAX_ACCELERATIONS, curve_collision_fn, max_time=5) #, curve_collision_fn=[])
+                with Profiler():
+                    curve = smooth_curve(curve, MAX_VELOCITIES, MAX_ACCELERATIONS,
+                                         curve_collision_fn, max_time=5) #, curve_collision_fn=[])
         path = [conf for t, conf in sample_curve(curve, time_step=step_size)]
         print('Steps: {} | Start: {:.3f} | End: {:.3f} | Knots: {}'.format(
             len(path), curve.x[0], curve.x[-1], len(curve.x)))
@@ -533,6 +537,8 @@ def main():
                         help='')
     parser.add_argument('-l', '--lock', action='store_false',
                         help='')
+    parser.add_argument('-r', '--reversible', action='store_true',
+                        help='')
     parser.add_argument('-s', '--seed', default=None, type=int, # None | 1
                         help='The random seed to use.')
     parser.add_argument('-n', '--num', default=10, type=int,
@@ -574,6 +580,7 @@ def main():
     resolutions = 1.*DEFAULT_RESOLUTION*np.ones(len(base_joints))
     plan_joints = base_joints[:2] if not args.orientation else base_joints
     holonomic = args.holonomic or (len(plan_joints) != 3)
+    #cost_fn = get_cost_fn()
 
     if args.cfree:
         obstacles = []
@@ -587,20 +594,25 @@ def main():
 
     #########################
 
+    # TODO: filter if straight-line path is feasible
     saver = WorldSaver()
     start_time = time.time()
-    profiler = Profiler(field='cumtime', num=50) # tottime | cumtime | None
-    profiler.save()
     with LockRenderer(lock=args.lock):
-        # TODO: draw the search tree
-        path = plan_base_joint_motion(robot, plan_joints, goal_conf[:len(plan_joints)], holonomic=holonomic,
-                                      obstacles=obstacles, self_collisions=False,
-                                      custom_limits=custom_limits, resolutions=resolutions[:len(plan_joints)],
-                                      use_aabb=True, cache=True, max_distance=MIN_PROXIMITY,
-                                      algorithm=args.algorithm, restarts=5, max_iterations=50, smooth=20) # 20 | None
+        with Profiler(field='tottime', num=25): # tottime | cumtime | None
+            # TODO: draw the search tree
+            path = plan_base_joint_motion(
+                robot, plan_joints, goal_conf[:len(plan_joints)],
+                holonomic=holonomic, reversible=args.reversible,
+                obstacles=obstacles, self_collisions=False,
+                custom_limits=custom_limits, resolutions=resolutions[:len(plan_joints)],
+                use_aabb=True, cache=True, max_distance=MIN_PROXIMITY,
+                weights=np.reciprocal(resolutions[:len(plan_joints)]),
+                circular={2: UNBOUNDED_LIMITS if holonomic else CIRCULAR_LIMITS},
+                algorithm=args.algorithm, verbose=True,
+                restarts=5, max_iterations=50,
+                smooth=0 if args.holonomic else None) # 20 | None
         saver.restore()
     #wait_for_duration(duration=1e-3)
-    profiler.restore()
 
     #########################
 

pybullet_tools/utils.py

@@ -1753,7 +1753,9 @@ def rescale_interval(value, old_interval=UNIT_LIMITS, new_interval=UNIT_LIMITS):
 
 def wrap_interval(value, interval=UNIT_LIMITS):
     lower, upper = interval
-    assert lower <= upper
+    if (lower == -INF) and (+INF == upper):
+        return value
+    assert -INF < lower <= upper < +INF
     return (value - lower) % (upper - lower) + lower
 
 def interval_distance(value1, value2, interval=UNIT_LIMITS):
@@ -1771,7 +1773,11 @@ def wrap_angle(theta, **kwargs):
     return wrap_interval(theta, interval=circular_interval(**kwargs))
 
 def circular_difference(theta2, theta1, **kwargs):
-    return wrap_angle(theta2 - theta1, **kwargs)
+    interval = circular_interval(**kwargs)
+    #extent = get_interval_extent(interval) # TODO: combine with motion_planners
+    extent = get_aabb_extent(interval)
+    diff_interval = Interval(-extent/2, +extent/2)
+    return wrap_interval(theta2 - theta1, interval=diff_interval)
 
 def base_values_from_pose(pose, tolerance=1e-3):
     (point, quat) = pose
@@ -3749,7 +3755,7 @@ def fn(q1, q2):
         for i in range(num_steps):
             positions = (1. / (num_steps - i)) * np.array(difference_fn(q2, q)) + q
             q = tuple(positions)
-            #q = tuple(wrap_positions(body, joints, positions))
+            #q = tuple(wrap_positions(body, joints, positions)) # TODO: need wrap - issue with adjust path though
             yield q
     return fn
 
@@ -4066,11 +4072,12 @@ def extend_fn(q1, q2):
         return path
     return extend_fn
 
-def get_dubins_extend_fn(body, joints, turning_radius=1e-1, # meters
+def get_dubins_extend_fn(body, joints, turning_radius=1e-3, # meters
                          step_size=0.5, # meters
                          **kwargs):
     assert len(joints) == 3
     import dubins
+    # TODO: reversible (Reeds-Shepp curves)
 
     def extend_fn(q1, q2):
         dubins_path = dubins.shortest_path(q1, q2, turning_radius) # dubins.path
@@ -4141,6 +4148,7 @@ def plan_nonholonomic_motion(body, joints, end_conf, obstacles=[], attachments=[
                                                linear_tol=linear_tol) #, angular_tol=angular_tol)
     extend_fn = get_nonholonomic_extend_fn(body, joints, resolutions=resolutions, reversible=reversible,
                                            linear_tol=linear_tol, angular_tol=angular_tol)
+    #extend_fn = get_dubins_extend_fn(body, joints, resolutions=resolutions, reversible=reversible)
     collision_fn = get_collision_fn(body, joints, obstacles, attachments,
                                     self_collisions, disabled_collisions,
                                     custom_limits=custom_limits, max_distance=max_distance,