def testHSVD(self):
A = np.array([[1., -2.], [3., -4.]])
B = np.array([[5.], [7.]])
C = np.array([[6., 8.]])
D = np.array([[9.]])
sys = ss(A, B, C, D)
hsv = hsvd(sys)
hsvtrue = np.array([24.42686, 0.5731395]) # from MATLAB
np.testing.assert_array_almost_equal(hsv, hsvtrue)
# Make sure default type values are correct
self.assertTrue(isinstance(hsv, np.ndarray))
self.assertFalse(isinstance(hsv, np.matrix))
# Check that using numpy.matrix does *not* affect answer
with warnings.catch_warnings(record=True) as w:
control.use_numpy_matrix(True)
self.assertTrue(issubclass(w[-1].category, UserWarning))
# Redefine the system (using np.matrix for storage)
sys = ss(A, B, C, D)
# Compute the Hankel singular value decomposition
hsv = hsvd(sys)
# Make sure that return type is correct
self.assertTrue(isinstance(hsv, np.ndarray))
self.assertFalse(isinstance(hsv, np.matrix))
# Go back to using the normal np.array representation
control.use_numpy_matrix(False)
import numpy as np
import math
from scipy.integrate import solve_ivp
import control
from drawcartpend import drawcartpend
from cartpend import cartpendSystem
import taichi as ti
ti.init(debug=True)
control.use_numpy_matrix(flag=False)
m = 1.0 # mass of ball
M = 5.0 # mass of cart
L = 2.0 # pendulum length
g = 9.8 # gravity
d = 4.0 # dampping term
#############################
# Linearized system and place pole
A = np.array([[0, 1, 0, 0], [0, -d, -(g * m) / M, 0], [0, 0, 0, 1],
[0, 0, (g * m + M * g) / (L * M), -0.1 * d]],
dtype=np.float)
B = np.array([[0], [1.0 / M], [0], [-1.0 / (L * M)]], dtype=np.float)
print("eig A", np.linalg.eig(A))
C = control.ctrb(A, B)
print("ctrb: ", C)
print("rank(ctrb): ", np.linalg.matrix_rank(C))
# Design a linear controller using pole-placement method
import numpy as np
import gym
import control
import gym_CartPole_BT
from gym_CartPole_BT.systems.cartpend import cartpend_ss
control.use_numpy_matrix(False)
env = gym.make('CartPole-BT-v0')
env.reset()
m = env.masspole # Mass of pendulum
M = env.masscart # Mass of cart
L = env.length # Length of pendulum
g = env.gravity # Acceleration due to gravity
d = env.friction # Damping coefficient for friction between cart and ground
s = 1 # 1 for pendulum up position or -1 for down position
# Generate state space model matrices for linearized system
A, B = cartpend_ss(m, M, L, g, d, s)
# Choose poles of desired closed-loop system
# The poles determine the speed of the controller in
# manipulating each state
p = [-1, -2, -5, -2.5]
# Compute gain matrix using pole placement
K = control.place(A, B, p)
def setUp(self):
# Use array instead of matrix (and save old value to restore at end)
control.use_numpy_matrix(False)
def matarrayout(request):
"""Switch the config to use np.ndarray and np.matrix as returns"""
restore = control.config.defaults['statesp.use_numpy_matrix']
control.use_numpy_matrix(request.param == "matrixout", warn=False)
yield
control.use_numpy_matrix(restore, warn=False)
Lateral Motion Model using the python-control package The model uses a bicycle model that assumes zero wheel slip! Script created by [email protected] and [email protected] as a project of TRAFFIC MODELLING, SIMULATION AND CONTROL subject """ from cmath import sqrt import control as ct import numpy as np from random import uniform # Base configuration ct.use_fbs_defaults() ct.use_numpy_matrix(False) class LateralModel: """ Class for keeping track of the agent movement in case of continuous SUMO state space. """ def __init__(self, state, lane_width, dt=0.1): """ Function to initiate the lateral Model, it will set the parameters. :param state: dict of system state :param lane_width: float, width of initial lane in meters [m] :param dt: timestep increment of the simulation """
import copy
from typing import Tuple, Union
import control as _ctrl
import numpy as _np
from numpy import inf
from .linalg import block, eye, matrix, norm, zeros, eig_max
from .quantizer import DynamicQuantizer as _DynamicQuantizer
from .quantizer import StaticQuantizer as _StaticQuantizer
from .quantizer import _ConnectionType
_ctrl.use_numpy_matrix(False)
__all__ = [
'IdealSystem',
'Controller',
'Plant',
]
# TODO: support control.input_output_response()
# TODO: define function returns control.InputOutputSystem
class Controller(object):
"""
An instance of `Controller` represents the state-space model
of controller K given by
K : { x(t+1) = A x(t) + B1 r(t) + B2 y(t)
{ u(t) = C x(t) + D1 r(t) + D2 y(t)
"""
def setUp(self):
ct.use_numpy_matrix(False)
original author: @ggleizer
modified by @asamant to include only relevant parts.
"""
import numpy as np
from numpy import random
import control as ct
import warnings
import scipy.linalg as la
from scipy import integrate
from traffic_models.etcutil import normalize_state_to_lyapunov
import logging
import cvxpy as cvx
ct.use_numpy_matrix(flag=False)
_MAX_TRIES_TIGHT_LYAPUNOV = 10
_ASSERT_SMALL_NUMBER = 1e-6
_LMIS_SMALL_IDENTITY_FACTOR = 1e-6
''' linearetc.py '''
''' Main building blocks for building your ETC implementation'''
'''
AUXILIARY
'''
def is_positive_definite(A):
return (la.eig(A)[0] > 0).all()
