rtc-tools 2.7.3__py3-none-any.whl

rtctools/data/csv.py

@@ -0,0 +1,150 @@
+import logging
+import sys
+from datetime import datetime
+from typing import Union
+
+import numpy as np
+
+logger = logging.getLogger("rtctools")
+
+
+def _boolean_to_nan(data, fname):
+    """
+    Empty columns are detected as boolean full of "False". We instead want this to be np.nan.
+    We cannot distinguish between explicitly desired boolean columns, so instead we convert all
+    boolean columns to np.nan, and raise a warning.
+    """
+    data = data.copy()
+
+    dtypes_in = []
+    for i in range(0, len(data.dtype)):
+        dtypes_in.append(data.dtype.descr[i])
+
+    convert_to_nan = []
+    dtypes_out = []
+    for i, name in enumerate(data.dtype.names):
+        if dtypes_in[i][1][1] == "b":
+            convert_to_nan.append(name)
+            dtypes_out.append((dtypes_in[i][0], "<f8"))
+        else:
+            dtypes_out.append(dtypes_in[i])
+
+    if convert_to_nan:
+        logger.warning(
+            "Column(s) {} were detected as boolean in '{}'; converting to NaN".format(
+                ", ".join(["'{}'".format(name) for name in convert_to_nan]), fname
+            )
+        )
+        data = data.astype(dtypes_out)
+        for name in convert_to_nan:
+            data[name] = np.nan
+
+    return data
+
+
+def _string_to_datetime(string: Union[str, bytes]) -> datetime:
+    """Convert a string to a datetime object."""
+    if isinstance(string, bytes):
+        string = string.decode("utf-8")
+    return datetime.strptime(string, "%Y-%m-%d %H:%M:%S")
+
+
+def _string_to_float(string: Union[str, bytes]) -> float:
+    """Convert a string to a float."""
+    if isinstance(string, bytes):
+        string = string.decode("utf-8")
+    string = string.replace(",", ".")
+    return float(string)
+
+
+def load(fname, delimiter=",", with_time=False):
+    """
+    Check delimiter of csv and read contents to an array. Assumes no date-time conversion needed.
+
+    :param fname:     Filename.
+    :param delimiter: CSV column delimiter.
+    :param with_time: Whether the first column is expected to contain time stamps.
+
+    :returns: A named numpy array with the contents of the file.
+    """
+    c = {}
+    if with_time:
+        c.update({0: _string_to_datetime})
+
+    # Check delimiter of csv file. If semicolon, check if decimal separator is
+    # a comma.
+    if delimiter == ";":
+        with open(fname, "rb") as csvfile:
+            # Read the first line, this should be a header. Count columns by
+            # counting separator.
+            sample_csvfile = csvfile.readline()
+            n_semicolon = sample_csvfile.count(b";")
+            # We actually only need one number to evaluate if commas are used as decimal
+            # separator, but certain csv writers don't use a decimal when the value has
+            # no meaningful decimal(e.g. 12.0 becomes 12) so we read the next 1024 bytes
+            # to make sure we catch a number.
+            sample_csvfile = csvfile.read(1024)
+            # Count the commas
+            n_comma_decimal = sample_csvfile.count(b",")
+            # If commas are used as decimal separator, we need additional
+            # converters.
+            if n_comma_decimal:
+                c.update({i + len(c): _string_to_float for i in range(1 + n_semicolon - len(c))})
+
+    # Read the csv file and convert to array
+    try:
+        if len(c):  # Converters exist, so use them.
+            try:
+                data = np.genfromtxt(
+                    fname, delimiter=delimiter, deletechars="", dtype=None, names=True, converters=c
+                )
+                return _boolean_to_nan(data, fname)
+            except (
+                np.lib._iotools.ConverterError
+            ):  # value does not conform to expected date-time format
+                type, value, traceback = sys.exc_info()
+                logger.error(
+                    "CSVMixin: converter of csv reader failed on {}: {}".format(fname, value)
+                )
+                raise ValueError(
+                    "CSVMixin: wrong date time or value format in {}. "
+                    "Should be %Y-%m-%d %H:%M:%S and numerical values everywhere.".format(fname)
+                )
+        else:
+            data = np.genfromtxt(fname, delimiter=delimiter, deletechars="", dtype=None, names=True)
+            return _boolean_to_nan(data, fname)
+    except ValueError:
+        # can occur when delimiter changes after first 1024 bytes of file,
+        # or delimiter is not , or ;
+        type, value, traceback = sys.exc_info()
+        logger.error("CSV: Value reader of csv reader failed on {}: {}".format(fname, value))
+        raise ValueError(
+            "CSV: could not read all values from {}. Used delimiter '{}'. "
+            "Please check delimiter (should be ',' or ';' throughout the file) "
+            "and if all values are numbers.".format(fname, delimiter)
+        )
+
+
+def save(fname, data, delimiter=",", with_time=False):
+    """
+    Write the contents of an array to a csv file.
+
+    :param fname:     Filename.
+    :param data:      A named numpy array with the data to write.
+    :param delimiter: CSV column delimiter.
+    :param with_time: Whether to output the first column of the data as time stamps.
+    """
+    if with_time:
+        data["time"] = [t.strftime("%Y-%m-%d %H:%M:%S") for t in data["time"]]
+        fmt = ["%s"] + (len(data.dtype.names) - 1) * ["%f"]
+    else:
+        fmt = len(data.dtype.names) * ["%f"]
+
+    np.savetxt(
+        fname,
+        data,
+        delimiter=delimiter,
+        header=delimiter.join(data.dtype.names),
+        fmt=fmt,
+        comments="",
+    )

