Skip to content

Simulation

cartpole.simulator

SimulatorInfo

Simulator info.

ATTRIBUTE DESCRIPTION
step_count

number of simulation steps

TYPE: int

integration_count

number of integration steps

TYPE: int

Source code in cartpole/simulator.py 
30
31
32
33
34
35
36
37
38
39
40
41
42
43
class SimulatorInfo(BaseModel):
    """
    Simulator info.

    Attributes
    ----------
    step_count: int
        number of simulation steps
    integration_count: int
        number of integration steps
    """

    step_count: int = 0
    integration_count: int = 0

Simulator

CartPole simulator (integration implemented using RK4 method).

For more information see: https://cartpole.robotics-lab.ru/dynamics-and-control

Source code in cartpole/simulator.py 
 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
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
class Simulator(CartPoleBase):
    """
    CartPole simulator (integration implemented using RK4 method).

    For more information see: https://cartpole.robotics-lab.ru/dynamics-and-control
    """

    def __init__(self, integration_step: float = 0.001):
        """
        Create simulator with some default config.

        Parameters
        ----------
        integration_step: float
            integration time step (s)
        """

        # set default config
        self._config = Config(
            hardware_limit=Limits(
                cart_position=0.5, cart_velocity=2.0, cart_acceleration=5.0
            ),
            control_limit=Limits(),
            parameters=Parameters(g=9.81, b=0, k=0.3),
        )

        self._state = State(error=Error.NEED_RESET)
        self._target = Target(velocity=0)

        self._integration_step = integration_step
        self._integration_count = 0
        self._step_count = 0

    def get_config(self) -> Config:
        """
        Return current config.
        """
        return self._config

    def set_config(self, config: Config) -> None:
        """
        Set new config. In case of invalid config, AssertionError is raised.
        """

        assert config.control_limit.stronger(config.hardware_limit)

        assert config.parameters.friction_coef is not None
        assert config.parameters.friction_coef >= 0

        assert config.parameters.mass_coef is not None
        assert config.parameters.mass_coef > 0

        self._config = config

    def _eval_acceleration_by_velocity(
        self, velocity: float, eps: float = 1e-6
    ) -> float:
        err = velocity - self._state.cart_velocity

        a = self._target.acceleration_or(self._config.control_limit.cart_acceleration)
        if abs(err) < a * self._integration_step:
            return snap_zero(err / self._integration_step, eps)

        return sgn(err) * a

    def _eval_cart_acceleration(self, eps: float = 1e-6) -> float:
        if self._target.position is not None:
            a = self._target.acceleration_or(
                self._config.control_limit.cart_acceleration
            )
            v = self._target.velocity_or(self._config.control_limit.cart_velocity)
            err = self._target.position - self._state.cart_position

            # print(f'_eval_cart_acceleration err={err}, v={v}, a={a}, state_v={self._state.cart_velocity}')
            if abs(err) < 1e-3:
                return self._eval_acceleration_by_velocity(0)
            if sgn(err) != sgn(self._state.cart_velocity):
                # stoppping to change direction
                return self._eval_acceleration_by_velocity(sgn(err) * v)
            else:
                # bang-bang strategy
                a_brake = self._state.cart_velocity**2 / (2 * abs(err))
                # print(f'a_brake = {a_brake}, a = {a}')
                if a_brake >= a:
                    return self._eval_acceleration_by_velocity(0)
                else:
                    return self._eval_acceleration_by_velocity(sgn(err) * v)

        if self._target.velocity is not None:
            return self._eval_acceleration_by_velocity(self._target.velocity, eps)

        if self._target.acceleration is not None:
            return self._target.acceleration

        raise Exception("At least one of the target is required")

    def _derivative(self, s: numpy.ndarray, a: float) -> numpy.ndarray:
        """
        Calculate derivative of the given state (used for integration).

        Parameters
        ----------
        s: numpy.ndarray
            state vector
        a: float
            cart acceleration (m/s^2)

        Returns
        -------
        :numpy.ndarray
            derivative of the given state
        """

        result = numpy.zeros(4)

        b = self._config.parameters.friction_coef
        k = self._config.parameters.mass_coef
        g = self._config.parameters.gravity

        result[0] = s[2]
        result[1] = s[3]
        result[2] = a
        result[3] = -b * s[3] - k * (a * numpy.cos(s[1]) + g * numpy.sin(s[1]))

        return result

    def reset(self, state: State = State()) -> None:
        """
        Reset simulator to the given state.
        """

        self._state = state
        self._target = Target(velocity=0)

        self._count = 0
        self._integration_count = 0

    def get_state(self) -> State:
        """
        Return current state.
        """

        return self._state

    def get_info(self) -> SimulatorInfo:
        """
        Return current simulator info.
        """

        return SimulatorInfo(
            step_count=self._step_count, integration_count=self._integration_count
        )

    def set_target(self, target: Target) -> State:
        """
        Set control target and return current state.
        """

        self._target = target

        if not self._state.error:
            self._state.cart_acceleration = self._eval_cart_acceleration()
            self._state.validate(self._config)

        return self.get_state()

    def advance(self, delta: float) -> None:
        """
        Make simulation step of the given length.
        For integration, RK4 method is used.

        Parameters
        ----------
        delta: float
            length of the simulation step (s)
        """

        if self._state.error:
            return

        s = self._state.numpy4()
        h = self._integration_step
        h_2 = h / 2

        integration_step_n = int(delta / h)

        for _ in range(integration_step_n):
            a = self._eval_cart_acceleration()

            k1 = self._derivative(s, a)
            k2 = self._derivative(s + k1 * h_2, a)
            k3 = self._derivative(s + k2 * h_2, a)
            k4 = self._derivative(s + k3 * h, a)

            s += (k1 + 2 * k2 + 2 * k3 + k4) * h / 6

            self._integration_count += 1

            self._state = State(
                cart_position=s[0],
                cart_velocity=s[2],
                cart_acceleration=a,
                pole_angle=s[1],
                pole_angular_velocity=s[3],
                stamp=self._integration_step * self._integration_count,
            )

            self._state.validate(self._config)

        self._step_count += 1

    def close(self) -> None:
        """
        Clear any resources used by the simulator.
        """
        pass

