Browse Source

Fix some issues

Marc Savioz 4 months ago
parent
commit
11ffba1287
3 changed files with 179849 additions and 18451 deletions
  1. 90 116
      software/astar.py
  2. 76 72
      wing/MH61-S01.scad
  3. 179683 18263
      wing/MH61-S01_core.stl

+ 90 - 116
software/astar.py

@@ -3,8 +3,13 @@ import numpy as np
 import matplotlib.pyplot as plt
 from mpl_toolkits.mplot3d import Axes3D
 from scipy import signal
-class Node:
 
+class Plane:
+    def __init__(self, position, orientation):
+        self.position = position
+        self.orientation = orientation
+
+class Node:
     def __init__(self, x, y, z, cost, pind):
         self.x = x
         self.y = y
@@ -17,59 +22,44 @@ class Node:
 
 def calc_final_path(ngoal, closedset, reso):
     # generate final course
-    rx, ry = [ngoal.x * reso], [ngoal.y * reso]
+    rx, ry, rz = [ngoal.x * reso], [ngoal.y * reso], [ngoal.z * reso]
     pind = ngoal.pind
     while pind != -1:
         n = closedset[pind]
         rx.append(n.x * reso)
         ry.append(n.y * reso)
+        rz.append(n.z * reso)
         pind = n.pind
-
-    return rx, ry
+    return rx, ry, rz
 
 
 def calc_heuristic(n1, n2):
     w = 1.0  # weight of heuristic
-    d = w * math.sqrt((n1.x - n2.x)**2 + (n1.y - n2.y)**2)
+    d = w * math.sqrt((n1.x - n2.x)**2 + (n1.y - n2.y)**2 + (n1.z-n2.z)**2)
     return d
 
 def calc_index(node, xwidth, xmin, ymin):
     return (node.y - ymin) * xwidth + (node.x - xmin)
 
 def get_motion_model():
-    # Always go forward one step (1) so cost is
-    # dx, dy, dz, cost
+    # Always go forward one step (10 meter) so cost is
+    # dl distance lateral
+    # dz vertical distance
+    # cost
     motion = [
-            # z = 0
-            [0, 1,  0, 1],
-            [1, 0,  0, 1],
-            [-1, 0, 0, 1],
-            [0, -1, 0, 1],
-            [-1, -1,0, math.sqrt(2)],
-            [1, 1,  0, math.sqrt(2)],
-            [-1, 1, 0, math.sqrt(2)],
-            [1, -1, 0, math.sqrt(2)],
-
-            # z = 1
-            [0, 1,  0.1, math.sqrt(2)],
-            [1, 0,  0.1, math.sqrt(2)],
-            [-1, 0, 0.1, math.sqrt(2)],
-            [0, -1, 0.1, math.sqrt(2)],
-            [-1, -1,0.1, math.sqrt(math.sqrt(2)+1)],
-            [1, 1,  0.1, math.sqrt(math.sqrt(2)+1)],
-            [-1, 1, 0.1, math.sqrt(math.sqrt(2)+1)],
-            [1,-1,  0.1,  math.sqrt(math.sqrt(2)+1)],
-
-            # z = -1
-            [0, 1,  -0.1, math.sqrt(2)],
-            [1, 0,  -0.1, math.sqrt(2)],
-            [-1, 0, -0.1, math.sqrt(2)],
-            [0, -1, -0.1, math.sqrt(2)],
-            [-1, -1,-0.1, math.sqrt(math.sqrt(2)+1)],
-            [1, 1,  -0.1, math.sqrt(math.sqrt(2)+1)],
-            [-1, 1, -0.1, math.sqrt(math.sqrt(2)+1)],
-            [1,-1,  -0.1,  math.sqrt(math.sqrt(2)+1)],
-
+            [0, 0, 1],
+            [1, 0, math.sqrt(2)],
+            [-1, 0,math.sqrt(2)],
+
+            # go up
+            [0, 1, math.sqrt(2)],
+            [1, 1, math.sqrt(math.sqrt(2)+1)],
+            [-1, 1, math.sqrt(math.sqrt(2)+1)],
+
+            # go down
+            [0, -1, math.sqrt(2)],
+            [1, -1, math.sqrt(math.sqrt(2)+1)],
+            [-1,-1, math.sqrt(math.sqrt(2)+1)],
             ]
     return motion
 