rtctools/data/interpolation/__init__.py

@@ -0,0 +1,3 @@
+from .bspline import *
+from .bspline1d import *
+from .bspline2d import *

rtctools/data/interpolation/bspline.py

@@ -0,0 +1,31 @@
+from casadi import if_else, logic_and
+
+
+class BSpline:
+    """
+    B-Spline base class.
+    """
+
+    def basis(self, t, x, k, i):
+        """
+        Evaluate the B-Spline basis function using Cox-de Boor recursion.
+
+        :param x: Point at which to evaluate.
+        :param k: Order of the basis function.
+        :param i: Knot number.
+
+        :returns: The B-Spline basis function of the given order, at the given knot, evaluated at
+            the given point.
+        """
+        if k == 0:
+            return if_else(logic_and(t[i] <= x, x < t[i + 1]), 1.0, 0.0)
+        else:
+            if t[i] < t[i + k]:
+                a = (x - t[i]) / (t[i + k] - t[i]) * self.basis(t, x, k - 1, i)
+            else:
+                a = 0.0
+            if t[i + 1] < t[i + k + 1]:
+                b = (t[i + k + 1] - x) / (t[i + k + 1] - t[i + 1]) * self.basis(t, x, k - 1, i + 1)
+            else:
+                b = 0.0
+            return a + b

rtctools/data/interpolation/bspline1d.py

