ode4.py

import math
import numpy as np
import pinocchio as pin

def f(M, tau, x, iL, iR, b=0.2022, r=0.0985):
    '''
    Returns the derivative of the state vector x

    Arguments:
        - M: the mass matrix of the differential drive
        - tau: the control input
        - x: the state vector [x, y, theta, w_l, w_r]
        - iL: the left wheel slip
        - iR: the right wheel slip
        - b: the wheel distance of the differential drive
        - r: the radius of the wheels
    '''

    # Split the state vector into the joint configuration and the joint velocity
    q = x[:3]
    v = x[3:]
    theta = q[2]

    S = (1 / (2 * b)) * np.array([
        [b * r * (1 - iL) * math.cos(theta), b * r * (1 - iR) * math.cos(theta)],
        [b * r * (1 - iL) * math.sin(theta), b * r * (1 - iR) * math.sin(theta)],
        [-2*r * (1 - iL), 2*r * (1 - iR)]
    ]) 
    
    B = np.array([
        [math.cos(theta), math.cos(theta)],
        [math.sin(theta), math.sin(theta)],
        [-b/2, b/2]
    ])
    
    Bbar = S.T @ B
    Mbar = S.T @ M @ S
    
    # Compute the derivative of the state vector
    dx = np.zeros_like(x)

    dx[:3] = S @ v                                  # [x_dot, y_dot, theta_dot]
    dx[3:] = np.linalg.inv(Mbar) @ Bbar @ tau       # [w_l_dot, w_r_dot]

    return dx 

# Runge-Kutta 4th order integration method
def ode4(M, x0, dt, tau, iL=0, iR=0):
    # First step
    s1 = f(M, tau, x0, iL, iR)
    x1 = x0 + dt/2*s1
    
    s2 = f(M, tau, x1, iL, iR)
    x2 = x0 + dt/2*s2
    
    s3 = f(M, tau, x2, iL, iR)
    x3 = x0 + dt*s3
    
    s4 = f(M, tau, x3, iL, iR)
    x = x0 + dt*(s1 + 2*s2 + 2*s3 + s4)/6

    return x