@@ -78,6 +68,9 @@ def calc_obstacle_map(ox, oy, oz, reso, rr):
     miny = round(min(oy))
     maxx = round(max(ox))
     maxy = round(max(oy))
+    maxz = round(max(oz))
+    minz = round(min(oz))
+
     #  print("minx:", minx)
     #  print("miny:", miny)
     #  print("maxx:", maxx)
@@ -85,89 +78,68 @@ def calc_obstacle_map(ox, oy, oz, reso, rr):
 
     xwidth = round(maxx - minx)
     ywidth = round(maxy - miny)
+    zwidth = int(round(maxz - minz))
+    print(zwidth)
     #  print("xwidth:", xwidth)
     #  print("ywidth:", ywidth)
 
     # obstacle map generation
-    obmap = [[False for i in range(ywidth)] for i in range(xwidth)]
+    obmap = [[[False for i in range(ywidth)] for i in range(xwidth)] for i in range(zwidth)]
+    print(obmap)
     for ix in range(xwidth):
         x = ix + minx
         for iy in range(ywidth):
             y = iy + miny
-            #  print(x, y)
-            for iox, ioy in zip(ox, oy):
-                d = math.sqrt((iox - x)**2 + (ioy - y)**2)
-                if d <= rr / reso:
-                    obmap[ix][iy] = True
-                    break
+            for iz in range(zwidth):
+                z = iz + minz
+                for iox, ioy, ioz in zip(ox, oy, oz):
+                    d = math.sqrt((iox - x)**2 + (ioy - y)**2 + (ioz-z)**2)
+                    if d <= rr / reso:
+                        try:
+                            obmap[ix][iy][iz] = True
+                            break
+                        except Exception:
+                            pass
 
     return obmap, minx, miny, maxx, maxy, xwidth, ywidth
 
+def manathan(a, b):
+    return 1
 
-def astar(sx, sy, sz, gx, gy, gz, ox, oy, oz, reso, rr):
-    nstart = Node(round(sx/reso), round(sy/reso), round(sz/reso), 0.0, -1)
-    ngoal = Node(round(sx/reso), round(sy/reso), round(sz/reso), 0.0, -1)
-    ox = [iox / reso for iox in ox]
-    oy = [ioy / reso for ioy in oy]
-    oz = [ioz / reso for ioz in oz]
-
-    obmap, minx, miny, maxx, maxy, xw, yw = calc_obstacle_map(ox, oy, oz, reso, rr)
+def astar(sx, sy, sz, gx, gy, gz, ox, oy, oz, rr):
 
+    orientation_norm = math.sqrt((gx-sx)**2+(gy-sy)**2)
+    orientation = ((gx-sx)/orientation_norm, (gy-sy)/orientation_norm)
+    print(orientation)
     motion = get_motion_model()
+    # Start at sx sy sz
+    pos = [sx, sy, sz]
+    plane = Plane(pos, orientation)
+    current_node = Node(sx, sy, sz, 0, None)
 
-    openset, closedset = dict(), dict()
-    openset[calc_index(nstart, xw, minx, miny)] = nstart
-
+    # Calculate next step
     while 1:
