add run/climb movement

software/pathTool/src/path/backward.py

@@ -1,7 +1,9 @@
 from collections import deque
 
 from lib import semicircle_generator
-from forward import g_steps, g_radius
+
+g_steps = 20
+g_radius = 25
 
 def path_generator():
     assert (g_steps % 4) == 0

software/pathTool/src/path/climb.py

@@ -0,0 +1,27 @@
+
+from collections import deque
+
+from lib import semicircle2_generator
+
+g_steps = 20
+y_radius = 20
+z_radius = 80
+x_radius = 30
+
+z_shift = -30
+
+def path_generator():
+    assert (g_steps % 4) == 0
+    halfsteps = int(g_steps/2)
+
+    rpath = [(x, y, z + z_shift) for x, y, z in semicircle2_generator(g_steps, y_radius, z_radius, x_radius)]
+    lpath = [(x, y, z + z_shift) for x, y, z in semicircle2_generator(g_steps, y_radius, z_radius, -x_radius)]
+
+    mir_rpath = deque(rpath)
+    mir_rpath.rotate(halfsteps)
+
+    mir_lpath = deque(lpath)
+    mir_lpath.rotate(halfsteps)
+
+    return [rpath, mir_rpath, rpath, mir_lpath, lpath, mir_lpath, ], "shift", 30, (0, halfsteps)
+    

software/pathTool/src/path/forward.py

@@ -4,7 +4,7 @@ from collections import deque
 from lib import semicircle_generator
 
 g_steps = 20
-g_radius = 20
+g_radius = 25
 
 def path_generator():
     assert (g_steps % 4) == 0

software/pathTool/src/path/forwardfast.py

@@ -0,0 +1,25 @@
+
+from collections import deque
+
+from lib import semicircle2_generator
+
+g_steps = 20
+y_radius = 50
+z_radius = 30
+x_radius = 10
+
+def path_generator():
+    assert (g_steps % 4) == 0
+    halfsteps = int(g_steps/2)
+
+    rpath = semicircle2_generator(g_steps, y_radius, z_radius, x_radius)
+    lpath = semicircle2_generator(g_steps, y_radius, z_radius, -x_radius)
+
+    mir_rpath = deque(rpath)
+    mir_rpath.rotate(halfsteps)
+
+    mir_lpath = deque(lpath)
+    mir_lpath.rotate(halfsteps)
+
+    return [rpath, mir_rpath, rpath, mir_lpath, lpath, mir_lpath, ], "shift", 20, (0, halfsteps)
+    

software/pathTool/src/path/lib.py

@@ -32,6 +32,35 @@ def semicircle_generator(radius, steps, reverse=False):
 
     return result
 
+def semicircle2_generator(steps, y_radius, z_radius, x_radius, reverse=False):
+    assert (steps % 4) == 0
+    halfsteps = int(steps/2)
+
+    step_angle = pi / halfsteps
+
+    result = []
+
+    # first half, move backward (only y change)
+    for i in range(halfsteps):
+        result.append((0, y_radius - i*y_radius*2/(halfsteps), 0))
+
+    # second half, move forward in semicircle shape (y, z change)
+    for i in range(halfsteps):
+        angle = pi - step_angle*i
+        y = y_radius * math.cos(angle)
+        z = z_radius * math.sin(angle)
+        x = x_radius * math.sin(angle)
+        result.append((x, y, z))
+
+    result = deque(result)
+    result.rotate(int(steps/4))
+
+    if reverse:
+        result = deque(reversed(result))
+        result.rotate(1)
+
+    return result
+
 def get_rotate_x_matrix(angle):
     angle = angle * pi / 180
     return np.matrix([
@@ -59,6 +88,10 @@ def get_rotate_z_matrix(angle):
         [0, 0, 0, 1],
     ])
 
+def matrix_mul(m, pt):
+    ptx = list(pt) + [1]
+    return list((m * np.matrix(ptx).T).T.flat)[:-1]
+
 def point_rotate_x(pt, angle):
     ptx = list(pt) + [1]
     return list((get_rotate_x_matrix(angle) * np.matrix(ptx).T).T.flat)[:-1]

software/pathTool/src/path/shiftleft.py

@@ -5,7 +5,7 @@ from lib import semicircle_generator
 from lib import path_rotate_z
 
 g_steps = 20
-g_radius = 20
+g_radius = 25
 
 def path_generator():
     assert (g_steps % 4) == 0

software/pathTool/src/path/shiftright.py

@@ -5,7 +5,7 @@ from lib import semicircle_generator
 from lib import path_rotate_z
 
 g_steps = 20
-g_radius = 20
+g_radius = 25
 
 def path_generator():
     assert (g_steps % 4) == 0

software/pathTool/src/path/turnleft.py

@@ -5,7 +5,7 @@ from lib import semicircle_generator
 from lib import path_rotate_z
 
 g_steps = 20
-g_radius = 20
+g_radius = 25
 
 def path_generator():
     assert (g_steps % 4) == 0

software/pathTool/src/path/turnright.py

@@ -5,7 +5,7 @@ from lib import semicircle_generator
 from lib import path_rotate_z
 
 g_steps = 20
-g_radius = 20
+g_radius = 25
 
 def path_generator():
     assert (g_steps % 4) == 0

software/pathTool/src/path/twist.py

@@ -0,0 +1,35 @@
+
+from collections import deque
+import math
+
+from lib import get_rotate_x_matrix, get_rotate_z_matrix
+
+g_steps = 20
+
+raise_angle = 3
+twist_x_angle = 20
+twise_y_angle = 12
+
+def path_generator():
+    assert (g_steps % 4) == 0
+
+    result = []
+
+    quarter = int(g_steps / 4)
+    step_x_angle = twist_x_angle / quarter
+    step_y_angle = twise_y_angle / quarter
+
+    m = get_rotate_x_matrix(raise_angle)
+    for i in range(quarter):
+        result.append(m * get_rotate_z_matrix(i*step_x_angle) * get_rotate_x_matrix(i*step_y_angle))
+
+    for i in range(quarter):
+        result.append(m * get_rotate_z_matrix((quarter-i)*step_x_angle) * get_rotate_x_matrix((quarter-i)*step_y_angle))
+
+    for i in range(quarter):
+        result.append(m * get_rotate_z_matrix(-i*step_x_angle) * get_rotate_x_matrix(i*step_y_angle))
+
+    for i in range(quarter):
+        result.append(m * get_rotate_z_matrix((-quarter+i)*step_x_angle) * get_rotate_x_matrix((quarter-i)*step_y_angle))
+
+    return result, "matrix", 50, [0, 10]