SpotMicro AI Controller (ROS 2 C++)

A high-performance, modular C++ controller for the SpotMicro quadruped robot platform. This project implements a fully autonomous control loop featuring Geometric Inverse Kinematics, Bezier Curve Gait Planning, and Lidar-based Obstacle Avoidance within a Gazebo simulation environment.

---

Table of Contents

1. Project Overview
2. System Architecture
3. Installation
4. Usage
5. Technical Documentation
6. Configuration & Tuning
7. File Structure

---

Project Overview

This project converts a standard URDF/Xacro description of a SpotMicro into an intelligent, walking robot. It replaces standard "ragdoll" physics with a rigid-body control system that:

• Walks: Implements a Trot Gait (diagonal pairs moving together).
• Balances: Uses a mathematical model to keep the body flat while legs move.
• Sees: Utilizes a simulated Lidar to detect walls.
• Reacts: Autonomously stops and reroutes when paths are blocked.

---

System Architecture

The control logic operates on a 50Hz loop (20ms) and follows this pipeline:

1. Perception Layer (scan_callback):

   • Reads sensor_msgs/LaserScan from Gazebo.
   • Filters rays in a 40-degree cone in front of the robot.
   • Flagging system: If distance < 0.5m, trigger OBSTACLE_DETECTED.

2. Logic Layer (Finite State Machine):

   • State A (Clear): Set forward velocity to 0.06 m/s.
   • State B (Blocked): Set forward velocity to 0.0 m/s, Yaw velocity to 0.4 rad/s (Turn Left).

3. Gait Planner (Bezier Curves):

   • Calculates the swing trajectory of the foot using a cubic Bezier curve.
   • Ensures the foot lifts 0.04m (Clearance) during the swing phase to avoid dragging.

4. Inverse Kinematics Engine (spot_math.hpp):

   • Input: Desired $(x, y, z)$ foot position relative to the shoulder.
   • Output: Exact angles for Shoulder, Leg, and Foot motors using trigonometric projection and the Law of Cosines.

5. Actuation Layer (ros2_control):

   • Publishes joint angles to specific topics (e.g., /spot/front_right_leg_controller/command).
   • Gazebo's physics engine applies PID torque to match these angles.

---

Installation

1. Prerequisites

This project requires ROS 2 (Humble, Iron, or Jazzy) and the Gazebo simulation stack.

sudo apt update
sudo apt install ros-$ROS_DISTRO-gazebo-ros-pkgs \
                 ros-$ROS_DISTRO-ros2-control \
                 ros-$ROS_DISTRO-ros2-controllers \
                 ros-$ROS_DISTRO-xacro \
                 ros-$ROS_DISTRO-robot-state-publisher \
                 ros-$ROS_DISTRO-joint-state-broadcaster

2. Build the Package

Clone this package into your ROS 2 workspace src folder.

cd ~/ros2_ws
colcon build --packages-select spot_micro_project
source install/setup.bash

---

Usage

1. Autonomous AI Mode

This command launches Gazebo, spawns the robot, loads the controllers, and starts the C++ AI node.

ros2 launch spot_micro_project start_robot.launch.py

Expected Behavior:

1. The robot spawns and stands up.
2. It immediately begins walking forward.
3. If you drop a box in front of it (Gazebo -> Insert -> Box), it will stop, turn left, and resume walking.

2. Manual Control (Teleoperation)

To manually drive the robot (or test body kinematics), run a teleop node in a separate terminal:

ros2 run teleop_twist_keyboard teleop_twist_keyboard

• i / ,: Move Forward / Backward
• j / l: Turn Left / Right
• q / z: Increase / Decrease Max Speed

---

Technical Documentation

Inverse Kinematics (IK)

The robot uses a custom 3-DOF analytical solver located in include/spot_micro_project/spot_math.hpp.

Link Lengths:

• $L_{hip} = 0.052m$
• $L_{upper} = 0.120m$
• $L_{lower} = 0.115m$

The Math: Given a target coordinate $(x, y, z)$ relative to the shoulder:

1. Shoulder Angle ($\theta_1$): $$\theta_1 = \text{atan2}(y, -z) - \text{atan2}(\sqrt{L_{yz}^2 - L_{hip}^2}, L_{hip})$$
2. Knee Angle ($\theta_3$): $$\cos(\theta_3) = \frac{R^2 - L_{upper}^2 - L_{lower}^2}{2 \cdot L_{upper} \cdot L_{lower}}$$
3. Thigh Angle ($\theta_2$): $$\theta_2 = \text{atan2}(z', x') - \text{acos}(\frac{L_{upper}^2 + R^2 - L_{lower}^2}{2 \cdot L_{upper} \cdot R})$$

Gait Generation

The robot utilizes a Time-Based Trot Gait.

• Cycle Time: 0.6 seconds.
• Phase Offset: Diagonal pairs are $180^\circ$ out of phase.
  • Pair 1: Front-Right + Rear-Left
  • Pair 2: Front-Left + Rear-Right

---

Configuration & Tuning

PID Values

Defined in config/joint_controllers.yaml. If the robot collapses, increase P. If it vibrates, increase D.

front_right_leg_controller:
  ros__parameters:
    p: 150.0  # Stiffness
    d: 10.0   # Damping
    i: 0.01   # Integral

Joint Map

This project uses a specific naming convention matching the spotmicroai.xacro:

Leg	Shoulder Joint	Leg Joint	Foot Joint
Front Right	front_right_shoulder	front_right_leg	front_right_foot
Front Left	front_left_shoulder	front_left_leg	front_left_foot
Rear Right	rear_right_shoulder	rear_right_leg	rear_right_foot
Rear Left	rear_left_shoulder	rear_left_leg	rear_left_foot

---

📂 File Structure

spot_micro/
├── CMakeLists.txt              # Compiler instructions
├── package.xml                 # ROS 2 Dependencies
├── README.md                   # Documentation
├── config/
│   ├── joint_controllers.yaml  # PID & Hardware Interface config
├── include/
│   └── spot_micro/
│       └── spot_math.hpp       # MATH LIB: IK & Bezier Logic
├── launch/
│   └── start_robot.launch.py   # Main Launch Script
├── meshes/                     # STL Geometry files
├── models/
│   ├── materials.xacro         # Colors
│   ├── spotmicroai.gazebo      # Lidar & IMU Plugins
│   └── spotmicroai.xacro       # Robot Description
└── src/
    └── spot_node.cpp           # MAIN EXECUTABLE: AI & Control Node

Project for ROS 2 C++ Architecture.

---

IMPORTANT : Credits

This repository contains modifications to the original project by Andrea Tosi, who provided the meshes and models folders. This is an open source project. The original project is licensed under the MIT License.
All modifications are also released under the MIT License. See LICENSE for details.