Plant Models

Plants represent the mathematical systems that are being controlled. OpenSMC provides 9 pre-defined plant models that include system dynamics, parameters, and built-in disturbance profiles.

Usage Example

from opensmc.plants import Quadrotor6DOF

# Create a 6-DOF Quadrotor plant model
plant = Quadrotor6DOF()

# Get the time-derivative of the state for a given state x and input u
x0 = [0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0]
dx_dt = plant.dynamics(t=0.0, x=x0, u=[0.5, 0.5, 0.5, 0.5])

Available Plants

plants

OpenSMC Plants — 9 dynamical system implementations.

Classes

DoubleIntegrator

Bases: Plant

Double integrator: xddot = (u + d) / J.

States: x = [position, velocity] Input: u (scalar force/torque) Output: y = position

Parameters

J : float Inertia (default 1.0). Liu (2017) uses J=2.

Source code in opensmc/plants/double_integrator.py

class DoubleIntegrator(Plant):
    """Double integrator: xddot = (u + d) / J.

    States: x = [position, velocity]
    Input:  u (scalar force/torque)
    Output: y = position

    Parameters
    ----------
    J : float
        Inertia (default 1.0). Liu (2017) uses J=2.
    """

    def __init__(self, J=1.0):
        super().__init__(n_states=2, n_inputs=1, n_outputs=1,
                         name="DoubleIntegrator")
        self.J = J

    def dynamics(self, t, x, u, d=None):
        """Compute xdot = [velocity, (u + d) / J].

        Parameters
        ----------
        t : float
            Time.
        x : ndarray (2,)
            State [position, velocity].
        u : float
            Control input (force or torque).
        d : float or None
            External disturbance (default 0).

        Returns
        -------
        xdot : ndarray (2,)
        """
        if d is None:
            d = 0.0
        u_scalar = float(np.ravel(u)[0]) if np.ndim(u) > 0 else float(u)
        # Disturbance acts on acceleration (index 1) if array, else scalar
        if np.ndim(d) > 0:
            d_scalar = float(d[1]) if len(d) > 1 else float(d[0])
        else:
            d_scalar = float(d)

        position_dot = x[1]
        velocity_dot = (u_scalar + d_scalar) / self.J

        return np.array([position_dot, velocity_dot])

    def output(self, x):
        """Output y = position."""
        return x[:1]

    def get_default_x0(self):
        """Default initial state: origin at rest."""
        return np.zeros(2)

Functions

dynamics(t, x, u, d=None)

Compute xdot = [velocity, (u + d) / J].

Parameters

t : float Time. x : ndarray (2,) State [position, velocity]. u : float Control input (force or torque). d : float or None External disturbance (default 0).

Returns

xdot : ndarray (2,)

Source code in opensmc/plants/double_integrator.py

def dynamics(self, t, x, u, d=None):
    """Compute xdot = [velocity, (u + d) / J].

    Parameters
    ----------
    t : float
        Time.
    x : ndarray (2,)
        State [position, velocity].
    u : float
        Control input (force or torque).
    d : float or None
        External disturbance (default 0).

    Returns
    -------
    xdot : ndarray (2,)
    """
    if d is None:
        d = 0.0
    u_scalar = float(np.ravel(u)[0]) if np.ndim(u) > 0 else float(u)
    # Disturbance acts on acceleration (index 1) if array, else scalar
    if np.ndim(d) > 0:
        d_scalar = float(d[1]) if len(d) > 1 else float(d[0])
    else:
        d_scalar = float(d)

    position_dot = x[1]
    velocity_dot = (u_scalar + d_scalar) / self.J

    return np.array([position_dot, velocity_dot])

get_default_x0()

Default initial state: origin at rest.

Source code in opensmc/plants/double_integrator.py

def get_default_x0(self):
    """Default initial state: origin at rest."""
    return np.zeros(2)

output(x)

Output y = position.

Source code in opensmc/plants/double_integrator.py

def output(self, x):
    """Output y = position."""
    return x[:1]

DoublePendulumCrane

Bases: Plant

Double-pendulum overhead crane (Qian & Yi 2015, Eq. 2.46).

Lagrangian formulation: M(q)qddot + C(q,qdot)qdot + G(q) = [u; 0; 0] where q = [x_trolley, phi, theta].

States: x = [x_trolley, x_dot, phi, phi_dot, theta, theta_dot] Input: u (scalar horizontal force on trolley) [N] Output: y = [x_trolley, phi, theta]

Parameters

M : float Trolley mass [kg] (default 50.0). m1 : float Hook mass [kg] (default 10.0). m2 : float Payload (cargo) mass [kg] (default 2.0). l1 : float Upper cable length [m] (default 3.0). l2 : float Lower cable length [m] (default 0.3). b : float Trolley friction [N*s/m] (default 0.0). g : float Gravitational acceleration [m/s^2] (default 9.81).

Source code in opensmc/plants/crane.py

class DoublePendulumCrane(Plant):
    """Double-pendulum overhead crane (Qian & Yi 2015, Eq. 2.46).

    Lagrangian formulation: M(q)*qddot + C(q,qdot)*qdot + G(q) = [u; 0; 0]
    where q = [x_trolley, phi, theta].

    States: x = [x_trolley, x_dot, phi, phi_dot, theta, theta_dot]
    Input:  u (scalar horizontal force on trolley) [N]
    Output: y = [x_trolley, phi, theta]

    Parameters
    ----------
    M : float
        Trolley mass [kg] (default 50.0).
    m1 : float
        Hook mass [kg] (default 10.0).
    m2 : float
        Payload (cargo) mass [kg] (default 2.0).
    l1 : float
        Upper cable length [m] (default 3.0).
    l2 : float
        Lower cable length [m] (default 0.3).
    b : float
        Trolley friction [N*s/m] (default 0.0).
    g : float
        Gravitational acceleration [m/s^2] (default 9.81).
    """

    def __init__(self, M=50.0, m1=10.0, m2=2.0, l1=3.0, l2=0.3,
                 b=0.0, g=9.81):
        super().__init__(n_states=6, n_inputs=1, n_outputs=3,
                         name="DoublePendulumCrane")
        self.M = M
        self.m1 = m1   # hook mass (mh in the notebook)
        self.m2 = m2    # payload mass (mc in the notebook)
        self.l1 = l1
        self.l2 = l2
        self.b = b
        self.g = g

    def mass_matrix(self, phi, theta):
        """Mass matrix M(q) -- 3x3, symmetric, positive definite."""
        M_t = self.M
        mh, mc = self.m1, self.m2
        l1, l2 = self.l1, self.l2

        cphi = np.cos(phi)
        ctheta = np.cos(theta)
        cpt = np.cos(phi - theta)

        M_mat = np.array([
            [M_t + mh + mc,
             -(mh + mc) * l1 * cphi,
             -mc * l2 * ctheta],
            [-(mh + mc) * l1 * cphi,
             (mh + mc) * l1**2,
             mc * l1 * l2 * cpt],
            [-mc * l2 * ctheta,
             mc * l1 * l2 * cpt,
             mc * l2**2]
        ])
        return M_mat

    def coriolis_matrix(self, phi, theta, dphi, dtheta):
        """Coriolis/centripetal matrix C(q, qdot) -- 3x3."""
        mh, mc = self.m1, self.m2
        l1, l2 = self.l1, self.l2

        sphi = np.sin(phi)
        stheta = np.sin(theta)
        spt = np.sin(phi - theta)

        C_mat = np.array([
            [self.b,
             (mh + mc) * l1 * sphi * dphi,
             mc * l2 * stheta * dtheta],
            [0.0,
             0.0,
             -mc * l1 * l2 * spt * dtheta],
            [0.0,
             mc * l1 * l2 * spt * dphi,
             0.0]
        ])
        return C_mat

    def gravity_vector(self, phi, theta):
        """Gravity vector G(q) -- (3,)."""
        mh, mc = self.m1, self.m2
        l1, l2 = self.l1, self.l2
        g = self.g

        return np.array([
            0.0,
            (mh + mc) * g * l1 * np.sin(phi),
            mc * g * l2 * np.sin(theta)
        ])

    def get_dynamics_components(self, x):
        """Decompose into affine-in-u form for each generalized coordinate.

        qddot = M^{-1} * ([u;0;0] - G - C*qdot)

        At the current state, returns f_vec, b_vec such that:
            qddot = f_vec + b_vec * u

        Returns
        -------
        f_vec : ndarray (3,)
            Drift terms (gravity + Coriolis, no input).
        b_vec : ndarray (3,)
            Input gain column (first column of M^{-1}).
        """
        dx = x[1]
        phi = x[2]
        dphi = x[3]
        theta = x[4]
        dtheta = x[5]

        dq = np.array([dx, dphi, dtheta])

        M_mat = self.mass_matrix(phi, theta)
        C_mat = self.coriolis_matrix(phi, theta, dphi, dtheta)
        G_vec = self.gravity_vector(phi, theta)

        M_inv = np.linalg.inv(M_mat)

        # f_vec = M^{-1} * (-G - C*dq)
        f_vec = M_inv @ (-G_vec - C_mat @ dq)

        # b_vec = M^{-1} * [1; 0; 0]
        b_vec = M_inv[:, 0]

        return f_vec, b_vec

    def dynamics(self, t, x, u, d=None):
        """Compute state derivatives.

        Parameters
        ----------
        t : float
            Time.
        x : ndarray (6,)
            State [x_trolley, x_dot, phi, phi_dot, theta, theta_dot].
        u : float
            Horizontal force on trolley [N].
        d : float or None
            External disturbance (added to force). Default 0.

        Returns
        -------
        xdot : ndarray (6,)
        """
        if d is None:
            d = 0.0
        u_val = float(np.ravel(u)[0]) if np.ndim(u) > 0 else float(u)
        d_val = float(np.ravel(d)[0]) if np.ndim(d) > 0 else float(d)
        u_total = u_val + d_val

        dx = x[1]
        phi = x[2]
        dphi = x[3]
        theta = x[4]
        dtheta = x[5]

        dq = np.array([dx, dphi, dtheta])

        M_mat = self.mass_matrix(phi, theta)
        C_mat = self.coriolis_matrix(phi, theta, dphi, dtheta)
        G_vec = self.gravity_vector(phi, theta)

        U_vec = np.array([u_total, 0.0, 0.0])

        # Solve M * qddot = U - G - C*dq
        ddq = np.linalg.solve(M_mat, U_vec - G_vec - C_mat @ dq)

        return np.array([dx, ddq[0], dphi, ddq[1], dtheta, ddq[2]])

    def output(self, x):
        """Output y = [x_trolley, phi, theta]."""
        return np.array([x[0], x[2], x[4]])

    def get_default_x0(self):
        """Default: trolley at origin, both pendulums hanging straight down."""
        return np.zeros(6)

Functions

coriolis_matrix(phi, theta, dphi, dtheta)

