curobo.wrap.reacher.evaluator module

This modules contains heuristics for scoring trajectories.

class TrajEvaluatorConfig(
max_acc: Tensor,
max_jerk: Tensor,
min_dt: Tensor,
max_dt: Tensor,
cost_weight: float = 0.01,
)

Bases: object

Configurable Parameters for Trajectory Evaluator.

max_acc: Tensor

Maximum acceleration for each joint.

max_jerk: Tensor

Maximum jerk for each joint.

min_dt: Tensor

Minimum allowed time step for trajectory. Trajectories with time step less than this value will be rejected.

max_dt: Tensor

Maximum allowed time step for trajectory. Trajectories with time step greater than this value will be rejected.

cost_weight: float = 0.01

Weight to scale smoothness cost, total cost = path length + cost_weight * smoothness cost.

static from_basic(
dof: int,
max_acc: float = 15.0,
max_jerk: float = 500.0,
cost_weight: float = 0.01,
min_dt: float = 0.001,
max_dt: float = 0.15,
tensor_args: TensorDeviceType = TensorDeviceType(device=device(type='cuda', index=0), dtype=torch.float32, collision_geometry_dtype=torch.float32, collision_gradient_dtype=torch.float32, collision_distance_dtype=torch.float32),
) → TrajEvaluatorConfig

Creates TrajEvaluatorConfig object from basic parameters.

Parameters:

  • dof – number of active joints in the robot.

  • max_acc – maximum acceleration for all joints. Treats this as same for all joints.

  • max_jerk – maximum jerk for all joints. Treats this as same for all joints.

  • cost_weight – weight to scale smoothness cost.

  • min_dt – minimum allowed time step between waypoints of a trajectory.

  • max_dt – maximum allowed time step between waypoints of a trajectory.

  • tensor_args – device and dtype for the tensors.

Returns:

Configured Parameters for Trajectory Evaluator.

Return type:

TrajEvaluatorConfig

compute_path_length(
vel,
traj_dt,
cspace_distance_weight,
) → Tensor

JIT compatible function to compute path length.

Parameters:

  • vel – joint space velocity tensor of shape (batch, horizon, dof).

  • traj_dt – dt of trajectory tensor of shape (batch, horizon) or (1, horizon) or (1, 1).

  • cspace_distance_weight – weight tensor of shape (dof).

Returns:

path length tensor of shape (batch).

Return type:

torch.Tensor

compute_path_length_cost(
vel,
cspace_distance_weight,
) → Tensor

JIT compatible function to compute path length cost without considering time step.

Parameters:

  • vel – joint space velocity tensor of shape (batch, horizon, dof).

  • cspace_distance_weight – weight tensor of shape (dof).

Returns:

path length cost tensor of shape (batch).

Return type:

torch.Tensor

smooth_cost(
abs_acc,
abs_jerk,
opt_dt,
) → Tensor

JIT compatible function to compute smoothness cost.

Parameters:

  • abs_acc – absolute acceleration tensor of shape (batch, horizon, dof).

  • abs_jerk – absolute jerk tensor of shape (batch, horizon, dof).

  • opt_dt – optimal time step tensor of shape (batch).

Returns:

smoothness cost tensor of shape (batch).

Return type:

torch.Tensor

compute_smoothness(
vel: Tensor,
acc: Tensor,
jerk: Tensor,
max_vel: Tensor,
max_acc: Tensor,
max_jerk: Tensor,
traj_dt: Tensor,
min_dt: float,
max_dt: float,
epsilon: float = 1e-06,
) → Tuple[Tensor, Tensor]

JIT compatible function to compute smoothness.

Parameters:

  • vel – joint space velocity tensor of shape (batch, horizon, dof).

  • acc – joint space acceleration tensor of shape (batch, horizon, dof).

  • jerk – joint space jerk tensor of shape (batch, horizon, dof).

  • max_vel – maximum velocity limits, used to find scaling factor for dt that pushes at least one joint to its limit, taking into account acceleration and jerk limits.

  • max_acc – maximum acceleration limits, used to find scaling factor for dt that pushes at least one joint to its limit, taking into account velocity and jerk limits.

  • max_jerk – maximum jerk limits, used to find scaling factor for dt that pushes at least one joint to its limit, taking into account velocity and acceleration limits.

  • traj_dt – dt of trajectory tensor of shape (batch, horizon) or (1, horizon)

  • min_dt – minimum delta time allowed between steps/waypoints in a trajectory.

  • max_dt – maximum delta time allowed between steps/waypoints in a trajectory.

  • epsilon – relaxes evaluation of velocity, acceleration, and jerk limits to allow for numerical errors.

Returns:

success label tensor of shape (batch) and

smoothness cost tensor of shape (batch)

Return type:

Tuple[torch.Tensor, torch.Tensor]

compute_smoothness_opt_dt(
vel,
acc,
jerk,
max_vel: Tensor,
max_acc: Tensor,
max_jerk: Tensor,
opt_dt: Tensor,
) → Tuple[Tensor, Tensor]

JIT compatible function to compute smoothness with pre-computed optimal time step.

Parameters:

  • vel – joint space velocity tensor of shape (batch, horizon, dof), not used in this implementation.

  • acc – joint space acceleration tensor of shape (batch, horizon, dof).

  • jerk – joint space jerk tensor of shape (batch, horizon, dof).

  • max_vel – maximum velocity limit, not used in this implementation.

  • max_acc – maximum acceleration limit, used to reject trajectories.

  • max_jerk – maximum jerk limit, used to reject trajectories.

  • opt_dt – optimal time step tensor of shape (batch).

Returns:

success label tensor of shape (batch) and

