curobo.wrap.reacher.trajopt module

class curobo.wrap.reacher.trajopt.TrajOptSolverConfig(robot_config: curobo.types.robot.RobotConfig, solver: curobo.wrap.wrap_base.WrapBase, rollout_fn: curobo.rollout.arm_reacher.ArmReacher, position_threshold: float, rotation_threshold: float, traj_tsteps: int, use_cspace_seed: bool = True, interpolation_type: curobo.util.trajectory.InterpolateType = <InterpolateType.LINEAR_CUDA: 'linear_cuda'>, interpolation_steps: int = 1000, world_coll_checker: Optional[curobo.geom.sdf.world.WorldCollision] = None, seed_ratio: Optional[Dict[str, int]] = None, num_seeds: int = 1, bias_node: Optional[Annotated[torch.Tensor, {'__torchtyping__': True, 'details': ('dof', torch.float32), 'cls_name': 'TensorType'}]] = None, interpolation_dt: float = 0.01, traj_evaluator_config: curobo.wrap.reacher.evaluator.TrajEvaluatorConfig = TrajEvaluatorConfig(max_acc=15.0, max_jerk=500.0, cost_weight=0.01, min_dt=0.001, max_dt=0.1), traj_evaluator: Optional[curobo.wrap.reacher.evaluator.TrajEvaluator] = None, evaluate_interpolated_trajectory: bool = True, cspace_threshold: float = 0.1, tensor_args: curobo.types.base.TensorDeviceType = TensorDeviceType(device=device(type='cuda', index=0), dtype=torch.float32), sync_cuda_time: bool = True, interpolate_rollout: Optional[curobo.rollout.arm_reacher.ArmReacher] = None, use_cuda_graph_metrics: bool = False, trim_steps: Optional[List[int]] = None, store_debug_in_result: bool = False, optimize_dt: bool = True, use_cuda_graph: bool = True)

Bases: object

Parameters:
  • robot_config (RobotConfig) –

  • solver (WrapBase) –

  • rollout_fn (ArmReacher) –

  • position_threshold (float) –

  • rotation_threshold (float) –

  • traj_tsteps (int) –

  • use_cspace_seed (bool) –

  • interpolation_type (InterpolateType) –

  • interpolation_steps (int) –

  • world_coll_checker (WorldCollision | None) –

  • seed_ratio (Dict[str, int] | None) –

  • num_seeds (int) –

  • bias_node (Tensor | None) –

  • interpolation_dt (float) –

  • traj_evaluator_config (TrajEvaluatorConfig) –

  • traj_evaluator (TrajEvaluator | None) –

  • evaluate_interpolated_trajectory (bool) –

  • cspace_threshold (float) –

  • tensor_args (TensorDeviceType) –

  • sync_cuda_time (bool) –

  • interpolate_rollout (ArmReacher | None) –

  • use_cuda_graph_metrics (bool) –

  • trim_steps (List[int] | None) –

  • store_debug_in_result (bool) –

  • optimize_dt (bool) –

  • use_cuda_graph (bool) –