Coriolis/centripetal matrix C(q, qdot) -- 3x3.

Source code in opensmc/plants/crane.py

def coriolis_matrix(self, phi, theta, dphi, dtheta):
    """Coriolis/centripetal matrix C(q, qdot) -- 3x3."""
    mh, mc = self.m1, self.m2
    l1, l2 = self.l1, self.l2

    sphi = np.sin(phi)
    stheta = np.sin(theta)
    spt = np.sin(phi - theta)

    C_mat = np.array([
        [self.b,
         (mh + mc) * l1 * sphi * dphi,
         mc * l2 * stheta * dtheta],
        [0.0,
         0.0,
         -mc * l1 * l2 * spt * dtheta],
        [0.0,
         mc * l1 * l2 * spt * dphi,
         0.0]
    ])
    return C_mat

dynamics(t, x, u, d=None)

Compute state derivatives.

Parameters

t : float Time. x : ndarray (6,) State [x_trolley, x_dot, phi, phi_dot, theta, theta_dot]. u : float Horizontal force on trolley [N]. d : float or None External disturbance (added to force). Default 0.

Returns

xdot : ndarray (6,)

Source code in opensmc/plants/crane.py

def dynamics(self, t, x, u, d=None):
    """Compute state derivatives.

    Parameters
    ----------
    t : float
        Time.
    x : ndarray (6,)
        State [x_trolley, x_dot, phi, phi_dot, theta, theta_dot].
    u : float
        Horizontal force on trolley [N].
    d : float or None
        External disturbance (added to force). Default 0.

    Returns
    -------
    xdot : ndarray (6,)
    """
    if d is None:
        d = 0.0
    u_val = float(np.ravel(u)[0]) if np.ndim(u) > 0 else float(u)
    d_val = float(np.ravel(d)[0]) if np.ndim(d) > 0 else float(d)
    u_total = u_val + d_val

    dx = x[1]
    phi = x[2]
    dphi = x[3]
    theta = x[4]
    dtheta = x[5]

    dq = np.array([dx, dphi, dtheta])

    M_mat = self.mass_matrix(phi, theta)
    C_mat = self.coriolis_matrix(phi, theta, dphi, dtheta)
    G_vec = self.gravity_vector(phi, theta)

    U_vec = np.array([u_total, 0.0, 0.0])

    # Solve M * qddot = U - G - C*dq
    ddq = np.linalg.solve(M_mat, U_vec - G_vec - C_mat @ dq)

    return np.array([dx, ddq[0], dphi, ddq[1], dtheta, ddq[2]])

get_default_x0()

Default: trolley at origin, both pendulums hanging straight down.

Source code in opensmc/plants/crane.py

def get_default_x0(self):
    """Default: trolley at origin, both pendulums hanging straight down."""
    return np.zeros(6)

get_dynamics_components(x)

Decompose into affine-in-u form for each generalized coordinate.

qddot = M^{-1} * ([u;0;0] - G - C*qdot)

At the current state, returns f_vec, b_vec such that: qddot = f_vec + b_vec * u

Returns

f_vec : ndarray (3,) Drift terms (gravity + Coriolis, no input). b_vec : ndarray (3,) Input gain column (first column of M^{-1}).

Source code in opensmc/plants/crane.py

def get_dynamics_components(self, x):
    """Decompose into affine-in-u form for each generalized coordinate.

    qddot = M^{-1} * ([u;0;0] - G - C*qdot)

    At the current state, returns f_vec, b_vec such that:
        qddot = f_vec + b_vec * u

    Returns
    -------
    f_vec : ndarray (3,)
        Drift terms (gravity + Coriolis, no input).
    b_vec : ndarray (3,)
        Input gain column (first column of M^{-1}).
    """
    dx = x[1]
    phi = x[2]
    dphi = x[3]
    theta = x[4]
    dtheta = x[5]

    dq = np.array([dx, dphi, dtheta])

    M_mat = self.mass_matrix(phi, theta)
    C_mat = self.coriolis_matrix(phi, theta, dphi, dtheta)
    G_vec = self.gravity_vector(phi, theta)

    M_inv = np.linalg.inv(M_mat)

    # f_vec = M^{-1} * (-G - C*dq)
    f_vec = M_inv @ (-G_vec - C_mat @ dq)

    # b_vec = M^{-1} * [1; 0; 0]
    b_vec = M_inv[:, 0]

    return f_vec, b_vec

gravity_vector(phi, theta)

Gravity vector G(q) -- (3,).

Source code in opensmc/plants/crane.py

def gravity_vector(self, phi, theta):
    """Gravity vector G(q) -- (3,)."""
    mh, mc = self.m1, self.m2
    l1, l2 = self.l1, self.l2
    g = self.g

    return np.array([
        0.0,
        (mh + mc) * g * l1 * np.sin(phi),
        mc * g * l2 * np.sin(theta)
    ])

mass_matrix(phi, theta)

Mass matrix M(q) -- 3x3, symmetric, positive definite.

Source code in opensmc/plants/crane.py

def mass_matrix(self, phi, theta):
    """Mass matrix M(q) -- 3x3, symmetric, positive definite."""
    M_t = self.M
    mh, mc = self.m1, self.m2
    l1, l2 = self.l1, self.l2

    cphi = np.cos(phi)
    ctheta = np.cos(theta)
    cpt = np.cos(phi - theta)

    M_mat = np.array([
        [M_t + mh + mc,
         -(mh + mc) * l1 * cphi,
         -mc * l2 * ctheta],
        [-(mh + mc) * l1 * cphi,
         (mh + mc) * l1**2,
         mc * l1 * l2 * cpt],
        [-mc * l2 * ctheta,
         mc * l1 * l2 * cpt,
         mc * l2**2]
    ])
    return M_mat

output(x)

Output y = [x_trolley, phi, theta].

Source code in opensmc/plants/crane.py

def output(self, x):
    """Output y = [x_trolley, phi, theta]."""
    return np.array([x[0], x[2], x[4]])

DualStageNanopositioner

Bases: Plant

Piezoelectric fine stage with two resonant modes.

x = [x1, x1_dot, x2, x2_dot]

• (x1, x1_dot): mode 1 displacement and velocity [m, m/s]
• (x2, x2_dot): mode 2 displacement and velocity [m, m/s]

Input: u (scalar voltage [V], clamped to [-u_max, u_max]) Output: y = x1 + x2 (total displacement [m])

Dynamics (per mode k)

x_k_ddot = -wn_k^2 * x_k - 2zeta_kwn_k * x_k_dot + Kp_k*wn_k^2 * u

Transfer function per mode: G_k(s) = Kp_k * wn_k^2 / (s^2 + 2zeta_kwn_k*s + wn_k^2)

Parameters

fn1 : float Mode 1 natural frequency [Hz] (default 780). fn2 : float Mode 2 natural frequency [Hz] (default 2340). zeta1 : float Mode 1 damping ratio (default 0.012). zeta2 : float Mode 2 damping ratio (default 0.008). Kp1 : float Mode 1 DC gain [m/V] (default 0.145e-6). Kp2 : float Mode 2 DC gain [m/V] (default 0.015e-6). u_max : float Maximum voltage magnitude [V] (default 20.0).

Source code in opensmc/plants/nanopositioner.py

class DualStageNanopositioner(Plant):
    """Piezoelectric fine stage with two resonant modes.

    States: x = [x1, x1_dot, x2, x2_dot]
        - (x1, x1_dot): mode 1 displacement and velocity [m, m/s]
        - (x2, x2_dot): mode 2 displacement and velocity [m, m/s]

    Input:  u (scalar voltage [V], clamped to [-u_max, u_max])
    Output: y = x1 + x2 (total displacement [m])

    Dynamics (per mode k)
    ---------------------
    x_k_ddot = -wn_k^2 * x_k - 2*zeta_k*wn_k * x_k_dot + Kp_k*wn_k^2 * u

    Transfer function per mode:
    G_k(s) = Kp_k * wn_k^2 / (s^2 + 2*zeta_k*wn_k*s + wn_k^2)

    Parameters
    ----------
    fn1 : float
        Mode 1 natural frequency [Hz] (default 780).
    fn2 : float
        Mode 2 natural frequency [Hz] (default 2340).
    zeta1 : float
        Mode 1 damping ratio (default 0.012).
    zeta2 : float
        Mode 2 damping ratio (default 0.008).
    Kp1 : float
        Mode 1 DC gain [m/V] (default 0.145e-6).
    Kp2 : float
        Mode 2 DC gain [m/V] (default 0.015e-6).
    u_max : float
        Maximum voltage magnitude [V] (default 20.0).
    """

    def __init__(self, fn1=780.0, fn2=2340.0, zeta1=0.012, zeta2=0.008,
                 Kp1=0.145e-6, Kp2=0.015e-6, u_max=20.0):
        super().__init__(n_states=4, n_inputs=1, n_outputs=1,
                         name="DualStageNanopositioner")
        self.fn1 = fn1
        self.fn2 = fn2
        self.zeta1 = zeta1
        self.zeta2 = zeta2
        self.Kp1 = Kp1
        self.Kp2 = Kp2
        self.u_max = u_max

        # Derived angular frequencies
        self.wn1 = 2.0 * np.pi * fn1
        self.wn2 = 2.0 * np.pi * fn2

    def dynamics(self, t, x, u, d=None):
        """Compute state derivatives.

        Parameters
        ----------
        t : float
            Time.
        x : ndarray (4,)
            State [x1, x1_dot, x2, x2_dot].
        u : float
            Input voltage [V] (clamped to [-u_max, u_max]).
        d : float or None
            Additive disturbance on voltage. Default 0.

        Returns
        -------
        xdot : ndarray (4,)
        """
        if d is None:
            d = 0.0
        u_val = float(np.ravel(u)[0]) if np.ndim(u) > 0 else float(u)
        d_val = float(np.ravel(d)[0]) if np.ndim(d) > 0 else float(d)
        u_clamped = np.clip(u_val + d_val, -self.u_max, self.u_max)

        wn1, wn2 = self.wn1, self.wn2
        z1, z2 = self.zeta1, self.zeta2
        Kp1, Kp2 = self.Kp1, self.Kp2

        x1, x1d, x2, x2d = x

        x1_ddot = -wn1**2 * x1 - 2 * z1 * wn1 * x1d + Kp1 * wn1**2 * u_clamped
        x2_ddot = -wn2**2 * x2 - 2 * z2 * wn2 * x2d + Kp2 * wn2**2 * u_clamped

        return np.array([x1d, x1_ddot, x2d, x2_ddot])

    def output(self, x):
        """Output y = x1 + x2 (total displacement)."""
        return np.array([x[0] + x[2]])

    def get_default_x0(self):
        """Default: zero displacement, zero velocity for both modes."""
        return np.zeros(4)

    def dc_gain(self):
        """Return the total DC gain Kp1 + Kp2 [m/V]."""
        return self.Kp1 + self.Kp2

    def state_space_matrices(self):
        """Return (A, B, C, D) state-space matrices.

        Returns
        -------
        A : ndarray (4, 4)
        B : ndarray (4, 1)
        C : ndarray (1, 4)
        D : ndarray (1, 1)
        """
        wn1, wn2 = self.wn1, self.wn2
        z1, z2 = self.zeta1, self.zeta2
        Kp1, Kp2 = self.Kp1, self.Kp2

        A = np.array([
            [0.0,         1.0,         0.0,         0.0],
            [-wn1**2,    -2*z1*wn1,    0.0,         0.0],
            [0.0,         0.0,         0.0,         1.0],
            [0.0,         0.0,        -wn2**2,     -2*z2*wn2]
        ])

        B = np.array([
            [0.0],
            [Kp1 * wn1**2],
            [0.0],
            [Kp2 * wn2**2]
        ])

        C = np.array([[1.0, 0.0, 1.0, 0.0]])

        D = np.array([[0.0]])

        return A, B, C, D

Functions

dc_gain()

Return the total DC gain Kp1 + Kp2 [m/V].

Source code in opensmc/plants/nanopositioner.py

def dc_gain(self):
    """Return the total DC gain Kp1 + Kp2 [m/V]."""
    return self.Kp1 + self.Kp2

dynamics(t, x, u, d=None)

Compute state derivatives.

Parameters

t : float Time. x : ndarray (4,) State [x1, x1_dot, x2, x2_dot]. u : float Input voltage [V] (clamped to [-u_max, u_max]). d : float or None Additive disturbance on voltage. Default 0.

Returns

xdot : ndarray (4,)

Source code in opensmc/plants/nanopositioner.py

def dynamics(self, t, x, u, d=None):
    """Compute state derivatives.

    Parameters
    ----------
    t : float
        Time.
    x : ndarray (4,)
        State [x1, x1_dot, x2, x2_dot].
    u : float
        Input voltage [V] (clamped to [-u_max, u_max]).
    d : float or None
        Additive disturbance on voltage. Default 0.

    Returns
    -------
    xdot : ndarray (4,)
    """
    if d is None:
        d = 0.0
    u_val = float(np.ravel(u)[0]) if np.ndim(u) > 0 else float(u)
    d_val = float(np.ravel(d)[0]) if np.ndim(d) > 0 else float(d)
    u_clamped = np.clip(u_val + d_val, -self.u_max, self.u_max)

    wn1, wn2 = self.wn1, self.wn2
    z1, z2 = self.zeta1, self.zeta2
    Kp1, Kp2 = self.Kp1, self.Kp2

    x1, x1d, x2, x2d = x

    x1_ddot = -wn1**2 * x1 - 2 * z1 * wn1 * x1d + Kp1 * wn1**2 * u_clamped
    x2_ddot = -wn2**2 * x2 - 2 * z2 * wn2 * x2d + Kp2 * wn2**2 * u_clamped

    return np.array([x1d, x1_ddot, x2d, x2_ddot])

get_default_x0()

Default: zero displacement, zero velocity for both modes.

Source code in opensmc/plants/nanopositioner.py

def get_default_x0(self):
    """Default: zero displacement, zero velocity for both modes."""
    return np.zeros(4)

output(x)

Output y = x1 + x2 (total displacement).

Source code in opensmc/plants/nanopositioner.py

def output(self, x):
    """Output y = x1 + x2 (total displacement)."""
    return np.array([x[0] + x[2]])

state_space_matrices()

Return (A, B, C, D) state-space matrices.

Returns

A : ndarray (4, 4) B : ndarray (4, 1) C : ndarray (1, 4) D : ndarray (1, 1)

Source code in opensmc/plants/nanopositioner.py

def state_space_matrices(self):
    """Return (A, B, C, D) state-space matrices.

    Returns
    -------
    A : ndarray (4, 4)
    B : ndarray (4, 1)
    C : ndarray (1, 4)
    D : ndarray (1, 1)
    """
    wn1, wn2 = self.wn1, self.wn2
    z1, z2 = self.zeta1, self.zeta2
    Kp1, Kp2 = self.Kp1, self.Kp2

    A = np.array([
        [0.0,         1.0,         0.0,         0.0],
        [-wn1**2,    -2*z1*wn1,    0.0,         0.0],
        [0.0,         0.0,         0.0,         1.0],
        [0.0,         0.0,        -wn2**2,     -2*z2*wn2]
    ])

    B = np.array([
        [0.0],
        [Kp1 * wn1**2],
        [0.0],
        [Kp2 * wn2**2]
    ])

    C = np.array([[1.0, 0.0, 1.0, 0.0]])

    D = np.array([[0.0]])

    return A, B, C, D

InvertedPendulum

Bases: Plant

Cart-pole inverted pendulum with 4 states.

States: x = [x_cart, x_cart_dot, theta, theta_dot] Input: u (scalar horizontal force on cart) Output: y = [x_cart, theta]

Dynamics

M_total = M + m - m * cos^2(theta)

x_cart_ddot = (u - bx_cart_dot + mltheta_dot^2sin(theta) - mgsin(theta)*cos(theta)) / M_total

theta_ddot = (-ucos(theta) + bx_cart_dotcos(theta) - mltheta_dot^2sin(theta)cos(theta) + (M+m)g*sin(theta)) / (l * M_total)

Parameters

M : float Cart mass [kg] (default 1.0). m : float Pendulum mass [kg] (default 0.1). l : float Pendulum half-length [m] (default 0.5). g : float Gravitational acceleration [m/s^2] (default 9.81). b : float Cart friction coefficient (default 0.1).

Source code in opensmc/plants/inverted_pendulum.py

class InvertedPendulum(Plant):
    """Cart-pole inverted pendulum with 4 states.

    States: x = [x_cart, x_cart_dot, theta, theta_dot]
    Input:  u (scalar horizontal force on cart)
    Output: y = [x_cart, theta]

    Dynamics
    --------
    M_total = M + m - m * cos^2(theta)

    x_cart_ddot = (u - b*x_cart_dot + m*l*theta_dot^2*sin(theta)
                   - m*g*sin(theta)*cos(theta)) / M_total

    theta_ddot  = (-u*cos(theta) + b*x_cart_dot*cos(theta)
                   - m*l*theta_dot^2*sin(theta)*cos(theta)
                   + (M+m)*g*sin(theta)) / (l * M_total)

    Parameters
    ----------
    M : float
        Cart mass [kg] (default 1.0).
    m : float
        Pendulum mass [kg] (default 0.1).
    l : float
        Pendulum half-length [m] (default 0.5).
    g : float
        Gravitational acceleration [m/s^2] (default 9.81).
    b : float
        Cart friction coefficient (default 0.1).
    """

    def __init__(self, M=1.0, m=0.1, l=0.5, g=9.81, b=0.1):
        super().__init__(n_states=4, n_inputs=1, n_outputs=2,
                         name="InvertedPendulum")
        self.M = M
        self.m = m
        self.l = l
        self.g = g
        self.b = b

    def dynamics(self, t, x, u, d=None):
        """Compute state derivatives.

        Parameters
        ----------
        t : float
        x : ndarray (4,)
        u : float or ndarray
        d : float or ndarray or None

        Returns
        -------
        xdot : ndarray (4,)
        """
        if d is None:
            d = 0.0

        # Robust scalar conversion
        u_val = float(np.ravel(u)[0]) if np.ndim(u) > 0 else float(u)

        if np.ndim(d) > 0:
            # If d is size 4, it acts on states. For inverted pendulum, 
            # disturbance usually acts on the force (matched) or theta_ddot.
            # We assume it matches u if size 1, else we pick the force-equivalent component.
            # Usually d[1] is xc_ddot and d[3] is th_ddot.
            # If it's matched disturbance from simulator padding, it's at index 0.
            d_val = float(d[0])
        else:
            d_val = float(d)

        u_total = u_val + d_val

        xc, xcd, th, thd = x
        M, m, l, g, b = self.M, self.m, self.l, self.g, self.b

        cth = np.cos(th)
        sth = np.sin(th)
        M_tot = M + m - m * cth**2

        xc_ddot = (u_total - b * xcd + m * l * thd**2 * sth
                   - m * g * sth * cth) / M_tot

        th_ddot = (-u_total * cth + b * xcd * cth
                   - m * l * thd**2 * sth * cth
                   + (M + m) * g * sth) / (l * M_tot)

        return np.array([xcd, xc_ddot, thd, th_ddot])

    def output(self, x):
        """Output y = [x_cart, theta]."""
        return np.array([x[0], x[2]])

    def get_default_x0(self):
        """Default: cart at origin, pendulum inverted upright (theta=0)."""
        return np.zeros(4)

Functions

dynamics(t, x, u, d=None)

Compute state derivatives.

Parameters

t : float x : ndarray (4,) u : float or ndarray d : float or ndarray or None

Returns

xdot : ndarray (4,)

Source code in opensmc/plants/inverted_pendulum.py

def dynamics(self, t, x, u, d=None):
    """Compute state derivatives.

    Parameters
    ----------
    t : float
    x : ndarray (4,)
    u : float or ndarray
    d : float or ndarray or None

    Returns
    -------
    xdot : ndarray (4,)
    """
    if d is None:
        d = 0.0

    # Robust scalar conversion
    u_val = float(np.ravel(u)[0]) if np.ndim(u) > 0 else float(u)

    if np.ndim(d) > 0:
        # If d is size 4, it acts on states. For inverted pendulum, 
        # disturbance usually acts on the force (matched) or theta_ddot.
        # We assume it matches u if size 1, else we pick the force-equivalent component.
        # Usually d[1] is xc_ddot and d[3] is th_ddot.
        # If it's matched disturbance from simulator padding, it's at index 0.
        d_val = float(d[0])
    else:
        d_val = float(d)

    u_total = u_val + d_val

    xc, xcd, th, thd = x
    M, m, l, g, b = self.M, self.m, self.l, self.g, self.b

    cth = np.cos(th)
    sth = np.sin(th)
    M_tot = M + m - m * cth**2

    xc_ddot = (u_total - b * xcd + m * l * thd**2 * sth
               - m * g * sth * cth) / M_tot

    th_ddot = (-u_total * cth + b * xcd * cth
               - m * l * thd**2 * sth * cth
               + (M + m) * g * sth) / (l * M_tot)

    return np.array([xcd, xc_ddot, thd, th_ddot])

get_default_x0()

Default: cart at origin, pendulum inverted upright (theta=0).

Source code in opensmc/plants/inverted_pendulum.py

def get_default_x0(self):
    """Default: cart at origin, pendulum inverted upright (theta=0)."""
    return np.zeros(4)

output(x)

Output y = [x_cart, theta].

Source code in opensmc/plants/inverted_pendulum.py

def output(self, x):
    """Output y = [x_cart, theta]."""
    return np.array([x[0], x[2]])