smoothness cost tensor of shape (batch).

Return type:

Tuple[torch.Tensor, torch.Tensor]

class TrajEvaluator(
config: TrajEvaluatorConfig | None = None,
)

Bases: TrajEvaluatorConfig

Trajectory Evaluator class that uses heuristics to score trajectories.

Initializes the TrajEvaluator object.

Parameters:

config – Configurable parameters for Trajectory Evaluator.

_compute_path_length(
js: JointState,
traj_dt: Tensor,
cspace_distance_weight: Tensor,
)

Compute path length from joint velocities across trajectory and dt between them.

Parameters:

  • js – joint state object with velocity tensor.

  • traj_dt – time step tensor of shape (batch, horizon) or (1, horizon) or (1, 1).

  • cspace_distance_weight – weight tensor of shape (dof).

Returns:

path length tensor of shape (batch).

Return type:

torch.Tensor

_check_smoothness(
js: JointState,
traj_dt: Tensor,
max_vel: Tensor,
) → Tuple[Tensor, Tensor]

Check smoothness of trajectory.

Parameters:

  • js – joint state object with velocity, acceleration and jerk tensors.

  • traj_dt – time step tensor of shape (batch, horizon) or (1, horizon) or (1, 1).

  • max_vel – maximum velocity limits, used to find scaling factor for dt that pushes at least one joint to its limit, taking into account acceleration and jerk limits.

Returns:

success label tensor of shape (batch) and

smoothness cost tensor of shape (batch).

Return type:

Tuple[torch.Tensor, torch.Tensor]

evaluate(
js: JointState,
traj_dt: Tensor,
cspace_distance_weight: Tensor,
max_vel: Tensor,
) → Tuple[Tensor, Tensor]

Evaluate trajectory based on smoothness and path length.

Parameters:

  • js – joint state object with velocity, acceleration and jerk tensors.

  • traj_dt – time step tensor of shape (batch, horizon) or (1, horizon) or (1, 1) or (batch, 1).

  • cspace_distance_weight – weight tensor of shape (dof).

  • max_vel – maximum velocity limits, used to find scaling factor for dt that pushes at least one joint to its limit, taking into account acceleration and jerk limits.

Returns:

success label tensor of shape (batch) and

total cost tensor of shape (batch) where total cost = (path length + cost_weight * smoothness cost).

Return type:

Tuple[torch.Tensor, torch.Tensor]

evaluate_interpolated_smootheness(
js: JointState,
opt_dt: Tensor,
cspace_distance_weight: Tensor,
max_vel: Tensor,
) → Tuple[Tensor, Tensor]

Evaluate trajectory based on smoothness and path length with pre-computed optimal dt.

Parameters:

  • js – joint state object with velocity, acceleration and jerk tensors.

  • opt_dt – optimal time step tensor of shape (batch).

  • cspace_distance_weight – weight tensor of shape (dof).

  • max_vel – maximum velocity limits, used to find scaling factor for dt that pushes at least one joint to its limit, taking into account acceleration and jerk limits.

Returns:

success label tensor of shape (batch) and

total cost tensor of shape (batch) where total cost = (path length + cost_weight * smoothness cost).

Return type:

Tuple[torch.Tensor, torch.Tensor]

evaluate_from_position(
js: JointState,
traj_dt: Tensor,
cspace_distance_weight: Tensor,
max_vel: Tensor,
skip_last_tstep: bool = False,
) → Tuple[Tensor, Tensor]

Evaluate trajectory by first computing velocity, acceleration and jerk from position.

Parameters:

  • js – joint state object with position tensor.

  • traj_dt – time step tensor of shape (batch, 1) or (1, 1).

  • cspace_distance_weight – weight tensor of shape (dof).

  • max_vel – maximum velocity limits, used to find scaling factor for dt that pushes at least one joint to its limit, taking into account acceleration and jerk limits.

  • skip_last_tstep – flag to skip last time step in trajectory.

Returns:

success label tensor of shape (batch) and

total cost tensor of shape (batch) where total cost = (path length + cost_weight * smoothness cost).

Return type:

Tuple[torch.Tensor, torch.Tensor]

cost_weight: float = 0.01

Weight to scale smoothness cost, total cost = path length + cost_weight * smoothness cost.

static from_basic(
dof: int,
max_acc: float = 15.0,
max_jerk: float = 500.0,
cost_weight: float = 0.01,
min_dt: float = 0.001,
max_dt: float = 0.15,
tensor_args: TensorDeviceType = TensorDeviceType(device=device(type='cuda', index=0), dtype=torch.float32, collision_geometry_dtype=torch.float32, collision_gradient_dtype=torch.float32, collision_distance_dtype=torch.float32),
) → TrajEvaluatorConfig

Creates TrajEvaluatorConfig object from basic parameters.

Parameters:

  • dof – number of active joints in the robot.

  • max_acc – maximum acceleration for all joints. Treats this as same for all joints.

  • max_jerk – maximum jerk for all joints. Treats this as same for all joints.

  • cost_weight – weight to scale smoothness cost.

  • min_dt – minimum allowed time step between waypoints of a trajectory.

  • max_dt – maximum allowed time step between waypoints of a trajectory.

  • tensor_args – device and dtype for the tensors.

Returns:

Configured Parameters for Trajectory Evaluator.

Return type:

TrajEvaluatorConfig

max_acc: Tensor

Maximum acceleration for each joint.

max_jerk: Tensor

Maximum jerk for each joint.

min_dt: Tensor

Minimum allowed time step for trajectory. Trajectories with time step less than this value will be rejected.

max_dt: Tensor

Maximum allowed time step for trajectory. Trajectories with time step greater than this value will be rejected.