robot_config: RobotConfig
solver: WrapBase
rollout_fn: ArmReacher
position_threshold: float
rotation_threshold: float
traj_tsteps: int
use_cspace_seed: bool = True
interpolation_type: InterpolateType = 'linear_cuda'
interpolation_steps: int = 1000
world_coll_checker: WorldCollision | None = None
seed_ratio: Dict[str, int] | None = None
num_seeds: int = 1
bias_node: Tensor | None = None
interpolation_dt: float = 0.01
traj_evaluator_config: TrajEvaluatorConfig = TrajEvaluatorConfig(max_acc=15.0, max_jerk=500.0, cost_weight=0.01, min_dt=0.001, max_dt=0.1)
traj_evaluator: TrajEvaluator | None = None
evaluate_interpolated_trajectory: bool = True
cspace_threshold: float = 0.1
tensor_args: TensorDeviceType = TensorDeviceType(device=device(type='cuda', index=0), dtype=torch.float32)
sync_cuda_time: bool = True
interpolate_rollout: ArmReacher | None = None
use_cuda_graph_metrics: bool = False
trim_steps: List[int] | None = None
store_debug_in_result: bool = False
optimize_dt: bool = True
use_cuda_graph: bool = True
static load_from_robot_config(robot_cfg, world_model=None, tensor_args=TensorDeviceType(device=device(type='cuda', index=0), dtype=torch.float32), position_threshold=0.005, rotation_threshold=0.05, cspace_threshold=0.05, world_coll_checker=None, base_cfg_file='base_cfg.yml', particle_file='particle_trajopt.yml', gradient_file='gradient_trajopt.yml', traj_tsteps=None, interpolation_type=InterpolateType.LINEAR_CUDA, interpolation_steps=10000, interpolation_dt=0.01, use_cuda_graph=True, self_collision_check=False, self_collision_opt=True, grad_trajopt_iters=None, num_seeds=2, seed_ratio={'bias': 0.0, 'end': 0.0, 'linear': 1.0, 'start': 0.0}, use_particle_opt=True, collision_checker_type=CollisionCheckerType.MESH, traj_evaluator_config=TrajEvaluatorConfig(max_acc=15.0, max_jerk=500.0, cost_weight=0.01, min_dt=0.001, max_dt=0.1), traj_evaluator=None, minimize_jerk=True, use_gradient_descent=False, collision_cache=None, n_collision_envs=None, use_es=None, es_learning_rate=0.1, use_fixed_samples=None, aux_rollout=None, evaluate_interpolated_trajectory=True, fixed_iters=None, store_debug=False, sync_cuda_time=True, collision_activation_distance=None, trajopt_dt=None, trim_steps=None, store_debug_in_result=False, smooth_weight=None, state_finite_difference_mode=None, filter_robot_command=False, optimize_dt=True, project_pose_to_goal_frame=True)
Parameters:
  • robot_cfg (str | Dict | RobotConfig) –

  • world_model (List[Dict] | List[WorldConfig] | Dict | WorldConfig | None) –

  • tensor_args (TensorDeviceType) –

  • position_threshold (float) –

  • rotation_threshold (float) –

  • cspace_threshold (float) –

  • base_cfg_file (str) –

  • particle_file (str) –

  • gradient_file (str) –

  • traj_tsteps (int | None) –

  • interpolation_type (InterpolateType) –

  • interpolation_steps (int) –

  • interpolation_dt (float) –

  • use_cuda_graph (bool) –

  • self_collision_check (bool) –

  • self_collision_opt (bool) –

  • grad_trajopt_iters (int | None) –

  • num_seeds (int) –

  • seed_ratio (Dict[str, int]) –

  • use_particle_opt (bool) –

  • collision_checker_type (CollisionCheckerType | None) –

  • traj_evaluator_config (TrajEvaluatorConfig) –

  • traj_evaluator (TrajEvaluator | None) –

  • minimize_jerk (bool) –

  • use_gradient_descent (bool) –

  • collision_cache (Dict[str, int] | None) –

  • n_collision_envs (int | None) –

  • use_es (bool | None) –

  • es_learning_rate (float | None) –

  • use_fixed_samples (bool | None) –

  • aux_rollout (ArmReacher | None) –

  • evaluate_interpolated_trajectory (bool) –

  • fixed_iters (bool | None) –

  • store_debug (bool) –

  • sync_cuda_time (bool) –

  • collision_activation_distance (float | None) –

  • trajopt_dt (float | None) –

  • trim_steps (List[int] | None) –

  • store_debug_in_result (bool) –

  • smooth_weight (List[float] | None) –

  • state_finite_difference_mode (str | None) –

  • filter_robot_command (bool) –

  • optimize_dt (bool) –

  • project_pose_to_goal_frame (bool) –

class curobo.wrap.reacher.trajopt.TrajResult(success: Annotated[torch.Tensor, {'__torchtyping__': True, 'details': ('batch', 'value', torch.bool), 'cls_name': 'TensorType'}], goal: curobo.rollout.rollout_base.Goal, solution: curobo.types.state.JointState, seed: Annotated[torch.Tensor, {'__torchtyping__': True, 'details': ('batch', 'dof', torch.float32), 'cls_name': 'TensorType'}], solve_time: float, debug_info: Any | None = None, metrics: curobo.rollout.rollout_base.RolloutMetrics | None = None, interpolated_solution: curobo.types.state.JointState | None = None, path_buffer_last_tstep: List[int] | None = None, position_error: Optional[Annotated[torch.Tensor, {'__torchtyping__': True, 'details': ('batch', 'value', torch.float32), 'cls_name': 'TensorType'}]] = None, rotation_error: Optional[Annotated[torch.Tensor, {'__torchtyping__': True, 'details': ('batch', 'value', torch.float32), 'cls_name': 'TensorType'}]] = None, cspace_error: Optional[Annotated[torch.Tensor, {'__torchtyping__': True, 'details': ('batch', 'value', torch.float32), 'cls_name': 'TensorType'}]] = None, smooth_error: Optional[Annotated[torch.Tensor, {'__torchtyping__': True, 'details': ('batch', 'value', torch.float32), 'cls_name': 'TensorType'}]] = None, smooth_label: Optional[Annotated[torch.Tensor, {'__torchtyping__': True, 'details': ('batch', 'value', torch.bool), 'cls_name': 'TensorType'}]] = None, optimized_dt: torch.Tensor | None = None, raw_solution: curobo.types.state.JointState | None = None, raw_action: torch.Tensor | None = None, goalset_index: torch.Tensor | None = None)

