In [2]:

    

import numpy as np

inf = np.inf
isclose = np.isclose
π = np.pi
arctan = np.arctan
sign = np.sign


    

In [3]:

    

class Device(object):
    def __init__(self):
        # position, velocity, acceleration
        self.x = 0
        self.v = 0
        
        # mass, drag, spring-constant, spring equilibrium point
        self.m = 1    # mass, inertial, moment of inertia
        self.b = 0    # drag
        self.k = 0    # spring
        self.x0 = 0   # equilibrium point
        
        # gain (maximum control), sensitivity
        self.g = 1
        self.s = 1
        
        self.t_step = 1/100
        
    def control_force(self,c):
        '''represents physical limitation of the control'''
        
        # bring instance variables into local scope
        g = self.g
        s = self.s
        
        def _control_force(g,s):
            if s == 0:
                def _fn(c):
                    return g * min(max(c,-1),1)
            elif s > 0:
                def _fn(c):
                    return g * arctan(s*c) / arctan(s)
            else:
                def _fn(c):
                    return sign(c) * g * (1 - arctan(s*(1-abs(c))) / arctan(s))
            return _fn

        return _control_force(g,s)(c)
        
    def external_force(self):
        '''external forces (drag, etc)'''
        
        # bring instance variables into local scope
        x = self.x
        v = self.v
        b = self.b
        k = self.k
        x0 = self.x0
        
        Fdrag = int(v<0) * b * v**2
        Fspring = -k * (x - x0)
        
        return Fdrag + Fspring
        
    def __call__(self,c,Δt):
        '''set the control to c for Δt time
        
        return the position and velocity after Δt has passed
        
        This device has a characteristic "mass" which gives
        it a certain momentum that the control must fight against
            
            ΣF = Δp / Δt
            ΣF = m * Δv / Δt
            v = v + (ΣF/m)*Δt
            x = x + v*Δt
        '''
        # bring instance variables into local scope
        x = self.x
        v = self.v
        m = self.m
        t_step = self.t_step
        
        Fcontrol = self.control_force(c)
        
        t = 0 
        while t < Δt:
            # sum of forces
            ΣF = Fcontrol + self.external_force()
            
            # change in velocity and position
            x = x + v*t_step
            v = v + (ΣF/m)*t_step
            
            t += t_step
        
        # save parameters to class instance
        self.x = x
        self.v = v
        
        return x,v