curobo.wrap.reacher.evaluator module¶
This modules contains heuristics for scoring trajectories.
- class TrajEvaluatorConfig( )¶
Bases:
object
Configurable Parameters for Trajectory Evaluator.
- 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),
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:
- compute_path_length(
- vel,
- traj_dt,
- cspace_distance_weight,
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:
- compute_path_length_cost(
- vel,
- cspace_distance_weight,
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:
- smooth_cost(
- abs_acc,
- abs_jerk,
- opt_dt,
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:
- 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,
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( ) 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:
- _check_smoothness(
- js: JointState,
- traj_dt: Tensor,
- max_vel: 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,
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,
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,
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),
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: