mip_sim.py 文件源码

def dynamics(q, u, p):
    """
    Returns state derivative qdot.
    Takes current state q, motor input torque u, and disturbance torque p.
    See <http://renaissance.ucsd.edu/courses/mae143c/MIPdynamics.pdf> (rederived with incline).

    """
    # Angle of pendulum in incline frame
    ang = q[2] - incline

    # Mass matrix
    M = np.array([
                  [(mass_wheel + mass_pend)*radius**2 + inertia_wheel, mass_pend*radius*cw_to_cm[1]*np.cos(ang)],
                  [mass_pend*radius*cw_to_cm[1]*np.cos(ang), inertia_pend + mass_pend*cw_to_cm[1]**2]
                ])

    # Gravity effect
    g = np.array([
                  -mass_pend*radius*cw_to_cm[1]*q[3]**2*np.sin(ang) + mass_wheel*radius*gravity[1]*np.sin(incline),
                  mass_pend*gravity[1]*cw_to_cm[1]*np.sin(q[2])
                ])

    # Friction force
    d = np.array([
                  -friction_wheel * (q[1] + np.arctan(q[1])),
                  friction_pend * q[3]
                ])

    # Dynamics
    accel_wheel_neg, accel_pend = npl.inv(M).dot(np.array([-u, p+u]) - g - d)

    return np.array([q[1], -accel_wheel_neg*radius, q[3], accel_pend])

################################################# SIMULATION

# Define time domain