Monday, March 18, 2024
Unwanted adventures in inverse kinematics
Given up on Marlin. I've installed straight GRBL on the RAMPS assembly and have written an amateur inverse kinematics routine in python to drive GRBL X, Y & Z as the delta towers C, B, A (which seems to be engineering convention, C being on the X axis). Once I get a bit of help, it's probably best to peel the control functions out of OpenFlexure, but their codebase is somewhat impenetrable. Suggestions welcome.
# Function to Calculate Tower Joint Positions:
# - calculate_tower_joint_positions calculates the positions of the tower joints based on the given TCP location.
# - It utilizes inverse kinematics to determine the heights of the tower joints above the XY plane.
# - The function takes a TCP location tuple (x, y, z) as input and returns a list of tower joint heights.
def calculate_tower_joint_positions(tcp_location):
link_length = 75 # units
radius = 35 # units
# Calculate the distance from the TCP to each tower base
tower_distances = [math.sqrt((tcp_location[0] - radius * math.cos(theta))**2 + (tcp_location[1] - radius * math.sin(theta))**2) for theta in [0, 2*math.pi/3, -2*math.pi/3]]
# Calculate tower joint heights above the XY plane
tower_joint_heights = [tcp_location[2] + math.sqrt(link_length**2 - dist**2) for dist in tower_distances]
return tower_joint_heights
# Test cases
test_locations = [(0, 0, 0), (-1, 1, 0), (0, 0, 10), (0, 20, 0), (5, 0, 0), (15, 0, 10)]
for location in test_locations:
tower_joint_heights = calculate_tower_joint_positions(location)
print("TCP Location:", location)
print("Tower Joint Heights:", tower_joint_heights)
print()
# - calculate_tower_joint_positions calculates the positions of the tower joints based on the given TCP location.
# - It utilizes inverse kinematics to determine the heights of the tower joints above the XY plane.
# - The function takes a TCP location tuple (x, y, z) as input and returns a list of tower joint heights.
def calculate_tower_joint_positions(tcp_location):
link_length = 75 # units
radius = 35 # units
# Calculate the distance from the TCP to each tower base
tower_distances = [math.sqrt((tcp_location[0] - radius * math.cos(theta))**2 + (tcp_location[1] - radius * math.sin(theta))**2) for theta in [0, 2*math.pi/3, -2*math.pi/3]]
# Calculate tower joint heights above the XY plane
tower_joint_heights = [tcp_location[2] + math.sqrt(link_length**2 - dist**2) for dist in tower_distances]
return tower_joint_heights
# Test cases
test_locations = [(0, 0, 0), (-1, 1, 0), (0, 0, 10), (0, 20, 0), (5, 0, 0), (15, 0, 10)]
for location in test_locations:
tower_joint_heights = calculate_tower_joint_positions(location)
print("TCP Location:", location)
print("Tower Joint Heights:", tower_joint_heights)
print()