Bases: Sequence

Parameters:
  • success (Tensor) –

  • goal (Goal) –

  • solution (JointState) –

  • seed (Tensor) –

  • solve_time (float) –

  • debug_info (Any | None) –

  • metrics (RolloutMetrics | None) –

  • interpolated_solution (JointState | None) –

  • path_buffer_last_tstep (List[int] | None) –

  • position_error (Tensor | None) –

  • rotation_error (Tensor | None) –

  • cspace_error (Tensor | None) –

  • smooth_error (Tensor | None) –

  • smooth_label (Tensor | None) –

  • optimized_dt (Tensor | None) –

  • raw_solution (JointState | None) –

  • raw_action (Tensor | None) –

  • goalset_index (Tensor | None) –

success: Tensor
goal: Goal
solution: JointState
seed: Tensor
solve_time: float
debug_info: Any | None = None
metrics: RolloutMetrics | None = None
interpolated_solution: JointState | None = None
path_buffer_last_tstep: List[int] | None = None
position_error: Tensor | None = None
rotation_error: Tensor | None = None
cspace_error: Tensor | None = None
smooth_error: Tensor | None = None
smooth_label: Tensor | None = None
optimized_dt: Tensor | None = None
raw_solution: JointState | None = None
raw_action: Tensor | None = None
goalset_index: Tensor | None = None
_abc_impl = <_abc._abc_data object>
class curobo.wrap.reacher.trajopt.TrajOptSolver(config)

Bases: TrajOptSolverConfig

Parameters:

config (TrajOptSolverConfig) –

get_all_rollout_instances()
Return type:

List[RolloutBase]

get_all_kinematics_instances()
Return type:

List[CudaRobotModel]

attach_object_to_robot(sphere_radius, sphere_tensor=None, link_name='attached_object')
Parameters:
  • sphere_radius (float) –

  • sphere_tensor (Tensor | None) –

  • link_name (str) –

Return type:

None

detach_object_from_robot(link_name='attached_object')
Parameters:

link_name (str) –

Return type:

None

update_goal_buffer(solve_state, goal)
Parameters:
solve_any(solve_type, goal, seed_traj=None, use_nn_seed=False, return_all_solutions=False, num_seeds=None, seed_success=None, newton_iters=None)
Parameters:
  • solve_type (ReacherSolveType) –

  • goal (Goal) –

  • seed_traj (JointState | None) –

  • use_nn_seed (bool) –

  • return_all_solutions (bool) –

  • num_seeds (int | None) –

  • seed_success (Tensor | None) –

  • newton_iters (int | None) –

Return type:

TrajResult

solve_from_solve_state(solve_state, goal, seed_traj=None, use_nn_seed=False, return_all_solutions=False, num_seeds=None, seed_success=None, newton_iters=None)
Parameters:
  • solve_state (ReacherSolveState) –

  • goal (Goal) –

  • seed_traj (JointState | None) –

  • use_nn_seed (bool) –

  • return_all_solutions (bool) –

  • num_seeds (int | None) –

  • seed_success (Tensor | None) –

  • newton_iters (int | None) –

solve_single(goal, seed_traj=None, use_nn_seed=False, return_all_solutions=False, num_seeds=None, newton_iters=None)
Parameters:
  • goal (Goal) –

  • seed_traj (JointState | None) –

  • use_nn_seed (bool) –

  • return_all_solutions (bool) –

  • num_seeds (int | None) –

  • newton_iters (int | None) –

Return type:

TrajResult

solve_goalset(goal, seed_traj=None, use_nn_seed=False, return_all_solutions=False, num_seeds=None, newton_iters=None)
Parameters:
  • goal (Goal) –

  • seed_traj (JointState | None) –

  • use_nn_seed (bool) –

  • return_all_solutions (bool) –

  • num_seeds (int | None) –

  • newton_iters (int | None) –

Return type:

TrajResult

solve_batch(goal, seed_traj=None, use_nn_seed=False, return_all_solutions=False, num_seeds=None, seed_success=None, newton_iters=None)
Parameters:
  • goal (Goal) –

  • seed_traj (JointState | None) –

  • use_nn_seed (bool) –

  • return_all_solutions (bool) –

  • num_seeds (int | None) –

  • seed_success (Tensor | None) –

  • newton_iters (int | None) –