PMSM

Bases: Plant

Surface-mounted PMSM in dq-frame with 4 states.

x = [i_d, i_q, omega, theta]

• i_d, i_q: d-axis and q-axis stator currents [A]
• omega: electrical rotor speed [rad/s]
• theta: electrical rotor position [rad]

Inputs: u = [v_d, v_q] (dq-frame voltages [V], clamped to [-V_max, V_max]) Outputs: y = [omega, theta] (speed and position)

Dynamics

L_d * di_d/dt = v_d - R_si_d + pomegaL_qi_q L_q * di_q/dt = v_q - R_si_q - pomega(L_di_d + psi_f) J * domega/dt = T_e - B*omega - T_L dtheta/dt = omega

Electromagnetic torque: T_e = (3/2) * p * (psi_fi_q + (L_d - L_q)i_di_q) For surface-mounted (L_d = L_q): T_e = (3/2)ppsi_fi_q

Parameters

Rs : float Stator resistance [Ohm] (default 1.2). Ld : float d-axis inductance [H] (default 6.8e-3). Lq : float q-axis inductance [H] (default 6.8e-3). psi_f : float PM flux linkage [Wb] (default 0.175). pp : int Number of pole pairs (default 4). J : float Rotor inertia [kgm^2] (default 0.003). B : float Viscous friction coefficient [Nms/rad] (default 0.001). TL : float Nominal load torque [Nm] (default 0.0). V_max : float DC bus voltage limit [V] (default 48.0).

Source code in opensmc/plants/pmsm.py

class PMSM(Plant):
    """Surface-mounted PMSM in dq-frame with 4 states.

    States: x = [i_d, i_q, omega, theta]
        - i_d, i_q: d-axis and q-axis stator currents [A]
        - omega: electrical rotor speed [rad/s]
        - theta: electrical rotor position [rad]

    Inputs: u = [v_d, v_q] (dq-frame voltages [V], clamped to [-V_max, V_max])
    Outputs: y = [omega, theta] (speed and position)

    Dynamics
    --------
    L_d * di_d/dt = v_d - R_s*i_d + p*omega*L_q*i_q
    L_q * di_q/dt = v_q - R_s*i_q - p*omega*(L_d*i_d + psi_f)
    J * domega/dt = T_e - B*omega - T_L
    dtheta/dt = omega

    Electromagnetic torque:
    T_e = (3/2) * p * (psi_f*i_q + (L_d - L_q)*i_d*i_q)
    For surface-mounted (L_d = L_q): T_e = (3/2)*p*psi_f*i_q

    Parameters
    ----------
    Rs : float
        Stator resistance [Ohm] (default 1.2).
    Ld : float
        d-axis inductance [H] (default 6.8e-3).
    Lq : float
        q-axis inductance [H] (default 6.8e-3).
    psi_f : float
        PM flux linkage [Wb] (default 0.175).
    pp : int
        Number of pole pairs (default 4).
    J : float
        Rotor inertia [kg*m^2] (default 0.003).
    B : float
        Viscous friction coefficient [N*m*s/rad] (default 0.001).
    TL : float
        Nominal load torque [N*m] (default 0.0).
    V_max : float
        DC bus voltage limit [V] (default 48.0).
    """

    def __init__(self, Rs=1.2, Ld=6.8e-3, Lq=6.8e-3, psi_f=0.175,
                 pp=4, J=0.003, B=0.001, TL=0.0, V_max=48.0):
        super().__init__(n_states=4, n_inputs=2, n_outputs=2,
                         name="PMSM")
        self.Rs = Rs
        self.Ld = Ld
        self.Lq = Lq
        self.psi_f = psi_f
        self.pp = pp
        self.J = J
        self.B = B
        self.TL = TL
        self.V_max = V_max

    def electromagnetic_torque(self, i_d, i_q):
        """Compute electromagnetic torque T_e.

        Parameters
        ----------
        i_d, i_q : float
            dq-axis currents [A].

        Returns
        -------
        T_e : float
            Electromagnetic torque [N*m].
        """
        return 1.5 * self.pp * (self.psi_f * i_q
                                + (self.Ld - self.Lq) * i_d * i_q)

    def torque_constant(self):
        """Return torque constant Kt = 1.5 * pp * psi_f [N*m/A].

        Valid for surface-mounted PMSM (Ld = Lq).
        """
        return 1.5 * self.pp * self.psi_f

    def electrical_time_constant(self):
        """Return electrical time constant L/R [s]."""
        return self.Ld / self.Rs

    def dynamics(self, t, x, u, d=None):
        """Compute state derivatives.

        Parameters
        ----------
        t : float
            Time.
        x : ndarray (4,)
            State [i_d, i_q, omega, theta].
        u : ndarray (2,)
            dq-frame voltages [v_d, v_q] [V].
        d : float or None
            Load torque disturbance (added to TL). Default 0.

        Returns
        -------
        xdot : ndarray (4,)
        """
        u = np.asarray(u, dtype=float).ravel()
        if len(u) < 2:
            u = np.concatenate([u, np.zeros(2 - len(u))])
        if d is None:
            d_val = 0.0
        else:
            d_val = float(d) if np.ndim(d) == 0 else float(np.asarray(d).ravel()[0])

        i_d, i_q, omega, theta = x

        # Clamp voltages to DC bus limit
        vd = np.clip(u[0], -self.V_max, self.V_max)
        vq = np.clip(u[1], -self.V_max, self.V_max)

        Rs = self.Rs
        Ld, Lq = self.Ld, self.Lq
        psi_f = self.psi_f
        pp = self.pp
        J, B = self.J, self.B
        TL = self.TL + d_val

        # Electrical dynamics
        did_dt = (vd - Rs * i_d + pp * omega * Lq * i_q) / Ld
        diq_dt = (vq - Rs * i_q - pp * omega * (Ld * i_d + psi_f)) / Lq

        # Electromagnetic torque
        Te = self.electromagnetic_torque(i_d, i_q)

        # Mechanical dynamics
        domega_dt = (Te - B * omega - TL) / J
        dtheta_dt = omega

        return np.array([did_dt, diq_dt, domega_dt, dtheta_dt])

    def output(self, x):
        """Output y = [omega, theta]."""
        return np.array([x[2], x[3]])

    def get_default_x0(self):
        """Default: zero currents, at rest."""
        return np.zeros(4)

Functions

dynamics(t, x, u, d=None)

Compute state derivatives.

Parameters

t : float Time. x : ndarray (4,) State [i_d, i_q, omega, theta]. u : ndarray (2,) dq-frame voltages [v_d, v_q][V]. d : float or None Load torque disturbance (added to TL). Default 0.

Returns

xdot : ndarray (4,)

Source code in opensmc/plants/pmsm.py

def dynamics(self, t, x, u, d=None):
    """Compute state derivatives.

    Parameters
    ----------
    t : float
        Time.
    x : ndarray (4,)
        State [i_d, i_q, omega, theta].
    u : ndarray (2,)
        dq-frame voltages [v_d, v_q] [V].
    d : float or None
        Load torque disturbance (added to TL). Default 0.

    Returns
    -------
    xdot : ndarray (4,)
    """
    u = np.asarray(u, dtype=float).ravel()
    if len(u) < 2:
        u = np.concatenate([u, np.zeros(2 - len(u))])
    if d is None:
        d_val = 0.0
    else:
        d_val = float(d) if np.ndim(d) == 0 else float(np.asarray(d).ravel()[0])

    i_d, i_q, omega, theta = x

    # Clamp voltages to DC bus limit
    vd = np.clip(u[0], -self.V_max, self.V_max)
    vq = np.clip(u[1], -self.V_max, self.V_max)

    Rs = self.Rs
    Ld, Lq = self.Ld, self.Lq
    psi_f = self.psi_f
    pp = self.pp
    J, B = self.J, self.B
    TL = self.TL + d_val

    # Electrical dynamics
    did_dt = (vd - Rs * i_d + pp * omega * Lq * i_q) / Ld
    diq_dt = (vq - Rs * i_q - pp * omega * (Ld * i_d + psi_f)) / Lq

    # Electromagnetic torque
    Te = self.electromagnetic_torque(i_d, i_q)

    # Mechanical dynamics
    domega_dt = (Te - B * omega - TL) / J
    dtheta_dt = omega

    return np.array([did_dt, diq_dt, domega_dt, dtheta_dt])

electrical_time_constant()

Return electrical time constant L/R [s].

Source code in opensmc/plants/pmsm.py

def electrical_time_constant(self):
    """Return electrical time constant L/R [s]."""
    return self.Ld / self.Rs

electromagnetic_torque(i_d, i_q)

Compute electromagnetic torque T_e.

Parameters

i_d, i_q : float dq-axis currents [A].

Returns

T_e : float Electromagnetic torque [N*m].

Source code in opensmc/plants/pmsm.py

def electromagnetic_torque(self, i_d, i_q):
    """Compute electromagnetic torque T_e.

    Parameters
    ----------
    i_d, i_q : float
        dq-axis currents [A].

    Returns
    -------
    T_e : float
        Electromagnetic torque [N*m].
    """
    return 1.5 * self.pp * (self.psi_f * i_q
                            + (self.Ld - self.Lq) * i_d * i_q)

get_default_x0()

Default: zero currents, at rest.

Source code in opensmc/plants/pmsm.py

def get_default_x0(self):
    """Default: zero currents, at rest."""
    return np.zeros(4)

output(x)

Output y = [omega, theta].

Source code in opensmc/plants/pmsm.py

def output(self, x):
    """Output y = [omega, theta]."""
    return np.array([x[2], x[3]])

torque_constant()

Return torque constant Kt = 1.5 * pp * psi_f [N*m/A].

Valid for surface-mounted PMSM (Ld = Lq).

Source code in opensmc/plants/pmsm.py

def torque_constant(self):
    """Return torque constant Kt = 1.5 * pp * psi_f [N*m/A].

    Valid for surface-mounted PMSM (Ld = Lq).
    """
    return 1.5 * self.pp * self.psi_f

Quadrotor6DOF

Bases: Plant

12-state rigid-body quadrotor with Newton-Euler dynamics.

x = [x, y, z, phi, theta, psi, vx, vy, vz, p, q, r]

• (x, y, z): inertial position
• (phi, theta, psi): Euler angles (roll, pitch, yaw)
• (vx, vy, vz): inertial velocity
• (p, q, r): body angular rates

u = [U1, U2, U3, U4]

• U1: total thrust [N]
• U2: roll torque [N*m]
• U3: pitch torque [N*m]
• U4: yaw torque [N*m]

Outputs: y = [x, y, z, phi, theta, psi]

Translational dynamics

ax = (cos(phi)sin(theta)cos(psi) + sin(phi)sin(psi)) * U1/m ay = (cos(phi)sin(theta)sin(psi) - sin(phi)cos(psi)) * U1/m az = -g + cos(phi)*cos(theta) * U1/m

