3DOF Robotic Manipulator

Kinematic Modeling & Real-Time Control

📌 Project Overview

An end-to-end mechatronic system featuring a 3-Degree-of-Freedom (3-DOF) robotic arm engineered for spatial positioning and trajectory execution. Built as a capstone mechatronics engineering project, this platform integrates analytical kinematic mathematical modeling, high-frequency closed-loop firmware control, and a modern Python graphical user interface.

andresalemn/uni-3dof-manipulator andresalemn/uni-3dof-manipulator
Key Technical Highlights
  • $\pm 1\text{ mm}$ Precision: Achieved sub-millimeter end-effector accuracy using closed-loop DC motor control with quadrature encoders and custom PID tuning.
  • Real-Time Trajectory Execution: Dynamic parametric trajectory generation allowing the manipulator to draw geometric primitives (circles, rectangles, triangles) with continuous velocity profiles.
  • Decoupled Architecture: Asynchronous system design splitting high-level mathematical calculations (Python) from real-time hardware execution (Arduino Mega firmware).
🔥 4M+ Views Showcase Demo on TikTok
@hec_moore Con esto terminamos la carrera de Ingeniería Mecatrónica You can call me Engineer! 🫡 #estudiantesingenieria #mecatronica #ingenieriamecatronica #robotica

🏗️ System Architecture

The project features a decoupled, two-tier architecture designed to separate high-level mathematical computations from real-time hardware execution:

flowchart TD
    subgraph Host ["💻 Host PC (Python / High-Level Layer)"]
        UI["CustomTkinter GUI\n(User Interface & Trajectory Selection)"]
        TP["Trajectory Planner\n(Parametric Waypoint Generator)"]
        KIN["Kinematic Engine\n(DH Matrices, Analytical IK & Jacobian)"]
        SER_PY["PySerial Interface\n(Command Formatting @ 115200 bps)"]

        UI --> KIN
        UI --> TP
        TP --> KIN
        KIN --> SER_PY
    end

    subgraph Comm ["🔌 Serial Bus"]
        SER_PY <-->|UART / RS-232| SER_ARD
    end

    subgraph Firmware ["⚡ Microcontroller Layer (Arduino Mega 2560)"]
        SER_ARD["Serial Command Parser & State Machine"]
        PID["Closed-Loop PID Control Loop\n(Encoder Interrupts & Position Error)"]
        PWM["PWM Motor Drivers"]
        BTN["Manual Push-Button Control\n(Auxiliary End-Effector Trigger)"]
        SERVO_LIB["Servo.h Driver"]

        SER_ARD --> PID
        PID --> PWM
        BTN --> SERVO_LIB
    end

    subgraph Hardware ["⚙️ Mechanical & Electrical Hardware"]
        MOTORS["3x DC Motors with Quadrature Encoders"]
        ROBOT["3-DOF Mechanical Linkage"]
        GRIPPER["Auxiliary Micro-Servo Gripper"]

        PWM --> MOTORS
        MOTORS --> ROBOT
        MOTORS -.->|Encoder Feedback Interrupts| PID
        SERVO_LIB --> GRIPPER
    end

    style Host fill:#1a1a2e,stroke:#4e54c8,stroke-width:2px,color:#fff
    style Firmware fill:#0f3460,stroke:#e94560,stroke-width:2px,color:#fff
    style Hardware fill:#16213e,stroke:#00b4d8,stroke-width:2px,color:#fff
    style Comm fill:#000,stroke:#ffa500,stroke-width:1px,color:#fff

Subsystem Breakdown

  1. High-Level Control (Python):
    • GUI (robot_interface.py): Built using CustomTkinter for real-time Cartesian coordinate entry, manual joint control, and visual feedback.
    • Kinematics Engine (FK_3DOF.py, IK_3DOF.py, dh_matrix.py, GetJacobian3.py): Solves analytical inverse kinematics and computes transformation and Jacobian matrices.
    • Trajectory Planner (Trajectories.py): Generates linear, circular, rectangular, and triangular parametric path waypoints with continuous velocity profiles.
  2. Communication Protocol:
    • Asynchronous serial interface over UART at $115,200\text{ bps}$. Packets transmit target joint angles and operational modes to the microcontroller.
  3. Low-Level Firmware (Arduino C++):
    • Non-blocking state machine running on an Arduino Mega 2560.
    • Reads hardware quadrature encoders via external interrupts for precise real-time angular feedback.
    • Executes independent PID loops per joint to regulate PWM output to motor drivers.

📐 Kinematics & Mathematical Modeling

The manipulator features 3 revolute joints. Spatial positioning of the end-effector is modeled rigorously using standard Denavit-Hartenberg (DH) parameters and closed-form analytical inverse kinematics.

