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 reduced state observer to estimate of the
unknown internal state of a given real system.
"""
import numpy as np
import sympy as sp
from ackrep_core import ResultContainer

j = 1j  # imaginary unit


class ProblemSpecification(object):
    # system symbols for setting up the equation of motion
    p1, p2, pdot1, pdot2 = sp.symbols("p1, p2, pdot1, pdot2")
    i = sp.Symbol("i")
    xx = sp.Matrix([p1, p2, pdot1, pdot2])  # states of system
    u = [i]  # input of system

    # equilibrium points for linearization of the nonlinear system
    eqrt = [(p1, 0.0017352055986820537), (p2, 0.040950414991103266), (i, 1)]
    xx0 = np.array([0, 0.4, 0, 0, 0.6, 0.2, 0.2])  # initial condition
    tt = np.linspace(0, 4, 1000)  # vector for the time axis for simulating
    poles_cl = [-15, -15, -15, -15]  # desired poles
    poles_o = [-15, -15, -15]  # poles of the observer dynamics

    @staticmethod
    def rhs(xx, uu):
        """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
        """
        m1 = 0.05  # mass of the iron ball in kg
        m2 = 0.04  # mass of the brass ball in kg
        kf = 10  # Spring constant in N/m
        g = 9.8  # acceleration of gravity in m/s^2
        k1 = 4e-5  # geometry constant
        k2 = 0.005  # air gap of magnet in m

        x1, x2, x3, x4 = xx
        u = uu

        p1_dot = (g * m1 - u[0] ** 2 * k1 / (k2 + x1) ** 2 - kf * (x1 - x2)) / m1
        p2_dot = (g * m2 + kf * (x1 - x2)) / m2

        ff = sp.Matrix([x3, x4, p1_dot, p2_dot])

        return ff

    @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: all estimated states correspond to the true states after 3 seconds at the latest
    :return:
    """
    P = ProblemSpecification
    res_eva = []
    for i in range(3):
        res_eva.append(all(abs(solution_data.xx[600:, i + 1] - solution_data.yy[600:, i] < 1e-2)))
    success = all(res_eva)
    return ResultContainer(success=success, score=1.0)