From f468403e3e9bfcb87db19c7f6f8555b1494a080e Mon Sep 17 00:00:00 2001 From: Sergim96 Date: Thu, 4 Dec 2025 10:45:53 +0000 Subject: [PATCH 1/3] fix remove_duplicates --- .../walkgen_surface_planner/SurfacePlanner.py | 12 +++++++- .../tools/geometry_utils.py | 30 ++++++++++++++++--- 2 files changed, 37 insertions(+), 5 deletions(-) diff --git a/walkgen_surface_planner/python/walkgen_surface_planner/SurfacePlanner.py b/walkgen_surface_planner/python/walkgen_surface_planner/SurfacePlanner.py index 8456b98..510fb13 100644 --- a/walkgen_surface_planner/python/walkgen_surface_planner/SurfacePlanner.py +++ b/walkgen_surface_planner/python/walkgen_surface_planner/SurfacePlanner.py @@ -46,6 +46,7 @@ from sl1m.problem_definition import Problem from sl1m.generic_solver import solve_MIP +from sl1m.solver import Solvers # --------------------------------- PROBLEM DEFINITION ---------------------------------------------------- paths = [ @@ -604,7 +605,16 @@ def run(self, q, gait_in, timings_in, bvref, target_foostep): # Solve MIP t0 = clock() - self.pb_data = solve_MIP(self.pb, costs=costs, com=self._com) + import traceback + + try: + self.pb_data = solve_MIP(self.pb, costs=costs, com=self._com, solver=Solvers.GUROBI) + except Exception as e: + print("Error while solving MIP:", e) + traceback.print_exc() + print("Skipping this MIP") + return self._selected_surfaces + t1 = clock() if self._RECORDING: self.profiler["timing_MIP"] = t1 - t0 diff --git a/walkgen_surface_processing/python/walkgen_surface_processing/tools/geometry_utils.py b/walkgen_surface_processing/python/walkgen_surface_processing/tools/geometry_utils.py index b1b698c..c6b86cc 100644 --- a/walkgen_surface_processing/python/walkgen_surface_processing/tools/geometry_utils.py +++ b/walkgen_surface_processing/python/walkgen_surface_processing/tools/geometry_utils.py @@ -223,8 +223,8 @@ def process_surfaces(surfacesIn, surfaces.pop(id_) # Add last surface remaining - if len(new_surfaces) > 0: - new_surfaces.append(surfaces[0].get_vertices_inner()) + # if len(new_surfaces) > 0: + new_surfaces.append(surfaces[0].get_vertices_inner()) return new_surfaces @@ -425,6 +425,28 @@ def order(points): output.pop(0) return [points[int(elt)].tolist() for elt in output] +# import visvalingamwyatt as vw +# def remove_duplicates(points, threshold=0.02): -def remove_duplicates(points): - return np.unique(points, axis=0) +# """ Decimate the number of point using visvalingamwyatt algorithm. +# """ +# # points = [[point.x, point.y] for point in hole.points ] +# simplifier = vw.Simplifier(points) +# points_filtered = simplifier.simplify(threshold=threshold) +# return np.array(points_filtered) + +def remove_duplicates(points, threshold=0.02): + filtered = [] + points = np.asarray(points) + + for p in points: + if len(filtered) == 0: + filtered.append(p) + continue + + # distance to all previously kept points + d = np.linalg.norm(np.array(filtered) - p, axis=1) + + if np.all(d > threshold): + filtered.append(p) + return np.array(filtered) From 8f505f06760836a020a5918ddc37c2a8655f9bca Mon Sep 17 00:00:00 2001 From: stonneau <4620966+stonneau@users.noreply.github.com> Date: Mon, 8 Dec 2025 14:24:55 +0000 Subject: [PATCH 2/3] removing non quasi static surfaces from planning but keeping them for removing ground area under --- .../tools/geometry_utils.py | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/walkgen_surface_processing/python/walkgen_surface_processing/tools/geometry_utils.py b/walkgen_surface_processing/python/walkgen_surface_processing/tools/geometry_utils.py index c6b86cc..5624a01 100644 --- a/walkgen_surface_processing/python/walkgen_surface_processing/tools/geometry_utils.py +++ b/walkgen_surface_processing/python/walkgen_surface_processing/tools/geometry_utils.py @@ -99,6 +99,12 @@ def reduce_surfaces(surface_list, n_points=None): return out_surface_list +def slope_quasi_static(surface): + n = surface.normal + z = np.array([0., 0., 1.]) + plane_angle_cos = np.dot(n, z) / (np.linalg.norm(n) * np.linalg.norm(z)) + theta = np.arccos(np.clip(plane_angle_cos, -1.0, 1.0)) + return (np.degrees(theta)<30) def process_surfaces(surfacesIn, polySize=10, @@ -197,7 +203,7 @@ def process_surfaces(surfacesIn, contours_intersect = [get_contour(vert_2D[:, 1:])] # No surface overllaping, keep initial surface. - if len(contours_intersect) == 0: + if len(contours_intersect) == 0 and slope_quasi_static(surfaces[id_]): new_surfaces.append(surfaces[id_].vertices_inner) # Surface overllaping, decompose the surface. @@ -214,9 +220,9 @@ def process_surfaces(surfacesIn, if method_type == DECOMPO_type.AREA or method_type == DECOMPO_type.AREA_CONVEX: # Keep the surface if the area > min_area # Or if the surface has not been decomposed - if get_Polygon(get_contour(vt)).area > min_area: + if get_Polygon(get_contour(vt)).area > min_area and slope_quasi_static(surfaces[id_]): new_surfaces.append(projection_surface(vt, surfaces[id_].equation)) - else: + elif slope_quasi_static(surfaces[id_]): new_surfaces.append(projection_surface(vt, surfaces[id_].equation)) # Remove the surface processed from the lists. From 81bfef19dd5daa70daad9effab8bd8def8e6b4c8 Mon Sep 17 00:00:00 2001 From: stonneau <4620966+stonneau@users.noreply.github.com> Date: Mon, 8 Dec 2025 17:35:52 +0000 Subject: [PATCH 3/3] duplicates removes some surfaces --- .../tools/geometry_utils.py | 46 +++++++++---------- 1 file changed, 23 insertions(+), 23 deletions(-) diff --git a/walkgen_surface_processing/python/walkgen_surface_processing/tools/geometry_utils.py b/walkgen_surface_processing/python/walkgen_surface_processing/tools/geometry_utils.py index 5624a01..5db9e5e 100644 --- a/walkgen_surface_processing/python/walkgen_surface_processing/tools/geometry_utils.py +++ b/walkgen_surface_processing/python/walkgen_surface_processing/tools/geometry_utils.py @@ -431,28 +431,28 @@ def order(points): output.pop(0) return [points[int(elt)].tolist() for elt in output] -# import visvalingamwyatt as vw -# def remove_duplicates(points, threshold=0.02): - -# """ Decimate the number of point using visvalingamwyatt algorithm. -# """ -# # points = [[point.x, point.y] for point in hole.points ] -# simplifier = vw.Simplifier(points) -# points_filtered = simplifier.simplify(threshold=threshold) -# return np.array(points_filtered) - +import visvalingamwyatt as vw def remove_duplicates(points, threshold=0.02): - filtered = [] - points = np.asarray(points) - for p in points: - if len(filtered) == 0: - filtered.append(p) - continue - - # distance to all previously kept points - d = np.linalg.norm(np.array(filtered) - p, axis=1) - - if np.all(d > threshold): - filtered.append(p) - return np.array(filtered) + """ Decimate the number of point using visvalingamwyatt algorithm. + """ + # points = [[point.x, point.y] for point in hole.points ] + simplifier = vw.Simplifier(points) + points_filtered = simplifier.simplify(threshold=threshold) + return np.array(points_filtered) + +# ~ def remove_duplicates(points, threshold=0.02): + # ~ filtered = [] + # ~ points = np.asarray(points) + + # ~ for p in points: + # ~ if len(filtered) == 0: + # ~ filtered.append(p) + # ~ continue + + # ~ # distance to all previously kept points + # ~ d = np.linalg.norm(np.array(filtered) - p, axis=1) + + # ~ if np.all(d > threshold): + # ~ filtered.append(p) + # ~ return np.array(filtered)