__init__(integration_step: float = 0.001)

Create simulator with some default config.

PARAMETER DESCRIPTION
integration_step

integration time step (s)

TYPE: float DEFAULT: 0.001

Source code in cartpole/simulator.py 
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
def __init__(self, integration_step: float = 0.001):
    """
    Create simulator with some default config.

    Parameters
    ----------
    integration_step: float
        integration time step (s)
    """

    # set default config
    self._config = Config(
        hardware_limit=Limits(
            cart_position=0.5, cart_velocity=2.0, cart_acceleration=5.0
        ),
        control_limit=Limits(),
        parameters=Parameters(g=9.81, b=0, k=0.3),
    )

    self._state = State(error=Error.NEED_RESET)
    self._target = Target(velocity=0)

    self._integration_step = integration_step
    self._integration_count = 0
    self._step_count = 0

get_config() -> Config

Return current config.

Source code in cartpole/simulator.py 
79
80
81
82
83
def get_config(self) -> Config:
    """
    Return current config.
    """
    return self._config

set_config(config: Config) -> None

Set new config. In case of invalid config, AssertionError is raised.

Source code in cartpole/simulator.py 
85
86
87
88
89
90
91
92
93
94
95
96
97
98
def set_config(self, config: Config) -> None:
    """
    Set new config. In case of invalid config, AssertionError is raised.
    """

    assert config.control_limit.stronger(config.hardware_limit)

    assert config.parameters.friction_coef is not None
    assert config.parameters.friction_coef >= 0

    assert config.parameters.mass_coef is not None
    assert config.parameters.mass_coef > 0

    self._config = config

reset(state: State = State()) -> None

Reset simulator to the given state.

Source code in cartpole/simulator.py 
172
173
174
175
176
177
178
179
180
181
def reset(self, state: State = State()) -> None:
    """
    Reset simulator to the given state.
    """

    self._state = state
    self._target = Target(velocity=0)

    self._count = 0
    self._integration_count = 0

get_state() -> State

Return current state.

Source code in cartpole/simulator.py 
183
184
185
186
187
188
def get_state(self) -> State:
    """
    Return current state.
    """

    return self._state

get_info() -> SimulatorInfo

Return current simulator info.

Source code in cartpole/simulator.py 
190
191
192
193
194
195
196
197
def get_info(self) -> SimulatorInfo:
    """
    Return current simulator info.
    """

    return SimulatorInfo(
        step_count=self._step_count, integration_count=self._integration_count
    )

set_target(target: Target) -> State

Set control target and return current state.

Source code in cartpole/simulator.py 
199
200
201
202
203
204
205
206
207
208
209
210
def set_target(self, target: Target) -> State:
    """
    Set control target and return current state.
    """

    self._target = target

    if not self._state.error:
        self._state.cart_acceleration = self._eval_cart_acceleration()
        self._state.validate(self._config)

    return self.get_state()

advance(delta: float) -> None

Make simulation step of the given length. For integration, RK4 method is used.

PARAMETER DESCRIPTION
delta

length of the simulation step (s)

TYPE: float

Source code in cartpole/simulator.py 
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
def advance(self, delta: float) -> None:
    """
    Make simulation step of the given length.
    For integration, RK4 method is used.

    Parameters
    ----------
    delta: float
        length of the simulation step (s)
    """

    if self._state.error:
        return

    s = self._state.numpy4()
    h = self._integration_step
    h_2 = h / 2

    integration_step_n = int(delta / h)

    for _ in range(integration_step_n):
        a = self._eval_cart_acceleration()

        k1 = self._derivative(s, a)
        k2 = self._derivative(s + k1 * h_2, a)
        k3 = self._derivative(s + k2 * h_2, a)
        k4 = self._derivative(s + k3 * h, a)

        s += (k1 + 2 * k2 + 2 * k3 + k4) * h / 6

        self._integration_count += 1

        self._state = State(
            cart_position=s[0],
            cart_velocity=s[2],
            cart_acceleration=a,
            pole_angle=s[1],
            pole_angular_velocity=s[3],
            stamp=self._integration_step * self._integration_count,
        )

        self._state.validate(self._config)

    self._step_count += 1

close() -> None

Clear any resources used by the simulator.

Source code in cartpole/simulator.py 
257
258
259
260
261
def close(self) -> None:
    """
    Clear any resources used by the simulator.
    """
    pass