@@ -0,0 +1,169 @@
+import numpy as np
+from casadi import SX, Function, if_else, inf, jacobian, logic_and, nlpsol, sum2, vertcat
+
+from .bspline import BSpline
+
+
+class BSpline1D(BSpline):
+    """
+    Arbitrary order, one-dimensional, non-uniform B-Spline implementation using Cox-de Boor
+    recursion.
+    """
+
+    def __init__(self, t, w, k=3):
+        """
+        Create a new 1D B-Spline object.
+
+        :param t: Knot vector.
+        :param w: Weight vector.
+        :param k: Spline order.
+        """
+
+        # Store arguments
+        self.__t = t
+        self.__w = w
+        self.__k = k
+
+    def __call__(self, x):
+        """
+        Evaluate the B-Spline at point x.
+
+        The support of this function is the half-open interval [t[0], t[-1]).
+
+        :param x: The point at which to evaluate.
+
+        :returns: The spline evaluated at the given point.
+        """
+        y = 0.0
+        for i in range(len(self.__t) - self.__k - 1):
+            y += if_else(
+                logic_and(x >= self.__t[i], x <= self.__t[i + self.__k + 1]),
+                self.__w[i] * self.basis(self.__t, x, self.__k, i),
+                0.0,
+            )
+        return y
+
+    @classmethod
+    def fit(
+        cls,
+        x,
+        y,
+        k=3,
+        monotonicity=0,
+        curvature=0,
+        num_test_points=100,
+        epsilon=1e-7,
+        delta=1e-4,
+        interior_pts=None,
+        ipopt_options=None,
+    ):
+        """
+        fit() returns a tck tuple like scipy.interpolate.splrep, but adjusts
+        the weights to meet the desired constraints to the curvature of the spline curve.
+
+        :param monotonicity:
+            - is an integer, magnitude is ignored
+            - if positive, causes spline to be monotonically increasing
+            - if negative, causes spline to be monotonically decreasing
+            - if 0, leaves spline monotonicity unconstrained
+
+        :param curvature:
+            - is an integer, magnitude is ignored
+            - if positive, causes spline curvature to be positive (convex)
+            - if negative, causes spline curvature to be negative (concave)
+            - if 0, leaves spline curvature unconstrained
+
+        :param num_test_points:
+            - sets the number of points that the constraints will be applied at across
+              the range of the spline
+
+        :param epsilon:
+            - offset of monotonicity and curvature constraints from zero, ensuring strict
+              monotonicity
+            - if epsilon is set to less than the tolerance of the solver, errors will result
+
+        :param delta:
+            - amount the first and last knots are extended outside the range of the splined points
+            - ensures that the spline evaluates correctly at the first and last nodes, as
+              well as the distance delta beyond these nodes
+
+        :param interior_pts:
+            - optional list of interior knots to use
+
+        :returns: A tuple of spline knots, weights, and order.
+        """
+        x = np.asarray(x)
+        y = np.asarray(y)
+        N = len(x)
+
+        if interior_pts is None:
+            # Generate knots: This algorithm is based on the Fitpack algorithm by p.dierckx
+            # The original code lives here: http://www.netlib.org/dierckx/
+            if k % 2 == 1:
+                interior_pts = x[k // 2 + 1 : -k // 2]
+            else:
+                interior_pts = (x[k // 2 + 1 : -k // 2] + x[k // 2 : -k // 2 - 1]) / 2
+        t = np.concatenate(
+            (np.full(k + 1, x[0] - delta), interior_pts, np.full(k + 1, x[-1] + delta))
+        )
+        num_knots = len(t)
+
+        # Casadi Variable Symbols
+        c = SX.sym("c", num_knots)
+        x_sym = SX.sym("x")
+
+        # Casadi Representation of Spline Function & Derivatives
+        expr = cls(t, c, k)(x_sym)
+        free_vars = [c, x_sym]
+        bspline = Function("bspline", free_vars, [expr])
+        J = jacobian(expr, x_sym)
+        # bspline_prime = Function('bspline_prime', free_vars, [J])
+        H = jacobian(J, x_sym)
+        bspline_prime_prime = Function("bspline_prime_prime", free_vars, [H])
+
+        # Objective Function
+        xpt = SX.sym("xpt")
+        ypt = SX.sym("ypt")
+        sq_diff = Function("sq_diff", [c, xpt, ypt], [(ypt - bspline(c, xpt)) ** 2])
+        sq_diff = sq_diff.map(N, "serial")
+        f = sum2(sq_diff(c, SX(x), SX(y)))
+
+        # Setup Curvature Constraints
+        delta_c_max = np.full(num_knots - 1, inf)
+        delta_c_min = np.full(num_knots - 1, -inf)
+        max_slope_slope = np.full(num_test_points, inf)
+        min_slope_slope = np.full(num_test_points, -inf)
+        if monotonicity != 0:
+            if monotonicity < 0:
+                delta_c_max = np.full(num_knots - 1, -epsilon)
+            else:
+                delta_c_min = np.full(num_knots - 1, epsilon)
+        if curvature != 0:
+            if curvature < 0:
+                max_slope_slope = np.full(num_test_points, -epsilon)
+            else:
+                min_slope_slope = np.full(num_test_points, epsilon)
+        monotonicity_constraints = vertcat(*[c[i + 1] - c[i] for i in range(num_knots - 1)])
+        x_linspace = np.linspace(x[0], x[-1], num_test_points)
+        curvature_constraints = vertcat(*[bspline_prime_prime(c, SX(x)) for x in x_linspace])
+        g = vertcat(monotonicity_constraints, curvature_constraints)
+        lbg = np.concatenate((delta_c_min, min_slope_slope))
+        ubg = np.concatenate((delta_c_max, max_slope_slope))
+
+        # Perform mini-optimization problem to calculate the the values of c
+        nlp = {"x": c, "f": f, "g": g}
+        my_solver = "ipopt"
+        solver = nlpsol(
+            "solver",
+            my_solver,
+            nlp,
+            {"print_time": 0, "expand": True, "ipopt": ipopt_options},
+        )
+        sol = solver(lbg=lbg, ubg=ubg)
+        stats = solver.stats()
+        return_status = stats["return_status"]
+        if return_status not in ["Solve_Succeeded", "Solved_To_Acceptable_Level", "SUCCESS"]:
+            raise Exception("Spline fitting failed with status {}".format(return_status))
+
+        # Return the new tck tuple
+        return (t, np.array(sol["x"]).ravel(), k)

rtctools/data/interpolation/bspline2d.py

@@ -0,0 +1,54 @@
+from casadi import if_else, logic_and
+
+from .bspline import BSpline
+
+
+class BSpline2D(BSpline):
+    """
+    Arbitrary order, two-dimensional, non-uniform B-Spline.
+    """
+
+    def __init__(self, tx, ty, w, kx=3, ky=3):
+        """
+        Create a new 2D B-Spline object.
+
+        :param tx: Knot vector in X direction.
+        :param ty: Knot vector in Y direction.
+        :param w:  Weight vector.
+        :param kx: Spline order in X direction.
+        :param ky: Spline order in Y direction.
+        """
+
+        # Store arguments
+        self.__tx = tx
+        self.__ty = ty
+        self.__w = w
+        self.__kx = kx
+        self.__ky = ky
+
+    def __call__(self, x, y):
+        """
+        Evaluate the B-Spline at point (x, y).
+
+        The support of this function is the half-open interval [tx[0], tx[-1]) x [ty[0], ty[-1]).
+
+        :param x: The coordinate of the point at which to evaluate.
+        :param y: The ordinate of the point at which to evaluate.
+
+        :returns: The spline evaluated at the given point.
+        """
+        z = 0.0
+        for i in range(len(self.__tx) - self.__kx - 1):
+            bx = if_else(
+                logic_and(x >= self.__tx[i], x <= self.__tx[i + self.__kx + 1]),
+                self.basis(self.__tx, x, self.__kx, i),
+                0.0,
+            )
+            for j in range(len(self.__ty) - self.__ky - 1):
+                by = if_else(
+                    logic_and(y >= self.__ty[j], y <= self.__ty[j + self.__ky + 1]),
+                    self.basis(self.__ty, y, self.__ky, j),
+                    0.0,
+                )
+                z += self.__w[i * (len(self.__ty) - self.__ky - 1) + j] * bx * by
+        return z