Mobile Robot Kinematics Library

A Python library for calculating the forward and inverse kinematics of fixed-wheel mobile robots.

Overview

This library supports kinematic calculations for robots with various fixed wheel configurations, for example:

  • Differential Drive Robots: Two independently driven wheels.
  • Omni-Wheel Robots: Four Mecanum wheels for holonomic motion.

    • Key features include:

    • Calculates robot velocities given wheel velocities (forward kinematics).
    • Determines required wheel velocities for desired robot motion (inverse kinematics).

    For the C++ version of this library, visit the GitHub repository.

    Theoretical Background

    This library is based on fundamental kinematic principles:

    • Forward Kinematics: Computes the robot's velocity using the equation:
      \[ \dot{\zeta} = \mathbf{R}_\theta^{-1} \cdot \mathbf{J}_1^{+} \cdot \mathbf{J}_2 \cdot \phi \]
    • Inverse Kinematics: Calculates wheel velocities required to achieve a desired robot motion:
      \[ \phi = \mathbf{J}_2^{+} \cdot \mathbf{J}_1 \cdot \mathbf{R}_\theta \cdot \dot{\zeta} \]

    Where:

    • \(\dot{\zeta}\): The state vector of robot velocities:
      \[ \dot{\zeta} = \begin{bmatrix} \dot{x} \\ \dot{y} \\ \dot{\phi} \end{bmatrix} \]
    • \(\phi\): Wheel velocity vector:
      \[ \phi = \begin{bmatrix} \phi_1 \\ \phi_2 \\ \vdots \\ \phi_n \end{bmatrix} \]
    • \(\mathbf{R}_\theta\): Rotation matrix to account for robot orientation in the world frame:
      \[ \mathbf{R}_\theta = \begin{bmatrix} \cos\theta & -\sin\theta & 0 \\ \sin\theta & \cos\theta & 0 \\ 0 & 0 & 1 \end{bmatrix} \]

    Example Usage in Python

    
from MobileRobotKinematics import MobileRobotKinematics

# Define a simple robot with two wheels
diffDrive = MobileRobot()
diffDrive.leftWheel = diffDrive.add_wheel(name="leftWheel", radius=0.25, L=0.5, alpha=pi/2, beta=0)
diffDrive.rightWheel = diffDrive.add_wheel(name="rightWheel", radius=0.25, L=0.5, alpha=3*pi/2, beta=0)

# Forward Kinematics: Calculate robot velocity
robot_velocity = diffDrive.calculate_robot_velocity([1.0, 1.2])
print(f"Robot Velocity: {robot_velocity}")

# Inverse Kinematics: Calculate wheel velocities
wheel_speeds = diffDrive.calculate_wheel_velocities([0.5, 0.0, 0.1])
print(f"Wheel Speeds: {wheel_speeds}")

    Example Usage in C++

    
#include 
#include 
#include 
#include "Wheel.hpp"
#include "MobileRobot.hpp"
#include 

int main() {
    // Create a Kiwi Drive robot with 3 omni-directional wheels
    Wheel wheel1("Wheel1", 0.1, 0.2, M_PI / 3, 0.0, 0);
    Wheel wheel2("Wheel2", 0.1, 0.2, M_PI, 0.0, 0);
    Wheel wheel3("Wheel3", 0.1, 0.2, -M_PI / 3, 0.0, 0);

    MobileRobot robot;
    robot.addWheel(wheel1);
    robot.addWheel(wheel2);
    robot.addWheel(wheel3);

    // Forward Kinematics: Calculate robot velocity
    std::vector wheelVelocities = {1.0, 1.2, 0.8};
    Eigen::Vector3d robotVelocity = robot.calculateRobotVelocity(wheelVelocities);
    std::cout << "Robot Velocity: (" << robotVelocity[0] << ", " 
              << robotVelocity[1] << ", " << robotVelocity[2] << ")\n";

    // Inverse Kinematics: Calculate wheel velocities
    Eigen::Vector3d desiredVelocity(0.5, 0.0, 0.1);
    std::vector wheelSpeeds = robot.calculateWheelVelocities(desiredVelocity);
    std::cout << "Wheel Speeds: ";
    for (double speed : wheelSpeeds) {
        std::cout << speed << " ";
    }
    std::cout << "\n";

    return 0;
}

    Repositories