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: choose die position of CuZn ball as the output.
Because it is a flat output. plan the trajectory of the CuZn ball.then let the real trajectory
of these ball converge towards the target trajectory by using the tracking controller based on error dynamics.
(These ball moves from 0.04m to 0.05m within 5 seconds.)
"""
import sympy as sp
import symbtools as st
import numpy as np
from symbtools import modeltools as mt
from ackrep_core import ResultContainer


class ProblemSpecification(object):
    # model parameters
    Np = 2  # number of active coordinates
    pp = st.symb_vector("p1:{0}".format(Np + 1))  # active coordinates
    ttheta = pp  # position of the bodies in m
    tthetad = st.time_deriv(ttheta, ttheta)  # velocities of the bodies in m/s
    tthetadd = st.time_deriv(ttheta, ttheta, order=2)  # accelerations in m/s^2
    F = sp.Symbol("F")  # nonlinear external magnetic force on the body
    u_F = F  # external force of system

    ii = sp.Symbol("ii")  # real current input of system
    u_i = ii
    tt = np.linspace(0, 5, 1000)  # time axis for state transition
    tt1 = np.linspace(-1, 6, 1000)  # time axis for full transition
    tt2 = np.linspace(0, 6, 1000)  # time axis for actual trajectory
    tt3 = np.linspace(3, 5, 400)  # time axis for evaluation of result

    # trajectory planning related parameters
    YA_p2 = [0.04, 0, 0, 0, 0]  # initial conditions of the CuZn-Ball p2 (position in m)
    YB_p2 = [0.06, 0, 0, 0, 0]  # final conditions of the CuZn-Ball p2 (position in m)
    t0 = 0  # start time in s
    tf = 5  # final time in s

    # tracking controller related parameters
    pol = [-10, -12, -15, -18]  # desired poles for error dynamics
    xx0 = np.array([0.005, 0.045, 0.02, 0.03])  # initial condition for simulation

    @staticmethod
    def rhs(xx, xxd, uu):
        """Right hand side of the equation of motion in nonlinear state space form
        :param xx:   system states
        :param xxd:  first derivation of system states
        :param uu:   system input
        :return mod:  system class
        """
        m1 = 0.05  # mass of the iron ball in kg
        m2 = 0.1  # mass of the brass ball in kg
        kf = 30  # Spring constant in N/m
        g = 9.8  # acceleration of gravity in m/s^2

        # kinetic energy of the bodies
        T0 = 0.5 * m1 * xxd[0] ** 2
        T1 = 0.5 * m2 * xxd[1] ** 2
        T = T0 + T1  # total kinetic energy

        # total potential energy
        V = -g * (m1 * xx[0] + m2 * xx[1]) + kf * 0.5 * (xx[0] - xx[1]) ** 2

        external_forces = [uu, 0]  # magnetic force as input

        # symbolic model of the system
        mod = mt.generate_symbolic_model(T, V, xx, external_forces)
        mod.calc_state_eq()

        return mod

    @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
        """
        u = uu

        return sp.Matrix([xx[1]])

    @staticmethod
    def force_current_function(xx, uu):
        k1 = 4e-5  # geometry constant

        k2 = 0.005  # air gap of magnet in m
        f_c_func = -k1 * uu**2 / (xx[0] + k2) ** 2
        return f_c_func


def evaluate_solution(solution_data):

    """Condition: the position of the CuZn-ball moves from 0.04m to 0.06m 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:, 1] - solution_data.ploy_p2(P.tt3)) < 1e-2)
    return ResultContainer(success=success, score=1.0)