control/controlSystem.py at master · ivapylibs/control · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
from util import Timer
import numpy as np

class controlSystem(object):

    def __init__(self, ceom, trajGenerator, trackBuilder, config=None):

        self.ceom = ceom
        self.trajGen = trajGenerator

        self.trackBuilder = trackBuilder
        self.trackSim = None
        self.simOut = []

        if config is not None:
            self.config = config

        self.iflag = False

    def setInitialState(self, istate):
        #TODO: Ensure that this always works
        state_size = self.trajGen.system.B.shape[0]
        if state_size == 2*istate.x.size:
            istate.x = np.pad(istate.x.flatten(), (0, istate.x.size), mode='constant').reshape((state_size, 1))

        self.istate = istate
        self.iflag = True

    def goto(self, fstate, duration=None):

        if not self.iflag:
            print("Error, no initial state")

        if len(self.simOut) == 0:
            tshift = 0
        else:
            tshift = self.simOut[-1].t[-1]

        if duration is not None:
            tspan = [0, duration]
            theTraj = self.trajGen.point2point(self.istate.x, fstate, tspan)
        else:
            theTraj = self.trajGen.point2point(self.istate.x, fstate)

        if self.trackSim is None:
            self.trackSim = self.trackBuilder.firstBuildFromStruct(self.istate, theTraj)
        else:
            self.trackBuilder.reconfigFromStruct(self.trackSim, self.istate, theTraj)

        simsol = self.trackSim.simulate()

        simsol.traj = theTraj
        simsol.istate = self.istate
        simsol.fstate = self.trackSim.getState()

        self.addSim(simsol)
        self.iflag = False

        return simsol


    '''=============================== follow ==============================
    %
    % @brief  Specify that the system should follow the specified
    %         trajectory.
    %
    % @param[in]  desTraj Desired trajectory structure.
    %
    % @param[out] simsol  Solution to the current simulated time span.
    %'''

    def follow(self, desTraj):

        if (not self.iflag):
            print('Error! Defined a terminal system state, but there is no initial state.')

        theTraj = self.trajGen.followPath(self.istate, desTraj)

        if self.trackSim is None:
            self.trackSim = self.trackBuilder.firstBuildFromStruct(self.istate, theTraj)
        else:
            self.trackBuilder.reconfigFromStruct(self.trackSim, self.istate, theTraj)

        with Timer(name="Sim"):
            simsol = self.trackSim.simulate()

        simsol.traj = theTraj
        simsol.istate = self.istate
        simsol.fstate = self.trackSim.getState()

        self.addSim(simsol)
        self.iflag = False

        return simsol

    # WIP to track trajectories without using feed forward
    # TODO: Test
    def track(self, desTraj):
        if (not self.iflag):
            print('Error! Defined a terminal system state, but there is no initial state.')

        if self.trackSim is None:
            self.trackSim = self.trackBuilder.firstBuildFromStruct(self.istate, desTraj)
        else:
            self.trackBuilder.reconfigFromStruct(self.trackSim, self.istate, desTraj)

        with Timer(name="Sim"):
            simsol = self.trackSim.simulate()

        simsol.traj = desTraj
        simsol.istate = self.istate
        simsol.fstate = self.trackSim.getState()

        self.addSim(simsol)
        self.iflag = False

        return simsol


    def addSim(self, simOut):
        #if len(self.simOut) == 0:
        self.simOut.append(simOut)