Arm Pathfinding

arm_control/src/arm_kinematics.py

@@ -1,8 +1,9 @@
 import numpy as np
 import math
+from scipy.spatial.transform import Rotation as R
 
-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
 
 arm_DH = [
     [0.0575,                0,     0, -90*math.pi/180], #vertical offset from base
@@ -341,8 +342,10 @@ def inverseKinematicsComputeJointAngles(ee_target, wrist_target, elbow_target, c
     wrist_proj = project(projection_line, wrist_target[:2])
     wrist_coordinates = (wrist_proj, wrist_target[2])
 
-    elbow_proj = project(projection_line, elbow_target[:2])
-    elbow_coordinates = (elbow_proj, elbow_target[2])
+    #elbow_proj = project(projection_line, elbow_target[:2])
+    #elbow_coordinates = (elbow_proj, elbow_target[2])
+    elbow_top_down_length = math.sqrt(elbow_target[0] ** 2 + elbow_target[1] **2)
+    elbow_coordinates = (elbow_top_down_length, elbow_target[2])
 
     true_base_coordinates = (0, 0, arm_DH[0][0])
 
@@ -425,15 +428,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) 
 
@@ -522,4 +526,6 @@ def legalIKPositionPicker(poses, cur_pose):
 
 def inverseKinematics(hand_pose, cur_pose):
 
+    return legalIKPositionPicker(inverseKinematicsAngleOptions(hand_pose, cur_pose), cur_pose)
+
     return legalIKPositionPicker(inverseKinematicsAngleOptions(hand_pose, cur_pose), cur_pose)

arm_control/src/arm_pathfinding.py

@@ -0,0 +1,90 @@
+import numpy as np
+import math
+
+max_vels = [.1, .1, .1, .1, .1] # waist shoulder elbow wrist hand maximum velocities (TEMPORARY VALUES)
+previous_end_joints = [0 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 joints 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
+        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, max_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):
+    half = time/2
+    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, max_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, polynomial):
+    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/view_arm_ref.py

@@ -4,11 +4,12 @@
 import math
 
 # q = [math.pi/2, 0, 0, math.pi/2, 0]
-# q = [0, 0, 0, 0, 0]
+#q = [0, 0, 0, 0, 0]
+q = [ 0.07144171, -1.22915937, -0.03627638, -0.67306168,  1.36934897]
 # q = [math.pi/2, 0, 0, math.pi/2, math.pi]
 # q = [math.pi/2, math.pi/4, math.pi/4, -math.pi/2, math.pi/4]
 # q = [math.pi/2, -math.pi/2, math.pi/3, math.pi/2, -math.pi/4]
-q = [0, math.pi/2, 0, 0, 0]
+#q = [0, math.pi/2, 0, 0, 0]
 
 Ts = kin._FK(q)