Rotational dynamics (Euler equations)

pdot = ((Iyy - Izz)qr + U2) / Ixx qdot = ((Izz - Ixx)pr + U3) / Iyy rdot = ((Ixx - Iyy)pq + U4) / Izz

Euler angle kinematics

phi_dot = p + qsin(phi)tan(theta) + rcos(phi)tan(theta) theta_dot = qcos(phi) - rsin(phi) psi_dot = (qsin(phi) + rcos(phi)) / cos(theta)

Parameters

mass : float Total mass [kg] (default 0.468). g : float Gravitational acceleration [m/s^2] (default 9.81). Ixx : float Roll moment of inertia [kgm^2] (default 4.856e-3). Iyy : float Pitch moment of inertia [kgm^2] (default 4.856e-3). Izz : float Yaw moment of inertia [kg*m^2] (default 8.801e-3). arm_length : float Motor arm length [m] (default 0.225).

Source code in opensmc/plants/quadrotor.py

class Quadrotor6DOF(Plant):
    """12-state rigid-body quadrotor with Newton-Euler dynamics.

    States: x = [x, y, z, phi, theta, psi, vx, vy, vz, p, q, r]
        - (x, y, z): inertial position
        - (phi, theta, psi): Euler angles (roll, pitch, yaw)
        - (vx, vy, vz): inertial velocity
        - (p, q, r): body angular rates

    Inputs: u = [U1, U2, U3, U4]
        - U1: total thrust [N]
        - U2: roll torque [N*m]
        - U3: pitch torque [N*m]
        - U4: yaw torque [N*m]

    Outputs: y = [x, y, z, phi, theta, psi]

    Translational dynamics
    ----------------------
    ax = (cos(phi)*sin(theta)*cos(psi) + sin(phi)*sin(psi)) * U1/m
    ay = (cos(phi)*sin(theta)*sin(psi) - sin(phi)*cos(psi)) * U1/m
    az = -g + cos(phi)*cos(theta) * U1/m

    Rotational dynamics (Euler equations)
    -------------------------------------
    pdot = ((Iyy - Izz)*q*r + U2) / Ixx
    qdot = ((Izz - Ixx)*p*r + U3) / Iyy
    rdot = ((Ixx - Iyy)*p*q + U4) / Izz

    Euler angle kinematics
    ----------------------
    phi_dot   = p + q*sin(phi)*tan(theta) + r*cos(phi)*tan(theta)
    theta_dot = q*cos(phi) - r*sin(phi)
    psi_dot   = (q*sin(phi) + r*cos(phi)) / cos(theta)

    Parameters
    ----------
    mass : float
        Total mass [kg] (default 0.468).
    g : float
        Gravitational acceleration [m/s^2] (default 9.81).
    Ixx : float
        Roll moment of inertia [kg*m^2] (default 4.856e-3).
    Iyy : float
        Pitch moment of inertia [kg*m^2] (default 4.856e-3).
    Izz : float
        Yaw moment of inertia [kg*m^2] (default 8.801e-3).
    arm_length : float
        Motor arm length [m] (default 0.225).
    """

    def __init__(self, mass=0.468, g=9.81, Ixx=4.856e-3, Iyy=4.856e-3,
                 Izz=8.801e-3, arm_length=0.225):
        super().__init__(n_states=12, n_inputs=4, n_outputs=6,
                         name="Quadrotor6DOF")
        self.mass = mass
        self.g = g
        self.Ixx = Ixx
        self.Iyy = Iyy
        self.Izz = Izz
        self.arm_length = arm_length

    def dynamics(self, t, x, u, d=None):
        """Compute state derivatives.

        Parameters
        ----------
        t : float
            Time.
        x : ndarray (12,)
            State vector.
        u : ndarray (4,)
            Inputs [U1, U2, U3, U4].
        d : ndarray (12,) or None
            Additive disturbance on state derivatives. Default 0.

        Returns
        -------
        xdot : ndarray (12,)
        """
        u = np.asarray(u, dtype=float).ravel()
        if len(u) < 4:
            u = np.concatenate([u, np.zeros(4 - len(u))])
        if d is None:
            d = np.zeros(12)
        else:
            d = np.asarray(d, dtype=float).ravel()
            if len(d) < 12:
                d = np.concatenate([d, np.zeros(12 - len(d))])

        # Unpack state
        phi, theta, psi = x[3], x[4], x[5]
        vx, vy, vz = x[6], x[7], x[8]
        p, q, r = x[9], x[10], x[11]

        # Unpack inputs
        U1, U2, U3, U4 = u[0], u[1], u[2], u[3]

        # Trig
        cphi = np.cos(phi);   sphi = np.sin(phi)
        cth  = np.cos(theta); sth  = np.sin(theta)
        cpsi = np.cos(psi);   spsi = np.sin(psi)
        tth  = np.tan(theta)

        mass = self.mass
        g = self.g
        Ixx, Iyy, Izz = self.Ixx, self.Iyy, self.Izz

        # Translational accelerations
        ax = (cphi * sth * cpsi + sphi * spsi) * U1 / mass
        ay = (cphi * sth * spsi - sphi * cpsi) * U1 / mass
        az = -g + cphi * cth * U1 / mass

        # Rotational dynamics (Euler equations)
        pdot = ((Iyy - Izz) * q * r + U2) / Ixx
        qdot = ((Izz - Ixx) * p * r + U3) / Iyy
        rdot = ((Ixx - Iyy) * p * q + U4) / Izz

        # Euler angle kinematics
        phi_dot = p + q * sphi * tth + r * cphi * tth
        theta_dot = q * cphi - r * sphi
        psi_dot = (q * sphi + r * cphi) / cth

        xdot = np.array([
            vx, vy, vz,
            phi_dot, theta_dot, psi_dot,
            ax, ay, az,
            pdot, qdot, rdot
        ])

        return xdot + d

    def output(self, x):
        """Output y = [x, y, z, phi, theta, psi]."""
        return x[:6]

    def get_default_x0(self):
        """Default: hover at origin with zero attitude and velocity."""
        return np.zeros(12)

    def hover_input(self):
        """Return the input that maintains hover equilibrium.

        Returns
        -------
        u_hover : ndarray (4,)
            [mass*g, 0, 0, 0]
        """
        return np.array([self.mass * self.g, 0.0, 0.0, 0.0])

Functions

dynamics(t, x, u, d=None)

Compute state derivatives.

Parameters

t : float Time. x : ndarray (12,) State vector. u : ndarray (4,) Inputs [U1, U2, U3, U4]. d : ndarray (12,) or None Additive disturbance on state derivatives. Default 0.

Returns

xdot : ndarray (12,)

Source code in opensmc/plants/quadrotor.py

def dynamics(self, t, x, u, d=None):
    """Compute state derivatives.

    Parameters
    ----------
    t : float
        Time.
    x : ndarray (12,)
        State vector.
    u : ndarray (4,)
        Inputs [U1, U2, U3, U4].
    d : ndarray (12,) or None
        Additive disturbance on state derivatives. Default 0.

    Returns
    -------
    xdot : ndarray (12,)
    """
    u = np.asarray(u, dtype=float).ravel()
    if len(u) < 4:
        u = np.concatenate([u, np.zeros(4 - len(u))])
    if d is None:
        d = np.zeros(12)
    else:
        d = np.asarray(d, dtype=float).ravel()
        if len(d) < 12:
            d = np.concatenate([d, np.zeros(12 - len(d))])

    # Unpack state
    phi, theta, psi = x[3], x[4], x[5]
    vx, vy, vz = x[6], x[7], x[8]
    p, q, r = x[9], x[10], x[11]

    # Unpack inputs
    U1, U2, U3, U4 = u[0], u[1], u[2], u[3]

    # Trig
    cphi = np.cos(phi);   sphi = np.sin(phi)
    cth  = np.cos(theta); sth  = np.sin(theta)
    cpsi = np.cos(psi);   spsi = np.sin(psi)
    tth  = np.tan(theta)

    mass = self.mass
    g = self.g
    Ixx, Iyy, Izz = self.Ixx, self.Iyy, self.Izz

    # Translational accelerations
    ax = (cphi * sth * cpsi + sphi * spsi) * U1 / mass
    ay = (cphi * sth * spsi - sphi * cpsi) * U1 / mass
    az = -g + cphi * cth * U1 / mass

    # Rotational dynamics (Euler equations)
    pdot = ((Iyy - Izz) * q * r + U2) / Ixx
    qdot = ((Izz - Ixx) * p * r + U3) / Iyy
    rdot = ((Ixx - Iyy) * p * q + U4) / Izz

    # Euler angle kinematics
    phi_dot = p + q * sphi * tth + r * cphi * tth
    theta_dot = q * cphi - r * sphi
    psi_dot = (q * sphi + r * cphi) / cth

    xdot = np.array([
        vx, vy, vz,
        phi_dot, theta_dot, psi_dot,
        ax, ay, az,
        pdot, qdot, rdot
    ])

    return xdot + d

get_default_x0()

Default: hover at origin with zero attitude and velocity.

Source code in opensmc/plants/quadrotor.py

def get_default_x0(self):
    """Default: hover at origin with zero attitude and velocity."""
    return np.zeros(12)

hover_input()

Return the input that maintains hover equilibrium.

Returns

u_hover : ndarray (4,) [mass*g, 0, 0, 0]

Source code in opensmc/plants/quadrotor.py

def hover_input(self):
    """Return the input that maintains hover equilibrium.

    Returns
    -------
    u_hover : ndarray (4,)
        [mass*g, 0, 0, 0]
    """
    return np.array([self.mass * self.g, 0.0, 0.0, 0.0])

output(x)

Output y = [x, y, z, phi, theta, psi].

Source code in opensmc/plants/quadrotor.py

def output(self, x):
    """Output y = [x, y, z, phi, theta, psi]."""
    return x[:6]

SinglePendulumCrane

Bases: Plant

Single-pendulum overhead crane (Qian & Yi 2015, Eq. 2.16).

States: x = [x_trolley, x_trolley_dot, phi, phi_dot] Input: u (scalar horizontal force on trolley) [N] Output: y = [x_trolley, phi]

Dynamics

A_ = M + m, B_ = m * l * cos(phi) den1 = A_ * l - B_ * cos(phi) den2 = B_ * cos(phi) - A_ * l (= -den1)

x_trolley_ddot = ((u + mlphi_dot^2*sin(phi)) * l + g * B_ * sin(phi)) / den1

phi_ddot = ((u + mlphi_dot^2*sin(phi)) * cos(phi) + g * A_ * sin(phi)) / den2

Parameters

M : float Trolley mass [kg] (default 1.0). m : float Payload mass [kg] (default 0.8). l : float Cable length [m] (default 0.305). g : float Gravitational acceleration [m/s^2] (default 9.81).

Source code in opensmc/plants/crane.py

