SERVOS!
1. Kinematic Architecture & Coordinate System
We define the physical arm using geometric parameters (Link lengths) and joint constraints.
Link Lengths:
L₁: Height of the base rotation plane to the shoulder joint.
L₂: Length of the upper arm (Shoulder to Elbow).
L₃: Length of the forearm (Elbow to Tip/End Effector).
Joint Angles (θ):
θ₀ (Base): Rotates around the Z-axis (horizontal plane sweep).
θ₁ (Shoulder): Rotates around the horizontal Y-axis (vertical swing).
θ₂ (Elbow): Rotates around the horizontal Y-axis (relative to the upper arm).
2. Analytical Inverse Kinematics Formulas
# ==========================================
# 4. INVERSE KINEMATICS ENGINE
# ==========================================
def calculate_ik(x, y, z):
"""Calculates necessary joint angles in degrees for target (X, Y, Z)."""
try:
# Base Angle
theta_0 = math.atan2(y, x)
# Intermediate projections
r = math.sqrt(x**2 + y**2)
z_rel = z - L1
D = math.sqrt(r**2 + z_rel**2)
# Workspace limit check
if D > (L2 + L3) or D < abs(L2 - L3):
raise ValueError("Target coordinate is out of physical reach.")
# Elbow Angle (Law of Cosines)
cos_theta_2 = (D**2 - L2**2 - L3**2) / (2 * L2 * L3)
theta_2 = math.acos(cos_theta_2)
# Shoulder Angle
alpha = math.atan2(z_rel, r)
cos_beta = (L2**2 + D**2 - L3**2) / (2 * L2 * D)
beta = math.acos(cos_beta)
theta_1 = alpha + beta
# Convert radians to system degrees
return {
"base": math.degrees(theta_0),
"shoulder": math.degrees(theta_1),
"elbow": math.degrees(theta_2)
}
except (ValueError, ZeroDivisionError):
return None
So, that should attempt to explain some things like formulas and the entire engine used during some of the source code.
It is not foolproof and may have bugs in it. So, be careful using it, i.e. as it may cause confounded scenarios within your builds depending on the length and mechanics of the build.
atan2 helps to prevent 0 division or division by 0 which Python3 hates to use. It hates it so much that it prevents scripts from running no matter how it is implemented.
Also, if you find the bug in it, please let me know. I have tested some of the source code so far while looking up parts to the mathematics and equations/formulas. If you would like to prove this Inverse Kinematics Engine incorrect, please be my guest.
Seth
P.S. I am always learning and trying to perform new ideas with the beagleboard SBCs.
So, basically…
- build a plane
- turn the plane into coordinates
- make the coordinates from 2D to 3D via angles from degrees
It seems simple when given the source code like this code above and then researching the internal structure of it all. For example, we have limits for the projections. So, if our engine was to move out of its recommended values, we would receive negative feedback like in our source code.
Oh and radians to degrees formula is done by adding a beforehand matrix of base, shoulder, and elbow for our 3 DoF Arm.
Something like this type of matrix would be used:
SERVO_MATRIX = {
"base": {"ch": 0, "min_deg": -90, "max_deg": 90, "min_us": 1000, "max_us": 2000}
}
Now, like some servos that I have come across, negative values in matrices are a disqualifying factor. And the minimum uS and maximum uS (microseconds) seems to be dedicated to the specific servo especially when utilizing various controls.
For instance, 1000 (say full left or 0) to 1500 (say middle or 90) to 2000 (say full right or 180) states only instances or examples and it is not 100% correct depending on your servo and servo controls.
The datasheet for your servo should explain a more in depth way to evaluating the 0 degree to 180 or 235 degrees of universal maneuvering. Also, like the controller I am using, maestro from Pololu, there is a chip onboard that needs to be ready by selecting the correct servo and servo settings.
So, for instance, in my Maestro Servo Control Center, I set Servo 0 to whatever is in my datasheet for my servos. Right now for this Arm, I am testing a DS Servo with particular data.
Unlike a S2003 (discontinued) from Futaba, this DS Servo does not use 0 to 180 degrees but does use 0 to 235 degrees for rotation parameters.
And unlike the S2003, this digital servo called the DS Servo utilizes the 500 uS to 2500 uS timing. Why you may ask…
Well, some say it is a unsafe attribute while others utilize this microseconds detail out of choice.
Earlier in my brief, I stated that these DS Servos are 0 to 235 degrees of freedom. I may be incorrect.
Do you know how I can test if these servos are 0 to 90 or 0 to 235 degrees?I also learned something new.
In Python3, there is an unpack operator: *.
See here:
init_angles = calculate_ik(*current_position)
So, for something like current_position = WAYPOINTS[0], where WAYPOINTS[0] is a small array or large array, the init_angles = calculate_ik(*current_position) unpacks the array into later used source code.
And all along, I figured * only applied to different languages and not to Python3 at all.
Here is a small example from stack overflow online:
def print_all(*values):
for item in values:
print(item)
# You can pass as many values as you want
print_all("Apple", "Banana", "Cherry")