This is the first blog in a series to develop a comprehensive suite of codes for robot arm software that could potentially become a realistic application in life or industry usage.
main.py and robot_arm.py
# Import necessary libraries
import speech_recognition as sr
import spacy
from robot_arm import RobotArm # Import the RobotArm module
# Initialize NLP model
nlp = spacy.load("en_core_web_sm")
# Initialize the robot arm with link lengths
robot_arm = RobotArm(link1_length=10, link2_length=10)
def listen_for_command():
# Initialize the recognizer
recognizer = sr.Recognizer()
# Use the microphone as the source for input
with sr.Microphone() as source:
print("Listening for command...")
audio = recognizer.listen(source)
# Convert audio to text
try:
command_text = recognizer.recognize_google(audio)
print(f"Command received: {command_text}")
return command_text
except sr.UnknownValueError:
print("Could not understand audio. Please try again.")
return None
except sr.RequestError as e:
print(f"Could not request results; {e}. Please check your network connection.")
return None
def process_command(command_text):
# Parse the command using NLP
doc = nlp(command_text)
# Extract the verb (action) and objects from the command
action = None
objects = []
position = None
for token in doc:
if token.pos_ == "VERB":
action = token.lemma_
if token.pos_ in ["NOUN", "PROPN"]:
objects.append(token.text.lower())
if token.like_num:
if position is None:
position = [float(token.text)]
else:
position.append(float(token.text))
# Define a dictionary of supported actions and their corresponding robot functions
actions = {
"lift": robot_arm.lift,
"bring": robot_arm.bring,
"move": robot_arm.move,
"grab": robot_arm.grab,
"put": robot_arm.put,
"go": robot_arm.move_to_position
}
# Check if the action is supported and execute the corresponding function
if action in actions:
if action == "go" and position and len(position) == 2:
print(f"Moving to position: ({position[0]}, {position[1]})")
actions[action](position[0], position[1])
elif objects:
print(f"Executing: {action} {', '.join(objects)}")
actions[action](*objects)
else:
print(f"Action '{action}' recognized, but no objects or position specified.")
else:
print(f"Action '{action}' not recognized. Supported actions are: {', '.join(actions.keys())}")
def main():
while True:
command_text = listen_for_command()
if command_text:
process_command(command_text)
else:
print("No valid command received. Listening again...")
if __name__ == "__main__":
main()
import numpy as np
from scipy.linalg import expm, logm
class RobotArm:
def __init__(self, link1_length, link2_length):
self.link1_length = link1_length
self.link2_length = link2_length
def skew(self, v):
"""Convert a 3D vector to a skew-symmetric matrix."""
return np.array([[0, -v[2], v[1]],
[v[2], 0, -v[0]],
[-v[1], v[0], 0]])
def forward_kinematics(self, theta1, theta2):
"""Compute the end-effector position given joint angles."""
T1 = self.joint_transform(self.link1_length, theta1)
T2 = self.joint_transform(self.link2_length, theta2)
T = np.dot(T1, T2)
return T[:2, 3] # Return x, y coordinates
def joint_transform(self, length, theta):
"""Compute the transformation matrix for a joint."""
c, s = np.cos(theta), np.sin(theta)
return np.array([[c, -s, 0, length*c],
[s, c, 0, length*s],
[0, 0, 1, 0],
[0, 0, 0, 1]])
def compute_jacobian(self, theta1, theta2):
"""Compute the Jacobian matrix."""
J = np.zeros((2, 2))
J[:, 0] = [-self.link1_length*np.sin(theta1) - self.link2_length*np.sin(theta1 + theta2),
self.link1_length*np.cos(theta1) + self.link2_length*np.cos(theta1 + theta2)]
J[:, 1] = [-self.link2_length*np.sin(theta1 + theta2),
self.link2_length*np.cos(theta1 + theta2)]
return J
def inverse_kinematics(self, target_x, target_y, max_iterations=100, tolerance=1e-3):
"""Compute inverse kinematics using Lie group methods."""
theta = np.array([0, 0]) # Initial guess
for _ in range(max_iterations):
current_pos = self.forward_kinematics(theta[0], theta[1])
error = np.array([target_x, target_y]) - current_pos
if np.linalg.norm(error) < tolerance:
return theta
J = self.compute_jacobian(theta[0], theta[1])
# Compute the twist
twist = np.linalg.pinv(J) @ error
# Convert twist to se(2) Lie algebra
se2_algebra = np.array([[0, -twist[1], twist[0]],
[twist[1], 0, 0],
[0, 0, 0]])
# Compute the incremental transformation
delta_T = expm(se2_algebra)
# Extract the rotation part
delta_R = delta_T[:2, :2]
# Convert rotation to angle
delta_theta = np.arctan2(delta_R[1, 0], delta_R[0, 0])
# Update joint angles
theta += np.array([delta_theta, delta_theta])
print("Inverse kinematics did not converge")
return None
def move_to_position(self, target_x, target_y):
angles = self.inverse_kinematics(target_x, target_y)
if angles is not None:
print(f"Moving to position ({target_x}, {target_y}) with angles: {np.degrees(angles[0]):.2f}°, {np.degrees(angles[1]):.2f}°")
# Here you would add the code to physically move the robot arm
else:
print("Cannot move to the specified position.")
def lift(self, *objects):
for obj in objects:
if self.is_within_reach(obj) and self.is_weight_supported(obj):
print(f"Lifting {obj}")
# Simulate the lifting process
self.perform_lift(obj)
print(f"{obj} has been lifted successfully.")
else:
print(f"Cannot lift {obj}. It may be out of reach or too heavy.")
def is_within_reach(self, obj):
# Placeholder logic to check if the object is within reach
# In a real scenario, this would involve sensor data or predefined coordinates
print(f"Checking if {obj} is within reach...")
return True # Assume all objects are within reach for this example
def is_weight_supported(self, obj):
# Placeholder logic to check if the object's weight is supported
# In a real scenario, this would involve weight sensors or predefined weight limits
print(f"Checking if {obj} is within weight limits...")
return True # Assume all objects are within weight limits for this example
def perform_lift(self, obj):
# Placeholder for the actual lifting mechanism
# This could involve sending commands to motors or actuators
print(f"Performing lift operation for {obj}...")
def bring(self, *objects):
print(f"Bringing {', '.join(objects)}")
def move(self, *objects):
print(f"Moving {', '.join(objects)}")
def grab(self, *objects):
print(f"Grabbing {', '.join(objects)}")
def put(self, *objects):
print(f"Putting {', '.join(objects)}")
# Example usage
if __name__ == "__main__":
arm = RobotArm(link1_length=10, link2_length=10)
arm.move_to_position(15, 5)What’s worth highlighting is the math behind arm movements. It includes Forward Kinematics: Uses transformation matrices to compute the end-effector’s position. Jacobian: Relates joint velocities to end-effector velocities and is used to compute the twist for correcting position errors. Inverse Kinematics: Uses Lie algebra and exponential maps to iteratively update joint angles and reach the target position.
next I will expand the files to make it more sophisticated and capable.
Naixian Zhang 