add run/climb movement

software/pathTool/src/config.py

@@ -0,0 +1,48 @@
+kLegMountLeftRightX = 29.87
+kLegMountOtherX = 22.41
+kLegMountOtherY = 55.41
+
+kLegRootToJoint1 = 20.75
+kLegJoint1ToJoint2 = 28.0
+kLegJoint2ToJoint3 = 42.6
+kLegJoint3ToTip = 89.07
+
+SIN30   = 0.5
+COS30   = 0.866
+SIN45   = 0.7071
+COS45   = 0.7071
+SIN15   = 0.2588
+COS15   = 0.9659
+
+STANDBY_Z = (kLegJoint3ToTip*COS15-kLegJoint2ToJoint3*SIN30)
+LEFTRIGHT_X = (kLegMountLeftRightX+kLegRootToJoint1+kLegJoint1ToJoint2+(kLegJoint2ToJoint3*COS30)+kLegJoint3ToTip*SIN15)
+OTHER_X = (kLegMountOtherX + (kLegRootToJoint1+kLegJoint1ToJoint2+(kLegJoint2ToJoint3*COS30)+kLegJoint3ToTip*SIN15)*COS45)
+OTHER_Y = (kLegMountOtherY + (kLegRootToJoint1+kLegJoint1ToJoint2+(kLegJoint2ToJoint3*COS30)+kLegJoint3ToTip*SIN15)*SIN45)
+
+defaultPosition = (
+    (OTHER_X, OTHER_Y, -STANDBY_Z),
+    (LEFTRIGHT_X, 0, -STANDBY_Z),
+    (OTHER_X, -OTHER_Y, -STANDBY_Z),
+    (-OTHER_X, -OTHER_Y, -STANDBY_Z),
+    (-LEFTRIGHT_X, 0, -STANDBY_Z),
+    (-OTHER_X, OTHER_Y, -STANDBY_Z),
+)
+
+mountPosition = (
+    (kLegMountOtherX, kLegMountOtherY, 0),
+    (kLegMountLeftRightX, 0, 0),
+    (kLegMountOtherX, -kLegMountOtherY, 0),
+    (-kLegMountOtherX, -kLegMountOtherY, 0),
+    (-kLegMountLeftRightX, 0, 0),
+    (-kLegMountOtherX, kLegMountOtherY, 0),
+)
+
+defaultAngle = (
+    -45, 0, 45, 135, 180, 225
+)
+
+angleLimitation = (
+    (-45, 45),
+    (-45, 75),
+    (-60, 60),
+)

software/pathTool/src/kinematics.py

@@ -0,0 +1,25 @@
+import math
+
+import config
+
+pi = math.acos(-1)
+
+def ik(to):
+    angles = []
+    x = to[0] - config.kLegRootToJoint1
+    y = to[1]
+
+    angles.append(math.atan2(y, x) * 180 / pi)
+
+    x = math.sqrt(x*x + y*y) - config.kLegJoint1ToJoint2
+    y = to[2]
+    ar = math.atan2(y, x)
+    lr2 = x*x + y*y
+    lr = math.sqrt(lr2)
+    a1 = math.acos((lr2 + config.kLegJoint2ToJoint3*config.kLegJoint2ToJoint3 - config.kLegJoint3ToTip*config.kLegJoint3ToTip)/(2*config.kLegJoint2ToJoint3*lr))
+    a2 = math.acos((lr2 - config.kLegJoint2ToJoint3*config.kLegJoint2ToJoint3 + config.kLegJoint3ToTip*config.kLegJoint3ToTip)/(2*config.kLegJoint3ToTip*lr))
+
+    angles.append((ar + a1) * 180 / pi)
+    angles.append(90 - ((a1 + a2)  * 180 / pi))
+
+    return angles

software/pathTool/src/main.py

@@ -3,6 +3,10 @@ import logging
 import os
 import sys
 
+import config
+import kinematics
+from path.lib import point_rotate_z, matrix_mul
+
 def collectPath(sub_folder):
     scripts = {}
     for script_name in [f[:-3] for f in sorted(os.listdir(sub_folder)) if f.endswith('.py') and os.path.isfile(os.path.join(sub_folder, f))]:
@@ -18,6 +22,55 @@ def show_detail(path, result):
     for i, p in enumerate(result):
         print("{:2d}  {:5.2f}, {:5.2f}, {:5.2f}".format(i, p[0], p[1], p[2]))
 
+def verify_points(pt):
+    angles = kinematics.ik(pt)
+
+    ok = True
+    failed = []
+    for i, angle in enumerate(angles):
+        if angle < config.angleLimitation[i][0] or angle > config.angleLimitation[i][1]:
+            ok = False
+            failed.append((i, angle))
+
+    return ok, failed
+
+def verify_path(path, params):
+    data, mode, _, _ = params
+    print("Verifying {}...".format(path))
+
+    all_ok = True
+    if mode == "shift":
+        # data: float[6][N][3]
+        assert(len(data) == 6)
+
+        for i in range(len(data[0])):
+            for j in range(6):
+                pt = [config.defaultPosition[j][k] - config.mountPosition[j][k] + data[j][i][k] for k in range(3)]
+                pt = point_rotate_z(pt, config.defaultAngle[j])
+                ok, failed = verify_points(pt)
+
+                if not ok:
+                    print("{}, {} failed: {}".format(i, j, failed))
+                    all_ok = False
+
+    elif mode == "matrix":
+        # data: np.matrix[N]
+        for i in range(len(data)):
+            for j in range(6):
+                pt = matrix_mul(data[i], config.defaultPosition[j])
+                for k in range(3):
+                    pt[k] -= config.mountPosition[j][k]
+                pt = point_rotate_z(pt, config.defaultAngle[j])
+
+                ok, failed = verify_points(pt)
+
+                if not ok:
+                    print("{}, {} failed: {}".format(i, j, failed))
+                    all_ok = False
+    
+    return all_ok
+
+
 def generate_c_body(path, params):
     data, mode, dur, entries = params
     result = "\nconst Locations {}_paths[] {{\n".format(path)
@@ -77,19 +130,24 @@ if __name__ == '__main__':
     # generate all paths
     results = {path: generator() for path, generator in paths.items()}
 
-    # output results
-    with open(args.out_path, "w") as f:
-        print("//", file=f)
-        print("// This file is generated, dont directly modify content...", file=f)
-        print("//", file=f)
-        print("namespace {", file=f)
-        for path, data in results.items():
-            print(generate_c_body(path, data), file=f)
-        print("}\n", file=f)
-        for path in results:
-            print(generate_c_def(path), file=f)
-
-    print("Result written to {}".format(args.out_path))
+    # verify all path is within safe angles
+    
+    verified = [1 for path, data in results.items() if not verify_path(path, data)]
+    if len(verified) > 0:
+        print("There were errors, exit...")
+    else:
+        # output results
+        with open(args.out_path, "w") as f:
+            print("//", file=f)
+            print("// This file is generated, dont directly modify content...", file=f)
+            print("//", file=f)
+            print("namespace {", file=f)
+            for path, data in results.items():
+                print(generate_c_body(path, data), file=f)
+            print("}\n", file=f)
+            for path in results:
+                print(generate_c_def(path), file=f)
 
+        print("Result written to {}".format(args.out_path))
 
 

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]