class SinglePendulumCrane(Plant):
    """Single-pendulum overhead crane (Qian & Yi 2015, Eq. 2.16).

    States: x = [x_trolley, x_trolley_dot, phi, phi_dot]
    Input:  u (scalar horizontal force on trolley) [N]
    Output: y = [x_trolley, phi]

    Dynamics
    --------
    A_ = M + m,  B_ = m * l * cos(phi)
    den1 = A_ * l - B_ * cos(phi)
    den2 = B_ * cos(phi) - A_ * l  (= -den1)

    x_trolley_ddot = ((u + m*l*phi_dot^2*sin(phi)) * l
                      + g * B_ * sin(phi)) / den1

    phi_ddot = ((u + m*l*phi_dot^2*sin(phi)) * cos(phi)
                + g * A_ * sin(phi)) / den2

    Parameters
    ----------
    M : float
        Trolley mass [kg] (default 1.0).
    m : float
        Payload mass [kg] (default 0.8).
    l : float
        Cable length [m] (default 0.305).
    g : float
        Gravitational acceleration [m/s^2] (default 9.81).
    """

    def __init__(self, M=1.0, m=0.8, l=0.305, g=9.81):
        super().__init__(n_states=4, n_inputs=1, n_outputs=2,
                         name="SinglePendulumCrane")
        self.M = M
        self.m = m
        self.l = l
        self.g = g

    def dynamics(self, t, x, u, d=None):
        """Compute state derivatives.

        Parameters
        ----------
        t : float
            Time.
        x : ndarray (4,)
            State [x_trolley, x_trolley_dot, phi, phi_dot].
        u : float
            Horizontal force on trolley [N].
        d : float or None
            External disturbance (added to force). Default 0.

        Returns
        -------
        xdot : ndarray (4,)
        """
        if d is None:
            d = 0.0
        u_val = float(np.ravel(u)[0]) if np.ndim(u) > 0 else float(u)
        d_val = float(np.ravel(d)[0]) if np.ndim(d) > 0 else float(d)
        u_total = u_val + d_val

        dx = x[1]
        phi = x[2]
        dphi = x[3]

        M, m, l, g = self.M, self.m, self.l, self.g

        st = np.sin(phi)
        ct = np.cos(phi)

        A_ = M + m
        B_ = m * l * ct

        den1 = A_ * l - B_ * ct      # (M+m)*l - m*l*cos^2(phi)
        den2 = B_ * ct - A_ * l      # -den1

        xddot = ((u_total + m * l * dphi**2 * st) * l
                 + g * B_ * st) / den1

        phiddot = ((u_total + m * l * dphi**2 * st) * ct
                   + g * A_ * st) / den2

        return np.array([dx, xddot, dphi, phiddot])

    def get_dynamics_components(self, x):
        """Decompose dynamics into affine-in-u form.

        x_ddot   = f1(x) + b1(x) * u
        phi_ddot = f2(x) + b2(x) * u

        Returns
        -------
        f1, b1, f2, b2 : float
            Drift and input-gain components.
        """
        phi = x[2]
        dphi = x[3]

        M, m, l, g = self.M, self.m, self.l, self.g

        st = np.sin(phi)
        ct = np.cos(phi)

        A_ = M + m
        B_ = m * l * ct
        D_ = m * l * st

        den1 = A_ * l - B_ * ct
        den2 = B_ * ct - A_ * l

        f1 = (D_ * dphi**2 * l + g * B_ * st) / den1
        b1 = l / den1

        f2 = (D_ * dphi**2 * ct + g * A_ * st) / den2
        b2 = ct / den2

        return f1, b1, f2, b2

    def output(self, x):
        """Output y = [x_trolley, phi]."""
        return np.array([x[0], x[2]])

    def get_default_x0(self):
        """Default: trolley at origin, no swing."""
        return np.zeros(4)

Functions

dynamics(t, x, u, d=None)

Compute state derivatives.

Parameters

t : float Time. x : ndarray (4,) State [x_trolley, x_trolley_dot, phi, phi_dot]. u : float Horizontal force on trolley [N]. d : float or None External disturbance (added to force). Default 0.

Returns

xdot : ndarray (4,)

Source code in opensmc/plants/crane.py

def dynamics(self, t, x, u, d=None):
    """Compute state derivatives.

    Parameters
    ----------
    t : float
        Time.
    x : ndarray (4,)
        State [x_trolley, x_trolley_dot, phi, phi_dot].
    u : float
        Horizontal force on trolley [N].
    d : float or None
        External disturbance (added to force). Default 0.

    Returns
    -------
    xdot : ndarray (4,)
    """
    if d is None:
        d = 0.0
    u_val = float(np.ravel(u)[0]) if np.ndim(u) > 0 else float(u)
    d_val = float(np.ravel(d)[0]) if np.ndim(d) > 0 else float(d)
    u_total = u_val + d_val

    dx = x[1]
    phi = x[2]
    dphi = x[3]

    M, m, l, g = self.M, self.m, self.l, self.g

    st = np.sin(phi)
    ct = np.cos(phi)

    A_ = M + m
    B_ = m * l * ct

    den1 = A_ * l - B_ * ct      # (M+m)*l - m*l*cos^2(phi)
    den2 = B_ * ct - A_ * l      # -den1

    xddot = ((u_total + m * l * dphi**2 * st) * l
             + g * B_ * st) / den1

    phiddot = ((u_total + m * l * dphi**2 * st) * ct
               + g * A_ * st) / den2

    return np.array([dx, xddot, dphi, phiddot])

get_default_x0()

Default: trolley at origin, no swing.

Source code in opensmc/plants/crane.py

def get_default_x0(self):
    """Default: trolley at origin, no swing."""
    return np.zeros(4)

get_dynamics_components(x)

Decompose dynamics into affine-in-u form.

x_ddot = f1(x) + b1(x) * u phi_ddot = f2(x) + b2(x) * u

Returns

f1, b1, f2, b2 : float Drift and input-gain components.

Source code in opensmc/plants/crane.py

def get_dynamics_components(self, x):
    """Decompose dynamics into affine-in-u form.

    x_ddot   = f1(x) + b1(x) * u
    phi_ddot = f2(x) + b2(x) * u

    Returns
    -------
    f1, b1, f2, b2 : float
        Drift and input-gain components.
    """
    phi = x[2]
    dphi = x[3]

    M, m, l, g = self.M, self.m, self.l, self.g

    st = np.sin(phi)
    ct = np.cos(phi)

    A_ = M + m
    B_ = m * l * ct
    D_ = m * l * st

    den1 = A_ * l - B_ * ct
    den2 = B_ * ct - A_ * l

    f1 = (D_ * dphi**2 * l + g * B_ * st) / den1
    b1 = l / den1

    f2 = (D_ * dphi**2 * ct + g * A_ * st) / den2
    b2 = ct / den2

    return f1, b1, f2, b2

output(x)

Output y = [x_trolley, phi].

Source code in opensmc/plants/crane.py

def output(self, x):
    """Output y = [x_trolley, phi]."""
    return np.array([x[0], x[2]])

SurfaceVessel

Bases: Plant

3-DOF marine surface vessel in body-fixed frame (Fossen 2011).

x = [x_n, y_n, psi, u_b, v_b, r]

• (x_n, y_n, psi): inertial (NED) position and heading [m, m, rad]
• (u_b, v_b, r): body-fixed surge, sway, yaw rate [m/s, m/s, rad/s]

tau = [tau_u, tau_v, tau_r]

• tau_u: surge force [N]
• tau_v: sway force [N]
• tau_r: yaw moment [N*m]

Outputs: y = [x_n, y_n, psi]

Dynamics

Kinematics: eta_dot = R(psi) * nu Kinetics: M * nu_dot = tau - C(nu)nu - D(nu)nu

where R(psi) is the 2D rotation matrix, M is the rigid-body + added mass matrix, C is the Coriolis matrix, and D contains linear + quadratic damping.

Parameters

m : float Vessel mass [kg] (default 23.8). Iz : float Yaw moment of inertia [kg*m^2] (default 1.76). xg : float CG offset from CO [m] (default 0.046). Xu : float Surge linear damping (default -0.72). Yv : float Sway linear damping (default -0.89). Nr : float Yaw linear damping (default -1.90). Xuu : float Surge quadratic damping (default -1.33). Yvv : float Sway quadratic damping (default -36.5). Nrr : float Yaw quadratic damping (default -0.75). Xudot : float Surge added mass (default -2.0). Yvdot : float Sway added mass (default -10.0). Nrdot : float Yaw added inertia (default -1.0).

Source code in opensmc/plants/surface_vessel.py

