Arm Pathfinding

arm_control/src/arm_kinematics.py

@@ -1,9 +1,10 @@
 import numpy as np
 import math
+from scipy.spatial.transform import Rotation as R
 
 # Define joint limits in Radians
-jointUpperLimits = [90*np.pi/180, 70*np.pi/180, 70*np.pi/180, 38*np.pi/180, 85*np.pi/180]      # rad
-jointLowerLimits = [-90*np.pi/180, -60*np.pi/180, -20*np.pi/180, -35*np.pi/180, -85*np.pi/180] # rad
+jointUpperLimits = [118.76*np.pi/180, 90*np.pi/180, 75*np.pi/180, 75*np.pi/180, np.pi]      # rad
+jointLowerLimits = [-118*np.pi/180, -60*np.pi/180, -70*np.pi/180, -75*np.pi/180, -np.pi] # rad
 
 # Define Denavit-Hartenberg parameters for the robot arm
 arm_DH = [
@@ -406,15 +407,16 @@ def inverseKinematicsJointPositions(hand_pose):
     basis_x = wrist_pose - shoulder_pose
     d = np.linalg.norm(basis_x)
     if not np.isclose(d, 0): 
-        basis_x = basis_x / d
-    w = np.linalg.norm(wrist_pose)
+        basis_x = basis_x / d #Unit vector
+    w = np.linalg.norm(wrist_pose) #length of vector
     if np.isclose(w, 0):
         pass
     arm_plane_normal = np.cross(basis_x, wrist_pose)
     basis_y = np.cross(arm_plane_normal, basis_x)
     basis_y = basis_y / np.linalg.norm(basis_y)
 
     # Get Position of Link intersections (circles)
+    #Notes: d points from wrist to shoulder
     elbow_basis_x = (d**2 - arm_DH[2][2]**2 + arm_DH[1][2]**2) / (2*d)
     elbow_basis_y = np.sqrt(arm_DH[1][2]**2 - elbow_basis_x**2) 
 
@@ -495,5 +497,4 @@ def legalIKPositionPicker(poses, cur_pose):
 
 
 def inverseKinematics(hand_pose, cur_pose):
-
-    return legalIKPositionPicker(inverseKinematicsAngleOptions(hand_pose), cur_pose)
+    return legalIKPositionPicker(inverseKinematicsAngleOptions(hand_pose), cur_pose)

arm_control/src/arm_pathfinding.py

@@ -0,0 +1,142 @@
+import numpy as np
+import math
+
+max_acc = [1, 1, 1, 1, 1] # waist shoulder elbow wrist hand maximum accelerations (TEMPORARY VALUES)
+max_vels = [0.1, 0.1, 0.1, 0.1, 0.1] # waist shoulder elbow wrist hand maximum accelerations (TEMPORARY VALUES)
+previous_end_joints = [None for i in range(5)]
+polynomials = [[0 for j in range(4)] for i in range(5)]
+total_motion_time = 0
+original_start_joints = [0 for i in range(5)]
+
+def pathfind(start_joints, end_joints, time):
+    """
+    Generates the path polynomial for each joint and calculates the required position for a given time remaining.
+    Note: If time is too long or maximum velocity is too high, it will reverse and then overshoot before landing
+    on the desired position.
+
+    Parameters
+    ----------
+        start_joints : list(5)
+            Current [waist, shoulder, elbow, wrist, hand] in radians
+        end_joints : list(5)
+            Desired [waist, shoulder, elbow, wrist, hand] in radians
+        time : float
+            Time until the arm should be at end_joints. Should go down by the time between each call of
+            this function until the arm reaches end_joints, at which point it should reset for the next
+            desired position.
+
+    Returns
+    -------
+        joints : list(5)
+            The next position the arm should move to in accordance with the path
+    """
+    if end_joints != previous_end_joints: #Setting up polynomials and saved variables
+        half = time / 2
+        vels = [max_acc[i] * half for i in range(5)] # waist shoulder elbow wrist hand maximum velocities 
+        for i in range(5):
+            if vels[i] > max_vels[i]:
+                vels[i] = max_vels[i]
+            if end_joints[i] < start_joints[i]:
+                vels[i] = -vels[i]
+        matrix = np.array([[time ** 6, time ** 5, time ** 4, time ** 3], 
+        [6 * time ** 5, 5 * time ** 4, 4 * time ** 3, 3 * time ** 2],
+        [6 * half ** 5, 5 * half ** 4, 4 * half ** 3, 3 * half ** 2],
+        [30 * half ** 4, 20 * half ** 3, 12 * half ** 2, 6 * half]])
+
+        for i in range(len(polynomials)):
+            points = np.array([end_joints[i] - start_joints[i], 0, vels[i], 0])
+            poly = np.linalg.solve(matrix, points)
+            for j in range(4):
+                polynomials[i][j] = poly[j]
+            previous_end_joints[i] = end_joints[i]
+            original_start_joints[i] = start_joints[i]
+
+        global total_motion_time
+        total_motion_time = time
+
+    joints = [] #Calculating positions
+    for i in range(len(polynomials)):
+        polynomial = polynomials[i]
+        delta_time = total_motion_time - time
+        delta_position = polynomial[0] * delta_time ** 6 + polynomial[1] * delta_time ** 5 + polynomial[2] * delta_time ** 4 + polynomial[3] * delta_time ** 3
+        joints.append(delta_position + original_start_joints[i])
+
+    return joints
+
+def pathfiningPolynomial(start_joints, end_joints, time):
+    """
+    Generates the path polynomial for each joint. Does NOT store these polynomials as global variables.
+    Note: If time is too long or maximum velocity is too high, it will reverse and then overshoot before landing
+    on the desired position.
+
+    Parameters
+    ----------
+        start_joints : list(5)
+            Current [waist, shoulder, elbow, wrist, hand] in radians
+        end_joints : list(5)
+            Desired [waist, shoulder, elbow, wrist, hand] in radians
+        time : float
+            Time until the arm should be at end_joints. 
+
+    Returns
+    -------
+        polynomials : list(5)(4)
+            The polynomial coefficients governing the motion of the waist, shoudler, elbow, 
+            wrist, and hand respectively.
+    """
+    half = time / 2
+    vels = [max_acc[i] * half for i in range(5)] # waist shoulder elbow wrist hand maximum velocities 
+    for i in range(5):
+        if vels[i] > max_vels[i]:
+            vels[i] = max_vels[i]
+        if end_joints[i] < start_joints[i]:
+            vels[i] = -vels[i]
+    polynomials = [[0 for j in range(4)] for i in range(5)]
+    matrix = np.array([[time ** 6, time ** 5, time ** 4, time ** 3], 
+        [6 * time ** 5, 5 * time ** 4, 4 * time ** 3, 3 * time ** 2],
+        [6 * half ** 5, 5 * half ** 4, 4 * half ** 3, 3 * half ** 2],
+        [30 * half ** 4, 20 * half ** 3, 12 * half ** 2, 6 * half]])
+
+    for i in range(len(polynomials)):
+        points = np.array([end_joints[i] - start_joints[i], 0, vels[i], 0])
+        poly = np.linalg.solve(matrix, points)
+        for j in range(4):
+            polynomials[i][j] = poly[j]
+
+    return polynomials
+
+def nextJointPosition(start_position, time_elapsed, polynomials):
+    """ Calculates the desired joint positions after time_elapsed time has passed since the
+    motion began, as govenerned by the polynomials.
+
+    Params
+    ------
+        start_joints : list(5)
+            The original position [waist, shoulder, elbow, wrist, hand] in radians when the
+            motion began.
+        time_elapsed : int
+            The time since the motion began
+        polynomials : list(5)(4)
+            The polynomial coefficients governing the motion of the waist, shoudler, elbow, 
+            wrist, and hand respectively.
+
+    Returns
+    -------
+        joints : list(5)
+            The next position the arm should move to in accordance with the path
+    """
+    joints = [] #Calculating positions
+    for i in range(len(polynomials)):
+        polynomial = polynomials[i]
+        delta_position = polynomial[0] * time_elapsed ** 6 + polynomial[1] * time_elapsed ** 5 + polynomial[2] * time_elapsed ** 4 + polynomial[3] * time_elapsed ** 3
+        joints.append(delta_position + start_position[i])
+
+    return joints
+
+if __name__ == '__main__':
+    time = 10
+    start = [0, 0, 0, 0, 0]
+    end = [-1.5, 2, 2, .5, .75]
+    for t in range(time, -1, -1):
+        start = pathfind(start, end, t)
+        print(start)

arm_control/src/arm_unit_tests.py

@@ -1,6 +1,7 @@
 import numpy as np
 import math
 import arm_kinematics
+import arm_pathfinding
 
 jointUpperLimits = [118.76*np.pi/180, 90*np.pi/180, 75*np.pi/180, 75*np.pi/180, np.pi]      # rad
 jointLowerLimits = [-125.97*np.pi/180, -60*np.pi/180, -70*np.pi/180, -75*np.pi/180, -np.pi] # rad
@@ -110,7 +111,104 @@ def test_inverseKinematics(num_samples = 1000, verbose=False):
     print(f"Ratio: {(num_samples - failed) / num_samples * 100}%")
     return failed
 
+def test_pathfind(num_samples = 1000, max_velocities=[0.1, 0.1, 0.1, 0.1, 0.1], verbose=False):
+    print("------------------------------------------------------------------")
+    print("-------------------------test_pathfind----------------------------")
+    print("------------------------------------------------------------------")
+
+    joints = ["Waist", "Shoulder", "Elbow", "Wrist", "Hand"]
+    num_failed = 0
+    for i in range(num_samples):
+        failed = False
+
+        start_joints = [np.random.random() * (jointUpperLimits[i]-jointLowerLimits[i]) + jointLowerLimits[i] for i in range(5)]
+        end_joints = [np.random.random() * (jointUpperLimits[i]-jointLowerLimits[i]) + jointLowerLimits[i] for i in range(5)]
+        differences = [end_joints[i] - start_joints[i] for i in range(5)]
+        min_time = max([abs((differences[i])/max_velocities[i]) for i in range(5)])
+        max_distance = max(abs(differences[i]) for i in range(5)) # Using furthest distance to get a reasonable max time
+        time = min_time + np.random.random() * max_distance * 10
+
+        if verbose:
+            print(f"Start Joints: {start_joints} End Joints: {end_joints} Time: {time}")
+
+        polynomials = arm_pathfinding.pathfiningPolynomial(start_joints, end_joints, time)
+        for k, polynomial in enumerate(polynomials):
+            if abs(sum([polynomial[j] * math.pow(time, 6 - j) for j in range(4)]) - differences[k]) > 0.001: # Checking final position
+                failed = True
+                if verbose:
+                    given = sum([polynomial[j] * math.pow(time, 6 - j) for j in range(4)])
+                    print(f"{joints[k]} polynomial failed final position.")
+                    print(f"Result: {given} Expected: {differences[k]} Difference: {differences[k] - given}")
+                break
+
+            if abs(abs(sum([polynomial[j] * (6 - j) * math.pow(time/2, 5 - j) for j in range(4)])) - max_velocities[k]) > 0.001: # Checking max velocity
+                failed = True
+                if verbose:
+                    given = sum([polynomial[j] * (6 - j) * math.pow(time/2, 5 - j) for j in range(4)])
+                    print(f"{joints[k]} polynomial failed midway velocity.")
+                    print(f"Result: {given} Expected: {max_velocities[k]} Difference: {max_velocities[k] - given}")
+                break
+
+            if abs(sum([polynomial[j] * (6 - j) * math.pow(time, 5 - j) for j in range(4)])) > 0.001: # Checking final velocity
+                failed = True
+                if verbose:
+                    given = sum([polynomial[j] * (6 - j) * math.pow(time, 5 - j) for j in range(4)])
+                    print(f"{joints[k]} polynomial failed final velocity.")
+                    print(f"Result: {given} Expected: 0")
+                break
+
+            if abs(sum([polynomial[j] * (6 - j) * (5 - j) * math.pow(time/2, 4 - j) for j in range(4)])) > 0.001: # Check halfway acceleration
+                failed = True
+                if verbose:
+                    given = sum([polynomial[j] * (6 - j) * (5 - j) * math.pow(time/2, 4 - j) for j in range(4)])
+                    print(f"{joints[k]} polynomial failed midway acceleration.")
+                    print(f"Result: {given} Expected: 0")
+                break
+
+        if not failed and abs(sum(start_joints[j] - arm_pathfinding.nextJointPosition(start_joints, 0, polynomials)[j] for j in range(5))) > 0.001:
+            failed = True
+            if verbose:
+                    given = arm_pathfinding.nextJointPosition(start_joints, 0, polynomials)
+                    print("nextJointPosition failed initial position.")
+                    print(f"Result: {given} Expected: {start_joints} Difference: {[start_joints[j] - given[j] for j in range(5)]}")
+
+        if not failed and abs(sum(start_joints[j] - arm_pathfinding.pathfind(start_joints, end_joints, time)[j] for j in range(5))) > 0.001:
+            failed = True
+            if verbose:
+                    given = arm_pathfinding.pathfind(start_joints, end_joints, 0)
+                    print("Pathfnd failed initial position.")
+                    print(f"Result: {given} Expected: {start_joints} Difference: {[start_joints[j] - given[j] for j in range(5)]}")
+
+        if not failed and abs(sum(end_joints[j] - arm_pathfinding.nextJointPosition(start_joints, time, polynomials)[j] for j in range(5))) > 0.001:
+            failed = True
+            if verbose:
+                    given = arm_pathfinding.nextJointPosition(start_joints, time, polynomials)
+                    print("nextJointPosition failed final position.")
+                    print(f"Result: {given} Expected: {end_joints} Difference: {[end_joints[j] - given[j] for j in range(5)]}")
+
+        if not failed and abs(sum(end_joints[j] - arm_pathfinding.pathfind(start_joints, end_joints, 0)[j] for j in range(5))) > 0.001:
+            failed = True
+            if verbose:
+                    given = arm_pathfinding.pathfind(start_joints, end_joints, time)
+                    print("Pathfind failed final position.")
+                    print(f"Result: {given} Expected: {end_joints} Difference: {[end_joints[j] - given[j] for j in range(5)]}")
+
+        print("------------------------------------------------------------------")
+        if failed:
+            num_failed += 1
+            print(f"Test Case {i + 1} failed.")
+        else:
+            print(f"Test Case {i + 1} passed.")
+
+        print("------------------------------------------------------------------")
+
+    print(f"Ratio: {(num_samples - num_failed) / num_samples * 100}%")
+    return num_failed
+
+
+
 if __name__=="__main__":
+    test_pathfind(verbose=True)
     test_inverseKinematicsJointPositions()
     test_inverseKinematicsComputeJointAngles()
     test_inverseKinematics()