Key Code Examples
1. PID Controller (Python)
import time
from typing import Optional
class PIDController:
"""Proportional-Integral-Derivative controller for robotics."""
def __init__(self, Kp: float, Ki: float, Kd: float,
output_limits: tuple = (None, None)):
self.Kp = Kp # Proportional gain
self.Ki = Ki # Integral gain
self.Kd = Kd # Derivative gain
self.output_min, self.output_max = output_limits
self._integral = 0.0
self._last_error = 0.0
self._last_time = None
def compute(self, setpoint: float, measured: float,
dt: Optional[float] = None) -> float:
"""Compute PID output."""
now = time.time()
error = setpoint - measured
# Time delta
if dt is None:
if self._last_time is None:
dt = 0.0
else:
dt = now - self._last_time
self._last_time = now
# Prevent division by zero
if dt <= 0:
dt = 0.001
# Proportional term
P = self.Kp * error
# Integral term (with anti-windup)
self._integral += error * dt
I = self.Ki * self._integral
# Derivative term (on measurement, not error — avoids setpoint kick)
d_measured = (measured - self._last_error) / dt
D = -self.Kd * d_measured
self._last_error = measured
# Total output
output = P + I + D
# Apply output limits (anti-windup)
if self.output_max is not None and output > self.output_max:
output = self.output_max
self._integral -= error * dt # Back-calculate anti-windup
elif self.output_min is not None and output < self.output_min:
output = self.output_min
self._integral -= error * dt
return output
def reset(self):
"""Reset integral and derivative terms."""
self._integral = 0.0
self._last_error = 0.0
self._last_time = None
# Example: Temperature control
pid = PIDController(Kp=2.0, Ki=0.5, Kd=0.1,
output_limits=(-100, 100))
setpoint = 75.0 # Target temperature
current_temp = 25.0
for step in range(50):
control = pid.compute(setpoint, current_temp)
# Simulate temperature change
current_temp += control * 0.1
print(f"Step {step}: Temp={current_temp:.1f}, Control={control:.1f}")
2. Robot Arm — Forward Kinematics
import numpy as np
from typing import List, Tuple
class RobotArm2D:
"""2D robot arm with forward and inverse kinematics."""
def __init__(self, link_lengths: List[float]):
self.link_lengths = link_lengths
self.num_joints = len(link_lengths)
def forward_kinematics(self, joint_angles: List[float]) -> List[Tuple[float, float]]:
"""Calculate end-effector position from joint angles.
DH Convention:
x = Σ L_i * cos(Σ θ_j) for j=0..i
y = Σ L_i * sin(Σ θ_j) for j=0..i
"""
positions = [(0.0, 0.0)] # Base position
x, y = 0.0, 0.0
cumulative_angle = 0.0
for i in range(self.num_joints):
cumulative_angle += joint_angles[i]
x += self.link_lengths[i] * np.cos(cumulative_angle)
y += self.link_lengths[i] * np.sin(cumulative_angle)
positions.append((x, y))
return positions
def inverse_kinematics_2link(self, target_x: float, target_y: float,
elbow_up: bool = True) -> Tuple[float, float]:
"""Analytical IK for 2-link arm."""
L1, L2 = self.link_lengths[0], self.link_lengths[1]
# Distance to target
D = np.sqrt(target_x**2 + target_y**2)
# Check reachability
if D > L1 + L2 or D < abs(L1 - L2):
raise ValueError(f"Target ({target_x}, {target_y}) unreachable")
# Cosine law for elbow angle
cos_elbow = (D**2 - L1**2 - L2**2) / (2 * L1 * L2)
cos_elbow = np.clip(cos_elbow, -1, 1)
elbow_angle = np.arccos(cos_elbow)
if not elbow_up:
elbow_angle = -elbow_angle
# Shoulder angle
base_angle = np.arctan2(target_y, target_x)
offset_angle = np.arctan2(L2 * np.sin(elbow_angle),
L1 + L2 * np.cos(elbow_angle))
shoulder_angle = base_angle - offset_angle
return shoulder_angle, elbow_angle
def jacobian(self, joint_angles: List[float]) -> np.ndarray:
"""Compute Jacobian matrix for velocity control."""
n = self.num_joints
J = np.zeros((2, n))
cumulative_angle = 0.0
for i in range(n):
cumulative_angle += joint_angles[i]
for j in range(i + 1):
J[0, j] -= self.link_lengths[i] * np.sin(cumulative_angle)
J[1, j] += self.link_lengths[i] * np.cos(cumulative_angle)
return J
# Example: 2-link robot arm
arm = RobotArm2D(link_lengths=[100, 80]) # mm
# Forward kinematics
angles = [np.radians(45), np.radians(30)]
positions = arm.forward_kinematics(angles)
print(f"End effector: ({positions[-1][0]:.1f}, {positions[-1][1]:.1f})")
# Inverse kinematics
target = (120, 80)
shoulder, elbow = arm.inverse_kinematics_2link(*target)
print(f"Joint angles: θ1={np.degrees(shoulder):.1f}°, θ2={np.degrees(elbow):.1f}°")
# Verify with FK
verification = arm.forward_kinematics([shoulder, elbow])
print(f"Verified: ({verification[-1][0]:.1f}, {verification[-1][1]:.1f})")
3. ROS Node — Publisher/Subscriber (Python)
#!/usr/bin/env python3
import rospy
from std_msgs.msg import String, Float32
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
class RobotController:
"""ROS node for autonomous robot navigation."""
def __init__(self):
rospy.init_node('robot_controller', anonymous=True)
# Publishers
self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
self.status_pub = rospy.Publisher('/robot_status', String, queue_size=10)
# Subscribers
self.lidar_sub = rospy.Subscriber('/scan', LaserScan,
self.lidar_callback)
self.target_sub = rospy.Subscriber('/target_position', String,
self.target_callback)
# State
self.obstacle_distance = float('inf')
self.target_reached = False
self.rate = rospy.Rate(10) # 10 Hz
rospy.loginfo("Robot controller initialized")
def lidar_callback(self, msg: LaserScan):
"""Process LiDAR data to find nearest obstacle."""
# Filter out invalid readings (inf and nan)
valid_ranges = [r for r in msg.ranges
if msg.range_min < r < msg.range_max]
if valid_ranges:
self.obstacle_distance = min(valid_ranges)
rospy.logdebug(f"Nearest obstacle: {self.obstacle_distance:.2f}m")
def target_callback(self, msg: String):
"""Receive navigation target."""
rospy.loginfo(f"New target: {msg.data}")
self.target_reached = False
def navigate(self):
"""Simple obstacle avoidance navigation."""
vel_msg = Twist()
if self.obstacle_distance < 0.5:
# Obstacle ahead — turn
vel_msg.linear.x = 0.0
vel_msg.angular.z = 0.5 # Turn left
rospy.logwarn("Obstacle detected! Turning...")
elif self.obstacle_distance < 1.0:
# Slow down
vel_msg.linear.x = 0.2
vel_msg.angular.z = 0.1
else:
# Clear path — move forward
vel_msg.linear.x = 0.5
vel_msg.angular.z = 0.0
self.cmd_vel_pub.publish(vel_msg)
def run(self):
"""Main control loop."""
while not rospy.is_shutdown():
self.navigate()
self.rate.sleep()
if __name__ == '__main__':
try:
controller = RobotController()
controller.run()
except rospy.ROSInterruptException:
pass
4. SLAM Algorithm — Grid Mapping
import numpy as np
from collections import deque
class GridSLAM:
"""Simultaneous Localization and Mapping using grid-based approach."""
def __init__(self, grid_size=(100, 100), resolution=0.1):
self.width, self.height = grid_size
self.resolution = resolution # meters per cell
# Occupancy grid: 0=unknown, 1=free, -1=occupied
self.grid = np.zeros(grid_size, dtype=np.float32)
# Log-odds representation for probabilistic updates
self.log_odds = np.zeros(grid_size, dtype=np.float32)
self.log_odds_occ = 0.85 # Log-odds for occupied
self.log_odds_free = -0.4 # Log-odds for free
def world_to_grid(self, x: float, y: float) -> tuple:
"""Convert world coordinates to grid cell."""
gx = int((x / self.resolution) + self.width // 2)
gy = int((y / self.resolution) + self.height // 2)
return np.clip(gx, 0, self.width-1), np.clip(gy, 0, self.height-1)
def grid_to_world(self, gx: int, gy: int) -> tuple:
"""Convert grid cell to world coordinates."""
x = (gx - self.width // 2) * self.resolution
y = (gy - self.height // 2) * self.resolution
return x, y
def update_scan(self, robot_x: float, robot_y: float,
robot_yaw: float, ranges: list,
angles: list, max_range: float = 10.0):
"""Update occupancy grid with LiDAR scan."""
robot_gx, robot_gy = self.world_to_grid(robot_x, robot_y)
for distance, angle in zip(ranges, angles):
if distance >= max_range:
continue
# Calculate hit position
hit_x = robot_x + distance * np.cos(robot_yaw + angle)
hit_y = robot_y + distance * np.sin(robot_yaw + angle)
hit_gx, hit_gy = self.world_to_grid(hit_x, hit_y)
# Ray tracing (Bresenham's line algorithm)
cells = self._bresenham(robot_gx, robot_gy, hit_gx, hit_gy)
# Update free space (all cells along ray except last)
for cx, cy in cells[:-1]:
self.log_odds[cx, cy] += self.log_odds_free
# Update occupied (last cell)
if cells:
self.log_odds[cells[-1]] += self.log_odds_occ
# Convert log-odds to probabilities
self.grid = 1.0 / (1.0 + np.exp(-self.log_odds))
def _bresenham(self, x0, y0, x1, y1):
"""Bresenham's line algorithm for ray tracing."""
points = []
dx = abs(x1 - x0)
dy = abs(y1 - y0)
sx = 1 if x0 < x1 else -1
sy = 1 if y0 < y1 else -1
err = dx - dy
while True:
points.append((x0, y0))
if x0 == x1 and y0 == y1:
break
e2 = 2 * err
if e2 > -dy:
err -= dy
x0 += sx
if e2 < dx:
err += dx
y0 += sy
return points
def get_map(self) -> np.ndarray:
"""Return current occupancy map."""
return self.grid.copy()
# Usage
slam = GridSLAM(grid_size=(200, 200), resolution=0.05)
slam.update_scan(robot_x=0, robot_y=0, robot_yaw=0,
ranges=[1.5, 2.0, 1.8, 5.0],
angles=[0, np.pi/4, np.pi/2, np.pi])
occupancy_map = slam.get_map()
5. Motor Control — PID Speed Controller
import RPi.GPIO as GPIO
import time
from collections import deque
class MotorController:
"""DC motor controller with encoder feedback and PID speed control."""
def __init__(self, pwm_pin, dir_pin1, dir_pin2, enc_pin_a, enc_pin_b):
# GPIO setup
GPIO.setmode(GPIO.BCM)
GPIO.setup(pwm_pin, GPIO.OUT)
GPIO.setup(dir_pin1, GPIO.OUT)
GPIO.setup(dir_pin2, GPIO.OUT)
GPIO.setup(enc_pin_a, GPIO.IN, pull_up_down=GPIO.PUD_UP)
GPIO.setup(enc_pin_b, GPIO.IN, pull_up_down=GPIO.PUD_UP)
self.pwm = GPIO.PWM(pwm_pin, 1000) # 1 kHz PWM
self.pwm.start(0)
self.dir_pin1 = dir_pin1
self.dir_pin2 = dir_pin2
# Encoder state
self.encoder_count = 0
GPIO.add_event_detect(enc_pin_a, GPIO.RISING,
callback=self._encoder_callback)
# PID variables
self.Kp, self.Ki, self.Kd = 1.0, 0.1, 0.01
self.integral = 0
self.last_error = 0
# Speed measurement
self.last_time = time.time()
self.last_count = 0
self.speed_buffer = deque(maxlen=10)
def _encoder_callback(self, channel):
"""Count encoder pulses."""
b = GPIO.input(self.dir_pin2)
if b > 0:
self.encoder_count += 1
else:
self.encoder_count -= 1
def get_speed(self) -> float:
"""Calculate current speed in RPM."""
now = time.time()
dt = now - self.last_time
if dt < 0.01:
return 0.0
delta_counts = self.encoder_count - self.last_count
# 200 counts per revolution (example encoder)
rpm = (delta_counts / 200.0) / dt * 60.0
self.speed_buffer.append(rpm)
self.last_time = now
self.last_count = self.encoder_count
return np.mean(self.speed_buffer)
def set_speed(self, target_rpm: float):
"""PID speed controller."""
current_rpm = self.get_speed()
error = target_rpm - current_rpm
# PID calculation
self.integral += error * 0.01
derivative = (error - self.last_error) / 0.01
output = (self.Kp * error + self.Ki * self.integral +
self.Kd * derivative)
self.last_error = error
# Set direction and PWM
if output >= 0:
GPIO.output(self.dir_pin1, GPIO.HIGH)
GPIO.output(self.dir_pin2, GPIO.LOW)
else:
GPIO.output(self.dir_pin1, GPIO.LOW)
GPIO.output(self.dir_pin2, GPIO.HIGH)
duty_cycle = min(abs(output), 100)
self.pwm.ChangeDutyCycle(duty_cycle)
def stop(self):
"""Stop motor and cleanup."""
self.pwm.ChangeDutyCycle(0)
GPIO.output(self.dir_pin1, GPIO.LOW)
GPIO.output(self.dir_pin2, GPIO.LOW)
def cleanup(self):
self.pwm.stop()
GPIO.cleanup()
6. Path Planning — A* on Grid
import heapq
import numpy as np
class AStarPlanner:
"""A* path planning for mobile robots."""
def __init__(self, occupancy_grid: np.ndarray):
self.grid = occupancy_grid
self.height, self.width = occupancy_grid.shape
def plan(self, start: tuple, goal: tuple) -> list:
"""Find path from start to goal using A*."""
def heuristic(a, b):
# Euclidean distance
return np.sqrt((a[0]-b[0])**2 + (a[1]-b[1])**2)
def get_neighbors(pos):
"""Get valid neighboring cells (8-connected)."""
neighbors = []
for dx, dy in [(-1,0), (1,0), (0,-1), (0,1),
(-1,-1), (-1,1), (1,-1), (1,1)]:
nx, ny = pos[0]+dx, pos[1]+dy
if 0 <= nx < self.height and 0 <= ny < self.width:
if self.grid[nx, ny] < 0.5: # Not occupied
neighbors.append((nx, ny))
return neighbors
# A* algorithm
open_set = [(heuristic(start, goal), 0, start)]
came_from = {}
g_score = {start: 0}
visited = set()
while open_set:
f, g, current = heapq.heappop(open_set)
if current == goal:
# Reconstruct path
path = [current]
while current in came_from:
current = came_from[current]
path.append(current)
return path[::-1]
if current in visited:
continue
visited.add(current)
for neighbor in get_neighbors(current):
cost = np.sqrt(2) if neighbor[0] != current[0] and \
neighbor[1] != current[1] else 1.0
tentative_g = g + cost
if tentative_g < g_score.get(neighbor, float('inf')):
came_from[neighbor] = current
g_score[neighbor] = tentative_g
f_score = tentative_g + heuristic(neighbor, goal)
heapq.heappush(open_set, (f_score, tentative_g, neighbor))
return None # No path found
def smooth_path(self, path: list, weight_data=0.5,
weight_smooth=0.3, tolerance=0.00001):
"""Gradient descent path smoothing."""
smoothed = [list(p) for p in path]
change = tolerance
while change >= tolerance:
change = 0.0
for i in range(1, len(path) - 1):
for j in range(2):
prev = smoothed[i][j]
# Attraction to original path
smoothed[i][j] += weight_data * (path[i][j] - smoothed[i][j])
# Smoothing
smoothed[i][j] += weight_smooth * (
smoothed[i-1][j] + smoothed[i+1][j]
- 2 * smoothed[i][j]
)
change += abs(prev - smoothed[i][j])
return [tuple(p) for p in smoothed]
7. Computer Vision — Object Detection (OpenCV)
import cv2
import numpy as np
class ObjectDetector:
"""Real-time object detection for robot vision."""
def __init__(self, model_path=None):
self.cap = cv2.VideoCapture(0)
self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
# HSV color ranges for object detection
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 detect_color_object(self, frame: np.ndarray,
color: str) -> dict:
"""Detect object by color using HSV thresholding."""
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
if color == 'red':
# Red wraps around in HSV, need two ranges
lower1 = np.array(self.color_ranges['red'][0])
upper1 = np.array(self.color_ranges['red'][1])
lower2 = np.array([160, 100, 100])
upper2 = np.array([180, 255, 255])
mask = cv2.inRange(hsv, lower1, upper1) | \
cv2.inRange(hsv, lower2, upper2)
else:
lower = np.array(self.color_ranges[color][0])
upper = np.array(self.color_ranges[color][1])
mask = cv2.inRange(hsv, lower, upper)
# Morphological operations
kernel = np.ones((5, 5), np.uint8)
mask = cv2.erode(mask, kernel, iterations=1)
mask = cv2.dilate(mask, kernel, iterations=2)
# Find contours
contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)
if contours:
largest = max(contours, key=cv2.contourArea)
if cv2.contourArea(largest) > 500: # Minimum area threshold
M = cv2.moments(largest)
if M["m00"] != 0:
cx = int(M["m10"] / M["m00"])
cy = int(M["m01"] / M["m00"])
return {
'found': True,
'center': (cx, cy),
'area': cv2.contourArea(largest),
'contour': largest,
}
return {'found': False}
def detect_aruco_marker(self, frame: np.ndarray) -> dict:
"""Detect ArUco markers for robot localization."""
aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_6X6_250)
aruco_params = cv2.aruco.DetectorParameters()
detector = cv2.aruco.ArucoDetector(aruco_dict, aruco_params)
corners, ids, rejected = detector.detectMarkers(frame)
if ids is not None and len(ids) > 0:
# Calculate marker center and orientation
corner = corners[0][0]
center = corner.mean(axis=0).astype(int)
# Vector from corner 0 to corner 1 gives orientation
direction = corner[1] - corner[0]
angle = np.arctan2(direction[1], direction[0])
return {
'found': True,
'marker_id': int(ids[0][0]),
'center': tuple(center),
'angle': np.degrees(angle),
}
return {'found': False}
def run(self):
"""Main detection loop."""
while True:
ret, frame = self.cap.read()
if not ret:
break
result = self.detect_color_object(frame, 'green')
if result['found']:
cv2.circle(frame, result['center'], 5, (0, 255, 0), -1)
cv2.drawContours(frame, [result['contour']], -1, (0, 255, 0), 2)
print(f"Object at: {result['center']}, Area: {result['area']:.0f}")
cv2.imshow('Robot Vision', frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
self.cap.release()
cv2.destroyAllWindows()
8. Kalman Filter for Sensor Fusion
import numpy as np
class KalmanFilter:
"""1D Kalman Filter for sensor fusion."""
def __init__(self, process_variance=1e-4, measurement_variance=0.1,
initial_estimate=0.0, initial_error=1.0):
self.Q = process_variance # Process noise covariance
self.R = measurement_variance # Measurement noise covariance
self.x = initial_estimate # State estimate
self.P = initial_error # Error covariance
def update(self, measurement: float) -> float:
"""Update filter with new measurement."""
# Prediction step
self.P = self.Q + self.P # Predict error covariance
# Update step
K = self.P / (self.P + self.R) # Kalman gain
self.x = self.x + K * (measurement - self.x) # Update estimate
self.P = (1 - K) * self.P # Update error covariance
return self.x
@property
def uncertainty(self) -> float:
return np.sqrt(self.P)
class SensorFusion:
"""Fuse multiple sensors using Kalman filters."""
def __init__(self):
self.position_filter = KalmanFilter(process_variance=1e-5,
measurement_variance=0.01)
self.velocity_filter = KalmanFilter(process_variance=1e-3,
measurement_variance=0.1)
def fuse_position(self, gps_position: float,
imu_position: float) -> float:
"""Fuse GPS and IMU position estimates."""
# GPS: accurate but noisy (high measurement variance)
gps_filtered = self.position_filter.update(gps_position)
# IMU: precise but drifts (lower measurement variance)
imu_filtered = self.position_filter.update(imu_filtered)
return imu_filtered
# Example: Position estimation with noisy sensors
kf = KalmanFilter(process_variance=1e-5, measurement_variance=0.04)
true_position = 0.0
measurements = []
estimates = []
np.random.seed(42)
for i in range(100):
# True position moves linearly
true_position = i * 0.1
# Noisy measurement (simulating sensor)
measurement = true_position + np.random.normal(0, 0.2)
# Kalman filter estimate
estimate = kf.update(measurement)
measurements.append(measurement)
estimates.append(estimate)
# Result: Filtered estimate is smoother and more accurate than raw measurements
print(f"Final estimate: {estimates[-1]:.3f}")
print(f"True position: {true_position:.3f}")
print(f"Uncertainty: ±{kf.uncertainty:.3f}")
9. Robot Operating System (ROS2) Service
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseStamped, Twist
from nav_msgs.msg import Odometry
from std_srvs.srv import Trigger
class NavigationServer(Node):
"""ROS2 navigation server with action-like behavior."""
def __init__(self):
super().__init__('navigation_server')
# Publishers
self.cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel', 10)
self.goal_pub = self.create_publisher(PoseStamped,
'/goal_pose', 10)
# Subscribers
self.odom_sub = self.create_subscription(
Odometry, '/odom', self.odom_callback, 10)
# Services
self.pause_srv = self.create_service(
Trigger, 'pause_navigation', self.pause_callback)
self.resume_srv = self.create_service(
Trigger, 'resume_navigation', self.resume_callback)
# State
self.current_pose = None
self.target_pose = None
self.is_paused = False
# PID gains for navigation
self.Kp_linear = 0.5
self.Kp_angular = 1.0
self.get_logger().info('Navigation server ready')
def odom_callback(self, msg: Odometry):
"""Update current position from odometry."""
self.current_pose = msg.pose.pose
if self.target_pose and not self.is_paused:
self._navigate_to_goal()
def set_goal(self, x: float, y: float, theta: float = 0.0):
"""Set navigation goal."""
self.target_pose = PoseStamped()
self.target_pose.header.frame_id = 'map'
self.target_pose.pose.position.x = x
self.target_pose.pose.position.y = y
self.goal_pub.publish(self.target_pose)
self.get_logger().info(f'New goal: ({x}, {y})')
def _navigate_to_goal(self):
"""Simple proportional navigation."""
dx = self.target_pose.pose.position.x - self.current_pose.position.x
dy = self.target_pose.pose.position.y - self.current_pose.position.y
distance = np.sqrt(dx**2 + dy**2)
target_angle = np.arctan2(dy, dx)
# Get current yaw from quaternion
current_yaw = self._get_yaw(self.current_pose.orientation)
angle_error = target_angle - current_yaw
# Normalize angle
angle_error = np.arctan2(np.sin(angle_error), np.cos(angle_error))
cmd = Twist()
if distance > 0.1: # 10cm tolerance
cmd.linear.x = min(self.Kp_linear * distance, 0.5)
cmd.angular.z = self.Kp_angular * angle_error
else:
self.get_logger().info('Goal reached!')
self.target_pose = None
self.cmd_vel_pub.publish(cmd)
def _get_yaw(self, quaternion) -> float:
"""Extract yaw angle from quaternion."""
siny_cosp = 2.0 * (quaternion.w * quaternion.z +
quaternion.x * quaternion.y)
cosy_cosp = 1.0 - 2.0 * (quaternion.y * quaternion.y +
quaternion.z * quaternion.z)
return np.arctan2(siny_cosp, cosy_cosp)
def pause_callback(self, request, response):
self.is_paused = True
response.success = True
response.message = 'Navigation paused'
return response
def resume_callback(self, request, response):
self.is_paused = False
response.success = True
response.message = 'Navigation resumed'
return response
def main():
rclpy.init()
node = NavigationServer()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
10. Arduino Robot — Line Follower
// line_follower.ino — Arduino line-following robot
#include
// Pin definitions
#define LEFT_MOTOR_FORWARD 5
#define LEFT_MOTOR_BACKWARD 6
#define RIGHT_MOTOR_FORWARD 9
#define RIGHT_MOTOR_BACKWARD 10
#define LEFT_MOTOR_ENABLE 3
#define RIGHT_MOTOR_ENABLE 11
// IR sensors
#define LEFT_SENSOR 2
#define RIGHT_SENSOR 4
#define CENTER_SENSOR 7
// PID constants
float Kp = 25.0;
float Ki = 0.5;
float Kd = 15.0;
// Robot parameters
int baseSpeed = 150; // Base motor speed (0-255)
int maxSpeed = 255;
int minSpeed = 0;
// PID variables
float lastError = 0;
float integral = 0;
unsigned long lastTime = 0;
void setup() {
Serial.begin(115200);
// Motor pins as output
pinMode(LEFT_MOTOR_FORWARD, OUTPUT);
pinMode(LEFT_MOTOR_BACKWARD, OUTPUT);
pinMode(RIGHT_MOTOR_FORWARD, OUTPUT);
pinMode(RIGHT_MOTOR_BACKWARD, OUTPUT);
pinMode(LEFT_MOTOR_ENABLE, OUTPUT);
pinMode(RIGHT_MOTOR_ENABLE, OUTPUT);
// Sensor pins as input
pinMode(LEFT_SENSOR, INPUT);
pinMode(RIGHT_SENSOR, INPUT);
pinMode(CENTER_SENSOR, INPUT);
Serial.println("Line Follower Robot Ready");
delay(2000); // Wait before starting
}
void readSensors(bool &left, bool ¢er, bool &right) {
// Read IR sensors (LOW = line detected)
left = !digitalRead(LEFT_SENSOR);
center = !digitalRead(CENTER_SENSOR);
right = !digitalRead(RIGHT_SENSOR);
}
float calculateError(bool left, bool center, bool right) {
// Error calculation based on sensor combination
if (center && !left && !right) return 0; // Centered
if (left && center && !right) return -1; // Slight left
if (!left && center && right) return 1; // Slight right
if (left && !center && !right) return -2; // Sharp left
if (!left && !center && right) return 2; // Sharp right
if (left && center && right) return 0; // Intersection
return lastError; // Lost line — use last error
}
float computePID(float error, float dt) {
integral += error * dt;
integral = constrain(integral, -100, 100); // Anti-windup
float derivative = (error - lastError) / dt;
float output = Kp * error + Ki * integral + Kd * derivative;
lastError = error;
return output;
}
void setMotors(int leftSpeed, int rightSpeed) {
// Left motor
if (leftSpeed >= 0) {
analogWrite(LEFT_MOTOR_ENABLE, leftSpeed);
digitalWrite(LEFT_MOTOR_FORWARD, HIGH);
digitalWrite(LEFT_MOTOR_BACKWARD, LOW);
} else {
analogWrite(LEFT_MOTOR_ENABLE, abs(leftSpeed));
digitalWrite(LEFT_MOTOR_FORWARD, LOW);
digitalWrite(LEFT_MOTOR_BACKWARD, HIGH);
}
// Right motor
if (rightSpeed >= 0) {
analogWrite(RIGHT_MOTOR_ENABLE, rightSpeed);
digitalWrite(RIGHT_MOTOR_FORWARD, HIGH);
digitalWrite(RIGHT_MOTOR_BACKWARD, LOW);
} else {
analogWrite(RIGHT_MOTOR_ENABLE, abs(rightSpeed));
digitalWrite(RIGHT_MOTOR_FORWARD, LOW);
digitalWrite(RIGHT_MOTOR_BACKWARD, HIGH);
}
}
void stopMotors() {
setMotors(0, 0);
}
void loop() {
bool left, center, right;
readSensors(left, center, right);
float error = calculateError(left, center, right);
unsigned long now = millis();
float dt = (now - lastTime) / 1000.0;
lastTime = now;
float correction = computePID(error, dt);
int leftSpeed = constrain(baseSpeed + correction, minSpeed, maxSpeed);
int rightSpeed = constrain(baseSpeed - correction, minSpeed, maxSpeed);
setMotors(leftSpeed, rightSpeed);
// Debug output
Serial.print("L:"); Serial.print(left);
Serial.print(" C:"); Serial.print(center);
Serial.print(" R:"); Serial.print(right);
Serial.print(" Err:"); Serial.print(error);
Serial.print(" Correction:"); Serial.println(correction);
delay(10); // 100 Hz control loop
}
Core Concepts
Robot Kinematics
Forward Kinematics: Given joint angles, compute end-effector position. Unique solution.
Inverse Kinematics: Given desired end-effector position, compute joint angles. May have 0, 1, or multiple solutions.
Denavit-Hartenberg (DH) Parameters: Standard method for describing robot arm geometry.
Jacobian Matrix: Relates joint velocities to end-effector velocities. Used for velocity control and singularity detection.
Sensors in Robotics
- LiDAR: Laser-based distance measurement — accurate mapping and localization
- IMU (Inertial Measurement Unit): Accelerometer + gyroscope + magnetometer — orientation and acceleration
- Encoders: Motor position/speed feedback — joint angle measurement
- Cameras: Visual perception — object detection, navigation, SLAM
- Ultrasonic: Short-range distance measurement — obstacle avoidance
- GPS: Global positioning — outdoor navigation (low precision)
- Bump/Tactile: Physical contact detection — safety and object manipulation
Actuators & Motors
- DC Motors: Simple, cheap — used with encoders for position control
- Servo Motors: Built-in feedback — hobby robots, RC applications
- Stepper Motors: Precise positioning without feedback — 3D printers, CNC
- Brushless DC (BLDC): High power-to-weight — drones, high-performance robots
- Pneumatic/Hydraulic: High force — industrial robots, heavy lifting
SLAM (Simultaneous Localization and Mapping)
Definition: Robot builds a map of unknown environment while simultaneously localizing itself within the map.
- EKF-SLAM: Extended Kalman Filter — classic approach
- GraphSLAM: Graph-based optimization — more accurate for large environments
- FastSLAM: Particle filter approach — handles loop closures
- ORB-SLAM: Visual SLAM using feature extraction
- Cartographer: Google's real-time 2D/3D SLAM library
Control Systems
Open-Loop Control: No feedback — commands sent without verification. Simple but inaccurate.
Closed-Loop Control: Uses feedback to adjust output. PID is the most common.
PID Components:
- P (Proportional): Corrects current error — reduces rise time
- I (Integral): Corrects accumulated error — eliminates steady-state error
- D (Derivative): Predicts future error — reduces overshoot and oscillation
Robot Perception
- Computer Vision: Object detection, segmentation, depth estimation
- Point Cloud Processing: 3D data from LiDAR — filtering, segmentation
- Sensor Fusion: Combining data from multiple sensors — Kalman filter, particle filter
- Deep Learning: CNNs for object detection, RNNs for sequence prediction
Path Planning Algorithms
- A*: Optimal with heuristic — grid-based, widely used
- Dijkstra: Optimal without heuristic — guarantees shortest path
- RRT (Rapidly-exploring Random Tree): Probabilistically complete — good for high-DOF robots
- DWA (Dynamic Window Approach): Real-time obstacle avoidance — velocity space sampling
- Potential Fields: Attractive goal + repulsive obstacles — simple but local minima issue
Robot Communication
- ROS/ROS2: Middleware framework for robot software — nodes, topics, services, actions
- MQTT: Lightweight publish-subscribe protocol — IoT and robot communication
- CAN Bus: Robust serial protocol — automotive and industrial robots
- Modbus: Industrial communication protocol — PLCs and sensors
- WebSocket: Real-time bidirectional communication — robot web interfaces
Worked Examples
Example 1: PID Controller Tuning
Problem: Tune PID gains for a temperature control system with setpoint=80°C.
Solution (Ziegler-Nichols Method):
- Step 1: Set Ki=0, Kd=0. Increase Kp until oscillation starts (Ku=10, Pu=2s)
- Step 2: Apply Ziegler-Nichols formulas:
- Kp = 0.6 × Ku = 0.6 × 10 = 6.0
- Ki = 2 × Kp / Pu = 2 × 6.0 / 2 = 6.0
- Kd = Kp × Pu / 8 = 6.0 × 2 / 8 = 1.5
- Step 3: Fine-tune manually:
- Too much overshoot → reduce Kp
- Slow response → increase Kp
- Steady-state error → increase Ki
- Oscillation → increase Kd
- Final: Kp=4.5, Ki=3.0, Kd=0.8 (after fine-tuning)
Example 2: Forward Kinematics Calculation
Problem: A 3-DOF robotic arm has link lengths L1=100mm, L2=80mm, L3=60mm. Joint angles are θ1=30°, θ2=45°, θ3=15°. Find end-effector position.
Solution:
- Convert angles to radians: θ1=0.524, θ2=0.785, θ3=0.262
- Cumulative angles:
- After joint 1: 30° (0.524 rad)
- After joint 2: 75° (1.309 rad)
- After joint 3: 90° (1.571 rad)
- Position calculation:
- x = 100×cos(30°) + 80×cos(75°) + 60×cos(90°)
- x = 86.60 + 20.71 + 0 = 107.31 mm
- y = 100×sin(30°) + 80×sin(75°) + 60×sin(90°)
- y = 50.00 + 77.27 + 60 = 187.27 mm
- End effector: (107.31, 187.27) mm
Example 3: A* Path Planning on Grid
Problem: Plan path from (0,0) to (5,5) on a 6×6 grid with obstacles at (2,2), (2,3), (3,2).
Solution:
- Grid:
S . . . . . . . . . . . . . X X . . . . X . . . . . . . . . . . . . . G
- A* Algorithm:
- Start: (0,0), g=0, h=10 (Manhattan), f=10
- Expand: (1,0) f=10, (0,1) f=10
- Continue expanding lowest f-cost nodes
- Obstacles at (2,2), (2,3), (3,2) blocked
- Optimal Path: (0,0)→(1,0)→(2,0)→(3,0)→(4,0)→(4,1)→(4,2)→(4,3)→(4,4)→(4,5)→(5,5)
- Path length: 10 steps
Example 4: Kalman Filter Sensor Fusion
Problem: Fuse GPS (±3m accuracy) and IMU (±0.1m drift per second) for position estimation.
Solution:
- GPS: High absolute accuracy, low update rate (1 Hz), noisy
- IMU: High relative accuracy, high update rate (100 Hz), drifts
- Kalman Filter Setup:
- State: [position, velocity]
- Process noise Q = 0.01 (model uncertainty)
- GPS measurement noise R = 9 (3m variance)
- IMU measurement noise R = 0.001 (high precision)
- Result: Combined accuracy ≈ ±0.3m, best of both sensors
Example 5: Robot Arm Trajectory Planning
Problem: Plan a smooth trajectory for a 2-link arm from (0°, 0°) to (90°, 45°) in 2 seconds.
Solution (Cubic Polynomial):
θ(t) = a₀ + a₁t + a₂t² + a₃t³
Constraints:
θ(0) = 0 → a₀ = 0
θ(2) = π/2 → a₀ + 2a₁ + 4a₂ + 8a₃ = π/2
θ'(0) = 0 → a₁ = 0
θ'(2) = 0 → a₁ + 4a₂ + 12a₃ = 0
Solution: a₀=0, a₁=0, a₂=3π/8, a₃=-π/4
Joint 1 trajectory: θ₁(t) = (3π/8)t² - (π/4)t³ for t ∈ [0, 2]
Practice Problems
Problem 1: Implement PID Controller
Write a PID controller for a self-balancing robot (inverted pendulum).
class BalancePID:
def __init__(self, Kp=50, Ki=0.1, Kd=2):
self.Kp, self.Ki, self.Kd = Kp, Ki, Kd
self.integral = 0
self.last_error = 0
self.last_time = time.time()
def compute(self, angle, target_angle=0):
error = target_angle - angle
now = time.time()
dt = now - self.last_time
self.integral += error * dt
self.integral = max(min(self.integral, 100), -100) # Anti-windup
derivative = (error - self.last_error) / dt if dt > 0 else 0
output = self.Kp * error + self.Ki * self.integral + self.Kd * derivative
self.last_error = error
self.last_time = now
return max(min(output, 255), -255) # PWM limit
Problem 2: DH Parameters for Robot Arm
Create DH parameter table for a 3-DOF robot arm and compute the transformation matrix.
import numpy as np
def dh_matrix(theta, d, a, alpha):
"""Compute DH transformation matrix."""
return np.array([
[np.cos(theta), -np.sin(theta)*np.cos(alpha), np.sin(theta)*np.sin(alpha), a*np.cos(theta)],
[np.sin(theta), np.cos(theta)*np.cos(alpha), -np.cos(theta)*np.sin(alpha), a*np.sin(theta)],
[0, np.sin(alpha), np.cos(alpha), d],
[0, 0, 0, 1]
])
# DH Parameters: [theta, d, a, alpha]
dh_params = [
[np.radians(30), 100, 0, np.radians(90)], # Joint 1
[np.radians(45), 0, 80, 0], # Joint 2
[np.radians(15), 0, 60, 0], # Joint 3
]
# Compute forward kinematics
T = np.eye(4)
for theta, d, a, alpha in dh_params:
T = T @ dh_matrix(theta, d, a, alpha)
print("End-effector position:", T[:3, 3])
print("Rotation matrix:\n", T[:3, :3])
Problem 3: Implement Obstacle Avoidance
Write a potential field algorithm for obstacle avoidance.
import numpy as np
class PotentialField:
def __init__(self, goal, obstacles, attractive_gain=1.0,
repulsive_gain=100.0, influence_radius=2.0):
self.goal = np.array(goal)
self.obstacles = [np.array(o) for o in obstacles]
self.Ka = attractive_gain
self.Kr = repulsive_gain
self.d0 = influence_radius
def attractive_force(self, position):
"""Pull toward goal."""
diff = self.goal - position
return self.Ka * diff
def repulsive_force(self, position):
"""Push away from obstacles."""
force = np.zeros(2)
for obs in self.obstacles:
diff = position - obs
dist = np.linalg.norm(diff)
if dist < self.d0:
force += self.Kr * (1/dist - 1/self.d0) * \
diff / dist**3
return force
def compute(self, position):
"""Total force = attractive + repulsive."""
return self.attractive_force(position) + self.repetitive_force(position)
Problem 4: Wheel Odometry Calculation
Calculate robot position from wheel encoder counts using differential drive kinematics.
class DifferentialDrive:
def __init__(self, wheel_radius=0.05, wheel_base=0.3):
self.r = wheel_radius # meters
self.L = wheel_base # distance between wheels
self.x = self.y = self.theta = 0
def update(self, left_ticks, right_ticks, ticks_per_rev=200):
"""Update position from encoder ticks."""
# Distance traveled per wheel
d_left = (left_ticks / ticks_per_rev) * 2 * np.pi * self.r
d_right = (right_ticks / ticks_per_rev) * 2 * np.pi * self.r
# Differential drive kinematics
d_center = (d_left + d_right) / 2
d_theta = (d_right - d_left) / self.L
# Update position
self.x += d_center * np.cos(self.theta + d_theta/2)
self.y += d_center * np.sin(self.theta + d_theta/2)
self.theta += d_theta
return (self.x, self.y, self.theta)
Problem 5: Implement Particle Filter
Write a simplified particle filter for robot localization.
import numpy as np
class ParticleFilter:
def __init__(self, num_particles=1000, map_size=(100, 100)):
self.num_particles = num_particles
self.particles = np.random.rand(num_particles, 3) # [x, y, theta]
self.particles[:, 0] *= map_size[0]
self.particles[:, 1] *= map_size[1]
self.particles[:, 2] *= 2 * np.pi
self.weights = np.ones(num_particles) / num_particles
def predict(self, v, omega, dt, noise=0.1):
"""Motion model update."""
self.particles[:, 0] += v * np.cos(self.particles[:, 2]) * dt
self.particles[:, 1] += v * np.sin(self.particles[:, 2]) * dt
self.particles[:, 2] += omega * dt
# Add noise
self.particles[:, :3] += np.random.normal(0, noise, (self.num_particles, 3))
def update(self, measurements, landmark_positions):
"""Weight update based on sensor measurements."""
for i, particle in enumerate(self.particles):
likelihood = 1.0
for dist, bearing, landmark in zip(measurements, landmark_positions):
dx = landmark[0] - particle[0]
dy = landmark[1] - particle[1]
expected_dist = np.sqrt(dx**2 + dy**2)
expected_bearing = np.arctan2(dy, dx) - particle[2]
# Gaussian likelihood
likelihood *= np.exp(-0.5 * ((dist - expected_dist) / 0.5)**2)
self.weights[i] = likelihood
self.weights += 1e-300 # Prevent division by zero
self.weights /= self.weights.sum()
def resample(self):
"""Low-variance resampling."""
indices = np.random.choice(self.num_particles,
self.num_particles,
p=self.weights)
self.particles = self.particles[indices]
self.weights = np.ones(self.num_particles) / self.num_particles
def estimate(self):
"""Return weighted mean position."""
return np.average(self.particles, weights=self.weights, axis=0)
Problem 6: Tune PID Gains
Given system response data, determine if PID gains need adjustment and suggest new values.
# System response analysis for PID tuning
def analyze_response(times, setpoints, measurements):
"""Analyze step response and suggest PID tuning."""
# Calculate metrics
final_value = measurements[-1]
steady_state_error = setpoints[-1] - final_value
overshoot = (max(measurements) - final_value) / final_value * 100
# Rise time (10% to 90%)
low = 0.1 * final_value
high = 0.9 * final_value
t_low = next(t for t, m in zip(times, measurements) if m >= low)
t_high = next(t for t, m in zip(times, measurements) if m >= high)
rise_time = t_high - t_low
# Settling time (within 2% of final)
tolerance = 0.02 * final_value
settling_time = max(t for t, m in zip(times, measurements)
if abs(m - final_value) > tolerance)
suggestions = []
if overshoot > 10:
suggestions.append("Increase Kd to reduce overshoot")
suggestions.append("Or decrease Kp")
if steady_state_error > 0.05 * final_value:
suggestions.append("Increase Ki to eliminate steady-state error")
if rise_time > 2.0:
suggestions.append("Increase Kp for faster response")
return {
'overshoot': overshoot,
'rise_time': rise_time,
'settling_time': settling_time,
'steady_state_error': steady_state_error,
'suggestions': suggestions
}
Problem 7: Implement Bresenham's Algorithm
Write Bressenham's line algorithm for robot path rasterization.
def bresenham_line(x0, y0, x1, y1):
"""Generate grid cells for a line from (x0,y0) to (x1,y1)."""
points = []
dx = abs(x1 - x0)
dy = abs(y1 - y0)
sx = 1 if x0 < x1 else -1
sy = 1 if y0 < y1 else -1
err = dx - dy
while True:
points.append((x0, y0))
if x0 == x1 and y0 == y1:
break
e2 = 2 * err
if e2 > -dy:
err -= dy
x0 += sx
if e2 < dx:
err += dx
y0 += sy
return points
# Example: Ray from (0,0) to (5,3)
print(bressenham_line(0, 0, 5, 3))
# [(0,0), (1,1), (2,1), (3,2), (4,2), (5,3)]
Problem 8: Robot Arm Inverse Kinematics
Write iterative inverse kinematics using Jacobian transpose method.
import numpy as np
class IterativeIK:
def __init__(self, link_lengths):
self.L = link_lengths
def solve(self, target, initial_guess=None, max_iters=100,
tolerance=0.01):
"""Iterative IK using Jacobian transpose."""
angles = initial_guess or [0.0] * len(self.L)
for i in range(max_iters):
# Forward kinematics
positions = self.forward_kinematics(angles)
current = np.array(positions[-1])
# Error
error = np.array(target) - current
if np.linalg.norm(error) < tolerance:
return angles
# Jacobian
J = self.jacobian(angles)
# Update (Jacobian transpose method)
damping = 0.1
delta = J.T @ error * 0.5
angles = [a + d for a, d in zip(angles, delta)]
return angles # Best effort
Problem 9: Implement ROS Message Callback
Write a synchronized callback for multiple ROS topics using message_filters.
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from message_filters import Subscriber, ApproximateTimeSynchronizer
from sensor_msgs.msg import Image, CameraInfo
class VisualOdometry(Node):
def __init__(self):
super().__init__('visual_odometry')
image_sub = Subscriber(self, Image, '/camera/image_raw')
info_sub = Subscriber(self, CameraInfo, '/camera/camera_info')
# Synchronize messages with 0.1s tolerance
sync = ApproximateTimeSynchronizer(
[image_sub, info_sub],
queue_size=10,
slop=0.1
)
sync.registerCallback(self.sync_callback)
self.get_logger().info('Visual odometry node started')
def sync_callback(self, image_msg, info_msg):
"""Process synchronized image + camera info."""
self.get_logger().info(
f'Image: {image_msg.width}x{image_msg.height}, '
f'K: {info_msg.k[:3]}'
)
# Process visual odometry here
def main():
rclpy.init()
node = VisualOdometry()
rclpy.spin(node)
rclpy.shutdown()
Problem 10: Simulate Sensor Noise
Write a function that adds realistic Gaussian noise to sensor readings.
import numpy as np
class SensorNoise:
@staticmethod
def add_gaussian_noise(measurement, std_dev):
"""Add Gaussian noise to a measurement."""
return measurement + np.random.normal(0, std_dev)
@staticmethod
def add_multiplicative_noise(measurement, factor):
"""Add proportional noise (e.g., 5% of reading)."""
return measurement * (1 + np.random.normal(0, factor))
@staticmethod
def quantize(measurement, resolution):
"""Simulate sensor quantization (e.g., 8-bit ADC)."""
levels = 2**8 - 1
return np.round(measurement * levels) / levels
@staticmethod
def add_outliers(data, probability=0.01, magnitude=10):
"""Add random outliers (sensor glitches)."""
mask = np.random.random(len(data)) < probability
data[mask] += np.random.normal(0, magnitude, mask.sum())
return data
# Example: Simulate LiDAR scan
distances = np.array([1.0, 2.0, 1.5, 3.0, 2.5] * 20)
noisy = SensorNoise.add_gaussian_noise(distances, std_dev=0.05)
print(f"True: {distances[:5]}")
print(f"Noisy: {noisy[:5]}")