class SurfaceVessel(Plant):
    """3-DOF marine surface vessel in body-fixed frame (Fossen 2011).

    States: x = [x_n, y_n, psi, u_b, v_b, r]
        - (x_n, y_n, psi): inertial (NED) position and heading [m, m, rad]
        - (u_b, v_b, r): body-fixed surge, sway, yaw rate [m/s, m/s, rad/s]

    Inputs: tau = [tau_u, tau_v, tau_r]
        - tau_u: surge force [N]
        - tau_v: sway force [N]
        - tau_r: yaw moment [N*m]

    Outputs: y = [x_n, y_n, psi]

    Dynamics
    --------
    Kinematics: eta_dot = R(psi) * nu
    Kinetics:   M * nu_dot = tau - C(nu)*nu - D(nu)*nu

    where R(psi) is the 2D rotation matrix, M is the rigid-body + added
    mass matrix, C is the Coriolis matrix, and D contains linear +
    quadratic damping.

    Parameters
    ----------
    m : float
        Vessel mass [kg] (default 23.8).
    Iz : float
        Yaw moment of inertia [kg*m^2] (default 1.76).
    xg : float
        CG offset from CO [m] (default 0.046).
    Xu : float
        Surge linear damping (default -0.72).
    Yv : float
        Sway linear damping (default -0.89).
    Nr : float
        Yaw linear damping (default -1.90).
    Xuu : float
        Surge quadratic damping (default -1.33).
    Yvv : float
        Sway quadratic damping (default -36.5).
    Nrr : float
        Yaw quadratic damping (default -0.75).
    Xudot : float
        Surge added mass (default -2.0).
    Yvdot : float
        Sway added mass (default -10.0).
    Nrdot : float
        Yaw added inertia (default -1.0).
    """

    def __init__(self, m=23.8, Iz=1.76, xg=0.046,
                 Xu=-0.72, Yv=-0.89, Nr=-1.90,
                 Xuu=-1.33, Yvv=-36.5, Nrr=-0.75,
                 Xudot=-2.0, Yvdot=-10.0, Nrdot=-1.0):
        super().__init__(n_states=6, n_inputs=3, n_outputs=3,
                         name="SurfaceVessel")
        self.m = m
        self.Iz = Iz
        self.xg = xg
        self.Xu = Xu
        self.Yv = Yv
        self.Nr = Nr
        self.Xuu = Xuu
        self.Yvv = Yvv
        self.Nrr = Nrr
        self.Xudot = Xudot
        self.Yvdot = Yvdot
        self.Nrdot = Nrdot

        # Mass matrix diagonal elements
        self.m11 = m - Xudot
        self.m22 = m - Yvdot
        self.m33 = Iz - Nrdot
        self.m23 = m * xg

    @staticmethod
    def rotation_matrix(psi):
        """2D rotation matrix R(psi): body -> NED.

        Parameters
        ----------
        psi : float
            Heading angle [rad].

        Returns
        -------
        R : ndarray (3, 3)
        """
        cp = np.cos(psi)
        sp = np.sin(psi)
        return np.array([
            [cp, -sp, 0.0],
            [sp,  cp, 0.0],
            [0.0, 0.0, 1.0]
        ])

    def dynamics(self, t, x, u, d=None):
        """Compute state derivatives.

        Parameters
        ----------
        t : float
            Time.
        x : ndarray (6,)
            State [x_n, y_n, psi, u_b, v_b, r].
        u : ndarray (3,)
            Inputs [tau_u, tau_v, tau_r].
        d : ndarray (3,) or None
            Additive disturbance forces/moment. Default 0.

        Returns
        -------
        xdot : ndarray (6,)
        """
        u_in = np.asarray(u, dtype=float).ravel()
        if len(u_in) < 3:
            u_in = np.concatenate([u_in, np.zeros(3 - len(u_in))])
        else:
            u_in = u_in[:3]
        if d is None:
            d_vec = np.zeros(3)
        else:
            d_vec = np.asarray(d, dtype=float).ravel()
            if len(d_vec) < 3:
                d_vec = np.concatenate([d_vec, np.zeros(3 - len(d_vec))])
            else:
                d_vec = d_vec[:3]

        tau = u_in + d_vec

        psi = x[2]
        ub  = x[3]
        vb  = x[4]
        r   = x[5]

        # Kinematics: eta_dot = R(psi) * nu
        R = self.rotation_matrix(psi)
        eta_dot = R @ np.array([ub, vb, r])

        m = self.m
        m11, m22, m33 = self.m11, self.m22, self.m33

        # Coriolis + centripetal
        c13 = -(m - self.Yvdot) * vb - m * self.xg * r
        c23 = (m - self.Xudot) * ub

        # Damping: linear + quadratic
        d1 = -self.Xu * ub - self.Xuu * abs(ub) * ub
        d2 = -self.Yv * vb - self.Yvv * abs(vb) * vb
        d3 = -self.Nr * r  - self.Nrr * abs(r) * r

        # Kinetics (simplified decoupled form)
        rhs1 = tau[0] + c13 * r - d1
        rhs2 = tau[1] + c23 * r - d2
        rhs3 = tau[2]           - d3

        nu_dot = np.array([rhs1 / m11,
                           rhs2 / m22,
                           rhs3 / m33])

        return np.concatenate([eta_dot, nu_dot])

    def output(self, x):
        """Output y = [x_n, y_n, psi]."""
        return x[:3]

    def get_default_x0(self):
        """Default: at origin, heading north, at rest."""
        return np.zeros(6)

Functions

dynamics(t, x, u, d=None)

Compute state derivatives.

Parameters

t : float Time. x : ndarray (6,) State [x_n, y_n, psi, u_b, v_b, r]. u : ndarray (3,) Inputs [tau_u, tau_v, tau_r]. d : ndarray (3,) or None Additive disturbance forces/moment. Default 0.

Returns

xdot : ndarray (6,)

Source code in opensmc/plants/surface_vessel.py

def dynamics(self, t, x, u, d=None):
    """Compute state derivatives.

    Parameters
    ----------
    t : float
        Time.
    x : ndarray (6,)
        State [x_n, y_n, psi, u_b, v_b, r].
    u : ndarray (3,)
        Inputs [tau_u, tau_v, tau_r].
    d : ndarray (3,) or None
        Additive disturbance forces/moment. Default 0.

    Returns
    -------
    xdot : ndarray (6,)
    """
    u_in = np.asarray(u, dtype=float).ravel()
    if len(u_in) < 3:
        u_in = np.concatenate([u_in, np.zeros(3 - len(u_in))])
    else:
        u_in = u_in[:3]
    if d is None:
        d_vec = np.zeros(3)
    else:
        d_vec = np.asarray(d, dtype=float).ravel()
        if len(d_vec) < 3:
            d_vec = np.concatenate([d_vec, np.zeros(3 - len(d_vec))])
        else:
            d_vec = d_vec[:3]

    tau = u_in + d_vec

    psi = x[2]
    ub  = x[3]
    vb  = x[4]
    r   = x[5]

    # Kinematics: eta_dot = R(psi) * nu
    R = self.rotation_matrix(psi)
    eta_dot = R @ np.array([ub, vb, r])

    m = self.m
    m11, m22, m33 = self.m11, self.m22, self.m33

    # Coriolis + centripetal
    c13 = -(m - self.Yvdot) * vb - m * self.xg * r
    c23 = (m - self.Xudot) * ub

    # Damping: linear + quadratic
    d1 = -self.Xu * ub - self.Xuu * abs(ub) * ub
    d2 = -self.Yv * vb - self.Yvv * abs(vb) * vb
    d3 = -self.Nr * r  - self.Nrr * abs(r) * r

    # Kinetics (simplified decoupled form)
    rhs1 = tau[0] + c13 * r - d1
    rhs2 = tau[1] + c23 * r - d2
    rhs3 = tau[2]           - d3

    nu_dot = np.array([rhs1 / m11,
                       rhs2 / m22,
                       rhs3 / m33])

    return np.concatenate([eta_dot, nu_dot])

get_default_x0()

Default: at origin, heading north, at rest.

Source code in opensmc/plants/surface_vessel.py

def get_default_x0(self):
    """Default: at origin, heading north, at rest."""
    return np.zeros(6)

output(x)

Output y = [x_n, y_n, psi].

Source code in opensmc/plants/surface_vessel.py

def output(self, x):
    """Output y = [x_n, y_n, psi]."""
    return x[:3]

rotation_matrix(psi) staticmethod

2D rotation matrix R(psi): body -> NED.

Parameters

psi : float Heading angle [rad].

Returns

R : ndarray (3, 3)

Source code in opensmc/plants/surface_vessel.py

@staticmethod
def rotation_matrix(psi):
    """2D rotation matrix R(psi): body -> NED.

    Parameters
    ----------
    psi : float
        Heading angle [rad].

    Returns
    -------
    R : ndarray (3, 3)
    """
    cp = np.cos(psi)
    sp = np.sin(psi)
    return np.array([
        [cp, -sp, 0.0],
        [sp,  cp, 0.0],
        [0.0, 0.0, 1.0]
    ])

TwoLinkArm

Bases: Plant

2-DOF planar robot manipulator (fully actuated).

x = [q1, q1_dot, q2, q2_dot]

• (q1, q1_dot): joint 1 angle and angular velocity [rad, rad/s]
• (q2, q2_dot): joint 2 angle and angular velocity [rad, rad/s]

Inputs: tau = [tau1, tau2] (joint torques [N*m]) Outputs: y = [q1, q2] (joint angles)

Dynamics

M(q) * qddot + C(q, qdot) * qdot + G(q) = tau + d

where M is the inertia matrix, C is the Coriolis/centrifugal vector, and G is the gravity vector.

Parameters

m1 : float Link 1 mass [kg] (default 1.0). m2 : float Link 2 mass [kg] (default 1.0). l1 : float Link 1 length [m] (default 1.0). l2 : float Link 2 length [m] (default 1.0). lc1 : float Link 1 center-of-mass distance [m] (default 0.5). lc2 : float Link 2 center-of-mass distance [m] (default 0.5). I1 : float Link 1 rotational inertia [kgm^2] (default 0.083). I2 : float Link 2 rotational inertia [kgm^2] (default 0.083). g : float Gravitational acceleration [m/s^2] (default 9.81). b1 : float Joint 1 viscous friction [Nms/rad] (default 0.0). b2 : float Joint 2 viscous friction [Nms/rad] (default 0.0).

Source code in opensmc/plants/two_link_arm.py