-        c_id = min(
-            openset, key=lambda o: openset[o].cost + calc_heuristic(ngoal, openset[o]))
-        current = openset[c_id]
-
-        # show graph
-        if show_animation:  # pragma: no cover
-            plt.plot(current.x * reso, current.y * reso, "xc")
-            if len(closedset.keys()) % 10 == 0:
-                plt.pause(0.001)
-
-        if current.x == ngoal.x and current.y == ngoal.y:
-            print("Find goal")
-            ngoal.pind = current.pind
-            ngoal.cost = current.cost
-            break
-
-        # Remove the item from the open set
-        del openset[c_id]
-        # Add it to the closed set
-        closedset[c_id] = current
-
-        # expand search grid based on motion model
-        for i, _ in enumerate(motion):
-            node = Node(current.x + motion[i][0],
-                        current.y + motion[i][1],
-                        current.z + motion[i][2],
-                        current.cost + motion[i][3], c_id)
-            n_id = calc_index(node, xw, minx, miny)
-
-            if n_id in closedset:
-                continue
-
-            if not verify_node(node, obmap, minx, miny, maxx, maxy):
-                continue
-
-            if n_id not in openset:
-                openset[n_id] = node  # Discover a new node
-            else:
-                if openset[n_id].cost >= node.cost:
-                    # This path is the best until now. record it!
-                    openset[n_id] = node
-
-    rx, ry = calc_final_path(ngoal, closedset, reso)
-
-    return rx, ry
+        g = 0
+        # Cost of step starts at infinity
+        cost = 1000
+        # Calculate G and H for each of the 9 option
+        for i in range(0, 9) :
+            # Get the manathan for the next 9 nodes
+
+            H = manathan(sx, gx)
+            g = g + motion[i][2]
+            print(g, H)
+
+    return rx, ry, rz
 
 show_animation = True
-goal = (22, 30)
-start = (2, 32)
+goal = (22, 30, 0)
+start = (2, 10, 0)
+
+ox = [10]
+oy = [20]
+oz = [0]
+
 # Set up grid and test data
 nx, ny = 64, 64
 x = range(nx)
@@ -181,7 +153,7 @@ filter = g/g.sum()
 
 demSmooth = signal.convolve(dem1,filter,mode='valid')
 # rescale so it lies between 0 and 1
-demSmooth = (demSmooth - demSmooth.min())/(demSmooth.max() - demSmooth.min())
+demSmooth = (demSmooth - demSmooth.min())/(demSmooth.max() - demSmooth.min())*10
 print(demSmooth)
 print(demSmooth[0,0])
 plt.imshow(demSmooth)
@@ -190,22 +162,24 @@ plt.plot(goal[0], goal[1],'bo' )
 # Add obstacles to list
 ox, oy, oz = [], [], []
 
-for x in range(0, len(demSmooth)):
-    for y in range(0, len(demSmooth)):
-        print(demSmooth[x,y])
-        ox.append(x)
-        oy.append(y)
-        oz.append(demSmooth[x, y])
+#for x in range(0, len(demSmooth)):
+#    for y in range(0, len(demSmooth)):
+#        ox.append(x)
+#        oy.append(y)
+#        oz.append(demSmooth[x, y])
+#
+print(oz)
+from mpl_toolkits.mplot3d import Axes3D
+fig = plt.figure()
+#ax = fig.add_subplot(111, projection='3d')
 
-if show_animation:  # pragma: no cover
-    plt.plot(ox, oy, ".k")
-    plt.plot(start[0], start[1], "xr")
-    plt.plot(goal[0], goal[1], "xb")
-    plt.axis("equal")
-rx, ry = astar(start[0], start[1], demSmooth[start[0], start[1]], goal[0], goal[1], demSmooth[goal[0], goal[1]], ox, oy, oz, 1, 1)
+
+rx, ry, rz = astar(start[0], start[1], start[2], goal[0], goal[1], goal[2], ox, oy, oz, 1)
 
 if show_animation:  # pragma: no cover
-    plt.plot(rx, ry, "-r")
+    #Axes3D.plot(rx, ry,rz)
+    print(rz)
+    print(ry)
     plt.show()
 
 

+ 76 - 72
wing/MH61-S01.scad

@@ -1,35 +1,33 @@
-// TODO: skew instead of rotate
-$fn         = 50;
-swept_angle = 15/45; // 
-twist       = -4;
-wingspan    = 500;
-angle_comp  = -0.7;
-core_span   = 70;
-CORE        = true;
+$fn			= 180;
+swept_angle	= 15/45; // 
+twist		= -4;
+wingspan	= 500;
+angle_comp	= -0.7;
+core_span	= 70;
+CORE		= true;
+WING		= true;
 
 module wing(size, tw) {
-    rotate([180,0,0])
-    linear_extrude(height=size, twist=tw, slices=60)
-    import("mh61_minikowski_3_25mm.dxf", center=true);
+	rotate([180,0,0])
+	linear_extrude(height=size, twist=tw, slices=60)
+	import("mh61_minikowski_3_25mm.dxf", center=true);
 }
 
