"""
Transfer blocks:
- have inputs and outputs
- have state variables
- are a subclass of ``TransferBlock`` |rarr| ``Block``
"""
import numpy as np
import scipy.signal
import math
from math import sin, cos, atan2, sqrt, pi
import matplotlib.pyplot as plt
from spatialmath import base
from bdsim.components import TransferBlock
[docs]class Integrator(TransferBlock):
"""
:blockname:`INTEGRATOR`
.. table::
:align: left
+------------+---------+---------+
| inputs | outputs | states |
+------------+---------+---------+
| 1 | 1 | N |
+------------+---------+---------+
| float, | float, | |
| A(N,) | A(N,) | |
+------------+---------+---------+
"""
nin = 1
nout = 1
[docs] def __init__(self, x0=0, min=None, max=None, **blockargs):
"""
Integrator.
:param x0: Initial state, defaults to 0
:type x0: array_like, optional
:param min: Minimum value of state, defaults to None
:type min: float or array_like, optional
:param max: Maximum value of state, defaults to None
:type max: float or array_like, optional
:param blockargs: |BlockOptions|
:type blockargs: dict
:return: an INTEGRATOR block
:rtype: Integrator instance
Output is the time integral of the input. The state can be a scalar or a
vector. The initial state, and type, is given by ``x0``. The shape of
the input signal must match ``x0``.
The minimum and maximum values can be:
- a scalar, in which case the same value applies to every element of
the state vector, or
- a vector, of the same shape as ``x0`` that applies elementwise to
the state.
"""
super().__init__(**blockargs)
if isinstance(x0, (int, float)):
self.nstates = 1
if min is None:
min = -math.inf
if max is None:
max = math.inf
else:
if isinstance(x0, np.ndarray):
if x0.ndim > 1:
raise ValueError('state must be a 1D vector')
else:
x0 = base.getvector(x0)
self.nstates = x0.shape[0]
if min is None:
min = [-math.inf] * self.nstates
elif len(min) != self.nstates:
raise ValueError('minimum bound length must match x0')
if max is None:
max = [math.inf] * self.nstates
elif len(max) != self.nstates:
raise ValueError('maximum bound length must match x0')
self._x0 = np.r_[x0]
self.min = np.r_[min]
self.max = np.r_[max]
print('nstates', self.nstates)
def output(self, t=None):
return [self._x]
def deriv(self):
xd = np.array(self.inputs)
for i in range(0, self.nstates):
if self._x[i] < self.min[i] or self._x[i] > self.max[i]:
xd[i] = 0
return xd
[docs]class PoseIntegrator(TransferBlock):
"""
:blockname:`POSEINTEGRATOR`
.. table::
:align: left
+------------+---------+---------+
| inputs | outputs | states |
+------------+---------+---------+
| 1 | 1 | N |
+------------+---------+---------+
| A(N,) | A(N,) | |
+------------+---------+---------+
"""
nin = 1
nout = 1
[docs] def __init__(self, x0=None, **blockargs):
r"""
Pose integrator
:param x0: Initial pose, defaults to null
:type x0: SE3, optional
:param blockargs: |BlockOptions|
:type blockargs: dict
:return: an INTEGRATOR block
:rtype: Integrator instance
This block integrates spatial velocity over time.
The block input is a spatial velocity as a 6-vector
:math:`(v_x, v_y, v_z, \omega_x, \omega_y, \omega_z)` and the output
is pose as an ``SE3`` instance.
.. note:: State is a velocity twist.
.. warning:: NOT WORKING YET
"""
super().__init__(**blockargs)
if x0 is None:
x0 = SE3()
self.nstates = 6
self._x0 = np.r_[x0]
print('nstates', self.nstates)
def output(self, t=None):
return [Twist3(self._x).SE3(1)]
def deriv(self):
xd = np.array(self.inputs)
for i in range(0, self.nstates):
if self._x[i] < self.min[i] or self._x[i] > self.max[i]:
xd[i] = 0
return xd
# ------------------------------------------------------------------------ #
[docs]class LTI_SS(TransferBlock):
"""
:blockname:`LTI_SS`
.. table::
:align: left
+------------+---------+---------+
| inputs | outputs | states |
+------------+---------+---------+
| 1 | 1 | nc |
+------------+---------+---------+
| float, | float, | |
| A(nb,) | A(nc,) | |
+------------+---------+---------+
"""
nin = 1
nout = 1
[docs] def __init__(self, A=None, B=None, C=None, x0=None, **blockargs):
r"""
State-space LTI dynamics.
:param N: numerator coefficients, defaults to 1
:type N: array_like, optional
:param D: denominator coefficients, defaults to [1,1]
:type D: array_like, optional
:param x0: initial states, defaults to None
:type x0: array_like, optional
:param blockargs: |BlockOptions|
:type blockargs: dict
:return: A SCOPE block
:rtype: LTI_SISO instance
Implements the dynamics of a single-input single-output (SISO) linear
time invariant (LTI) system described by numerator and denominator
polynomial coefficients.
Coefficients are given in the order from highest order to zeroth
order, ie. :math:`2s^2 - 4s +3` is ``[2, -4, 3]``.
Only proper transfer functions, where order of numerator is less
than denominator are allowed.
The order of the states in ``x0`` is consistent with controller canonical
form.
Examples::
LTI_SISO(N=[1,2], D=[2, 3, -4])
is the transfer function :math:`\frac{s+2}{2s^2+3s-4}`.
"""
#print('in SS constructor')
assert A.shape[0] == A.shape[1], 'A must be square'
n = A.shape[0]
if len(B.shape) == 1:
nin = 1
B = B.reshape((n, 1))
else:
nin = B.shape[1]
assert B.shape[0] == n, 'B must have same number of rows as A'
if len(C.shape) == 1:
nout = 1
assert C.shape[0] == n, 'C must have same number of columns as A'
C = C.reshape((1, n))
else:
nout = C.shape[0]
assert C.shape[1] == n, 'C must have same number of columns as A'
super().__init__(**blockargs)
self.A = A
self.B = B
self.C = C
self.nstates = A.shape[0]
if x0 is None:
self._x0 = np.zeros((self.nstates,))
else:
self._x0 = x0
def output(self, t=None):
return list(self.C @ self._x)
def deriv(self):
return self.A @ self._x + self.B @ np.array(self.inputs)
# ------------------------------------------------------------------------ #
[docs]class LTI_SISO(LTI_SS):
"""
:blockname:`LTI_SISO`
.. table::
:align: left
+------------+---------+---------+
| inputs | outputs | states |
+------------+---------+---------+
| 1 | 1 | n |
+------------+---------+---------+
| float | float | |
+------------+---------+---------+
"""
nin = 1
nout = 1
[docs] def __init__(self, N=1, D=[1, 1], x0=None, **blockargs):
r"""
SISO LTI dynamics.
:param N: numerator coefficients, defaults to 1
:type N: array_like, optional
:param D: denominator coefficients, defaults to [1,1]
:type D: array_like, optional
:param x0: initial states, defaults to None
:type x0: array_like, optional
:param blockargs: |BlockOptions|
:type blockargs: dict
:return: A SCOPE block
:rtype: LTI_SISO instance
Implements the dynamics of a single-input single-output (SISO) linear
time invariant (LTI) system described by numerator and denominator
polynomial coefficients.
Coefficients are given in the order from highest order to zeroth
order, ie. :math:`2s^2 - 4s +3` is ``[2, -4, 3]``.
Only proper transfer functions, where order of numerator is less
than denominator are allowed.
The order of the states in ``x0`` is consistent with controller canonical
form.
Examples::
LTI_SISO(N=[1, 2], D=[2, 3, -4])
is the transfer function :math:`\frac{s+2}{2s^2+3s-4}`.
"""
#print('in SISO constscutor')
if not isinstance(N, list):
N = [N]
if not isinstance(D, list):
D = [D]
self.N = N
self.D = N
n = len(D) - 1
nn = len(N)
if x0 is None:
x0 = np.zeros((n,))
assert nn <= n, 'direct pass through is not supported'
# convert to numpy arrays
N = np.r_[np.zeros((len(D) - len(N),)), np.array(N)]
D = np.array(D)
# normalize the coefficients to obtain
#
# b_0 s^n + b_1 s^(n-1) + ... + b_n
# ---------------------------------
# a_0 s^n + a_1 s^(n-1) + ....+ a_n
# normalize so leading coefficient of denominator is one
# D0 = D[0]
# D = D / D0
# N = N / D0
# A = np.eye(len(D) - 1, k=1) # control canonic (companion matrix) form
# A[-1, :] = -D[1:]
# B = np.zeros((n, 1))
# B[-1] = 1
# C = (N[1:] - N[0] * D[1:]).reshape((1, n))
A, B, C, D = scipy.signal.tf2ss(N, D)
self.num = N
self.den = D
if len(np.flatnonzero(D)) > 0:
raise ValueError('D matrix is not zero')
super().__init__(A=A, B=B, C=C, x0=x0, **blockargs)
if self.verbose:
print('A=', A)
print('B=', B)
print('C=', C)
def change_param(self, param, newvalue):
if param == 'num':
self.num = newvalue
elif param == 'den':
self.den = newvalue
self.A, self.B, self.C, self.D = scipy.signal.tf2ss(self.num, self.den)
self.add_param('num', change_param)
self.add_param('den', change_param)
if __name__ == "__main__":
import pathlib
import os.path
exec(open(os.path.join(pathlib.Path(
__file__).parent.absolute(), "test_transfers.py")).read())