Automatic Control Knowledge Repository

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
system description: a two-body floating system is considered. The above iron ball undergoes
a magnetic force generated by a current. A CuZn ball below is attached to that iron ball
by a spring with the spring constant kf.

problem specification for control problem: design of the LQ controller to control and
stabilize position of the both balls.
"""
import numpy as np
import sympy as sp
from ackrep_core import ResultContainer

from system_models.two_mass_floating_bodies_system.system_model import Model

j = 1j  # imaginary unit


class ProblemSpecification(object):
    # system symbols for setting up the equation of motion
    model = Model()
    x1, x2, x3, x4 = model.xx_symb
    xx = sp.Matrix(model.xx_symb)  # states of system
    u1 = model.uu_symb[0]  # input of system
    u = [u1]

    # equilibrium points for linearization of the nonlinear system
    eqrt = [(x1, 0.01), (x2, 0.049), (x3, 0), (x4, 0), (u1, 5)]
    xx0 = np.array([0.02, 0.052, 0, 0])  # initial condition
    tt = np.linspace(0, 10, 1000)  # vector for the time axis for simulating
    yr = 0  # reference output
    q = np.diag([30, 30, 2, 2])  # desired poles
    r = np.diag([0.08])  # initial condition

    def rhs(model):
        """Right hand side of the equation of motion in nonlinear state space form
        :param xx:   system states
        :param uu:   system input
        :return:     nonlinear state space
        """

        return sp.Matrix(model.get_rhs_symbolic_num_params())

    @staticmethod
    def output_func(xx, uu):
        """output equation of the system
        :param xx:   system states
        :param uu:   system input (not used in this case)
        :return:     output equation y = x1
        """
        x1, x2, x3, x4 = xx
        u = uu

        return sp.Matrix([x1])


def evaluate_solution(solution_data):
    """
    Condition: the both balls return to their original points after 3 seconds at the latest
    :param solution_data: solution data of problem of solution
    :return:
    """
    P = ProblemSpecification
    success = all(abs(solution_data.res[600:, 0] - [P.eqrt[0][1]] * 400) < 1e-2) and all(
        abs(solution_data.res[600:, 1] - [P.eqrt[1][1]] * 400) < 1e-2
    )

    return ResultContainer(success=success, score=1.0)