diff --git a/arm_control/src/arm_kinematics.py b/arm_control/src/arm_kinematics.py index 3bd33e04..db7bee1a 100644 --- a/arm_control/src/arm_kinematics.py +++ b/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,8 +407,8 @@ 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) @@ -415,6 +416,7 @@ def inverseKinematicsJointPositions(hand_pose): 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) \ No newline at end of file + return legalIKPositionPicker(inverseKinematicsAngleOptions(hand_pose), cur_pose) diff --git a/arm_control/src/arm_pathfinding.py b/arm_control/src/arm_pathfinding.py new file mode 100644 index 00000000..52d1edfc --- /dev/null +++ b/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) diff --git a/arm_control/src/arm_unit_tests.py b/arm_control/src/arm_unit_tests.py index 7c7cbdaa..043bfa4f 100644 --- a/arm_control/src/arm_unit_tests.py +++ b/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()