📖 Click to expand full mathematical derivation (DH Parameters, Analytical IK & Jacobian Matrix)
1. Denavit-Hartenberg (DH) Parameters

Each link transformation $T_{i-1}^i$ is computed from the homogeneous DH transformation matrix:

$$ T_{i-1}^i = \begin{bmatrix} \cos\theta_i & -\sin\theta_i \cos\alpha_i & \sin\theta_i \sin\alpha_i & a_i \cos\theta_i \\ \sin\theta_i & \cos\theta_i \cos\alpha_i & -\cos\theta_i \sin\alpha_i & a_i \sin\theta_i \\ 0 & \sin\alpha_i & \cos\alpha_i & d_i \\ 0 & 0 & 0 & 1 \end{bmatrix} $$
Link ($i$) $\theta_i$ (rad) $d_i$ (cm) $a_i$ (cm) $\alpha_i$ (rad)
1 $\theta_1^*$ $L_1 = 15.0$ $0$ $+\pi/2$
2 $\theta_2^*$ $0$ $L_2 = 22.0$ $0$
3 $\theta_3^*$ $d = -6.5$ $L_3 = 18.0$ $0$
2. Analytical Inverse Kinematics (IK)

Given a target Cartesian coordinate $(x_c, y_c, z_c)$, the joint angles $(\theta_1, \theta_2, \theta_3)$ are computed analytically:

  • Base Angle ($\theta_1$):
    $$r = \sqrt{x_c^2 + y_c^2 - d^2}, \quad \theta_1 = \text{atan2}(y_c, x_c) - \text{atan2}(d, r)$$
  • Elbow Angle ($\theta_3$):
    $$\cos\theta_3 = \frac{x_c^2 + y_c^2 - d^2 + (z_c - L_1)^2 - L_2^2 - L_3^2}{2 L_2 L_3}, \quad \theta_3 = \text{atan2}(\sqrt{1 - \cos^2\theta_3}, \cos\theta_3)$$
  • Shoulder Angle ($\theta_2$):
    $$s = -(z_c - L_1), \quad \theta_2 = \text{atan2}(s, r) - \text{atan2}(L_3 \sin\theta_3, L_2 + L_3 \cos\theta_3)$$
3. Differential Kinematics & Velocity Control

Cartesian velocity transformation via the $6 \times 3$ Jacobian matrix $J(\mathbf{q})$:

$$\mathbf{v} = J(\mathbf{q}) \dot{\mathbf{q}}$$ $$ J(\mathbf{q}) = \begin{bmatrix} \mathbf{z}_0 \times (\mathbf{o}_3 - \mathbf{o}_0) & \mathbf{z}_1 \times (\mathbf{o}_3 - \mathbf{o}_1) & \mathbf{z}_2 \times (\mathbf{o}_3 - \mathbf{o}_2) \\ \mathbf{z}_0 & \mathbf{z}_1 & \mathbf{z}_2 \end{bmatrix} $$

⚡ Firmware & Hardware Control

The low-level controller handles interrupt-driven encoder decoding, state parsing, and closed-loop motor control on the Arduino Mega 2560.

  • High-Frequency Encoder ISRs: Quadrature encoders generate high-frequency pulses on external interrupt pins, maintaining exact angular position tracking for all 3 DC motors.
  • Closed-Loop PID Control: Individual discrete-time PID position control loops continuously compute error $e(t) = \theta_{target} - \theta_{current}$ to modulate motor PWM driver signals.

🖼️ Hardware & Circuit Gallery

Left: Assembled 3-DOF mechanical manipulator arm. Right: Control electronics, H-bridge motor drivers, and Arduino Mega wiring board.
💡 Rapid Prototyping & Engineering Trade-Offs
  • 3-Week Capstone Constraint: The entire system—from mechanical machining to kinematic math and custom firmware—was developed within a strict 3-week window.
  • Auxiliary Servo Gripper Trade-Off: The end-effector included a micro-servo tool for object grasping. Due to time constraints and to avoid potential timer/interrupt contention between Arduino's Servo.h library and the high-frequency motor encoder ISRs, the team made a strategic trade-off: gripper control was offloaded to a direct physical push-button trigger rather than passing through serial GUI commands. This allowed us to guarantee zero-latency end-effector actuation without compromising the primary 3-DOF kinematic control loop.

👥 Team & Contributions

  • Andrés Alemán – Control Systems, Kinematic Modeling & Lead Software Development (@andresalemn)
  • Héctor Moore – Mechanical Design, CAD Assembly & Physical Fabrication
  • Ricardo Villanueva – Power Electronics, Circuit Integration & Hardware Testing