def interp(x, xp, fp, left=None, right=None, period=None):
"""
One-dimensional linear interpolation, analogous to numpy.interp().
Returns the one-dimensional piecewise linear interpolant to a function with given discrete data points (xp, fp),
evaluated at x.
See syntax here: https://numpy.org/doc/stable/reference/generated/numpy.interp.html
Specific notes: xp is assumed to be sorted.
"""
if not is_casadi_type([x, xp, fp], recursive=True):
return _onp.interp(x=x,
xp=xp,
fp=fp,
left=left,
right=right,
period=period)
else:
### If xp or x are CasADi types, this is unsupported :(
if is_casadi_type([x, xp], recursive=True):
raise NotImplementedError(
"Unfortunately, CasADi doesn't yet support a dispatch for x or xp as CasADi types."
)
### Handle period argument
if period is not None:
if any(logical_or(xp < 0, xp > period)):
raise NotImplementedError(
"Haven't yet implemented handling for if xp is outside the period."
) # Not easy to implement because casadi doesn't have a sort feature.
x = _cas.mod(x, period)
### Make sure x isn't an int
if isinstance(x, int):
x = float(x)
### Make sure that x is an iterable
try:
x[0]
except TypeError:
x = array([x], dtype=float)
### Make sure xp is an iterable
xp = array(xp, dtype=float)
### Do the interpolation
f = _cas.interp1d(xp, fp, x)
### Handle left/right
if left is not None:
f = where(x < xp[0], left, f)
if right is not None:
f = where(x > xp[-1], right, f)
### Return
return f
def rotation_matrix_3D(angle, axis, _axis_already_normalized=False):
"""
Gives the 3D rotation matrix from an angle and an axis.
An implmentation of https://en.wikipedia.org/wiki/Rotation_matrix#Rotation_matrix_from_axis_and_angle
:param angle: can be one angle or a vector (1d ndarray) of angles. Given in radians. # TODO note deprecated functionality; must be scalar
Direction corresponds to the right-hand rule.
:param axis: a 1d numpy array of length 3 (x,y,z). Represents the angle.
:param _axis_already_normalized: boolean, skips normalization for speed if you flag this true.
:return:
* If angle is a scalar, returns a 3x3 rotation matrix.
* If angle is a vector, returns a 3x3xN rotation matrix.
"""
if not _axis_already_normalized:
axis = axis / linalg.norm(axis)
sintheta = _onp.sin(angle)
costheta = _onp.cos(angle)
cpm = array([
[0, -axis[2], axis[1]],
[axis[2], 0, -axis[0]],
[-axis[1], axis[0], 0],
]) # The cross product matrix of the rotation axis vector
outer_axis = linalg.outer(axis, axis)
rot_matrix = costheta * _onp.eye(3) + sintheta * cpm + (
1 - costheta) * outer_axis
return rot_matrix
def rotation_matrix_2D(angle, ):
"""
Gives the 2D rotation matrix associated with a counterclockwise rotation about an angle.
Args:
angle: Angle by which to rotate. Given in radians.
Returns: The 2D rotation matrix
"""
sintheta = _onp.sin(angle)
costheta = _onp.cos(angle)
rotation_matrix = array([[costheta, -sintheta], [sintheta, costheta]])
return rotation_matrix
def rotation_matrix_from_euler_angles(roll_angle: Union[float,
_onp.ndarray] = 0,
pitch_angle: Union[float,
_onp.ndarray] = 0,
yaw_angle: Union[float,
_onp.ndarray] = 0,
as_array: bool = True):
"""
Yields the rotation matrix that corresponds to a given Euler angle rotation.
Note: This uses the standard (yaw, pitch, roll) Euler angle rotation, where:
* First, a rotation about x is applied (roll)
* Second, a rotation about y is applied (pitch)
* Third, a rotation about z is applied (yaw)
In other words: R = R_z(yaw) @ R_y(pitch) @ R_x(roll).
Note: To use this, pre-multiply your vector to go from body axes to earth axes.
Example:
>>> vector_earth = rotation_matrix_from_euler_angles(np.pi / 4, np.pi / 4, np.pi / 4) @ vector_body
See notes:
http://planning.cs.uiuc.edu/node102.html
Args:
roll_angle: The roll angle, which is a rotation about the x-axis. [radians]
pitch_angle: The pitch angle, which is a rotation about the y-axis. [radians]
yaw_angle: The yaw angle, which is a rotation about the z-axis. [radians]
as_array:
Returns:
"""
sa = sin(yaw_angle)
ca = cos(yaw_angle)
sb = sin(pitch_angle)
cb = cos(pitch_angle)
sc = sin(roll_angle)
cc = cos(roll_angle)
rot = [[ca * cb, ca * sb * sc - sa * cc, ca * sb * cc + sa * sc],
[sa * cb, sa * sb * sc + ca * cc, sa * sb * cc - ca * sc],
[-sb, cb * sc, cb * cc]]
if as_array:
return array(rot)
else:
return rot
def rotation_matrix_2D(
angle,
as_array: bool = True,
):
"""
Gives the 2D rotation matrix associated with a counterclockwise rotation about an angle.
Args:
angle: Angle by which to rotate. Given in radians.
as_array: Determines whether to return an array-like or just a simple list of lists.
Returns: The 2D rotation matrix
"""
s = sin(angle)
c = cos(angle)
rot = [[c, -s], [s, c]]
if as_array:
return array(rot)
else:
return rot
def finite_difference_coefficients(
x: _onp.ndarray,
x0: float = 0,
derivative_degree: int = 1,
) -> _onp.ndarray:
"""
Computes the weights (coefficients) in compact finite differece formulas for any order of derivative
and to any order of accuracy on one-dimensional grids with arbitrary spacing.
(Wording above is taken from the paper below, as are docstrings for parameters.)
Modified from an implementation of:
Fornberg, Bengt, "Generation of Finite Difference Formulas on Arbitrarily Spaced Grids". Oct. 1988.
Mathematics of Computation, Volume 51, Number 184, pages 699-706.
PDF: https://www.ams.org/journals/mcom/1988-51-184/S0025-5718-1988-0935077-0/S0025-5718-1988-0935077-0.pdf
More detail: https://en.wikipedia.org/wiki/Finite_difference_coefficient
Args:
derivative_degree: The degree of the derivative that you are interested in obtaining. (denoted "M" in the
paper)
x: The grid points (not necessarily uniform or in order) that you want to obtain weights for. You must
provide at least as many grid points as the degree of the derivative that you're interested in, plus 1.
The order of accuracy of your derivative depends in part on the number of grid points that you provide.
Specifically:
order_of_accuracy = n_grid_points - derivative_degree
(This is in general; can be higher in special cases.)
For example, if you're evaluating a second derivative and you provide three grid points, you'll have a
first-order-accurate answer.
(x is denoted "alpha" in the paper)
x0: The location that you are interested in obtaining a derivative at. This need not be on a grid point.
Complexity is O(derivative_degree * len(x) ^ 2)
Returns: A 1D ndarray corresponding to the coefficients that should be placed on each grid point. In other words,
the approximate derivative at `x0` is the dot product of `coefficients` and the function values at each of the
grid points `x`.
"""
### Check inputs
if derivative_degree < 1:
return ValueError(
"The parameter derivative_degree must be an integer >= 1.")
expected_order_of_accuracy = length(x) - derivative_degree
if expected_order_of_accuracy < 1:
return ValueError(
"You need to provide at least (derivative_degree+1) grid points in the x vector."
)
### Implement algorithm; notation from paper in docstring.
N = length(x) - 1
delta = _onp.zeros(shape=(derivative_degree + 1, N + 1, N + 1), dtype="O")
delta[0, 0, 0] = 1
c1 = 1
for n in range(
1, N + 1
): # TODO make this algorithm more efficient; we only need to store a fraction of this data.
c2 = 1
for v in range(n):
c3 = x[n] - x[v]
c2 = c2 * c3
# if n <= M: # Omitted because d is initialized to zero.
# d[n, n - 1, v] = 0
for m in range(min(n, derivative_degree) + 1):
delta[m, n, v] = ((x[n] - x0) * delta[m, n - 1, v] -
m * delta[m - 1, n - 1, v]) / c3
for m in range(min(n, derivative_degree) + 1):
delta[m, n,
n] = (c1 / c2 * (m * delta[m - 1, n - 1, n - 1] -
(x[n - 1] - x0) * delta[m, n - 1, n - 1]))
c1 = c2
coefficients_object_array = delta[derivative_degree, -1, :]
coefficients = array([
*coefficients_object_array
]) # Reconstructs using aerosandbox.numpy to intelligently type
return coefficients
def interpn(points: Tuple[_onp.ndarray],
values: _onp.ndarray,
xi: _onp.ndarray,
method: str = "linear",
bounds_error=True,
fill_value=_onp.NaN) -> _onp.ndarray:
"""
Performs multidimensional interpolation on regular grids. Analogue to scipy.interpolate.interpn().
See syntax here: https://docs.scipy.org/doc/scipy/reference/generated/scipy.interpolate.interpn.html
Args:
points: The points defining the regular grid in n dimensions. Tuple of coordinates of each axis.
values: The data on the regular grid in n dimensions.
xi: The coordinates to sample the gridded data at.
method: The method of interpolation to perform. one of:
* "bspline" (Note: differentiable and suitable for optimization - made of piecewise-cubics. For other
applications, other interpolators may be faster. Not monotonicity-preserving - may overshoot.)
* "linear" (Note: differentiable, but not suitable for use in optimization w/o subgradient treatment due
to C1-discontinuity)
* "nearest" (Note: NOT differentiable, don't use in optimization. Fast.)
bounds_error: If True, when interpolated values are requested outside of the domain of the input data,
a ValueError is raised. If False, then fill_value is used.
fill_value: If provided, the value to use for points outside of the interpolation domain. If None,
values outside the domain are extrapolated.
Returns: Interpolated values at input coordinates.
"""
### Check input types for points and values
if is_casadi_type([points, values], recursive=True):
raise TypeError(
"The underlying dataset (points, values) must consist of NumPy arrays."
)
### Check dimensions of points
for points_axis in points:
points_axis = array(points_axis)
if not len(points_axis.shape) == 1:
raise ValueError(
"`points` must consist of a tuple of 1D ndarrays defining the coordinates of each axis."
)
### Check dimensions of values
implied_values_shape = tuple(len(points_axis) for points_axis in points)
if not values.shape == implied_values_shape:
raise ValueError(f"""
The shape of `values` should be {implied_values_shape}.
""")
if ( ### NumPy implementation
not is_casadi_type([points, values, xi], recursive=True)) and (
(method == "linear") or (method == "nearest")):
xi = _onp.array(xi).reshape((-1, len(implied_values_shape)))
return _interpolate.interpn(points=points,
values=values,
xi=xi,
method=method,
bounds_error=bounds_error,
fill_value=fill_value)
elif ( ### CasADi implementation
(method == "linear") or (method == "bspline")):
### Add handling to patch a specific bug in CasADi that occurs when `values` is all zeros.
### For more information, see: https://github.com/casadi/casadi/issues/2837
if method == "bspline" and all(values == 0):
return zeros_like(xi)
### If xi is an int or float, promote it to an array
if isinstance(xi, int) or isinstance(xi, float):
xi = array([xi])
### If xi is a NumPy array and 1D, convert it to 2D for this.
if not is_casadi_type(xi, recursive=False) and len(xi.shape) != 2:
xi = _onp.reshape(xi, (-1, 1))
### Check that xi is now 2D
if not len(xi.shape) == 2:
raise ValueError(
"`xi` must have the shape (n_points, n_dimensions)!")
### Transpose xi so that xi.shape is [n_points, n_dimensions].
n_dimensions = len(points)
if not len(points) in xi.shape:
raise ValueError(
"`xi` must have the shape (n_points, n_dimensions)!")
if not xi.shape[1] == n_dimensions:
xi = xi.T
assert xi.shape[1] == n_dimensions
### Calculate the minimum and maximum values along each axis.
axis_values_min = [_onp.min(axis_values) for axis_values in points]
axis_values_max = [_onp.max(axis_values) for axis_values in points]
### If fill_value is None, project the xi back onto the nearest point in the domain.
if fill_value is None:
for axis in range(n_dimensions):
xi[:, axis] = where(xi[:, axis] > axis_values_max[axis],
axis_values_max[axis], xi[:, axis])
xi[:, axis] = where(xi[:, axis] < axis_values_min[axis],
axis_values_min[axis], xi[:, axis])
### Check bounds_error
if bounds_error:
if isinstance(xi, _cas.MX):
raise ValueError(
"Can't have the `bounds_error` flag as True if `xi` is of cas.MX type."
)
for axis in range(n_dimensions):
if any(
logical_or(xi[:, axis] > axis_values_max[axis],
xi[:, axis] < axis_values_min[axis])):
raise ValueError(
f"One of the requested xi is out of bounds in dimension {axis}"
)
### Do the interpolation
values_flattened = _onp.ravel(values, order='F')
interpolator = _cas.interpolant('Interpolator', method, points,
values_flattened)
fi = interpolator(xi.T).T
### If fill_value is a scalar, replace all out-of-bounds xi with that value.
if fill_value is not None:
for axis in range(n_dimensions):
fi = where(xi[:, axis] > axis_values_max[axis], fill_value, fi)
fi = where(xi[:, axis] < axis_values_min[axis], fill_value, fi)
### If DM output (i.e. a numeric value), convert that back to an array
if isinstance(fi, _cas.DM):
if fi.shape == (1, 1):
return float(fi)
else:
return _onp.array(fi, dtype=float).reshape(-1)
return fi
else:
raise ValueError("Bad value of `method`!")
def rotation_matrix_3D(angle: Union[float, _onp.ndarray],
axis: Union[_onp.ndarray, List, str],
as_array: bool = True,
axis_already_normalized: bool = False):
"""
Yields the rotation matrix that corresponds to a rotation by a specified amount about a given axis.
An implementation of https://en.wikipedia.org/wiki/Rotation_matrix#Rotation_matrix_from_axis_and_angle
Args:
angle: The angle to rotate by. [radians]
Direction of rotation corresponds to the right-hand rule.
Can be vectorized.
axis: The axis to rotate about. [ndarray]
Can be vectorized; be sure axis[0] yields all the x-components, etc.
as_array: boolean, returns a 3x3 array-like if True, and a list-of-lists otherwise.
If you are intending to use this function vectorized, it is recommended you flag this False. (Or test before
proceeding.)
axis_already_normalized: boolean, skips axis normalization for speed if you flag this true.
Returns:
The rotation matrix, with type according to the parameter `as_array`.
"""
s = sin(angle)
c = cos(angle)
if isinstance(axis, str):
if axis.lower() == "x":
rot = [[1, 0, 0], [0, c, -s], [0, s, c]]
elif axis.lower() == "y":
rot = [[c, 0, s], [0, 1, 0], [-s, 0, c]]
elif axis.lower() == "z":
rot = [[c, -s, 0], [s, c, 0], [0, 0, 1]]
else:
raise ValueError(
"If `axis` is a string, it must be `x`, `y`, or `z`.")
else:
ux = axis[0]
uy = axis[1]
uz = axis[2]
if not axis_already_normalized:
norm = (ux**2 + uy**2 + uz**2)**0.5
ux = ux / norm
uy = uy / norm
uz = uz / norm
rot = [[
c + ux**2 * (1 - c), ux * uy * (1 - c) - uz * s,
ux * uz * (1 - c) + uy * s
],
[
uy * ux * (1 - c) + uz * s, c + uy**2 * (1 - c),
uy * uz * (1 - c) - ux * s
],
[
uz * ux * (1 - c) - uy * s, uz * uy * (1 - c) + ux * s,
c + uz**2 * (1 - c)
]]
if as_array:
return array(rot)
else:
return rot