-module rod()
-{
-    translate([200,0,8])
-    rotate([0,90,0])
-    cylinder(r=1., h=300, center=true);
+module rod() {
+	translate([200,0,8])
+	rotate([0,90,0])
+	cylinder(r=1., h=300, center=true);
 }
 
-module elevon()
-{
-    rotate([90, 0, 0])
-    hull(){
-        translate([310, 8, 0])
-        cylinder(r=2, h=40);
-        
-        translate([350, 8, 0])
-        cylinder(r=0.25, h=40);
-    }
+module elevon() {
+	rotate([90, 0, 0])
+	hull(){
+		translate([310, 8, 0])
+		cylinder(r=2, h=40);
+		
+		translate([350, 8, 0])
+		cylinder(r=0.25, h=40);
+	}
 }
 
 
@@ -46,39 +44,44 @@ module rod_support()
    rod();
 }
 
-// Support
+// Support containing avionics
+// TODO: Full blown support
+// For the moment, just an attaching surface
 module support(span)
 {
-    union(){
-        color([0.2,0.2,0.6], alpha=0.55){
-            translate([14, span/2, -0])
-            rotate([-90,angle_comp,0])
-            scale([1.25, 1.15   , 1])
-            wing(span);
-            
-            translate([-20,0,-9])
-            cube([30, span, 5], center=true);
-            
-          //  rod_support();
-        }
-    }
+	translate([-20,0,-9])
+		cube([40, span, 5], center=true);
 }
 
-// elevons 
-// TODO
-
-M = [ [1, 0, -swept_angle, 0],
-      [0, 1, 0, 0],// The "0.7" is the skew value; pushed along the y axis
-      [0, 0, 1, 0],
-      [0, 0, 0, 1]] ;
+M = [
+	[1, 0, -swept_angle, 0],
+	[0, 1, 0, 0],// The "0.7" is the skew value; pushed along the y axis
+	[0, 0, 1, 0],
+	[0, 0, 0, 1]];
 
-
-if(CORE)
-{
-    difference()
-    {
-        support(core_span);
-        
+//scale(0.25)
+if(CORE){
+    difference(){
+		color([0.2,0.7,0.6], alpha=0.7)
+		union(){
+			rotate([-90,0,0])
+			multmatrix(M){
+			rotate([0,0,-0.4])
+			resize([158, 25, 0])
+			wing(core_span/2, -0.25);
+			}
+		
+			mirror([0,1,1])
+			multmatrix(M) {
+				
+			rotate([0,0,-0.4])
+				resize([158, 25, 0])
+				wing(core_span/2, -0.25);
+			}
+			support(core_span);
+		}
+		
+		translate([-2,0,0]){
         rotate([-90,0,0])
         multmatrix(M) {
             wing(wingspan/2, twist);
@@ -88,24 +91,25 @@ if(CORE)
         multmatrix(M) {
             wing(wingspan/2, twist);
         }
-    }
-} else {
-    
-    elevon();
-    
-    mirror([0,1,0])
-    elevon();
-    
-    rod_support();
-    support(core_span);
-    rotate([-90,0,0]){
-        multmatrix(M) {
-            wing(wingspan/2, twist);
+		}
+}
+if(WING)
+    {
+		
+		elevon();
+		mirror([0,1,0])
+		elevon();
+		
+	color([0.5,0.1,0.5], alpha=0.18)
+		rotate([-90,0,0]){
+			multmatrix(M) {
+			wing(wingspan/2, twist);
+			}
+			
+			mirror([0,0,1])
+			multmatrix(M) {
+				wing(wingspan/2, twist);
+			}
         }
     }
-
-    mirror([0,1,1])
-    multmatrix(M) {
-        wing(wingspan/2, twist);
-    }
 }

File diff suppressed because it is too large
+ 179683 - 18263
wing/MH61-S01_core.stl