import abc
import numpy.typing as npt
[docs]
class ArrayLike(abc.ABC):
"""Abstract class for a generic Array wrapper. Every method should be implemented for every data type."""
"""This class has to implemented the following operators: """
@abc.abstractmethod
[docs]
def __add__(self, other):
pass
@abc.abstractmethod
[docs]
def __radd__(self, other):
pass
@abc.abstractmethod
[docs]
def __sub__(self, other):
pass
@abc.abstractmethod
[docs]
def __rsub__(self, other):
pass
@abc.abstractmethod
[docs]
def __mul__(self, other):
pass
@abc.abstractmethod
[docs]
def __rmul__(self, other):
pass
@abc.abstractmethod
[docs]
def __matmul__(self, other):
pass
@abc.abstractmethod
[docs]
def __rmatmul__(self, other):
pass
@abc.abstractmethod
[docs]
def __neg__(self):
pass
@abc.abstractmethod
[docs]
def __getitem__(self, item):
pass
@abc.abstractmethod
[docs]
def __truediv__(self, other):
pass
@property
@abc.abstractmethod
[docs]
def T(self):
"""
Returns: Transpose of the array
"""
pass
[docs]
def __len__(self):
return len(self.array)
[docs]
def __repr__(self):
return self.array.__repr__()
[docs]
def as_list(self):
for i in range(len(self.array)):
yield self.array[i]
[docs]
class ArrayLikeFactory(abc.ABC):
"""Abstract class for a generic Array wrapper. Every method should be implemented for every data type."""
@abc.abstractmethod
[docs]
def zeros(self, *x: npt.ArrayLike) -> npt.ArrayLike:
"""
Args:
x (npt.ArrayLike): matrix dimension
Returns:
npt.ArrayLike: zero matrix of dimension x
"""
pass
@abc.abstractmethod
[docs]
def eye(self, x: npt.ArrayLike) -> npt.ArrayLike:
"""
Args:
x (npt.ArrayLike): matrix dimension
Returns:
npt.ArrayLike: identity matrix of dimension x
"""
pass
@abc.abstractmethod
[docs]
def asarray(self, x: npt.ArrayLike) -> npt.ArrayLike:
"""
Args:
x (npt.ArrayLike): array
Returns:
npt.ArrayLike: array
"""
pass
@abc.abstractmethod
[docs]
def ones_like(self, x: npt.ArrayLike) -> npt.ArrayLike:
"""
Args:
x (npt.ArrayLike): array
Returns:
npt.ArrayLike: one array with the same shape as x
"""
pass
@abc.abstractmethod
[docs]
def zeros_like(self, x: npt.ArrayLike) -> npt.ArrayLike:
"""
Args:
x (npt.ArrayLike): array
Returns:
npt.ArrayLike: one array with the same shape as x
"""
pass
@abc.abstractmethod
[docs]
def tile(self, x: npt.ArrayLike, reps: tuple) -> npt.ArrayLike:
"""
Args:
x (npt.ArrayLike): input array
reps (tuple): repetition factors for each dimension
Returns:
npt.ArrayLike: tiled array
"""
pass
[docs]
class SpatialMath:
"""Class implementing the main geometric functions used for computing rigid-body algorithm
Args:
ArrayLike: abstract class describing a generic Array wrapper. It needs to be implemented for every data type
"""
def __init__(self, factory: ArrayLikeFactory):
[docs]
self._factory = factory
@property
[docs]
def factory(self) -> ArrayLikeFactory:
return self._factory
@abc.abstractmethod
[docs]
def vertcat(self, x: npt.ArrayLike) -> npt.ArrayLike:
"""
Args:
x (npt.ArrayLike): elements
Returns:
npt.ArrayLike: vertical concatenation of elements x
"""
pass
@abc.abstractmethod
[docs]
def horzcat(self, x: npt.ArrayLike) -> npt.ArrayLike:
"""
Args:
x (npt.ArrayLike): elements
Returns:
npt.ArrayLike: horizontal concatenation of elements x
"""
pass
@abc.abstractmethod
[docs]
def concatenate(self, x: npt.ArrayLike, axis: int) -> npt.ArrayLike:
"""
Args:
x (npt.ArrayLike): elements
axis (int): axis along which to concatenate
Returns:
npt.ArrayLike: concatenation of elements x along axis
"""
pass
@abc.abstractmethod
[docs]
def stack(self, x: npt.ArrayLike, axis: int) -> npt.ArrayLike:
"""
Args:
x (npt.ArrayLike): elements
axis (int): axis along which to stack
Returns:
npt.ArrayLike: stacked elements x along axis
"""
pass
@abc.abstractmethod
[docs]
def tile(self, x: npt.ArrayLike, reps: tuple) -> npt.ArrayLike:
"""
Args:
x (npt.ArrayLike): input array
reps (tuple): repetition factors for each dimension
Returns:
npt.ArrayLike: tiled array
"""
pass
@abc.abstractmethod
[docs]
def transpose(self, x: npt.ArrayLike, dims: tuple) -> npt.ArrayLike:
"""
Args:
x (npt.ArrayLike): input array
dims (tuple): permutation of dimensions
Returns:
npt.ArrayLike: transposed array
"""
pass
@abc.abstractmethod
[docs]
def inv(self, x: npt.ArrayLike) -> npt.ArrayLike:
"""
Args:
x (npt.ArrayLike): input array
Returns:
npt.ArrayLike: inverse of the array
"""
pass
@abc.abstractmethod
[docs]
def mtimes(self, x: npt.ArrayLike, y: npt.ArrayLike) -> npt.ArrayLike:
pass
@abc.abstractmethod
[docs]
def sin(self, x: npt.ArrayLike) -> npt.ArrayLike:
"""
Args:
x (npt.ArrayLike): angle value
Returns:
npt.ArrayLike: sin value of x
"""
pass
@abc.abstractmethod
[docs]
def cos(self, x: npt.ArrayLike) -> npt.ArrayLike:
"""
Args:
x (npt.ArrayLike): angle value
Returns:
npt.ArrayLike: cos value of angle x
"""
pass
@abc.abstractmethod
[docs]
def skew(self, x):
pass
[docs]
def R_from_axis_angle(self, axis, q):
"""
axis: (...,3) - normalized axis vector, batched if q is batched
q : (...,) - rotation angle, batched or scalar
returns: (...,3,3) - rotation matrix
"""
c = self.cos(q)
s = self.sin(q)
# Build rotation matrix components
I = self.factory.eye(q.shape + (3,))
K = self.skew(axis) # skew-symmetric matrix
K_squared = K @ K # K²
ones = self.factory.ones_like(c)
return I + self.sxm(s, K) + self.sxm((ones - c), K_squared)
def tile(self, x: npt.ArrayLike, reps: tuple) -> npt.ArrayLike:
"""
Args:
x (npt.ArrayLike): input array
reps (tuple): repetition factors for each dimension
Returns:
npt.ArrayLike: tiled array
"""
return self.factory.tile(x, reps)
[docs]
def sxm(self, s: npt.ArrayLike, m: npt.ArrayLike) -> npt.ArrayLike:
"""Computes scalar multiplication with a matrix
Args:
s (npt.ArrayLike): scalar value
m (npt.ArrayLike): matrix to be multiplied
Returns:
npt.ArrayLike: result of the multiplication
"""
s = s[..., None, None]
return s * m
[docs]
def Rx(self, q: npt.ArrayLike) -> npt.ArrayLike:
"""
Args:
q (npt.ArrayLike): angle value
Returns:
npt.ArrayLike: rotation matrix around x axis
"""
c, s = self.cos(q), self.sin(q)
one = self.factory.ones_like(c)
zero = self.factory.zeros_like(c)
row0 = self.stack([one, zero, zero], axis=-1)
row1 = self.stack([zero, c, -s], axis=-1)
row2 = self.stack([zero, s, c], axis=-1)
return self.stack([row0, row1, row2], axis=-2)
[docs]
def Ry(self, q: npt.ArrayLike) -> npt.ArrayLike:
"""
Args:
q (npt.ArrayLike): angle value
Returns:
npt.ArrayLike: rotation matrix around y axis
"""
c, s = self.cos(q), self.sin(q)
one = self.factory.ones_like(c)
zero = self.factory.zeros_like(c)
row0 = self.stack([c, zero, s], axis=-1)
row1 = self.stack([zero, one, zero], axis=-1)
row2 = self.stack([-s, zero, c], axis=-1)
return self.stack([row0, row1, row2], axis=-2)
[docs]
def Rz(self, q: npt.ArrayLike) -> npt.ArrayLike:
"""
Args:
q (npt.ArrayLike): angle value
Returns:
npt.ArrayLike: rotation matrix around z axis
"""
c, s = self.cos(q), self.sin(q)
one = self.factory.ones_like(c)
zero = self.factory.zeros_like(c)
row0 = self.stack([c, -s, zero], axis=-1)
row1 = self.stack([s, c, zero], axis=-1)
row2 = self.stack([zero, zero, one], axis=-1)
return self.stack([row0, row1, row2], axis=-2)
[docs]
def H_revolute_joint(
self,
xyz: npt.ArrayLike,
rpy: npt.ArrayLike,
axis: npt.ArrayLike,
q: npt.ArrayLike,
) -> npt.ArrayLike:
"""
Args:
xyz (npt.ArrayLike): joint origin in the urdf
rpy (npt.ArrayLike): joint orientation in the urdf
axis (npt.ArrayLike): joint axis in the urdf
q (npt.ArrayLike): joint angle value
Returns:
npt.ArrayLike: Homogeneous transform
"""
# Check if q is batched to determine if we need to handle batching
q_is_batched = q.ndim > 0 and q.shape != ()
if q_is_batched:
# make all the joint properties batched
batch_size = q.shape[0]
# Ensure xyz is batched using standard numpy-style broadcasting
if xyz.ndim == 1:
xp = self._xp(xyz.array)
xyz_batched = xp.tile(xyz.array[None, :], (batch_size, 1))
xyz = self.factory.asarray(xyz_batched)
# Ensure rpy is batched
if rpy.ndim == 1:
xp = self._xp(rpy.array)
rpy_batched = xp.tile(rpy.array[None, :], (batch_size, 1))
rpy = self.factory.asarray(rpy_batched)
# Ensure axis is batched
if axis.ndim == 1:
xp = self._xp(axis.array)
axis_batched = xp.tile(axis.array[None, :], (batch_size, 1))
axis = self.factory.asarray(axis_batched)
R_rpy = self.R_from_RPY(rpy)
R_axis = self.R_from_axis_angle(axis, q)
R = R_rpy @ R_axis
return self.homogeneous(R, xyz)
[docs]
def homogeneous(self, R, p):
# Ensure p has the right shape for concatenation
if p.ndim == R.ndim - 1:
p = self.factory.asarray(p.array[..., None]) # Add last dimension
top = self.concatenate([R, p], axis=-1) # (...,3,4)
zeros_row = self.factory.zeros_like(R[..., :1, :]) # (...,1,3)
ones_col = self.factory.ones_like(R[..., :1, :1]) # (...,1,1)
bottom = self.concatenate([zeros_row, ones_col], axis=-1) # (...,1,4)
return self.concatenate([top, bottom], axis=-2) # (...,4,4)
[docs]
def H_prismatic_joint(
self,
xyz: npt.ArrayLike,
rpy: npt.ArrayLike,
axis: npt.ArrayLike,
q: npt.ArrayLike,
) -> npt.ArrayLike:
"""
Args:
xyz (npt.ArrayLike): joint origin in the urdf
rpy (npt.ArrayLike): joint orientation in the urdf
axis (npt.ArrayLike): joint axis in the urdf
q (npt.ArrayLike): joint angle value
Returns:
npt.ArrayLike: Homogeneous transform
"""
R = self.R_from_RPY(rpy)
p = xyz + q * axis
return self.homogeneous(R, p)
[docs]
def H_from_Pos_RPY(self, xyz: npt.ArrayLike, rpy: npt.ArrayLike) -> npt.ArrayLike:
"""
Args:
xyz (npt.ArrayLike): translation vector
rpy (npt.ArrayLike): rotation as rpy angles
Returns:
npt.ArrayLike: Homegeneous transform
"""
R = self.R_from_RPY(rpy)
return self.homogeneous(R, xyz)
[docs]
def R_from_RPY(self, rpy: npt.ArrayLike) -> npt.ArrayLike:
"""
Args:
rpy (npt.ArrayLike): rotation as rpy angles
Returns:
npt.ArrayLike: Rotation matrix
"""
return self.Rz(rpy[..., 2]) @ self.Ry(rpy[..., 1]) @ self.Rx(rpy[..., 0])
[docs]
def X_revolute_joint(
self,
xyz: npt.ArrayLike,
rpy: npt.ArrayLike,
axis: npt.ArrayLike,
q: npt.ArrayLike,
) -> npt.ArrayLike:
"""
Args:
xyz (npt.ArrayLike): joint origin in the urdf
rpy (npt.ArrayLike): joint orientation in the urdf
axis (npt.ArrayLike): joint axis in the urdf
q (npt.ArrayLike): joint angle value
Returns:
npt.ArrayLike: Spatial transform of a revolute joint given its rotation angle
"""
# TODO: give Featherstone reference
T = self.H_revolute_joint(xyz, rpy, axis, q)
R = self.swapaxes(T[..., :3, :3], -1, -2)
p = self.mxv(-R, T[..., :3, 3])
return self.spatial_transform(R, p)
[docs]
def X_prismatic_joint(
self,
xyz: npt.ArrayLike,
rpy: npt.ArrayLike,
axis: npt.ArrayLike,
q: npt.ArrayLike,
) -> npt.ArrayLike:
"""
Args:
xyz (npt.ArrayLike): joint origin in the urdf
rpy (npt.ArrayLike): joint orientation in the urdf
axis (npt.ArrayLike): joint axis in the urdf
q (npt.ArrayLike): joint angle value
Returns:
npt.ArrayLike: Spatial transform of a prismatic joint given its increment
"""
T = self.H_prismatic_joint(xyz, rpy, axis, q)
R = T[:3, :3].T
p = -T[:3, :3].T @ T[:3, 3]
return self.spatial_transform(R, p)
[docs]
def X_fixed_joint(self, xyz: npt.ArrayLike, rpy: npt.ArrayLike) -> npt.ArrayLike:
"""
Args:
xyz (npt.ArrayLike): joint origin in the urdf
rpy (npt.ArrayLike): joint orientation in the urdf
Returns:
npt.ArrayLike: Spatial transform of a fixed joint
"""
T = self.H_from_Pos_RPY(xyz, rpy)
R = T[:3, :3].T
p = -T[:3, :3].T @ T[:3, 3]
return self.spatial_transform(R, p)
[docs]
def _X_from_H(self, T):
R = self.swapaxes(T[..., :3, :3], -1, -2)
p = -(R @ T[..., :3, 3:4])[..., :, 0]
return self.spatial_transform(R, p)
[docs]
def spatial_inertia(
self,
inertia_matrix: npt.ArrayLike,
mass: npt.ArrayLike,
c: npt.ArrayLike,
rpy: npt.ArrayLike,
) -> npt.ArrayLike:
"""
Args:
inertia_matrix (npt.ArrayLike): inertia values from urdf
mass (npt.ArrayLike): mass value from urdf
c (npt.ArrayLike): origin of the link from urdf
rpy (npt.ArrayLike): orientation of the link from the urdf
Returns:
npt.ArrayLike: the 6x6 inertia matrix expressed at the origin of the link (with rotation)
"""
# Compute components
Sc = self.skew(c)
R = self.R_from_RPY(rpy)
mass_I3 = self.sxm(mass, self.factory.eye(3))
mass_Sc = self.sxm(mass, Sc)
mass_Sc_T = self.swapaxes(mass_Sc, -1, -2)
rotated_inertia = R @ inertia_matrix @ self.swapaxes(R, -1, -2)
Sc_squared = Sc @ self.swapaxes(Sc, -1, -2)
bottom_right = rotated_inertia + self.sxm(mass, Sc_squared)
# Correct block placement:
top = self.concatenate([mass_I3, mass_Sc_T], axis=-1) # (...,3,6)
bottom = self.concatenate([mass_Sc, bottom_right], axis=-1) # (...,3,6)
return self.concatenate([top, bottom], axis=-2) # (...,6,6)
[docs]
def spatial_inertia_with_parameters(self, inertia_matrix, mass, c, rpy):
"""
Args:
I (npt.ArrayLike): inertia values parametric
mass (npt.ArrayLike): mass value parametric
c (npt.ArrayLike): origin of the link parametric
rpy (npt.ArrayLike): orientation of the link from urdf
Returns:
npt.ArrayLike: the 6x6 inertia matrix parametric expressed at the origin of the link (with rotation)
"""
Sc = self.skew(c)
R = self.R_from_RPY(rpy)
mass_I3 = self.sxm(mass, self.factory.eye(3))
mass_Sc = self.sxm(mass, Sc)
mass_Sc_T = self.swapaxes(mass_Sc, -1, -2)
rotated_inertia = R @ inertia_matrix @ self.swapaxes(R, -1, -2)
Sc_squared = Sc @ self.swapaxes(Sc, -1, -2)
bottom_right = rotated_inertia + self.sxm(mass, Sc_squared)
# Correct block placement:
top = self.concatenate([mass_I3, mass_Sc_T], axis=-1) # (...,3,6)
bottom = self.concatenate([mass_Sc, bottom_right], axis=-1) # (...,3,6)
return self.concatenate([top, bottom], axis=-2) # (...,6,6)
[docs]
def spatial_skew(self, v: npt.ArrayLike) -> npt.ArrayLike:
"""
Args:
v (npt.ArrayLike): 6D vector
Returns:
npt.ArrayLike: spatial skew matrix
"""
omega = v[..., 3:] # Angular part
vel = v[..., :3] # Linear part
skew_omega = self.skew(omega)
skew_vel = self.skew(vel)
zeros = self.factory.zeros_like(skew_omega)
top = self.concatenate([skew_omega, skew_vel], axis=-1) # (...,3,6)
bottom = self.concatenate([zeros, skew_omega], axis=-1) # (...,3,6)
return self.concatenate([top, bottom], axis=-2) # (...,6,6)
[docs]
def spatial_skew_star(self, v: npt.ArrayLike) -> npt.ArrayLike:
"""
Args:
v (npt.ArrayLike): 6D vector
Returns:
npt.ArrayLike: negative spatial skew matrix traspose
"""
# return -self.spatial_skew(v).T
return -self.swapaxes(self.spatial_skew(v), -1, -2)
[docs]
def adjoint(self, H: npt.ArrayLike) -> npt.ArrayLike:
"""
Args:
H (npt.ArrayLike): Homogeneous transform
Returns:
npt.ArrayLike: adjoint matrix
"""
R = H[..., :3, :3]
p = H[..., :3, 3]
return self.spatial_transform(R, p)
[docs]
def adjoint_derivative(self, H: npt.ArrayLike, v: npt.ArrayLike) -> npt.ArrayLike:
"""
Args:
H (npt.ArrayLike): Homogeneous transform
v (npt.ArrayLike): 6D twist
Returns:
npt.ArrayLike: adjoint matrix derivative
"""
R = H[..., :3, :3]
p = H[..., :3, 3]
v_linear = v[..., :3]
v_angular = v[..., 3:]
Rdot = self.skew(v_angular) @ R
if v_angular.shape[-1] == 3:
# promote to column for consistent matmul semantics
omega_col = v_angular[..., None]
v_linear = v_linear[..., None]
else:
omega_col = v_angular
v_linear = v_linear
pdot = v_linear - self.skew(p) @ omega_col
Z = self.factory.zeros_like(R)
S = self.skew(pdot) @ R + self.skew(p) @ Rdot
top = self.concatenate([Rdot, S], axis=-1) # (...,3,6)
bottom = self.concatenate([Z, Rdot], axis=-1) # (...,3,6)
return self.concatenate([top, bottom], axis=-2) # (...,6,6)
[docs]
def mxv(self, m: npt.ArrayLike, v: npt.ArrayLike) -> npt.ArrayLike:
"""
Args:
m (npt.ArrayLike): Matrix
v (npt.ArrayLike): Vector
Returns:
npt.ArrayLike: Result of matrix-vector multiplication
"""
res = m @ v[..., None]
return res[..., 0] # Remove the extra dimension
[docs]
def vxs(self, v: npt.ArrayLike, s: npt.ArrayLike) -> npt.ArrayLike:
"""
Args:
v (npt.ArrayLike): Vector
s (npt.ArrayLike): Scalar
Returns:
npt.ArrayLike: Result of vector cross product with scalar multiplication
"""
if v.shape[-1] == 1:
v = v[..., 0]
s = s[..., None] # Add extra dimension
return v * s
[docs]
def adjoint_inverse(self, H: npt.ArrayLike) -> npt.ArrayLike:
"""
Args:
H (npt.ArrayLike): Homogeneous transform
Returns:
npt.ArrayLike: adjoint matrix
"""
R = H[..., :3, :3]
p = H[..., :3, 3:4]
RT = self.swapaxes(R, -1, -2)
return self.spatial_transform(RT, -(RT @ p)[..., :, 0])
[docs]
def adjoint_inverse_derivative(
self, H: npt.ArrayLike, v: npt.ArrayLike
) -> npt.ArrayLike:
"""
Args:
H (npt.ArrayLike): Homogeneous transform
v (npt.ArrayLike): 6D twist
Returns:
npt.ArrayLike: adjoint matrix derivative
"""
R = H[..., :3, :3]
p = H[..., :3, 3]
R_dot = self.skew(v[..., 3:]) @ R
p_dot = v[..., :3] - self.skew(p) @ v[..., 3:]
R_T = self.swapaxes(R, -1, -2)
R_dot_T = self.swapaxes(R_dot, -1, -2)
Z = self.factory.zeros_like(R)
TR = -R_dot_T @ self.skew(p) - R_T @ self.skew(p_dot)
top = self.concatenate([R_dot_T, TR], axis=-1) # (...,3,6)
bottom = self.concatenate([Z, R_dot_T], axis=-1) # (...,3,6)
return self.concatenate([top, bottom], axis=-2) # (...,6,6)
[docs]
def adjoint_mixed(self, H: npt.ArrayLike) -> npt.ArrayLike:
"""
Args:
H (npt.ArrayLike): Homogeneous transform
Returns:
npt.ArrayLike: adjoint matrix
"""
R = H[..., :3, :3]
Z = self.factory.zeros_like(R)
return self.concatenate(
[
self.concatenate([R, Z], axis=-1),
self.concatenate([Z, R], axis=-1),
],
axis=-2,
)
[docs]
def adjoint_mixed_inverse(self, H: npt.ArrayLike) -> npt.ArrayLike:
"""
Args:
H (npt.ArrayLike): Homogeneous transform
Returns:
npt.ArrayLike: adjoint matrix
"""
RT = self.swapaxes(H[..., :3, :3], -1, -2)
Z = self.zeros_like(RT)
return self.concatenate(
[self.concatenate([RT, Z], axis=-1), self.concatenate([Z, RT], axis=-1)],
axis=-2,
)
[docs]
def adjoint_mixed_derivative(
self, H: npt.ArrayLike, v: npt.ArrayLike
) -> npt.ArrayLike:
"""
Args:
H (npt.ArrayLike): Homogeneous transform
v (npt.ArrayLike): 6D twist
Returns:
npt.ArrayLike: adjoint matrix derivative
"""
R = H[..., :3, :3]
omega = v[..., 3:]
R_dot = self.skew(omega) @ R
Z = self.factory.zeros_like(R_dot)
top = self.concatenate([R_dot, Z], axis=-1) # (...,3,6)
bottom = self.concatenate([Z, R_dot], axis=-1) # (...,3,6)
return self.concatenate([top, bottom], axis=-2) # (...,6,6)
[docs]
def adjoint_mixed_inverse_derivative(
self, H: npt.ArrayLike, v: npt.ArrayLike
) -> npt.ArrayLike:
"""
Args:
H (npt.ArrayLike): Homogeneous transform
v (npt.ArrayLike): 6D twist
Returns:
npt.ArrayLike: adjoint matrix derivative
"""
R = H[:3, :3]
R_dot = self.skew(v[3:]) @ R
X = self.factory.zeros(6, 6)
X[:3, :3] = R_dot.T
X[3:6, 3:6] = R_dot.T
return X
[docs]
def homogeneous_inverse(self, H: npt.ArrayLike) -> npt.ArrayLike:
"""
Args:
H (npt.ArrayLike): Homogeneous transform
Returns:
npt.ArrayLike: inverse of the homogeneous transform
"""
R = H[..., :3, :3] # (...,3,3)
p = H[..., :3, 3:4] # (...,3,1)
R_T = self.swapaxes(R, -1, -2) # (...,3,3)
Rp = -(R_T @ p) # (...,3,1)
top = self.concatenate([R_T, Rp], axis=-1) # (...,3,4)
last_row = self.factory.zeros(H.shape[:-2] + (1, 4))
last_row = last_row + self.factory.asarray([0, 0, 0, 1])
return self.concatenate([top, last_row], axis=-2) # (...,4,4)
[docs]
def zeros(self, *x: int) -> npt.ArrayLike:
"""
Args:
x (int): dimension
Returns:
npt.ArrayLike: zero matrix of dimension x
"""
return self.factory.zeros(*x)
[docs]
def eye(self, x: int) -> npt.ArrayLike:
"""
Args:
x (int): dimension
Returns:
npt.ArrayLike: identity matrix of dimension x
"""
return self.factory.eye(x)
[docs]
def asarray(self, x: npt.ArrayLike) -> npt.ArrayLike:
"""
Args:
x (npt.ArrayLike): array
Returns:
npt.ArrayLike: array
"""
return self.factory.asarray(x)
[docs]
def zeros_like(self, x: npt.ArrayLike) -> npt.ArrayLike:
"""
Args:
x (npt.ArrayLike): array
Returns:
npt.ArrayLike: zero array with the same shape as x
"""
return self.factory.zeros_like(x)
[docs]
def ones_like(self, x: npt.ArrayLike) -> npt.ArrayLike:
"""
Args:
x (npt.ArrayLike): array
Returns:
npt.ArrayLike: one array with the same shape as x
"""
return self.factory.ones_like(x)