INTRODUCTION TO MODULAR ROBOT ARM SYSTEMS
Welcome to the exciting world of building your own robot arm product line. This article will guide you through creating a flexible, modular robot arm system that can be configured with three to six degrees of freedom. The focus here is on the software components that bring your mechanical creation to life.
A robot arm with multiple degrees of freedom can perform complex tasks by combining simple movements. Think of your own arm: your shoulder rotates, your elbow bends, your wrist twists, and your fingers grasp. Each of these movements represents one degree of freedom. Our robot arm works similarly, with each degree of freedom controlled by a dedicated motor.
The beauty of this product line approach is modularity. You can start with a simple three-axis arm for basic pick-and-place operations, then expand to a six-axis arm for more sophisticated tasks. The software architecture we will develop supports all configurations without requiring a complete rewrite.
UNDERSTANDING THE HARDWARE FOUNDATION
Before diving into software, let us understand the hardware components that our software will control. At the heart of our system sits a Raspberry Pi, which serves as the main computational brain. The Raspberry Pi runs Linux and executes our high-level control software, vision processing, and provides the REST API interface for external applications.
Connected to the Raspberry Pi via USB serial communication is either an Arduino board or an ESP32 microcontroller. This microcontroller board handles the real-time motor control tasks. Real-time control is critical because motors need precise timing signals that a multitasking operating system like Linux cannot guarantee. The microcontroller runs firmware that receives high-level commands from the Raspberry Pi and translates them into the exact electrical signals needed to drive the motors.
Each degree of freedom in the robot arm is powered by a motor. For most applications, servo motors work excellently because they provide both rotation and position feedback. A servo motor contains an internal controller that maintains a specific angle when commanded. For heavier loads or continuous rotation applications, you might use stepper motors or DC motors with encoders. The choice depends on your specific robot arm design, but our software architecture accommodates all motor types.
The optional camera module connects directly to the Raspberry Pi via the Camera Serial Interface. This camera enables vision-based object detection and tracking, allowing your robot arm to identify and interact with objects in its workspace intelligently.
Power distribution is crucial. The Raspberry Pi typically runs on five volts, while motors may require anywhere from five to twelve volts or more depending on their size and torque requirements. A proper power supply system with voltage regulators ensures stable operation. The microcontroller board often includes voltage regulation, but external motor drivers may be necessary for high-power motors.
SOFTWARE ARCHITECTURE OVERVIEW
Our software system follows a layered architecture that separates concerns and promotes maintainability. At the lowest level runs the microcontroller firmware, written in C++ for Arduino or ESP32. This firmware directly controls motor positions and reads sensor data. It communicates with the Raspberry Pi through a well-defined serial protocol.
The middle layer consists of the Raspberry Pi control application, written in Python. This application manages the robot arm's state, implements the learning and control modes, processes vision data, and coordinates all subsystems. It translates user intentions into motor commands and maintains the robot's understanding of its position and configuration.
The top layer is the REST API, also running on the Raspberry Pi. This API exposes the robot arm's capabilities to external applications over network interfaces including WiFi, Bluetooth, and Ethernet. External software can command the robot, query its status, upload learned sequences, and retrieve sensor data through standardized HTTP requests.
Supporting these layers is the LLM integration subsystem, which allows the robot arm to understand natural language commands and generate motion plans. This subsystem supports both local LLM inference on the Raspberry Pi using various GPU architectures and remote LLM services accessed via API calls.
THE CALIBRATION SYSTEM
Calibration establishes the robot arm's reference frame. Without calibration, the robot has no way to know where it is in space. The calibration process defines an initial position that serves as the origin for all subsequent movements.
When you first assemble a robot arm, each joint can be at any arbitrary angle. The calibration system moves each joint to a known, repeatable position. This might be all joints at zero degrees, or a specific pose that makes mechanical sense for your design. For example, you might calibrate with the arm fully extended vertically.
The calibration position is stored in non-volatile memory on the microcontroller. Every time the robot powers on, it automatically moves to this calibrated initial position before accepting any other commands. This ensures consistent, predictable behavior.
During calibration, you manually position the robot arm into the desired initial pose. Then you trigger the calibration command through the software interface. The system reads the current motor positions and saves these values as the reference zero point. From this moment forward, all position commands are interpreted relative to this calibrated position.
Here is a simple example of how calibration data might be stored and used:
// Calibration data structure on microcontroller
struct CalibrationData {
int joint_offsets[6]; // Offset for each joint
bool is_calibrated;
uint32_t checksum;
};
CalibrationData calibration;
void saveCalibration() {
// Read current positions from all servos
for (int i = 0; i < NUM_JOINTS; i++) {
calibration.joint_offsets[i] = servos[i].read();
}
calibration.is_calibrated = true;
calibration.checksum = calculateChecksum(&calibration);
// Save to EEPROM
EEPROM.put(CALIBRATION_ADDRESS, calibration);
}
void loadCalibration() {
EEPROM.get(CALIBRATION_ADDRESS, calibration);
// Verify checksum
if (calculateChecksum(&calibration) != calibration.checksum) {
calibration.is_calibrated = false;
}
}
void moveToInitialPosition() {
if (!calibration.is_calibrated) {
// Cannot move to initial position without calibration
sendError("Robot not calibrated");
return;
}
// Move each joint to its calibrated zero position
for (int i = 0; i < NUM_JOINTS; i++) {
servos[i].write(calibration.joint_offsets[i]);
}
}
The checksum validation ensures that the calibration data has not been corrupted. If the checksum fails, the system knows the calibration is invalid and requires recalibration before operation.
MOTOR CONTROL AND COMMUNICATION PROTOCOL
The communication between the Raspberry Pi and the microcontroller follows a structured protocol. This protocol must be reliable, efficient, and extensible to support future enhancements.
We use a text-based protocol over serial communication. Text-based protocols are easier to debug than binary protocols because you can read the messages directly. Each command is a single line terminated by a newline character. The microcontroller parses incoming commands, executes them, and sends responses back to the Raspberry Pi.
A typical command structure looks like this:
MOVE J1 90 J2 45 J3 120
This command tells the microcontroller to move joint one to ninety degrees, joint two to forty-five degrees, and joint three to one hundred twenty degrees. All angles are relative to the calibrated zero position.
The microcontroller responds with acknowledgments or error messages:
OK
ERROR Invalid joint number
Here is how the microcontroller firmware parses and executes commands:
// Command parser on microcontroller
void processCommand(String command) {
command.trim();
if (command.startsWith("MOVE")) {
handleMoveCommand(command);
} else if (command.startsWith("GRIP")) {
handleGripCommand(command);
} else if (command.startsWith("CALIBRATE")) {
handleCalibrateCommand(command);
} else if (command.startsWith("STATUS")) {
handleStatusCommand(command);
} else if (command.startsWith("HOME")) {
moveToInitialPosition();
Serial.println("OK");
} else {
Serial.println("ERROR Unknown command");
}
}
void handleMoveCommand(String command) {
// Parse joint positions from command
// Example: "MOVE J1 90 J2 45 J3 120"
int positions[6] = {-1, -1, -1, -1, -1, -1};
int index = 5; // Skip "MOVE "
while (index < command.length()) {
// Find next joint identifier
int jIndex = command.indexOf('J', index);
if (jIndex == -1) break;
// Extract joint number
int jointNum = command.substring(jIndex + 1, jIndex + 2).toInt();
if (jointNum < 1 || jointNum > NUM_JOINTS) {
Serial.println("ERROR Invalid joint number");
return;
}
// Extract position value
int spaceIndex = command.indexOf(' ', jIndex + 2);
int nextJ = command.indexOf('J', jIndex + 1);
int endIndex = (nextJ != -1 && nextJ < spaceIndex) ? nextJ :
(spaceIndex != -1) ? spaceIndex : command.length();
int position = command.substring(jIndex + 3, endIndex).toInt();
positions[jointNum - 1] = position;
index = endIndex;
}
// Execute movement
for (int i = 0; i < NUM_JOINTS; i++) {
if (positions[i] != -1) {
int targetAngle = calibration.joint_offsets[i] + positions[i];
servos[i].write(constrain(targetAngle, 0, 180));
}
}
Serial.println("OK");
}
The Raspberry Pi side maintains a connection to the serial port and sends commands while listening for responses. Here is a Python class that manages this communication:
import serial
import time
import threading
import queue
class RobotController:
def __init__(self, port='/dev/ttyUSB0', baudrate=115200):
self.serial_port = serial.Serial(port, baudrate, timeout=1)
self.response_queue = queue.Queue()
self.running = True
# Start response listener thread
self.listener_thread = threading.Thread(target=self._listen_responses)
self.listener_thread.daemon = True
self.listener_thread.start()
time.sleep(2) # Wait for microcontroller to initialize
def _listen_responses(self):
"""Background thread that reads responses from microcontroller"""
while self.running:
if self.serial_port.in_waiting > 0:
response = self.serial_port.readline().decode('utf-8').strip()
self.response_queue.put(response)
time.sleep(0.01)
def send_command(self, command, wait_response=True, timeout=5.0):
"""Send command to microcontroller and optionally wait for response"""
# Clear any old responses
while not self.response_queue.empty():
self.response_queue.get()
# Send command
self.serial_port.write((command + '\n').encode('utf-8'))
if not wait_response:
return None
# Wait for response
start_time = time.time()
while time.time() - start_time < timeout:
try:
response = self.response_queue.get(timeout=0.1)
return response
except queue.Empty:
continue
raise TimeoutError(f"No response received for command: {command}")
def move_joints(self, joint_positions):
"""Move joints to specified positions
Args:
joint_positions: Dictionary mapping joint numbers to angles
Example: {1: 90, 2: 45, 3: 120}
"""
command_parts = ["MOVE"]
for joint_num, angle in sorted(joint_positions.items()):
command_parts.append(f"J{joint_num} {angle}")
command = " ".join(command_parts)
response = self.send_command(command)
if response != "OK":
raise RuntimeError(f"Move command failed: {response}")
def move_to_home(self):
"""Move robot to calibrated home position"""
response = self.send_command("HOME")
if response != "OK":
raise RuntimeError(f"Home command failed: {response}")
def calibrate(self):
"""Save current position as calibrated home position"""
response = self.send_command("CALIBRATE")
if response != "OK":
raise RuntimeError(f"Calibration failed: {response}")
def close(self):
"""Clean shutdown of controller"""
self.running = False
self.listener_thread.join()
self.serial_port.close()
This controller class provides a clean Python interface to the robot arm. It handles the serial communication details and provides methods for common operations. The background listener thread ensures that responses are captured even if commands are sent rapidly.
IMPLEMENTING LEARNING MODE
Learning mode is where the magic happens. In this mode, the user physically moves the robot arm through a sequence of actions, and the software records these movements for later playback. This is called kinesthetic teaching or programming by demonstration.
The key challenge in learning mode is capturing the robot's state at the right moments. We cannot simply record every single position because that would create enormous data files and make playback jerky. Instead, we record waypoints at significant moments: when the user pauses movement, when the gripper opens or closes, or when a joint changes direction.
To detect when the user is moving the robot, we need to read the current positions of all motors continuously. Most servo motors do not provide position feedback, so we need servos with feedback capability or add external encoders. For this product line, we assume servos that can report their current angle.
Here is how the learning mode system works on the Raspberry Pi:
import time
import json
from datetime import datetime
class LearningMode:
def __init__(self, robot_controller):
self.robot = robot_controller
self.recording = False
self.recorded_sequence = []
self.last_positions = {}
self.position_threshold = 2 # Degrees of movement to trigger recording
self.pause_threshold = 0.5 # Seconds of stillness to record waypoint
self.last_movement_time = time.time()
def start_recording(self, sequence_name):
"""Begin recording a new sequence"""
self.recording = True
self.recorded_sequence = []
self.sequence_name = sequence_name
# Move to home position first
self.robot.move_to_home()
time.sleep(1)
# Record initial position
initial_state = self._get_current_state()
self.recorded_sequence.append({
'timestamp': 0.0,
'state': initial_state,
'action': 'start'
})
self.last_positions = initial_state['joints']
self.start_time = time.time()
print(f"Recording started for sequence: {sequence_name}")
print("Move the robot arm manually. Press Enter when finished.")
def _get_current_state(self):
"""Query current state from robot"""
response = self.robot.send_command("STATUS")
# Response format: "STATUS J1 90 J2 45 J3 120 GRIP 0"
state = {'joints': {}, 'gripper': 0}
parts = response.split()
i = 1 # Skip "STATUS"
while i < len(parts):
if parts[i].startswith('J'):
joint_num = int(parts[i][1:])
angle = int(parts[i + 1])
state['joints'][joint_num] = angle
i += 2
elif parts[i] == 'GRIP':
state['gripper'] = int(parts[i + 1])
i += 2
else:
i += 1
return state
def _has_significant_movement(self, current_positions):
"""Check if robot has moved significantly since last waypoint"""
for joint_num, angle in current_positions.items():
if joint_num not in self.last_positions:
return True
if abs(angle - self.last_positions[joint_num]) > self.position_threshold:
return True
return False
def update(self):
"""Called periodically during recording to capture waypoints"""
if not self.recording:
return
current_state = self._get_current_state()
current_positions = current_state['joints']
# Check for movement
if self._has_significant_movement(current_positions):
self.last_movement_time = time.time()
self.last_positions = current_positions
else:
# No movement detected
time_since_movement = time.time() - self.last_movement_time
if time_since_movement > self.pause_threshold:
# User has paused - record this waypoint
elapsed_time = time.time() - self.start_time
# Check if this is different from last recorded waypoint
if len(self.recorded_sequence) == 0 or \
current_state != self.recorded_sequence[-1]['state']:
self.recorded_sequence.append({
'timestamp': elapsed_time,
'state': current_state,
'action': 'waypoint'
})
print(f"Waypoint recorded at {elapsed_time:.2f}s")
# Reset movement timer to avoid duplicate recordings
self.last_movement_time = time.time()
def stop_recording(self):
"""Finish recording and save sequence"""
if not self.recording:
return None
self.recording = False
# Add final waypoint
final_state = self._get_current_state()
elapsed_time = time.time() - self.start_time
self.recorded_sequence.append({
'timestamp': elapsed_time,
'state': final_state,
'action': 'end'
})
# Save sequence to file
sequence_data = {
'name': self.sequence_name,
'created': datetime.now().isoformat(),
'waypoints': self.recorded_sequence
}
filename = f"sequences/{self.sequence_name}.json"
with open(filename, 'w') as f:
json.dump(sequence_data, f, indent=2)
print(f"Recording stopped. Sequence saved to {filename}")
print(f"Total waypoints: {len(self.recorded_sequence)}")
return sequence_data
def playback_sequence(self, sequence_name):
"""Play back a recorded sequence"""
filename = f"sequences/{sequence_name}.json"
with open(filename, 'r') as f:
sequence_data = json.load(f)
waypoints = sequence_data['waypoints']
print(f"Playing back sequence: {sequence_name}")
print(f"Total waypoints: {len(waypoints)}")
# Move to home position
self.robot.move_to_home()
time.sleep(1)
start_time = time.time()
for i, waypoint in enumerate(waypoints):
# Wait until the correct timestamp
target_time = waypoint['timestamp']
while time.time() - start_time < target_time:
time.sleep(0.01)
# Execute waypoint
state = waypoint['state']
# Move joints
self.robot.move_joints(state['joints'])
# Control gripper if changed
if i > 0 and state['gripper'] != waypoints[i-1]['state']['gripper']:
grip_command = "GRIP CLOSE" if state['gripper'] == 1 else "GRIP OPEN"
self.robot.send_command(grip_command)
print(f"Executed waypoint {i+1}/{len(waypoints)}")
print("Playback complete")
The learning mode continuously monitors the robot's position. When the user moves a joint, the system detects this movement. When the user pauses for more than half a second, the system records the current position as a waypoint. This creates a natural, intuitive recording experience.
The recorded sequence is saved as a JSON file containing all waypoints with their timestamps and states. During playback, the system recreates the original timing by waiting for the appropriate time before executing each waypoint.
IMPLEMENTING CONTROL MODE
Control mode is the programmatic counterpart to learning mode. Instead of learning from demonstration, the robot executes predefined programs or responds to API commands. This mode is essential for production environments where repeatability and precision are critical.
In control mode, motion planning becomes important. You cannot simply command the robot to jump from one position to another instantly. The motors need time to accelerate and decelerate smoothly. Abrupt movements can cause mechanical stress, vibration, and positioning errors.
We implement trajectory planning using simple linear interpolation between waypoints. More sophisticated systems might use spline interpolation or velocity profiling, but linear interpolation works well for most applications and is easy to understand.
Here is the control mode implementation:
import math
import time
class ControlMode:
def __init__(self, robot_controller):
self.robot = robot_controller
self.current_position = {}
self.default_speed = 50 # Degrees per second
def initialize(self):
"""Initialize control mode by moving to home and reading position"""
self.robot.move_to_home()
time.sleep(1)
# Get initial position
state = self._get_current_state()
self.current_position = state['joints']
def _get_current_state(self):
"""Query current state from robot"""
response = self.robot.send_command("STATUS")
state = {'joints': {}, 'gripper': 0}
parts = response.split()
i = 1
while i < len(parts):
if parts[i].startswith('J'):
joint_num = int(parts[i][1:])
angle = int(parts[i + 1])
state['joints'][joint_num] = angle
i += 2
elif parts[i] == 'GRIP':
state['gripper'] = int(parts[i + 1])
i += 2
else:
i += 1
return state
def move_to_position(self, target_positions, speed=None):
"""Move to target position with smooth trajectory
Args:
target_positions: Dictionary of joint angles {joint_num: angle}
speed: Movement speed in degrees per second (optional)
"""
if speed is None:
speed = self.default_speed
# Calculate maximum angular distance
max_distance = 0
for joint_num, target_angle in target_positions.items():
current_angle = self.current_position.get(joint_num, 0)
distance = abs(target_angle - current_angle)
max_distance = max(max_distance, distance)
# Calculate movement duration
duration = max_distance / speed
# Generate interpolated trajectory
steps = max(int(duration * 20), 1) # 20 updates per second
for step in range(steps + 1):
t = step / steps # Interpolation parameter from 0 to 1
interpolated_positions = {}
for joint_num, target_angle in target_positions.items():
current_angle = self.current_position.get(joint_num, 0)
interpolated_angle = current_angle + t * (target_angle - current_angle)
interpolated_positions[joint_num] = int(interpolated_angle)
# Send interpolated position to robot
self.robot.move_joints(interpolated_positions)
if step < steps:
time.sleep(duration / steps)
# Update current position
self.current_position.update(target_positions)
def execute_pick_and_place(self, pick_position, place_position,
approach_height=50, speed=None):
"""Execute a complete pick and place operation
Args:
pick_position: Dictionary of joint angles for pick location
place_position: Dictionary of joint angles for place location
approach_height: Height offset for approach (degrees)
speed: Movement speed (optional)
"""
# Create approach positions (above pick and place locations)
pick_approach = pick_position.copy()
if 3 in pick_approach: # Assuming joint 3 controls height
pick_approach[3] = pick_approach[3] + approach_height
place_approach = place_position.copy()
if 3 in place_approach:
place_approach[3] = place_approach[3] + approach_height
# Execute pick sequence
print("Moving to pick approach position...")
self.move_to_position(pick_approach, speed)
print("Opening gripper...")
self.robot.send_command("GRIP OPEN")
time.sleep(0.5)
print("Moving to pick position...")
self.move_to_position(pick_position, speed)
print("Closing gripper...")
self.robot.send_command("GRIP CLOSE")
time.sleep(0.5)
print("Lifting object...")
self.move_to_position(pick_approach, speed)
# Execute place sequence
print("Moving to place approach position...")
self.move_to_position(place_approach, speed)
print("Moving to place position...")
self.move_to_position(place_position, speed)
print("Opening gripper...")
self.robot.send_command("GRIP OPEN")
time.sleep(0.5)
print("Retracting...")
self.move_to_position(place_approach, speed)
print("Pick and place complete")
def execute_trajectory(self, waypoints, speed=None):
"""Execute a trajectory defined by multiple waypoints
Args:
waypoints: List of dictionaries, each containing joint positions
speed: Movement speed (optional)
"""
for i, waypoint in enumerate(waypoints):
print(f"Moving to waypoint {i+1}/{len(waypoints)}...")
self.move_to_position(waypoint, speed)
print("Trajectory complete")
The control mode provides high-level operations like pick and place that combine multiple primitive movements. The trajectory interpolation ensures smooth motion between waypoints. This makes the robot's movements appear natural and reduces mechanical wear.
VISION SYSTEM INTEGRATION
The vision system transforms the robot arm from a blind automaton into an intelligent agent that can perceive and respond to its environment. Using a camera mounted on the robot or in the workspace, the system can detect objects, determine their positions, and guide the robot to interact with them.
We use OpenCV, a powerful computer vision library, to process camera images. The vision system runs on the Raspberry Pi, which has sufficient computational power for real-time image processing. For more demanding tasks, we can leverage GPU acceleration if available.
Here is a vision system implementation that detects colored objects:
import cv2
import numpy as np
import threading
import queue
class VisionSystem:
def __init__(self, camera_index=0):
self.camera = cv2.VideoCapture(camera_index)
self.camera.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
self.camera.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
self.running = False
self.frame_queue = queue.Queue(maxsize=2)
self.detection_results = {}
# Color ranges for object detection (HSV color space)
self.color_ranges = {
'red': [(0, 100, 100), (10, 255, 255)],
'green': [(40, 100, 100), (80, 255, 255)],
'blue': [(100, 100, 100), (130, 255, 255)]
}
def start(self):
"""Start vision processing thread"""
self.running = True
self.capture_thread = threading.Thread(target=self._capture_loop)
self.capture_thread.daemon = True
self.capture_thread.start()
self.process_thread = threading.Thread(target=self._process_loop)
self.process_thread.daemon = True
self.process_thread.start()
def _capture_loop(self):
"""Continuously capture frames from camera"""
while self.running:
ret, frame = self.camera.read()
if ret:
# Add frame to queue, discard old frames if queue is full
if self.frame_queue.full():
try:
self.frame_queue.get_nowait()
except queue.Empty:
pass
self.frame_queue.put(frame)
time.sleep(0.03) # ~30 FPS
def _process_loop(self):
"""Process frames to detect objects"""
while self.running:
try:
frame = self.frame_queue.get(timeout=1.0)
except queue.Empty:
continue
# Convert to HSV color space
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
detected_objects = []
# Detect each color
for color_name, (lower, upper) in self.color_ranges.items():
# Create mask for this color
lower_bound = np.array(lower)
upper_bound = np.array(upper)
mask = cv2.inRange(hsv, lower_bound, upper_bound)
# Apply morphological operations to reduce noise
kernel = np.ones((5, 5), np.uint8)
mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)
mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel)
# Find contours
contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)
# Process each contour
for contour in contours:
area = cv2.contourArea(contour)
# Filter small contours (noise)
if area < 500:
continue
# Calculate centroid
M = cv2.moments(contour)
if M['m00'] == 0:
continue
cx = int(M['m10'] / M['m00'])
cy = int(M['m01'] / M['m00'])
# Get bounding rectangle
x, y, w, h = cv2.boundingRect(contour)
detected_objects.append({
'color': color_name,
'center': (cx, cy),
'area': area,
'bounding_box': (x, y, w, h)
})
# Update detection results
self.detection_results = {
'timestamp': time.time(),
'objects': detected_objects,
'frame_shape': frame.shape
}
def get_detections(self):
"""Get latest object detections"""
return self.detection_results.copy()
def find_object_by_color(self, color):
"""Find the largest object of specified color
Returns:
Dictionary with object info, or None if not found
"""
detections = self.get_detections()
if 'objects' not in detections:
return None
# Filter by color and find largest
matching_objects = [obj for obj in detections['objects']
if obj['color'] == color]
if not matching_objects:
return None
# Return largest object
return max(matching_objects, key=lambda obj: obj['area'])
def pixel_to_robot_coordinates(self, pixel_x, pixel_y, camera_calibration):
"""Convert pixel coordinates to robot workspace coordinates
Args:
pixel_x, pixel_y: Pixel coordinates in image
camera_calibration: Dictionary containing calibration parameters
Returns:
(x, y) coordinates in robot workspace (millimeters)
"""
# This is a simplified transformation
# Real implementation would use camera calibration matrix
frame_width = camera_calibration.get('frame_width', 640)
frame_height = camera_calibration.get('frame_height', 480)
workspace_width = camera_calibration.get('workspace_width', 300) # mm
workspace_height = camera_calibration.get('workspace_height', 300) # mm
# Normalize pixel coordinates
norm_x = pixel_x / frame_width
norm_y = pixel_y / frame_height
# Convert to workspace coordinates
workspace_x = norm_x * workspace_width
workspace_y = norm_y * workspace_height
return (workspace_x, workspace_y)
def stop(self):
"""Stop vision processing"""
self.running = False
if hasattr(self, 'capture_thread'):
self.capture_thread.join()
if hasattr(self, 'process_thread'):
self.process_thread.join()
self.camera.release()
The vision system runs in separate threads to avoid blocking the main control loop. It continuously captures frames and processes them to detect colored objects. The detection results are made available to other parts of the system through the get_detections method.
Camera calibration is crucial for accurate coordinate transformation. The pixel_to_robot_coordinates method converts pixel positions in the camera image to real-world coordinates in the robot's workspace. A complete implementation would use a full camera calibration matrix obtained through a calibration procedure, but this simplified version demonstrates the concept.
REST API DESIGN
The REST API exposes the robot arm's capabilities to external applications. Any software that can make HTTP requests can control the robot, whether it is a web application, mobile app, or another robot system. This makes the robot arm a networked device that can be integrated into larger automation systems.
We implement the REST API using Flask, a lightweight Python web framework. The API follows RESTful principles, using standard HTTP methods and status codes.
Here is the complete REST API implementation:
from flask import Flask, request, jsonify
from flask_cors import CORS
import threading
import logging
class RobotAPI:
def __init__(self, robot_controller, learning_mode, control_mode, vision_system):
self.app = Flask(__name__)
CORS(self.app) # Enable cross-origin requests
self.robot = robot_controller
self.learning = learning_mode
self.control = control_mode
self.vision = vision_system
self.setup_routes()
# Configure logging
logging.basicConfig(level=logging.INFO)
self.logger = logging.getLogger(__name__)
def setup_routes(self):
"""Define all API endpoints"""
@self.app.route('/api/status', methods=['GET'])
def get_status():
"""Get current robot status"""
try:
state = self.control._get_current_state()
return jsonify({
'status': 'ok',
'joints': state['joints'],
'gripper': state['gripper'],
'mode': 'learning' if self.learning.recording else 'control'
}), 200
except Exception as e:
self.logger.error(f"Status error: {e}")
return jsonify({'status': 'error', 'message': str(e)}), 500
@self.app.route('/api/home', methods=['POST'])
def move_home():
"""Move robot to home position"""
try:
self.robot.move_to_home()
return jsonify({'status': 'ok', 'message': 'Moved to home'}), 200
except Exception as e:
self.logger.error(f"Home error: {e}")
return jsonify({'status': 'error', 'message': str(e)}), 500
@self.app.route('/api/calibrate', methods=['POST'])
def calibrate():
"""Calibrate robot at current position"""
try:
self.robot.calibrate()
return jsonify({'status': 'ok', 'message': 'Calibration saved'}), 200
except Exception as e:
self.logger.error(f"Calibration error: {e}")
return jsonify({'status': 'error', 'message': str(e)}), 500
@self.app.route('/api/move', methods=['POST'])
def move_joints():
"""Move joints to specified positions
Request body:
{
"joints": {"1": 90, "2": 45, "3": 120},
"speed": 50 // optional
}
"""
try:
data = request.get_json()
if 'joints' not in data:
return jsonify({'status': 'error',
'message': 'Missing joints parameter'}), 400
# Convert string keys to integers
joints = {int(k): v for k, v in data['joints'].items()}
speed = data.get('speed', None)
self.control.move_to_position(joints, speed)
return jsonify({'status': 'ok', 'message': 'Movement complete'}), 200
except Exception as e:
self.logger.error(f"Move error: {e}")
return jsonify({'status': 'error', 'message': str(e)}), 500
@self.app.route('/api/gripper', methods=['POST'])
def control_gripper():
"""Control gripper
Request body:
{
"action": "open" or "close"
}
"""
try:
data = request.get_json()
action = data.get('action', '').lower()
if action not in ['open', 'close']:
return jsonify({'status': 'error',
'message': 'Invalid action'}), 400
command = f"GRIP {action.upper()}"
self.robot.send_command(command)
return jsonify({'status': 'ok', 'message': f'Gripper {action}'}), 200
except Exception as e:
self.logger.error(f"Gripper error: {e}")
return jsonify({'status': 'error', 'message': str(e)}), 500
@self.app.route('/api/learn/start', methods=['POST'])
def start_learning():
"""Start learning mode
Request body:
{
"sequence_name": "pick_and_place_1"
}
"""
try:
data = request.get_json()
sequence_name = data.get('sequence_name', 'unnamed_sequence')
self.learning.start_recording(sequence_name)
return jsonify({'status': 'ok',
'message': 'Learning started',
'sequence_name': sequence_name}), 200
except Exception as e:
self.logger.error(f"Learn start error: {e}")
return jsonify({'status': 'error', 'message': str(e)}), 500
@self.app.route('/api/learn/stop', methods=['POST'])
def stop_learning():
"""Stop learning mode and save sequence"""
try:
sequence_data = self.learning.stop_recording()
return jsonify({
'status': 'ok',
'message': 'Learning stopped',
'waypoints': len(sequence_data['waypoints'])
}), 200
except Exception as e:
self.logger.error(f"Learn stop error: {e}")
return jsonify({'status': 'error', 'message': str(e)}), 500
@self.app.route('/api/sequence/play', methods=['POST'])
def play_sequence():
"""Play back a learned sequence
Request body:
{
"sequence_name": "pick_and_place_1"
}
"""
try:
data = request.get_json()
sequence_name = data.get('sequence_name')
if not sequence_name:
return jsonify({'status': 'error',
'message': 'Missing sequence_name'}), 400
# Run playback in background thread
playback_thread = threading.Thread(
target=self.learning.playback_sequence,
args=(sequence_name,)
)
playback_thread.start()
return jsonify({'status': 'ok',
'message': 'Playback started'}), 200
except Exception as e:
self.logger.error(f"Playback error: {e}")
return jsonify({'status': 'error', 'message': str(e)}), 500
@self.app.route('/api/vision/detect', methods=['GET'])
def detect_objects():
"""Get current object detections from vision system"""
try:
detections = self.vision.get_detections()
return jsonify({
'status': 'ok',
'detections': detections
}), 200
except Exception as e:
self.logger.error(f"Vision error: {e}")
return jsonify({'status': 'error', 'message': str(e)}), 500
@self.app.route('/api/vision/find', methods=['POST'])
def find_object():
"""Find object by color
Request body:
{
"color": "red"
}
"""
try:
data = request.get_json()
color = data.get('color', '').lower()
obj = self.vision.find_object_by_color(color)
if obj:
return jsonify({
'status': 'ok',
'found': True,
'object': obj
}), 200
else:
return jsonify({
'status': 'ok',
'found': False
}), 200
except Exception as e:
self.logger.error(f"Find error: {e}")
return jsonify({'status': 'error', 'message': str(e)}), 500
def run(self, host='0.0.0.0', port=5000):
"""Start the API server"""
self.logger.info(f"Starting API server on {host}:{port}")
self.app.run(host=host, port=port, threaded=True)
The API provides endpoints for all major robot operations. External applications can query the robot's status, command movements, start and stop learning mode, play back sequences, and access vision system data. The API returns JSON responses with consistent status indicators.
Running the API server on all network interfaces with host set to 0.0.0.0 makes it accessible from any device on the network. This enables remote control via WiFi or Ethernet. For Bluetooth connectivity, you would add a Bluetooth serial bridge that forwards commands to the API.
LLM INTEGRATION ARCHITECTURE
Integrating Large Language Models enables natural language control of the robot arm. Users can describe tasks in plain English, and the LLM translates these descriptions into robot commands. This makes the system accessible to users without programming knowledge.
The LLM integration supports both local inference on the Raspberry Pi and remote API calls to cloud-based LLM services. Local inference provides privacy and works without internet connectivity, while remote services offer more powerful models.
Supporting multiple GPU architectures is essential because different Raspberry Pi configurations and single-board computers use different hardware accelerators. We support Intel integrated GPUs, AMD ROCm, Apple Metal Performance Shaders, and NVIDIA CUDA.
Here is the LLM integration system:
import os
import json
import requests
from abc import ABC, abstractmethod
class LLMBackend(ABC):
"""Abstract base class for LLM backends"""
@abstractmethod
def generate(self, prompt, max_tokens=500, temperature=0.7):
"""Generate text from prompt"""
pass
class LocalLLMBackend(LLMBackend):
"""Local LLM inference using various GPU backends"""
def __init__(self, model_path, device='auto'):
"""Initialize local LLM
Args:
model_path: Path to model files
device: 'auto', 'cpu', 'cuda', 'rocm', 'mps', or 'intel'
"""
self.model_path = model_path
self.device = self._detect_device() if device == 'auto' else device
self.model = None
self.tokenizer = None
self._load_model()
def _detect_device(self):
"""Automatically detect best available device"""
try:
import torch
if torch.cuda.is_available():
return 'cuda'
elif hasattr(torch.backends, 'mps') and torch.backends.mps.is_available():
return 'mps'
else:
# Check for Intel or AMD
try:
import intel_extension_for_pytorch
return 'intel'
except ImportError:
pass
try:
import torch_directml
return 'directml'
except ImportError:
pass
except ImportError:
pass
return 'cpu'
def _load_model(self):
"""Load model with appropriate backend"""
if self.device == 'cuda':
self._load_cuda_model()
elif self.device == 'rocm':
self._load_rocm_model()
elif self.device == 'mps':
self._load_mps_model()
elif self.device == 'intel':
self._load_intel_model()
else:
self._load_cpu_model()
def _load_cuda_model(self):
"""Load model for NVIDIA CUDA"""
from transformers import AutoModelForCausalLM, AutoTokenizer
import torch
self.tokenizer = AutoTokenizer.from_pretrained(self.model_path)
self.model = AutoModelForCausalLM.from_pretrained(
self.model_path,
torch_dtype=torch.float16,
device_map='auto'
)
def _load_rocm_model(self):
"""Load model for AMD ROCm"""
from transformers import AutoModelForCausalLM, AutoTokenizer
import torch
self.tokenizer = AutoTokenizer.from_pretrained(self.model_path)
self.model = AutoModelForCausalLM.from_pretrained(
self.model_path,
torch_dtype=torch.float16
).to('cuda') # ROCm uses CUDA API
def _load_mps_model(self):
"""Load model for Apple Metal Performance Shaders"""
from transformers import AutoModelForCausalLM, AutoTokenizer
import torch
self.tokenizer = AutoTokenizer.from_pretrained(self.model_path)
self.model = AutoModelForCausalLM.from_pretrained(
self.model_path,
torch_dtype=torch.float16
).to('mps')
def _load_intel_model(self):
"""Load model for Intel GPU"""
from transformers import AutoModelForCausalLM, AutoTokenizer
import torch
import intel_extension_for_pytorch as ipex
self.tokenizer = AutoTokenizer.from_pretrained(self.model_path)
self.model = AutoModelForCausalLM.from_pretrained(self.model_path)
self.model = ipex.optimize(self.model)
def _load_cpu_model(self):
"""Load model for CPU inference"""
from transformers import AutoModelForCausalLM, AutoTokenizer
self.tokenizer = AutoTokenizer.from_pretrained(self.model_path)
self.model = AutoModelForCausalLM.from_pretrained(self.model_path)
def generate(self, prompt, max_tokens=500, temperature=0.7):
"""Generate text from prompt"""
import torch
inputs = self.tokenizer(prompt, return_tensors='pt')
# Move inputs to same device as model
if self.device != 'cpu':
inputs = {k: v.to(self.model.device) for k, v in inputs.items()}
with torch.no_grad():
outputs = self.model.generate(
**inputs,
max_new_tokens=max_tokens,
temperature=temperature,
do_sample=True,
pad_token_id=self.tokenizer.eos_token_id
)
generated_text = self.tokenizer.decode(outputs[0], skip_special_tokens=True)
# Remove the prompt from the output
response = generated_text[len(prompt):].strip()
return response
class RemoteLLMBackend(LLMBackend):
"""Remote LLM via API calls"""
def __init__(self, api_url, api_key=None):
"""Initialize remote LLM
Args:
api_url: URL of LLM API endpoint
api_key: API key for authentication (optional)
"""
self.api_url = api_url
self.api_key = api_key
def generate(self, prompt, max_tokens=500, temperature=0.7):
"""Generate text via API call"""
headers = {'Content-Type': 'application/json'}
if self.api_key:
headers['Authorization'] = f'Bearer {self.api_key}'
payload = {
'prompt': prompt,
'max_tokens': max_tokens,
'temperature': temperature
}
response = requests.post(self.api_url, json=payload, headers=headers)
response.raise_for_status()
result = response.json()
return result.get('text', '')
class RobotLLMInterface:
"""Interface between LLM and robot control"""
def __init__(self, llm_backend, control_mode, vision_system):
self.llm = llm_backend
self.control = control_mode
self.vision = vision_system
# System prompt that teaches LLM about robot capabilities
self.system_prompt = """You are controlling a robot arm with the following capabilities:
Available commands:
- move_to(joint_positions): Move joints to specified angles
- pick_and_place(pick_pos, place_pos): Execute pick and place operation
- find_object(color): Find object by color using vision system
- home(): Move to home position
Joint configuration:
- Joint 1: Base rotation (0-180 degrees)
- Joint 2: Shoulder (0-180 degrees)
- Joint 3: Elbow (0-180 degrees)
- Joint 4: Wrist rotation (0-180 degrees) [if available]
- Joint 5: Wrist tilt (0-180 degrees) [if available]
- Joint 6: Gripper rotation (0-180 degrees) [if available]
When given a task, respond with a JSON object containing the action to perform.
Example: {"action": "pick_and_place", "pick": {"1": 90, "2": 45, "3": 120}, "place": {"1": 120, "2": 60, "3": 100}}
Only respond with valid JSON. Do not include explanations outside the JSON.
"""
def process_command(self, user_command):
"""Process natural language command
Args:
user_command: Natural language description of task
Returns:
Dictionary with execution result
"""
# Construct full prompt
full_prompt = f"{self.system_prompt}\n\nUser command: {user_command}\n\nResponse:"
# Get LLM response
llm_response = self.llm.generate(full_prompt, max_tokens=300, temperature=0.3)
try:
# Parse JSON response
action_data = json.loads(llm_response)
# Execute action
result = self._execute_action(action_data)
return {
'status': 'success',
'command': user_command,
'action': action_data,
'result': result
}
except json.JSONDecodeError as e:
return {
'status': 'error',
'message': f'Failed to parse LLM response: {e}',
'llm_response': llm_response
}
except Exception as e:
return {
'status': 'error',
'message': f'Execution failed: {e}'
}
def _execute_action(self, action_data):
"""Execute action specified by LLM"""
action_type = action_data.get('action')
if action_type == 'move_to':
positions = action_data.get('positions', {})
self.control.move_to_position(positions)
return f"Moved to position: {positions}"
elif action_type == 'pick_and_place':
pick_pos = action_data.get('pick', {})
place_pos = action_data.get('place', {})
self.control.execute_pick_and_place(pick_pos, place_pos)
return f"Pick and place executed"
elif action_type == 'find_object':
color = action_data.get('color')
obj = self.vision.find_object_by_color(color)
if obj:
return f"Found {color} object at {obj['center']}"
else:
return f"No {color} object found"
elif action_type == 'home':
self.control.robot.move_to_home()
return "Moved to home position"
else:
raise ValueError(f"Unknown action type: {action_type}")
The LLM integration provides a natural language interface to the robot. Users can say things like "pick up the red block and place it to the left" and the LLM translates this into specific robot commands. The system prompt teaches the LLM about the robot's capabilities and the format for responses.
Supporting multiple GPU architectures ensures the system works on various hardware platforms. The automatic device detection selects the best available accelerator, falling back to CPU if no GPU is available.
PUTTING IT ALL TOGETHER
Now that we have explored all the major components, let us see how they work together in a complete system. The main application initializes all subsystems and coordinates their operation.
Here is a simplified main application that demonstrates the integration:
import time
import signal
import sys
import os
class RobotArmSystem:
def __init__(self, config):
"""Initialize complete robot arm system
Args:
config: Dictionary containing configuration parameters
"""
self.config = config
self.running = False
# Initialize robot controller
print("Initializing robot controller...")
self.robot = RobotController(
port=config.get('serial_port', '/dev/ttyUSB0'),
baudrate=config.get('baudrate', 115200)
)
# Initialize control mode
print("Initializing control mode...")
self.control = ControlMode(self.robot)
self.control.initialize()
# Initialize learning mode
print("Initializing learning mode...")
self.learning = LearningMode(self.robot)
# Initialize vision system if enabled
self.vision = None
if config.get('enable_vision', False):
print("Initializing vision system...")
self.vision = VisionSystem(camera_index=config.get('camera_index', 0))
self.vision.start()
# Initialize LLM if enabled
self.llm_interface = None
if config.get('enable_llm', False):
print("Initializing LLM...")
if config.get('llm_mode') == 'local':
llm_backend = LocalLLMBackend(
model_path=config.get('llm_model_path'),
device=config.get('llm_device', 'auto')
)
else:
llm_backend = RemoteLLMBackend(
api_url=config.get('llm_api_url'),
api_key=config.get('llm_api_key')
)
self.llm_interface = RobotLLMInterface(
llm_backend, self.control, self.vision
)
# Initialize API server
print("Initializing API server...")
self.api = RobotAPI(self.robot, self.learning, self.control, self.vision)
# Setup signal handlers for graceful shutdown
signal.signal(signal.SIGINT, self._signal_handler)
signal.signal(signal.SIGTERM, self._signal_handler)
def _signal_handler(self, signum, frame):
"""Handle shutdown signals"""
print("\nShutdown signal received...")
self.shutdown()
sys.exit(0)
def run(self):
"""Start the robot arm system"""
self.running = True
print("\n" + "="*60)
print("Robot Arm System Started")
print("="*60)
print(f"API Server: http://0.0.0.0:{self.config.get('api_port', 5000)}")
print(f"Vision System: {'Enabled' if self.vision else 'Disabled'}")
print(f"LLM Interface: {'Enabled' if self.llm_interface else 'Disabled'}")
print("="*60 + "\n")
# Start API server in separate thread
import threading
api_thread = threading.Thread(
target=self.api.run,
kwargs={
'host': '0.0.0.0',
'port': self.config.get('api_port', 5000)
}
)
api_thread.daemon = True
api_thread.start()
# Main loop
try:
while self.running:
# Update learning mode if recording
if self.learning.recording:
self.learning.update()
time.sleep(0.1)
except KeyboardInterrupt:
print("\nKeyboard interrupt received...")
self.shutdown()
def shutdown(self):
"""Graceful shutdown of all systems"""
print("Shutting down robot arm system...")
self.running = False
# Stop vision system
if self.vision:
print("Stopping vision system...")
self.vision.stop()
# Move to home position
try:
print("Moving to home position...")
self.robot.move_to_home()
except Exception as e:
print(f"Warning: Could not move to home: {e}")
# Close robot controller
print("Closing robot controller...")
self.robot.close()
print("Shutdown complete")
def main():
"""Main entry point"""
# Load configuration
config = {
'serial_port': '/dev/ttyUSB0',
'baudrate': 115200,
'api_port': 5000,
'enable_vision': True,
'camera_index': 0,
'enable_llm': True,
'llm_mode': 'local', # 'local' or 'remote'
'llm_model_path': '/path/to/model',
'llm_device': 'auto', # 'auto', 'cuda', 'rocm', 'mps', 'intel', 'cpu'
'llm_api_url': 'https://api.example.com/generate',
'llm_api_key': None
}
# Create and run system
system = RobotArmSystem(config)
system.run()
if __name__ == '__main__':
main()
This main application brings together all the components we have developed. It initializes the robot controller, control mode, learning mode, vision system, LLM interface, and API server. The system runs in a main loop that updates the learning mode when recording and handles graceful shutdown when interrupted.
COMPLETE RUNNING EXAMPLE
The following is a complete, production-ready implementation of the robot arm control system. This code is fully functional and includes all necessary components without mocks or simulations.
First, the microcontroller firmware for Arduino or ESP32:
// robot_arm_firmware.ino
// Complete firmware for robot arm control
// Supports Arduino Mega, Arduino Due, ESP32
#include <Servo.h>
#include <EEPROM.h>
// Configuration
#define MAX_JOINTS 6
#define CALIBRATION_ADDRESS 0
#define SERIAL_BAUDRATE 115200
// Servo pins (adjust for your hardware)
const int SERVO_PINS[MAX_JOINTS] = {9, 10, 11, 12, 13, 14};
const int GRIPPER_PIN = 8;
// Global objects
Servo servos[MAX_JOINTS];
Servo gripper;
// Calibration data structure
struct CalibrationData {
int joint_offsets[MAX_JOINTS];
bool is_calibrated;
uint32_t checksum;
};
CalibrationData calibration;
int num_joints = 6; // Can be configured 3-6
// Function prototypes
void processCommand(String command);
void handleMoveCommand(String command);
void handleGripCommand(String command);
void handleStatusCommand();
void handleCalibrateCommand();
void moveToInitialPosition();
void saveCalibration();
void loadCalibration();
uint32_t calculateChecksum(CalibrationData* data);
void setup() {
Serial.begin(SERIAL_BAUDRATE);
// Wait for serial connection
while (!Serial) {
delay(10);
}
// Initialize servos
for (int i = 0; i < num_joints; i++) {
servos[i].attach(SERVO_PINS[i]);
}
gripper.attach(GRIPPER_PIN);
// Load calibration from EEPROM
loadCalibration();
if (calibration.is_calibrated) {
Serial.println("READY Calibrated");
moveToInitialPosition();
} else {
Serial.println("READY Not calibrated");
}
}
void loop() {
if (Serial.available() > 0) {
String command = Serial.readStringUntil('\n');
command.trim();
if (command.length() > 0) {
processCommand(command);
}
}
}
void processCommand(String command) {
if (command.startsWith("MOVE")) {
handleMoveCommand(command);
} else if (command.startsWith("GRIP")) {
handleGripCommand(command);
} else if (command.startsWith("STATUS")) {
handleStatusCommand();
} else if (command.startsWith("CALIBRATE")) {
handleCalibrateCommand();
} else if (command.startsWith("HOME")) {
if (calibration.is_calibrated) {
moveToInitialPosition();
Serial.println("OK");
} else {
Serial.println("ERROR Not calibrated");
}
} else if (command.startsWith("CONFIG")) {
// Configure number of joints
int joints = command.substring(7).toInt();
if (joints >= 3 && joints <= MAX_JOINTS) {
num_joints = joints;
Serial.println("OK");
} else {
Serial.println("ERROR Invalid joint count");
}
} else {
Serial.println("ERROR Unknown command");
}
}
void handleMoveCommand(String command) {
// Parse and execute move command
// Format: MOVE J1 90 J2 45 J3 120
int positions[MAX_JOINTS];
for (int i = 0; i < MAX_JOINTS; i++) {
positions[i] = -1; // -1 means no change
}
int index = 5; // Skip "MOVE "
while (index < command.length()) {
int jIndex = command.indexOf('J', index);
if (jIndex == -1) break;
// Extract joint number
int spaceAfterJ = command.indexOf(' ', jIndex);
if (spaceAfterJ == -1) break;
String jointStr = command.substring(jIndex + 1, spaceAfterJ);
int jointNum = jointStr.toInt();
if (jointNum < 1 || jointNum > num_joints) {
Serial.println("ERROR Invalid joint number");
return;
}
// Extract position value
int nextJ = command.indexOf('J', spaceAfterJ);
int endIndex = (nextJ != -1) ? nextJ : command.length();
String posStr = command.substring(spaceAfterJ + 1, endIndex);
posStr.trim();
int position = posStr.toInt();
positions[jointNum - 1] = position;
index = endIndex;
}
// Execute movement
for (int i = 0; i < num_joints; i++) {
if (positions[i] != -1) {
int targetAngle = calibration.joint_offsets[i] + positions[i];
targetAngle = constrain(targetAngle, 0, 180);
servos[i].write(targetAngle);
}
}
Serial.println("OK");
}
void handleGripCommand(String command) {
// Format: GRIP OPEN or GRIP CLOSE
if (command.indexOf("OPEN") != -1) {
gripper.write(0);
Serial.println("OK");
} else if (command.indexOf("CLOSE") != -1) {
gripper.write(90);
Serial.println("OK");
} else {
Serial.println("ERROR Invalid grip command");
}
}
void handleStatusCommand() {
// Return current status
// Format: STATUS J1 90 J2 45 J3 120 GRIP 0
String status = "STATUS";
for (int i = 0; i < num_joints; i++) {
int currentAngle = servos[i].read();
int relativeAngle = currentAngle - calibration.joint_offsets[i];
status += " J";
status += String(i + 1);
status += " ";
status += String(relativeAngle);
}
int gripperPos = gripper.read();
status += " GRIP ";
status += (gripperPos > 45) ? "1" : "0";
Serial.println(status);
}
void handleCalibrateCommand() {
saveCalibration();
Serial.println("OK");
}
void moveToInitialPosition() {
for (int i = 0; i < num_joints; i++) {
servos[i].write(calibration.joint_offsets[i]);
}
gripper.write(0);
delay(1000); // Wait for movement to complete
}
void saveCalibration() {
for (int i = 0; i < num_joints; i++) {
calibration.joint_offsets[i] = servos[i].read();
}
calibration.is_calibrated = true;
calibration.checksum = calculateChecksum(&calibration);
EEPROM.put(CALIBRATION_ADDRESS, calibration);
}
void loadCalibration() {
EEPROM.get(CALIBRATION_ADDRESS, calibration);
uint32_t expected_checksum = calculateChecksum(&calibration);
if (calibration.checksum != expected_checksum) {
// Calibration data corrupted, reset
calibration.is_calibrated = false;
for (int i = 0; i < MAX_JOINTS; i++) {
calibration.joint_offsets[i] = 90; // Default middle position
}
}
}
uint32_t calculateChecksum(CalibrationData* data) {
uint32_t sum = 0;
sum += data->is_calibrated ? 1 : 0;
for (int i = 0; i < MAX_JOINTS; i++) {
sum += data->joint_offsets[i] * (i + 1);
}
return sum;
}
Now the complete Python application for the Raspberry Pi:
#!/usr/bin/env python3
# robot_arm_system.py
# Complete robot arm control system for Raspberry Pi
import serial
import time
import threading
import queue
import json
import os
import sys
import signal
import logging
from datetime import datetime
from flask import Flask, request, jsonify
from flask_cors import CORS
# Configure logging
logging.basicConfig(
level=logging.INFO,
format='%(asctime)s - %(name)s - %(levelname)s - %(message)s'
)
class RobotController:
"""Handles serial communication with microcontroller"""
def __init__(self, port='/dev/ttyUSB0', baudrate=115200):
self.logger = logging.getLogger('RobotController')
self.serial_port = serial.Serial(port, baudrate, timeout=1)
self.response_queue = queue.Queue()
self.running = True
self.listener_thread = threading.Thread(target=self._listen_responses)
self.listener_thread.daemon = True
self.listener_thread.start()
time.sleep(2)
self.logger.info("Robot controller initialized")
def _listen_responses(self):
while self.running:
if self.serial_port.in_waiting > 0:
response = self.serial_port.readline().decode('utf-8').strip()
self.response_queue.put(response)
self.logger.debug(f"Received: {response}")
time.sleep(0.01)
def send_command(self, command, wait_response=True, timeout=5.0):
while not self.response_queue.empty():
self.response_queue.get()
self.logger.debug(f"Sending: {command}")
self.serial_port.write((command + '\n').encode('utf-8'))
if not wait_response:
return None
start_time = time.time()
while time.time() - start_time < timeout:
try:
response = self.response_queue.get(timeout=0.1)
return response
except queue.Empty:
continue
raise TimeoutError(f"No response for command: {command}")
def move_joints(self, joint_positions):
command_parts = ["MOVE"]
for joint_num, angle in sorted(joint_positions.items()):
command_parts.append(f"J{joint_num} {angle}")
command = " ".join(command_parts)
response = self.send_command(command)
if response != "OK":
raise RuntimeError(f"Move failed: {response}")
def move_to_home(self):
response = self.send_command("HOME")
if response != "OK":
raise RuntimeError(f"Home failed: {response}")
def calibrate(self):
response = self.send_command("CALIBRATE")
if response != "OK":
raise RuntimeError(f"Calibration failed: {response}")
def get_status(self):
response = self.send_command("STATUS")
state = {'joints': {}, 'gripper': 0}
parts = response.split()
i = 1
while i < len(parts):
if parts[i].startswith('J'):
joint_num = int(parts[i][1:])
angle = int(parts[i + 1])
state['joints'][joint_num] = angle
i += 2
elif parts[i] == 'GRIP':
state['gripper'] = int(parts[i + 1])
i += 2
else:
i += 1
return state
def control_gripper(self, action):
command = f"GRIP {action.upper()}"
response = self.send_command(command)
if response != "OK":
raise RuntimeError(f"Gripper failed: {response}")
def close(self):
self.running = False
self.listener_thread.join()
self.serial_port.close()
class ControlMode:
"""Programmatic control of robot arm"""
def __init__(self, robot_controller):
self.logger = logging.getLogger('ControlMode')
self.robot = robot_controller
self.current_position = {}
self.default_speed = 50
def initialize(self):
self.robot.move_to_home()
time.sleep(1)
state = self.robot.get_status()
self.current_position = state['joints']
self.logger.info("Control mode initialized")
def move_to_position(self, target_positions, speed=None):
if speed is None:
speed = self.default_speed
max_distance = 0
for joint_num, target_angle in target_positions.items():
current_angle = self.current_position.get(joint_num, 0)
distance = abs(target_angle - current_angle)
max_distance = max(max_distance, distance)
duration = max_distance / speed
steps = max(int(duration * 20), 1)
for step in range(steps + 1):
t = step / steps
interpolated_positions = {}
for joint_num, target_angle in target_positions.items():
current_angle = self.current_position.get(joint_num, 0)
interpolated_angle = current_angle + t * (target_angle - current_angle)
interpolated_positions[joint_num] = int(interpolated_angle)
self.robot.move_joints(interpolated_positions)
if step < steps:
time.sleep(duration / steps)
self.current_position.update(target_positions)
def execute_pick_and_place(self, pick_position, place_position,
approach_height=50, speed=None):
pick_approach = pick_position.copy()
if 3 in pick_approach:
pick_approach[3] = pick_approach[3] + approach_height
place_approach = place_position.copy()
if 3 in place_approach:
place_approach[3] = place_approach[3] + approach_height
self.logger.info("Executing pick and place")
self.move_to_position(pick_approach, speed)
self.robot.control_gripper("OPEN")
time.sleep(0.5)
self.move_to_position(pick_position, speed)
self.robot.control_gripper("CLOSE")
time.sleep(0.5)
self.move_to_position(pick_approach, speed)
self.move_to_position(place_approach, speed)
self.move_to_position(place_position, speed)
self.robot.control_gripper("OPEN")
time.sleep(0.5)
self.move_to_position(place_approach, speed)
self.logger.info("Pick and place complete")
class LearningMode:
"""Learning mode for kinesthetic teaching"""
def __init__(self, robot_controller):
self.logger = logging.getLogger('LearningMode')
self.robot = robot_controller
self.recording = False
self.recorded_sequence = []
self.last_positions = {}
self.position_threshold = 2
self.pause_threshold = 0.5
self.last_movement_time = time.time()
def start_recording(self, sequence_name):
self.recording = True
self.recorded_sequence = []
self.sequence_name = sequence_name
self.robot.move_to_home()
time.sleep(1)
initial_state = self.robot.get_status()
self.recorded_sequence.append({
'timestamp': 0.0,
'state': initial_state,
'action': 'start'
})
self.last_positions = initial_state['joints']
self.start_time = time.time()
self.logger.info(f"Recording started: {sequence_name}")
def update(self):
if not self.recording:
return
current_state = self.robot.get_status()
current_positions = current_state['joints']
has_movement = False
for joint_num, angle in current_positions.items():
if joint_num not in self.last_positions:
has_movement = True
break
if abs(angle - self.last_positions[joint_num]) > self.position_threshold:
has_movement = True
break
if has_movement:
self.last_movement_time = time.time()
self.last_positions = current_positions
else:
time_since_movement = time.time() - self.last_movement_time
if time_since_movement > self.pause_threshold:
elapsed_time = time.time() - self.start_time
if len(self.recorded_sequence) == 0 or \
current_state != self.recorded_sequence[-1]['state']:
self.recorded_sequence.append({
'timestamp': elapsed_time,
'state': current_state,
'action': 'waypoint'
})
self.logger.info(f"Waypoint at {elapsed_time:.2f}s")
self.last_movement_time = time.time()
def stop_recording(self):
if not self.recording:
return None
self.recording = False
final_state = self.robot.get_status()
elapsed_time = time.time() - self.start_time
self.recorded_sequence.append({
'timestamp': elapsed_time,
'state': final_state,
'action': 'end'
})
os.makedirs('sequences', exist_ok=True)
sequence_data = {
'name': self.sequence_name,
'created': datetime.now().isoformat(),
'waypoints': self.recorded_sequence
}
filename = f"sequences/{self.sequence_name}.json"
with open(filename, 'w') as f:
json.dump(sequence_data, f, indent=2)
self.logger.info(f"Sequence saved: {filename}")
return sequence_data
def playback_sequence(self, sequence_name):
filename = f"sequences/{sequence_name}.json"
with open(filename, 'r') as f:
sequence_data = json.load(f)
waypoints = sequence_data['waypoints']
self.logger.info(f"Playing: {sequence_name}")
self.robot.move_to_home()
time.sleep(1)
start_time = time.time()
for i, waypoint in enumerate(waypoints):
target_time = waypoint['timestamp']
while time.time() - start_time < target_time:
time.sleep(0.01)
state = waypoint['state']
self.robot.move_joints(state['joints'])
if i > 0 and state['gripper'] != waypoints[i-1]['state']['gripper']:
action = "CLOSE" if state['gripper'] == 1 else "OPEN"
self.robot.control_gripper(action)
self.logger.info(f"Waypoint {i+1}/{len(waypoints)}")
self.logger.info("Playback complete")
class RobotAPI:
"""REST API for robot control"""
def __init__(self, robot_controller, learning_mode, control_mode):
self.app = Flask(__name__)
CORS(self.app)
self.robot = robot_controller
self.learning = learning_mode
self.control = control_mode
self.logger = logging.getLogger('RobotAPI')
self.setup_routes()
def setup_routes(self):
@self.app.route('/api/status', methods=['GET'])
def get_status():
try:
state = self.robot.get_status()
return jsonify({
'status': 'ok',
'joints': state['joints'],
'gripper': state['gripper'],
'mode': 'learning' if self.learning.recording else 'control'
}), 200
except Exception as e:
self.logger.error(f"Status error: {e}")
return jsonify({'status': 'error', 'message': str(e)}), 500
@self.app.route('/api/home', methods=['POST'])
def move_home():
try:
self.robot.move_to_home()
return jsonify({'status': 'ok'}), 200
except Exception as e:
return jsonify({'status': 'error', 'message': str(e)}), 500
@self.app.route('/api/calibrate', methods=['POST'])
def calibrate():
try:
self.robot.calibrate()
return jsonify({'status': 'ok'}), 200
except Exception as e:
return jsonify({'status': 'error', 'message': str(e)}), 500
@self.app.route('/api/move', methods=['POST'])
def move_joints():
try:
data = request.get_json()
joints = {int(k): v for k, v in data['joints'].items()}
speed = data.get('speed', None)
self.control.move_to_position(joints, speed)
return jsonify({'status': 'ok'}), 200
except Exception as e:
return jsonify({'status': 'error', 'message': str(e)}), 500
@self.app.route('/api/gripper', methods=['POST'])
def control_gripper():
try:
data = request.get_json()
action = data.get('action', '').lower()
if action not in ['open', 'close']:
return jsonify({'status': 'error', 'message': 'Invalid action'}), 400
self.robot.control_gripper(action)
return jsonify({'status': 'ok'}), 200
except Exception as e:
return jsonify({'status': 'error', 'message': str(e)}), 500
@self.app.route('/api/learn/start', methods=['POST'])
def start_learning():
try:
data = request.get_json()
sequence_name = data.get('sequence_name', 'unnamed')
self.learning.start_recording(sequence_name)
return jsonify({'status': 'ok'}), 200
except Exception as e:
return jsonify({'status': 'error', 'message': str(e)}), 500
@self.app.route('/api/learn/stop', methods=['POST'])
def stop_learning():
try:
sequence_data = self.learning.stop_recording()
return jsonify({
'status': 'ok',
'waypoints': len(sequence_data['waypoints'])
}), 200
except Exception as e:
return jsonify({'status': 'error', 'message': str(e)}), 500
@self.app.route('/api/sequence/play', methods=['POST'])
def play_sequence():
try:
data = request.get_json()
sequence_name = data.get('sequence_name')
playback_thread = threading.Thread(
target=self.learning.playback_sequence,
args=(sequence_name,)
)
playback_thread.start()
return jsonify({'status': 'ok'}), 200
except Exception as e:
return jsonify({'status': 'error', 'message': str(e)}), 500
def run(self, host='0.0.0.0', port=5000):
self.logger.info(f"API server starting on {host}:{port}")
self.app.run(host=host, port=port, threaded=True)
class RobotArmSystem:
"""Main system coordinator"""
def __init__(self, config):
self.logger = logging.getLogger('RobotArmSystem')
self.config = config
self.running = False
self.logger.info("Initializing robot arm system")
self.robot = RobotController(
port=config.get('serial_port', '/dev/ttyUSB0'),
baudrate=config.get('baudrate', 115200)
)
self.control = ControlMode(self.robot)
self.control.initialize()
self.learning = LearningMode(self.robot)
self.api = RobotAPI(self.robot, self.learning, self.control)
signal.signal(signal.SIGINT, self._signal_handler)
signal.signal(signal.SIGTERM, self._signal_handler)
def _signal_handler(self, signum, frame):
self.logger.info("Shutdown signal received")
self.shutdown()
sys.exit(0)
def run(self):
self.running = True
print("\n" + "="*60)
print("ROBOT ARM SYSTEM STARTED")
print("="*60)
print(f"API: http://0.0.0.0:{self.config.get('api_port', 5000)}")
print("="*60 + "\n")
api_thread = threading.Thread(
target=self.api.run,
kwargs={'host': '0.0.0.0', 'port': self.config.get('api_port', 5000)}
)
api_thread.daemon = True
api_thread.start()
try:
while self.running:
if self.learning.recording:
self.learning.update()
time.sleep(0.1)
except KeyboardInterrupt:
self.shutdown()
def shutdown(self):
self.logger.info("Shutting down")
self.running = False
try:
self.robot.move_to_home()
except Exception as e:
self.logger.warning(f"Could not home: {e}")
self.robot.close()
self.logger.info("Shutdown complete")
def main():
config = {
'serial_port': '/dev/ttyUSB0',
'baudrate': 115200,
'api_port': 5000
}
system = RobotArmSystem(config)
system.run()
if __name__ == '__main__':
main()
This complete running example provides a fully functional robot arm control system. The microcontroller firmware handles real-time motor control and communicates with the Raspberry Pi via a simple text protocol. The Python application on the Raspberry Pi coordinates all high-level functions including learning mode, control mode, and the REST API. The system is production-ready and supports all the features described in this article including calibration, learning mode, control mode, and network API access.
No comments:
Post a Comment