"""
Transfer blocks:
- have inputs and outputs
- have state variables
- are a subclass of ``TransferBlock`` |rarr| ``Block``
"""
import numpy as np
import math
from math import sin, cos, atan2, sqrt, pi
import matplotlib.pyplot as plt
import inspect
from spatialmath import base, Twist3, SE3
from bdsim.components import ClockedBlock
# ------------------------------------------------------------------------
[docs]class ZOH(ClockedBlock):
"""
:blockname:`ZOH`
.. table::
:align: left
+------------+---------+---------+
| inputs | outputs | states |
+------------+---------+---------+
| 1 | 1 | N |
+------------+---------+---------+
| float, | float, | |
| A(N,) | A(N,) | |
+------------+---------+---------+
"""
nin = 1
nout = 1
[docs] def __init__(self, clock, x0=0, **blockargs):
"""
Zero-order hold.
:param clock: clock source
:type clock: Clock
:param x0: Initial value of the hold, defaults to 0
:type x0: array_like, optional
:param blockargs: |BlockOptions|
:type blockargs: dict
:return: a ZOH block
:rtype: Integrator instance
Output is the input at the previous clock time. The state can be a scalar or a
vector, this is given by the type of ``x0``.
.. note:: If input is not a scalar, ``x0`` must have the shape of the
input signal.
"""
self.type = 'sampler'
super().__init__(nin=1, nout=1, clock=clock, **blockargs)
x0 = base.getvector(x0)
self._x0 = x0
self.ndstates = len(x0)
# print('nstates', self.nstates)
def output(self, t=None):
# print('* output, x is ', self._x)
return [self._x]
[docs] def next(self):
xnext = np.array(self.inputs)
return xnext
# ------------------------------------------------------------------------
[docs]class DIntegrator(ClockedBlock):
"""
:blockname:`DINTEGRATOR`
.. table::
:align: left
+------------+---------+---------+
| inputs | outputs | states |
+------------+---------+---------+
| 1 | 1 | N |
+------------+---------+---------+
| float, | float, | |
| A(N,) | A(N,) | |
+------------+---------+---------+
"""
nin = 1
nout = 1
[docs] def __init__(self, clock, x0=0, gain=1.0, min=None, max=None, **blockargs):
"""
Discrete-time integrator.
:param clock: clock source
:type clock: Clock
: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
Create a discrete-time integrator block.
Output is the time integral of the input. The state can be a scalar or a
vector, this is given by the type of ``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__(clock=clock, **blockargs)
if isinstance(x0, (int, float)):
self.ndstates = 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.ndstates = 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]
self.gain = gain
self.ndstates = len(x0)
def output(self, t=None):
return [self._x]
[docs] def next(self):
xnext = self._x + self.gain * self.clock.T * np.array(self.inputs[0])
return xnext
[docs]class DPoseIntegrator(ClockedBlock):
"""
:blockname:`DPOSEINTEGRATOR`
.. table::
:align: left
+------------+---------+---------+
| inputs | outputs | states |
+------------+---------+---------+
| 1 | 1 | N |
+------------+---------+---------+
| A(6,) | SE3 | |
+------------+---------+---------+
"""
nin = 1
nout = 1
inlabels = ('ν',)
outlabels = ('ξ',)
[docs] def __init__(self, clock, x0=None, **blockargs):
r"""
Discrete-time spatial velocity integrator.
:param clock: clock source
:type clock: Clock
:param x0: Initial pose, defaults to null
:type x0: SE3, optional
:param blockargs: |BlockOptions|
:type blockargs: dict
:return: an DPOSEINTEGRATOR 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.
"""
super().__init__(clock=clock, **blockargs)
if x0 is None:
x0 = Twist3()
elif isinstance(x0, SE3):
x0 = Twist3(x0).A
elif isinstance(x0, Twist3):
x0 = x0.A
elif isvector(x0, 6):
x0 = getvector(x0, 6)
self.ndstates = 6
self._x0 = x0
print('nstates', self.nstates, x0)
def output(self, t=None):
return [Twist3(self._x).SE3()]
[docs] def next(self):
T_delta = SE3.Delta(self.inputs[0] * self.clock.T)
pose = Twist3(self._x).SE3() * T_delta
return Twist3(pose).A
# ------------------------------------------------------------------------ #
# @block
# class LTI_SS(TransferBlock):
# """
# :blockname:`LTI_SS`
# .. table::
# :align: left
# +------------+---------+---------+
# | inputs | outputs | states |
# +------------+---------+---------+
# | 1 | 01 | nc |
# +------------+---------+---------+
# | float, | float, | |
# | A(nb,) | A(nc,) | |
# +------------+---------+---------+
# """
# def __init__(self, *inputs, A=None, B=None, C=None, x0=None, verbose=False, **blockargs):
# r"""
# :param ``*inputs``: Optional incoming connections
# :type ``*inputs``: Block or Plug
# :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 zero
# :type x0: array_like, optional
# :param ``**blockargs``: |BlockOptions|
# :return: A SCOPE block
# :rtype: LTI_SISO instance
# Create a state-space LTI block.
# Describes 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')
# self.type = 'LTI SS'
# 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__(nin=nin, nout=nout, inputs=inputs, **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)
# # ------------------------------------------------------------------------ #
# @block
# class LTI_SISO(LTI_SS):
# """
# :blockname:`LTI_SISO`
# .. table::
# :align: left
# +------------+---------+---------+
# | inputs | outputs | states |
# +------------+---------+---------+
# | 1 | 1 | n |
# +------------+---------+---------+
# | float | float | |
# +------------+---------+---------+
# """
# def __init__(self, N=1, D=[1, 1], *inputs, x0=None, verbose=False, **blockargs):
# r"""
# :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 ``*inputs``: Optional incoming connections
# :type ``*inputs``: Block or Plug
# :param x0: initial states, defaults to zero
# :type x0: array_like, optional
# :param ``**blockargs``: |BlockOptions|
# :return: A SCOPE block
# :rtype: LTI_SISO instance
# Create a SISO LTI block.
# Describes 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))
# if verbose:
# print('A=', A)
# print('B=', B)
# print('C=', C)
# super().__init__(A=A, B=B, C=C, x0=x0, **blockargs)
# self.type = 'LTI'
# if __name__ == "__main__":
# import pathlib
# import os.path
# exec(open(os.path.join(pathlib.Path(
# __file__).parent.absolute(), "test_transfers.py")).read())