robometric_frame.trajectory_quality.curvature_change

Curvature Change metric for robotics policy trajectory evaluation.

Curvature Change measures trajectory smoothness while accounting for robot orientation, particularly valuable for car-like mobile robots where curvature relates to turning radius constraints.

Reference:

J.-H. Hwang, R. C. Arkin, and D.-S. Kwon, “Mobile robots at your fingertip: Bezier curve on-line trajectory generation for supervisory control,” in Proceedings 2003 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2003), IEEE, 2004.

Classes

CurvatureChange(**kwargs)

Compute Curvature Change for robotics policy trajectory evaluation.

class robometric_frame.trajectory_quality.curvature_change.CurvatureChange(**kwargs)

Compute Curvature Change for robotics policy trajectory evaluation.

Curvature Change is calculated as:

\[CC = \frac{1}{L-2} \sum_{i=1}^{L-2} |\kappa_{i+1} - \kappa_i|, \quad \kappa_i = \frac{\theta_{i+1} - \theta_i}{\|\mathbf{p}_{i+1} - \mathbf{p}_i\|_2}\]

where \(\mathbf{p}_i\) are trajectory positions, \(\theta_i\) are orientations (heading angles), and \(\kappa_i\) is the curvature at segment \(i\). Unlike path smoothness, this metric incorporates angular velocity and is particularly useful for evaluating car-like mobile robots where curvature relates to turning radius constraints.

This metric accumulates curvature change values across multiple trajectories and returns the average when compute() is called.

Parameters:

**kwargs (Any) – Additional keyword arguments passed to the base Metric class.

Example

>>> from robometric_frame.trajectory_quality import CurvatureChange
>>> import torch
>>> metric = CurvatureChange()
>>> # Straight line motion (constant orientation)
>>> positions = torch.tensor([[0.0, 0.0], [1.0, 0.0], [2.0, 0.0], [3.0, 0.0]])
>>> orientations = torch.tensor([0.0, 0.0, 0.0, 0.0])
>>> metric.update(positions, orientations)
>>> metric.compute()
tensor(0.0000)

Example (with turn):

>>> # Path with a turn
>>> metric = CurvatureChange()
>>> positions = torch.tensor([
...     [0.0, 0.0],
...     [1.0, 0.0],
...     [2.0, 0.0],
...     [3.0, 1.0]
... ])
>>> # Orientations change from 0 to π/4 radians
>>> orientations = torch.tensor([0.0, 0.0, 0.0, 0.785])
>>> metric.update(positions, orientations)
>>> result = metric.compute()

Example (batched):

>>> # Batch of trajectory pairs - shape (B, L, D)
>>> metric = CurvatureChange()
>>> positions_batch = torch.tensor([
...     [[0.0, 0.0], [1.0, 0.0], [2.0, 0.0], [3.0, 0.0]],
...     [[0.0, 0.0], [0.0, 1.0], [0.0, 2.0], [0.0, 3.0]]
... ])
>>> orientations_batch = torch.tensor([
...     [0.0, 0.0, 0.0, 0.0],
...     [1.57, 1.57, 1.57, 1.57]
... ])
>>> metric.update(positions_batch, orientations_batch)
>>> result = metric.compute()

Example (circular path):

>>> # Circular motion with constant curvature
>>> metric = CurvatureChange()
>>> import math
>>> angles = torch.linspace(0, math.pi/2, 10)
>>> positions = torch.stack([torch.cos(angles), torch.sin(angles)], dim=1)
>>> orientations = angles + math.pi/2  # Tangent direction
>>> metric.update(positions, orientations)
>>> result = metric.compute()  # Should be small for smooth circular motion

Example (distributed):

>>> # In distributed training, metrics are automatically synced
>>> metric = CurvatureChange()
>>> # On GPU 0
>>> pos_gpu0 = torch.tensor([[0.0, 0.0], [1.0, 0.0], [2.0, 0.0]])
>>> ori_gpu0 = torch.tensor([0.0, 0.0, 0.0])
>>> metric.update(pos_gpu0, ori_gpu0)
>>> # On GPU 1
>>> pos_gpu1 = torch.tensor([[0.0, 0.0], [0.0, 1.0], [0.0, 2.0]])
>>> ori_gpu1 = torch.tensor([1.57, 1.57, 1.57])
>>> metric.update(pos_gpu1, ori_gpu1)
>>> # Final result aggregates across all GPUs
>>> result = metric.compute()

full_state_update: bool = False

total_curvature_change: Tensor

num_trajectories: Tensor

__init__(**kwargs)

Initialize the CurvatureChange metric.

update(positions, orientations)

Update metric state with new trajectory or batch of trajectories.

Parameters:

• positions (Tensor) –

  Position trajectory tensor of shape (…, L, D) where: - … represents any number of batch dimensions (can be empty) - L is the number of points (must be >= 3) - D is the spatial dimensionality (typically 2 for mobile robots)

  Examples of valid shapes: - (L, D): Single trajectory - (B, L, D): Batch of B trajectories - (B, T, L, D): Batch of B sequences with T slices each

  Points should be ordered chronologically along the L dimension.

• orientations (Tensor) –

  Orientation (heading angle) tensor of shape (…, L) where: - … represents the same batch dimensions as positions - L is the number of points (must match positions) - Values are heading angles in radians

  Must have the same batch dimensions and L as positions, but without the spatial dimension D.

Raises:

ValueError – If trajectories have invalid shape, mismatched shapes, or insufficient points.

Return type:

None

compute()

Compute the average Curvature Change across all trajectories.

Return type:

Tensor

Returns:

Average curvature change as a scalar tensor. Lower values indicate smoother trajectories with more consistent turning behavior.

Raises:

RuntimeError – If no trajectories have been recorded.

training: bool