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

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.

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.