Return type:

TrajResult

solve_batch_goalset(goal, seed_traj=None, use_nn_seed=False, return_all_solutions=False, num_seeds=None, seed_success=None, newton_iters=None)
Parameters:
  • goal (Goal) –

  • seed_traj (JointState | None) –

  • use_nn_seed (bool) –

  • return_all_solutions (bool) –

  • num_seeds (int | None) –

  • seed_success (Tensor | None) –

  • newton_iters (int | None) –

Return type:

TrajResult

solve_batch_env(goal, seed_traj=None, use_nn_seed=False, return_all_solutions=False, num_seeds=None, seed_success=None, newton_iters=None)
Parameters:
  • goal (Goal) –

  • seed_traj (JointState | None) –

  • use_nn_seed (bool) –

  • return_all_solutions (bool) –

  • num_seeds (int | None) –

  • seed_success (Tensor | None) –

  • newton_iters (int | None) –

Return type:

TrajResult

solve_batch_env_goalset(goal, seed_traj=None, use_nn_seed=False, return_all_solutions=False, num_seeds=None, seed_success=None, newton_iters=None)
Parameters:
  • goal (Goal) –

  • seed_traj (JointState | None) –

  • use_nn_seed (bool) –

  • return_all_solutions (bool) –

  • num_seeds (int | None) –

  • seed_success (Tensor | None) –

  • newton_iters (int | None) –

Return type:

TrajResult

solve(goal, seed_traj=None, use_nn_seed=False, return_all_solutions=False, num_seeds=None, newton_iters=None)

Only for single goal

Parameters:
  • goal (Goal) – _description_

  • seed_traj (Optional[JointState], optional) – _description_. Defaults to None.

  • use_nn_seed (bool, optional) – _description_. Defaults to False.

  • return_all_solutions (bool) –

  • num_seeds (int | None) –

  • newton_iters (int | None) –

Raises:

NotImplementedError – _description_

Returns:

_description_

Return type:

TrajResult

_get_result(result, return_all_solutions, goal, seed_traj, num_seeds, batch_mode=False)
Parameters:
  • result (WrapResult) –

  • return_all_solutions (bool) –

  • goal (Goal) –

  • seed_traj (JointState) –

  • num_seeds (int) –

  • batch_mode (bool) –

batch_solve(goal, seed_traj=None, seed_success=None, use_nn_seed=False, return_all_solutions=False, num_seeds=None)

Only for single goal

Parameters:
  • goal (Goal) – _description_

  • seed_traj (Optional[JointState], optional) – _description_. Defaults to None.

  • use_nn_seed (bool, optional) – _description_. Defaults to False.

  • seed_success (Tensor | None) –

  • return_all_solutions (bool) –

  • num_seeds (int | None) –

Raises:

NotImplementedError – _description_

Returns:

_description_

Return type:

TrajResult

get_linear_seed(start_state, goal_state)
get_start_seed(start_state)
_get_seed_numbers(num_seeds)
get_seed_set(goal, seed_traj=None, seed_success=None, num_seeds=None, batch_mode=False)
Parameters:
  • goal (Goal) –

  • seed_traj (JointState | Tensor | None) –

  • seed_success (Tensor | None) –

  • num_seeds (int | None) –

  • batch_mode (bool) –

get_seeds(start_state, goal_state, num_seeds=None)
get_bias_seed(start_state, goal_state)
get_interpolated_trajectory(traj_state)
Parameters:

traj_state (JointState) –

calculate_trajectory_dt(trajectory)
Parameters:

trajectory (JointState) –

Return type:

Tensor

reset_seed()
reset_cuda_graph()
reset_shape()
property kinematics: CudaRobotModel
property retract_config
fk(q)
Parameters:

q (Tensor) –

Return type:

CudaRobotModelState

property solver_dt
update_solver_dt(dt, base_dt=None, max_dt=None, base_ratio=None)
Parameters:
  • dt (float | Tensor) –

  • base_dt (float | None) –

  • max_dt (float | None) –

  • base_ratio (float | None) –

compute_metrics(opt_trajectory, interpolated_trajectory)
Parameters:
  • opt_trajectory (bool) –

  • interpolated_trajectory (bool) –

get_full_js(active_js)
Parameters:

active_js (JointState) –

Return type:

JointState

update_pose_cost_metric(metric)
Parameters:

metric (PoseCostMetric) –