class TwoLinkArm(Plant):
    """2-DOF planar robot manipulator (fully actuated).

    States: x = [q1, q1_dot, q2, q2_dot]
        - (q1, q1_dot): joint 1 angle and angular velocity [rad, rad/s]
        - (q2, q2_dot): joint 2 angle and angular velocity [rad, rad/s]

    Inputs: tau = [tau1, tau2] (joint torques [N*m])
    Outputs: y = [q1, q2] (joint angles)

    Dynamics
    --------
    M(q) * qddot + C(q, qdot) * qdot + G(q) = tau + d

    where M is the inertia matrix, C is the Coriolis/centrifugal vector,
    and G is the gravity vector.

    Parameters
    ----------
    m1 : float
        Link 1 mass [kg] (default 1.0).
    m2 : float
        Link 2 mass [kg] (default 1.0).
    l1 : float
        Link 1 length [m] (default 1.0).
    l2 : float
        Link 2 length [m] (default 1.0).
    lc1 : float
        Link 1 center-of-mass distance [m] (default 0.5).
    lc2 : float
        Link 2 center-of-mass distance [m] (default 0.5).
    I1 : float
        Link 1 rotational inertia [kg*m^2] (default 0.083).
    I2 : float
        Link 2 rotational inertia [kg*m^2] (default 0.083).
    g : float
        Gravitational acceleration [m/s^2] (default 9.81).
    b1 : float
        Joint 1 viscous friction [N*m*s/rad] (default 0.0).
    b2 : float
        Joint 2 viscous friction [N*m*s/rad] (default 0.0).
    """

    def __init__(self, m1=1.0, m2=1.0, l1=1.0, l2=1.0,
                 lc1=0.5, lc2=0.5, I1=0.083, I2=0.083,
                 g=9.81, b1=0.0, b2=0.0):
        super().__init__(n_states=4, n_inputs=2, n_outputs=2,
                         name="TwoLinkArm")
        self.m1 = m1
        self.m2 = m2
        self.l1 = l1
        self.l2 = l2
        self.lc1 = lc1
        self.lc2 = lc2
        self.I1 = I1
        self.I2 = I2
        self.g = g
        self.b1 = b1
        self.b2 = b2

    def mass_matrix(self, q1, q2):
        """Compute 2x2 inertia matrix M(q).

        Parameters
        ----------
        q1, q2 : float
            Joint angles [rad].

        Returns
        -------
        M : ndarray (2, 2)
            Symmetric positive-definite inertia matrix.
        """
        m1, m2 = self.m1, self.m2
        l1 = self.l1
        lc1, lc2 = self.lc1, self.lc2
        I1, I2 = self.I1, self.I2

        c2 = np.cos(q2)

        M11 = m1 * lc1**2 + I1 + m2 * (l1**2 + lc2**2 + 2 * l1 * lc2 * c2) + I2
        M12 = m2 * (lc2**2 + l1 * lc2 * c2) + I2
        M22 = m2 * lc2**2 + I2

        return np.array([[M11, M12],
                         [M12, M22]])

    def coriolis_vector(self, q1, q2, q1dot, q2dot):
        """Compute Coriolis/centrifugal vector C(q, qdot)*qdot.

        Parameters
        ----------
        q1, q2 : float
            Joint angles [rad].
        q1dot, q2dot : float
            Joint angular velocities [rad/s].

        Returns
        -------
        Cv : ndarray (2,)
            Coriolis/centrifugal terms.
        """
        s2 = np.sin(q2)
        h = self.m2 * self.l1 * self.lc2 * s2

        c1 = -2 * h * q1dot * q2dot - h * q2dot**2
        c2 = h * q1dot**2

        return np.array([c1, c2])

    def coriolis_matrix(self, q1, q2, q1dot, q2dot):
        """Compute 2x2 Coriolis matrix C(q, qdot) using Christoffel symbols.

        This is the unique C matrix satisfying the skew-symmetry property:
        Mdot - 2C is skew-symmetric (Slotine & Li, Property 7.1).

        Parameters
        ----------
        q1, q2 : float
            Joint angles [rad].
        q1dot, q2dot : float
            Joint angular velocities [rad/s].

        Returns
        -------
        C : ndarray (2, 2)
        """
        s2 = np.sin(q2)
        h = self.m2 * self.l1 * self.lc2 * s2

        C11 = -h * q2dot
        C12 = -h * (q1dot + q2dot)
        C21 = h * q1dot
        C22 = 0.0

        return np.array([[C11, C12],
                         [C21, C22]])

    def gravity_vector(self, q1, q2):
        """Compute gravity vector G(q).

        Parameters
        ----------
        q1, q2 : float
            Joint angles [rad].

        Returns
        -------
        G : ndarray (2,)
        """
        m1, m2 = self.m1, self.m2
        l1 = self.l1
        lc1, lc2 = self.lc1, self.lc2
        g = self.g

        c1 = np.cos(q1)
        c12 = np.cos(q1 + q2)

        g1 = (m1 * lc1 + m2 * l1) * g * c1 + m2 * lc2 * g * c12
        g2 = m2 * lc2 * g * c12

        return np.array([g1, g2])

    def dynamics(self, t, x, u, d=None):
        """Compute state derivatives.

        M(q)*qddot = tau - C(q,qdot)*qdot - G(q) - friction + d

        Parameters
        ----------
        t : float
            Time.
        x : ndarray (4,)
            State [q1, q1_dot, q2, q2_dot].
        u : ndarray (2,)
            Joint torques [tau1, tau2].
        d : ndarray (4,) or None
            Disturbance. If provided, d[1] and d[3] are added as
            torque disturbances on joints 1 and 2 respectively. Default 0.

        Returns
        -------
        xdot : ndarray (4,)
        """
        u = np.asarray(u, dtype=float).ravel()
        if len(u) < 2:
            u = np.concatenate([u, np.zeros(2 - len(u))])
        if d is None:
            d = np.zeros(4)
        else:
            d = np.asarray(d, dtype=float).ravel()
            if len(d) < 4:
                d = np.concatenate([d, np.zeros(4 - len(d))])

        q1, q1dot, q2, q2dot = x

        M = self.mass_matrix(q1, q2)
        Cv = self.coriolis_vector(q1, q2, q1dot, q2dot)
        G = self.gravity_vector(q1, q2)

        friction = np.array([self.b1 * q1dot, self.b2 * q2dot])

        # tau + disturbance - Coriolis - gravity - friction
        rhs = u - Cv - G - friction + d[[1, 3]]
        qddot = np.linalg.solve(M, rhs)

        return np.array([q1dot, qddot[0], q2dot, qddot[1]])

    def output(self, x):
        """Output y = [q1, q2]."""
        return np.array([x[0], x[2]])

    def get_default_x0(self):
        """Default: both joints at zero angle, at rest."""
        return np.zeros(4)

Functions

coriolis_matrix(q1, q2, q1dot, q2dot)

Compute 2x2 Coriolis matrix C(q, qdot) using Christoffel symbols.

This is the unique C matrix satisfying the skew-symmetry property: Mdot - 2C is skew-symmetric (Slotine & Li, Property 7.1).

Parameters

q1, q2 : float Joint angles [rad]. q1dot, q2dot : float Joint angular velocities [rad/s].

Returns

C : ndarray (2, 2)

Source code in opensmc/plants/two_link_arm.py

def coriolis_matrix(self, q1, q2, q1dot, q2dot):
    """Compute 2x2 Coriolis matrix C(q, qdot) using Christoffel symbols.

    This is the unique C matrix satisfying the skew-symmetry property:
    Mdot - 2C is skew-symmetric (Slotine & Li, Property 7.1).

    Parameters
    ----------
    q1, q2 : float
        Joint angles [rad].
    q1dot, q2dot : float
        Joint angular velocities [rad/s].

    Returns
    -------
    C : ndarray (2, 2)
    """
    s2 = np.sin(q2)
    h = self.m2 * self.l1 * self.lc2 * s2

    C11 = -h * q2dot
    C12 = -h * (q1dot + q2dot)
    C21 = h * q1dot
    C22 = 0.0

    return np.array([[C11, C12],
                     [C21, C22]])

coriolis_vector(q1, q2, q1dot, q2dot)

Compute Coriolis/centrifugal vector C(q, qdot)*qdot.

Parameters

q1, q2 : float Joint angles [rad]. q1dot, q2dot : float Joint angular velocities [rad/s].

Returns

Cv : ndarray (2,) Coriolis/centrifugal terms.

Source code in opensmc/plants/two_link_arm.py

def coriolis_vector(self, q1, q2, q1dot, q2dot):
    """Compute Coriolis/centrifugal vector C(q, qdot)*qdot.

    Parameters
    ----------
    q1, q2 : float
        Joint angles [rad].
    q1dot, q2dot : float
        Joint angular velocities [rad/s].

    Returns
    -------
    Cv : ndarray (2,)
        Coriolis/centrifugal terms.
    """
    s2 = np.sin(q2)
    h = self.m2 * self.l1 * self.lc2 * s2

    c1 = -2 * h * q1dot * q2dot - h * q2dot**2
    c2 = h * q1dot**2

    return np.array([c1, c2])

dynamics(t, x, u, d=None)

Compute state derivatives.

M(q)qddot = tau - C(q,qdot)qdot - G(q) - friction + d

Parameters

t : float Time. x : ndarray (4,) State [q1, q1_dot, q2, q2_dot]. u : ndarray (2,) Joint torques [tau1, tau2]. d : ndarray (4,) or None Disturbance. If provided, d[1] and d[3] are added as torque disturbances on joints 1 and 2 respectively. Default 0.

Returns

xdot : ndarray (4,)

Source code in opensmc/plants/two_link_arm.py

def dynamics(self, t, x, u, d=None):
    """Compute state derivatives.

    M(q)*qddot = tau - C(q,qdot)*qdot - G(q) - friction + d

    Parameters
    ----------
    t : float
        Time.
    x : ndarray (4,)
        State [q1, q1_dot, q2, q2_dot].
    u : ndarray (2,)
        Joint torques [tau1, tau2].
    d : ndarray (4,) or None
        Disturbance. If provided, d[1] and d[3] are added as
        torque disturbances on joints 1 and 2 respectively. Default 0.

    Returns
    -------
    xdot : ndarray (4,)
    """
    u = np.asarray(u, dtype=float).ravel()
    if len(u) < 2:
        u = np.concatenate([u, np.zeros(2 - len(u))])
    if d is None:
        d = np.zeros(4)
    else:
        d = np.asarray(d, dtype=float).ravel()
        if len(d) < 4:
            d = np.concatenate([d, np.zeros(4 - len(d))])

    q1, q1dot, q2, q2dot = x

    M = self.mass_matrix(q1, q2)
    Cv = self.coriolis_vector(q1, q2, q1dot, q2dot)
    G = self.gravity_vector(q1, q2)

    friction = np.array([self.b1 * q1dot, self.b2 * q2dot])

    # tau + disturbance - Coriolis - gravity - friction
    rhs = u - Cv - G - friction + d[[1, 3]]
    qddot = np.linalg.solve(M, rhs)

    return np.array([q1dot, qddot[0], q2dot, qddot[1]])

get_default_x0()

Default: both joints at zero angle, at rest.

Source code in opensmc/plants/two_link_arm.py

def get_default_x0(self):
    """Default: both joints at zero angle, at rest."""
    return np.zeros(4)

gravity_vector(q1, q2)

Compute gravity vector G(q).

Parameters

q1, q2 : float Joint angles [rad].

Returns

G : ndarray (2,)

Source code in opensmc/plants/two_link_arm.py

def gravity_vector(self, q1, q2):
    """Compute gravity vector G(q).

    Parameters
    ----------
    q1, q2 : float
        Joint angles [rad].

    Returns
    -------
    G : ndarray (2,)
    """
    m1, m2 = self.m1, self.m2
    l1 = self.l1
    lc1, lc2 = self.lc1, self.lc2
    g = self.g

    c1 = np.cos(q1)
    c12 = np.cos(q1 + q2)

    g1 = (m1 * lc1 + m2 * l1) * g * c1 + m2 * lc2 * g * c12
    g2 = m2 * lc2 * g * c12

    return np.array([g1, g2])

mass_matrix(q1, q2)

Compute 2x2 inertia matrix M(q).

Parameters

q1, q2 : float Joint angles [rad].

Returns

M : ndarray (2, 2) Symmetric positive-definite inertia matrix.

Source code in opensmc/plants/two_link_arm.py

def mass_matrix(self, q1, q2):
    """Compute 2x2 inertia matrix M(q).

    Parameters
    ----------
    q1, q2 : float
        Joint angles [rad].

    Returns
    -------
    M : ndarray (2, 2)
        Symmetric positive-definite inertia matrix.
    """
    m1, m2 = self.m1, self.m2
    l1 = self.l1
    lc1, lc2 = self.lc1, self.lc2
    I1, I2 = self.I1, self.I2

    c2 = np.cos(q2)

    M11 = m1 * lc1**2 + I1 + m2 * (l1**2 + lc2**2 + 2 * l1 * lc2 * c2) + I2
    M12 = m2 * (lc2**2 + l1 * lc2 * c2) + I2
    M22 = m2 * lc2**2 + I2

    return np.array([[M11, M12],
                     [M12, M22]])

output(x)

Output y = [q1, q2].

Source code in opensmc/plants/two_link_arm.py

def output(self, x):
    """Output y = [q1, q2]."""
    return np.array([x[0], x[2]])