control.py

import numpy as np

class ControlSpace:
    """
    A generic state space with states x and control variable u.  
    The primary function of this class is to define the transformation from the current 
    state x to the next state xnext by applying a control u.
    """
    def __init__(self):
        pass
    def state_dimension(self):
        """Returns the dimension of the state space associated with this."""
        raise NotImplementedError()
    def control_dimension(self):
        """Returns the set from which controls should be sampled"""
        raise NotImplementedError()
    def next_state(self,x,u):
        """Produce the next state after applying control u to state x"""
        raise NotImplementedError()
    def interpolator(self,x,u):
        """Returns the interpolator (e.g., a trajectory) that goes from x to
        self.nextState(x,u)."""
        raise NotImplementedError()
    def connection(self,x,y):
        """Returns a sequence of controls that connects x to y, if
        applicable.  Return None if no such sequence exists."""
        return None

class Roomba(ControlSpace):
    """
    Class that describes the dynamics of a roomba-like robot.
    """
    def __init__(self):
        self.max_speed = 10
        self.max_acc = 5.0
        self.max_d_acc = 3.0
        self.max_steer = np.radians(360)
        self.max_d_steer = np.radians(180)

    def state_dimension(self):
        # State is [x, y, theta]
        return 3

    def control_dimension(self):
        # Control is [v, steering_angle]
        return 2

    def derivative(self, x, u):
        """
        Compute the derivative of the state x with respect to control u.
        State x: [x, y, theta]
        Control u: [v, steering_angle]
        """
        x_pos, y_pos, theta = x
        v, steering_angle = u

        # Dynamics equations
        dx_pos = v * np.cos(theta)
        dy_pos = v * np.sin(theta)
        dtheta = steering_angle

        return np.array([dx_pos, dy_pos, dtheta])

    def next_state(self, x, u, dt):
        """
        Compute the next state given the current state and control input.
        """
        dxdt = u[0] * np.cos(x[2])
        dydt = u[0] * np.sin(x[2])
        dthetadt = u[1]
        
        # Update state using Euler integration
        new_x = x[0] + dxdt * dt
        new_y = x[1] + dydt * dt
        new_theta = x[2] + dthetadt * dt

        # Return the predicted next state
        return np.array([new_x, new_y, new_theta])