v2: adding support for apf 2d

testV2/.gitignore

@@ -0,0 +1,10 @@
+bin/
+lib/
+include/
+share/
+.DS_Store
+pyvenv.cfg
+__pycache__/*
+__pycache__/
+__pycache__*
+*.prof

testV2/Makefile

@@ -0,0 +1,20 @@
+env:
+	python3 -m venv .
+	./bin/pip3 install numpy matplotlib PyQt5 opencv-python snakeviz open3d
+
+run:
+	./bin/python3 main.py
+
+prof:
+	./bin/python3 prof.py field_calculate.prof 40
+	./bin/snakeviz 40_field_calculate.prof
+
+clear:
+	rm -rf ./bin
+	rm -rf ./etc
+	rm -rf ./include
+	rm -rf ./lib
+	rm -rf ./lib64
+	rm -rf ./share
+	rm -rf ./__pycache__
+	rm -rf ./pyvenv.cfg

testV2/README.md

@@ -0,0 +1,3 @@
+# Links
+https://www.desmos.com/calculator/vo9bz6ca5y
+

testV2/apf.py

@@ -0,0 +1,78 @@
+"""
+Title: Calculate APF
+Author: Giovanni Rasera
+"""
+
+import numpy as np
+
+DEFAULT_DIM_SIZE: int = 100
+
+# TODO: ragi control parameters
+α = 50
+β = 140
+s = 8
+
+class Obstacle2D():
+    def __init__(self, x: float, y: float):
+        print(f"Obstacle2D({x}, {y})")
+        self.x = np.array(x)
+        self.y = np.array(y)
+
+class Goal2D():
+    def __init__(self, x: float, y: float):
+        print(f"Goal2D({x}, {y})")
+        self.x = np.array(x)
+        self.y = np.array(y)
+
+def calc_field(p0: np.array, v_1: np.array, v_2: np.array, c: float = 57.0, t: float = 1) -> np.array:
+    v1_m = np.array([0, 0])
+    v2_m = np.array([0, 0])
+    v1possible = False
+    v2possible = False
+    if (v_1[0] != 0 or v_1[1] != 0):
+        v1_m = v_1 / ((v_1[0] ** 2 + v_1[1] ** 2)**(1/2))
+        v1possible = True
+
+    if (v_2[0] != 0 or v_2[1] != 0):
+        v2_m = v_2 / ((v_2[0] ** 2 + v_2[1] ** 2)**(1/2))
+        v2possible = True
+
+    if(v1possible and v2possible):
+        v1_n = 1 / (np.dot(v_1, v_1) + c) * t
+        v2_n = 1 / (np.dot(v_2, v_2) + c) * t
+        v_3 = p0 + (c + 1) * (v1_m * v1_n + v2_m * v2_n)
+        return v_3
+    else:
+        return p0
+
+class APF2D():
+
+
+    def __init__(self, s_ize: int = DEFAULT_DIM_SIZE):
+        print(f"APF2D([{s_ize}][{s_ize}])")
+        self.s_ize = s_ize
+        self.a = np.array([i+0.5 for i in range(0, s_ize+1)])
+        self.b = np.array([i+0.5 for i in range(0, s_ize+1)])
+        self.halfPoints = np.array([(y, x) for x in self.a for y in self.b])
+        # goal
+        self.goal = np.array([5, 0])
+        # obstacle
+        self.obstacle = np.array([5, 10])
+        self.v1 = np.array([(xp1 - self.goal[0], yp1 - self.goal[1]) for (xp1, yp1) in self.halfPoints])
+        self.v2 = np.array([(-(xp2 - self.obstacle[0]), -(yp2 - self.obstacle[1])) for (xp2, yp2) in self.halfPoints])
+
+        self.c = 100.0
+        self.t = 0.5
+        self.calculate()
+
+    # TODO: ragi this needs to update the field ???
+    def update_goal(self, new_goal : np.array) -> int:
+        self.goal = new_goal
+        return 0
+
+    def update_obstacle(self, new_obstacle: np.array) -> int:
+        self.obstacle = new_obstacle
+        return 0
+
+    def calculate(self) -> None:
+        self.v3 = np.array([calc_field(p, v1, v2, self.c, self.t) for p, v1, v2 in zip(self.halfPoints, self.v1, self.v2)])

testV2/main.py

@@ -0,0 +1,27 @@
+"""
+Title: Calculate APF
+Author: Giovanni Rasera
+"""
+
+from apf import *
+from viz import *
+import time
+
+def main(gridSize=10):
+    field = APF2D(gridSize)
+
+    # Draw
+    # vis, data3d = draw(field)
+    # vis.create_window(width=1000, height=1000)
+    # # Add geometry
+    # for m in data3d:
+    #     vis.add_geometry(m)
+    # vis.poll_events()
+    # vis.update_renderer()
+
+    # # Exit
+    # input("Enter to exit...")
+    # vis.destroy_window()
+
+if __name__ == "__main__":
+    main(10)

testV2/prof.py

@@ -0,0 +1,27 @@
+"""
+Title: Calculate APF
+Author: Giovanni Rasera
+"""
+
+import cProfile
+import argparse
+from main import main
+from apf import *
+
+def run(profiler: cProfile.Profile, gridSize: int):
+    # Profile algo
+    profiler.enable()
+    main(gridSize=gridSize)
+    profiler.disable()
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser(prog='Profiling APF2D')
+    parser.add_argument('filename', help='filename to dump profiling')
+    parser.add_argument('size', help='size of the APF grid', type=int, default=DEFAULT_DIM_SIZE)
+    args = parser.parse_args()
+
+    profiler = cProfile.Profile()
+
+    run(profiler, args.size)
+
+    profiler.dump_stats(f"{args.size}_{args.filename}")

testV2/viz.py

@@ -0,0 +1,84 @@
+"""
+Title: Calculate APF
+Author: Giovanni Rasera
+"""
+
+import numpy as np
+import open3d as o3d
+import copy
+from apf import *
+
+def create_grid(size=100, step=1):
+    """
+    Create a square grid of given size on the XY plane.
+    Lines run from (0,0) to (size,size).
+    """
+    lines = []
+    points = []
+
+    # Vertical lines
+    for x in range(0, size + 1, step):
+        points.append([x, 0, 0])
+        points.append([x, size, 0])
+        lines.append([len(points)-2, len(points)-1])
+
+    # Horizontal lines
+    for y in range(0, size + 1, step):
+        points.append([0, y, 0])
+        points.append([size, y, 0])
+        lines.append([len(points)-2, len(points)-1])
+
+    line_set = o3d.geometry.LineSet(
+        points=o3d.utility.Vector3dVector(np.array(points)),
+        lines=o3d.utility.Vector2iVector(np.array(lines))
+    )
+
+    # Optional: color the grid grey
+    colors = [[0.7, 0.7, 0.7] for _ in lines]
+    line_set.colors = o3d.utility.Vector3dVector(colors)
+
+    return line_set
+
+def create_ball(position, radius=1.0, color=[1, 0, 0]):
+    """
+    Creates a sphere (ball) at a given 3D position.
+    """
+    sphere = o3d.geometry.TriangleMesh.create_sphere(radius=radius)
+    sphere.compute_vertex_normals()
+    sphere.paint_uniform_color(color)
+    sphere.translate(position)
+
+    return sphere
+
+def create_from_array(arr, radius=0.05, color=[0, 0, 0]):
+    balls = []
+    for p in arr:
+        ball = create_ball([p[0], p[1], 0], radius=radius, color=color)
+        balls.append(ball)
+    return balls
+
+def draw(field: APF2D):
+    # Build scene
+    grid = create_grid(field.s_ize*2, step=1)
+    first_point = create_ball([0, 0, 0], radius=0.05)
+    last_point = create_ball([field.s_ize, field.s_ize, 0], radius=0.05, color=[1, 0, 0])
+
+    # # Goal
+    goal_point = create_ball([field.goal[0], field.goal[1], 0], radius=0.05, color=[0, 1, 0])
+    obstacle_point = create_ball([field.obstacle[0], field.obstacle[1], 0], radius=0.05, color=[1, 0, 0])
+
+    points_p0 = create_from_array(field.halfPoints)
+    points_v3 = create_from_array(field.v3, radius=0.02, color=[0, 1, 1])
+    # # Obstacles
+    # obstacles_points = []
+    # for obs in field.obstacles:
+    #     obs_point = create_ball([int(obs.x), int(obs.y), 0], radius=obs.r, color=[1, 0, 0])
+    #     obstacles_points.append(obs_point)
+
+    # # Field Speeds
+    # vector_field = create_vector_field(field)
+
+    vis = o3d.visualization.Visualizer()
+    data3d = [grid, *points_p0, *points_v3, goal_point, obstacle_point, first_point, last_point]
+
+    return vis, data3d