text: from __future__ import annotations
  from typing import overload, Optional
  
  import numpy as np
  from numpy.typing import ArrayLike
  
  
  # Contract brainstorm:
  # * initial arguments are copied, unless the fromvector constructor is used
  #   with copy=False. This allows the pose to "view" a regular numpy array and
  #   provide high-level functionality
  #   - this makes it easy to "wrap" an array for use in your control or
  #     whatever, but take advantage of the provided high-level features
  # * modification of properties modifies the object
  # * modification of values returned by functions does not modify the object
  # * setting/getting properties does not create new objects
  
  # Types
  # NOTE: not currently used
  Matrix3 = np.ndarray
  Matrix2 = np.ndarray
  Vector2 = np.ndarray
  
  
  def rotation_matrix(θ: float) -> np.ndarray:
      """2D rotation matrix: rotates points counter-clockwise."""
      return np.array([[np.cos(θ), -np.sin(θ)], [np.sin(θ), np.cos(θ)]])
  
  
  def angle_from_rotation_matrix(R: np.ndarray) -> float:
      """Get the angle represented by the rotation matrix."""
      assert R.shape == (2, 2), "Rotation matrix must have shape (2, 2)."
      # Using arctan allows us to properly recover the sign/quadrant as well as
      # the magnitude of the angle, where R[1, 0] = sin(θ), R[0, 0] = cos(θ).
      return np.arctan2(R[1, 0], R[0, 0])
  
  
  def skew(x: float) -> np.ndarray:
      """Construct 2x2 skew-symmetric matrix from scalar x."""
      return np.array([[0, -x], [x, 0]])
  
  
  # NOTE: experimental; not currently used
  class CachedRotation:
      def __init__(self, angle: float) -> None:
          self.angle = angle
          self.R = rotation_matrix(angle)
  
      def rotation(self, angle: float) -> np.ndarray:
          if not np.isclose(self.angle, angle):
              self.angle = angle
              self.R = rotation_matrix(angle)
          return self.R
  
  
  class Transform:
      """Encodes a pose (position and orientation) in two dimensions."""
  
      def __init__(self, position: ArrayLike = (0, 0), angle: float = 0) -> None:
          self.vector = np.zeros(3)
          self.vector[:2] = position
          self.vector[2] = angle
  
          # Cache the rotation matrix and corresponding angle for fast
          # multiplication. This is updated during multiplication if the angle
          # has changed. This allows us to keep R cached but also in sync even if
          # the data in the underlying vector array is modified.
          self._cache_rotation(angle)
  
      @classmethod
      def eye(cls) -> "Transform":
          """Identity pose."""
          return cls(position=(0, 0), angle=0)
  
      @classmethod
      def fromvector(cls, vector: ArrayLike, copy: bool = False) -> "Transform":
          self = cls.__new__(cls)
          self.vector = np.array(vector, copy=copy)
          assert self.vector.shape == (3,)
          return self
  
      def _cache_rotation(self, angle: float) -> None:
          """Initialize cached rotation matrix."""
          self._angle = angle
          self._R = rotation_matrix(angle)
  
      def _get_cached_rotation(self) -> np.ndarray:
          """Return cached rotation matrix, updating if needed."""
          if not np.isclose(self._angle, self.vector[2]):
              self._cache_rotation(self.vector[2])
          return self._R
  
      @overload
      def __matmul__(self, other: np.ndarray) -> np.ndarray:
          ...
  
      @overload
      def __matmul__(self, other: "Transform") -> "Transform":
          ...
  
      def __matmul__(self, other):
          if isinstance(other, np.ndarray):
              assert other.shape[0] == 2, "Position array must have first-dimension of 2."
              R = self._get_cached_rotation()
  
              # if we are rotating multiple vectors at once, then we need to
              # adjust dimensions to satisfy broadcasting rules
              if other.ndim > 1:
                  return R @ other + self.position[:, np.newaxis]
              else:
                  return R @ other + self.position
          elif isinstance(other, Transform):
              angle = self.angle + other.angle
              position = self @ other.position
              return Transform(position, angle)
          raise TypeError(f"Cannot multiply Transform with {type(other)}.")
  
      def inverse(self) -> "Transform":
          """Get the inverse transform, such T @ T.inverse() is the identity transform."""
          R = self._get_cached_rotation()
          angle = -self.angle
          position = -R.T @ self.position
          return Transform(position, angle)
  
      @property
      def angle(self) -> float:
          return self.vector[2]
  
      @angle.setter
      def angle(self, angle: float) -> None:
          self.vector[2] = angle
  
      @property
      def position(self) -> np.ndarray:
          return self.vector[:2]
  
      @position.setter
      def position(self, position: ArrayLike) -> None:
          self.vector[:2] = position
  
      def rotation(self) -> np.ndarray:
          """Returns a copy of the 2x2 rotation matrix."""
          return rotation_matrix(self.angle)
  
      def matrix(self) -> np.ndarray:
          """Returns a copy of the 3x3 homogeneous transformation matrix."""
          T = np.eye(3)
          T[:2, :2] = self._get_cached_rotation()
          T[:2, 2] = self.position
          return T
  
      @property
      def x(self) -> float:
          """The x component of position."""
          return self.vector[0]
  
      @x.setter
      def x(self, x: float) -> None:
          self.vector[0] = x
  
      @property
      def y(self) -> float:
          """The y component of position."""
          return self.vector[1]
  
      @y.setter
      def y(self, y: float) -> None:
          self.vector[1] = y
  
      def __str__(self) -> str:
          return str(tuple(self.vector))
  
      def __repr__(self) -> str:
          return f"Transform(position=({self.x}, {self.y}), angle={self.angle})"
  
      def __array__(self) -> np.ndarray:
          return self.vector
  
  
  class Twist:
      """Spatial velocity.
  
      This represents the velocity of one frame w.r.t. to another. The twist is a
      3-dimensional quantity consisting of (v, ω), where v is 2D linear velocity
      and ω is the scalar angular velocity.
      """
  
      def __init__(self, linear: ArrayLike = (0, 0), angular: float = 0) -> None:
          self.vector = np.zeros(3)
          self.vector[:2] = linear
          self.vector[2] = angular
  
      @classmethod
      def zero(cls) -> "Twist":
          """Zero twist."""
          return cls(linear=(0, 0), angular=0)
  
      @classmethod
      def fromvector(cls, vector: ArrayLike, copy: bool = False) -> "Twist":
          self = cls.__new__(cls)
          self.vector = np.array(vector, copy=copy)
          assert self.vector.shape == (3,)
          return self
  
      @classmethod
      def rigid(cls, V_ab: "Twist", T_ab: Transform, T_bc: Transform) -> "Twist":
          """Calculate twist of Frame C w.r.t. A, when C is rigidly attached to B."""
          ω_ba_a = V_ab.angular
          ω_ca_a = ω_ba_a
          v_ca_a = skew(ω_ba_a) @ T_ab.rotation @ T_bc.position + V_ab.linear
          return cls(v_ca_a, ω_ca_a)
  
      @property
      def linear(self) -> np.ndarray:
          return self.vector[:2]
  
      @linear.setter
      def linear(self, linear: ArrayLike) -> None:
          self.vector[:2] = linear
  
      @property
      def angular(self) -> float:
          return self.vector[2]
  
      @angular.setter
      def angular(self, angular: float) -> None:
          self.vector[2] = angular
  
  
  class Wrench:
      """Spatial force."""
  
      def __init__(self, force: ArrayLike = (0, 0), torque: float = 0) -> None:
          self.vector = np.zeros(3)
          self.vector[:2] = force
          self.vector[2] = torque
  
      @classmethod
      def fromvector(cls, vector: ArrayLike, copy: bool = False) -> "Wrench":
          self = cls.__new__(cls)
          self.vector = np.array(vector, copy=copy)
          assert self.vector.shape == (3,)
          return self
  
      @property
      def force(self) -> np.ndarray:
          return self.vector[:2]
  
      @force.setter
      def force(self, force: ArrayLike) -> None:
          self.vector[:2] = force
  
      @property
      def torque(self) -> float:
          return self.vector[2]
  
      @torque.setter
      def torque(self, torque: float) -> None:
          self.vector[2] = torque
  
  
  class CartesianState:
      """Cartesian state consisting of 2D pose and twist."""
  
      def __init__(self, pose: Transform, twist: Optional[Twist] = None) -> None:
          # copy the data
          self.vector = np.zeros(6)
          self.vector[:3] = pose.vector
  
          if twist is not None:
              self.vector[3:] = twist.vector
  
          # create new pose and twist objects that view the state's vector
          self.pose = Transform.fromvector(self.vector[:3], copy=False)
          self.twist = Twist.fromvector(self.vector[3:], copy=False)
  
      @classmethod
      def fromvector(cls, vector: ArrayLike, copy: bool = False) -> "CartesianState":
          self = cls.__new__(cls)
          self.vector = np.array(vector, copy=copy)
  
          assert self.vector.shape == (6,)
  
          self.pose = Transform.fromvector(self.vector[:3], copy=False)
          self.twist = Twist.fromvector(self.vector[3:], copy=False)
          return self
  