curobo.wrap.reacher.trajopt module

Trajectory optimization module contains TrajOptSolver class which can optimize for minimum-jerk trajectories by first running a particle-based solver (MPPI) and then refining with a gradient-based solver (L-BFGS). The module also provides linear interpolation functions for generating seeds from start joint configuration to goal joint configurations. To generate trajectories for reaching Cartesian poses or joint configurations, use the higher-level wrapper MotionGen.

Trajectory Optimization uses joint positions as optimization variables with cost terms for avoiding world collisions, self-collisions, and joint limits. The joint velocities, accelerations, and jerks are computed using five point stencil. A squared l2-norm cost term on joint accelerations and jerks is used to encourage smooth trajectories. A cost term for the terminal state to reach either a Cartesian pose or joint configuration is also used. Read Technical Report for more details.

class TrajOptSolverConfig(
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 = 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 | None = None,
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, collision_geometry_dtype=torch.float32, collision_gradient_dtype=torch.float32, collision_distance_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,
)

Bases: object

Configuration parameters for TrajOptSolver.

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 | None = None
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, collision_geometry_dtype=torch.float32, collision_gradient_dtype=torch.float32, collision_distance_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: str | Dict | RobotConfig,
world_model: List[Dict] | List[WorldConfig] | Dict | WorldConfig | None = None,
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),
position_threshold: float = 0.005,
rotation_threshold: float = 0.05,
cspace_threshold: float = 0.05,
world_coll_checker=None,
base_cfg_file: str = 'base_cfg.yml',
particle_file: str = 'particle_trajopt.yml',
gradient_file: str = 'gradient_trajopt.yml',
traj_tsteps: int | None = None,
interpolation_type: InterpolateType = InterpolateType.LINEAR_CUDA,
interpolation_steps: int = 10000,
interpolation_dt: float = 0.01,
minimum_trajectory_dt: float | None = None,
use_cuda_graph: bool = True,
self_collision_check: bool = False,
self_collision_opt: bool = True,
grad_trajopt_iters: int | None = None,
num_seeds: int = 2,
seed_ratio: Dict[str, int] = {'bias': 0.0, 'end': 0.0, 'linear': 1.0, 'start': 0.0},
use_particle_opt: bool = True,
collision_checker_type: CollisionCheckerType | None = CollisionCheckerType.MESH,
traj_evaluator_config: TrajEvaluatorConfig | None = None,
traj_evaluator: TrajEvaluator | None = None,
minimize_jerk: bool = True,
use_gradient_descent: bool = False,
collision_cache: Dict[str, int] | None = None,
n_collision_envs: int | None = None,
use_es: bool | None = None,
es_learning_rate: float | None = 0.1,
use_fixed_samples: bool | None = None,
aux_rollout: ArmReacher | None = None,
evaluate_interpolated_trajectory: bool = True,
fixed_iters: bool | None = None,
store_debug: bool = False,
sync_cuda_time: bool = True,
collision_activation_distance: float | None = None,
trajopt_dt: float | None = None,
trim_steps: List[int] | None = None,
store_debug_in_result: bool = False,
smooth_weight: List[float] | None = None,
state_finite_difference_mode: str | None = None,
filter_robot_command: bool = False,
optimize_dt: bool = True,
project_pose_to_goal_frame: bool = True,
)

Load TrajOptSolver configuration from robot configuration.

Parameters:

  • robot_cfg – Robot configuration to use for motion generation. This can be a path to a yaml file, a dictionary, or an instance of RobotConfig. See Supported Robots for a list of available robots. You can also create a a configuration file for your robot using Configuring a New Robot.

  • world_model – World configuration to use for motion generation. This can be a path to a yaml file, a dictionary, or an instance of WorldConfig. See Collision World Representation for more details.

  • tensor_args – Numerical precision and compute device to use for motion generation.

  • position_threshold – Position threshold between target position and reached position in meters. 0.005 is a good value for most tasks (5mm).

  • rotation_threshold – Rotation threshold between target orientation and reached orientation. The metric is q^T * q, where q is the quaternion difference between target and reached orientation. The metric is not easy to interpret and a future release will provide a more intuitive metric. For now, use 0.05 as a good value.

  • cspace_threshold – Joint space threshold in radians for revolute joints and meters for linear joints between reached joint configuration and target joint configuration used to measure success. Default of 0.05 has been found to be a good value for most cases.

  • world_coll_checker – Instance of world collision checker to use for motion generation. Leaving this to None will create a new instance of world collision checker using the provided attr:world_model.

  • base_cfg_file – Base configuration file containing convergence and constraint criteria to measure success.

  • particle_file – Optimizer configuration file to use for particle-based optimization during trajectory optimization.

  • gradient_file – Optimizer configuration file to use for gradient-based optimization during trajectory optimization.

  • trajopt_tsteps – Number of waypoints to use for trajectory optimization. Default of 32 is found to be a good number for most cases.

  • interpolation_type – Interpolation type to use for generating dense waypoints from optimized trajectory. Default of curobo.util.trajectory.InterpolateType.LINEAR_CUDA is found to be a good choice for most cases. Other suitable options for real robot execution are curobo.util.trajectory.InterpolateType.QUINTIC and curobo.util.trajectory.InterpolateType.CUBIC.

  • interpolation_steps – Buffer size to use for storing interpolated trajectory. Default of 5000 is found to be a good number for most cases.

  • interpolation_dt – Time step in seconds to use for generating interpolated trajectory from optimized trajectory. Change this if you want to generate a trajectory with a fixed timestep between waypoints.

  • minimum_trajectory_dt – Minimum time step in seconds allowed for trajectory optimization.

  • use_cuda_graph – Record compute ops as cuda graphs and replay recorded graphs where implemented. This can speed up execution by upto 10x. Default of True is recommended. Enabling this will prevent changing solve type or batch size after the first call to the solver.

  • self_collision_check – Enable self collision checks for generated motions. Default of True is recommended. Set this to False to debug planning failures. Setting this to False will also set self_collision_opt to False.

  • self_collision_opt – Enable self collision cost during optimization (IK, TrajOpt). Default of True is recommended.

  • grad_trajopt_iters – Number of L-BFGS iterations to run trajectory optimization.

  • num_seeds – Number of seeds to use for trajectory optimization per problem.

  • seed_ratio – Ratio of linear and bias seeds to use for trajectory optimization. Linear seed will generate a linear interpolated trajectory from start state to IK solutions. Bias seed will add a mid-waypoint through the retract configuration. Default of 1.0 linear and 0.0 bias is recommended. This can be changed to 0.5 linear and 0.5 bias, along with changing num_seeds to 2.

  • trajopt_particle_opt – Enable particle-based optimization during trajectory optimization. Default of True is recommended as particle-based optimization moves the interpolated seeds away from bad local minima.

  • collision_checker_type – Type of collision checker to use for motion generation. Default of CollisionCheckerType.MESH supports world represented by Cuboids and Meshes. See Collision World Representation for more details.

  • traj_evaluator_config – Configuration for trajectory evaluator. Default of None will create a new instance of TrajEvaluatorConfig. After trajectory optimization across many seeds, the best trajectory is selected based on this configuration. This evaluator also checks if the optimized dt is within curobo.wrap.reacher.evaluator.TrajEvaluatorConfig.max_dt. This check is needed to measure smoothness of the optimized trajectory as bad trajectories can have very high dt to fit within velocity, acceleration, and jerk limits.

  • traj_evaluator – Instance of trajectory evaluator to use for trajectory optimization. Default of None will create a new instance of TrajEvaluator. In case you want to use a custom evaluator, you can create a child instance of TrajEvaluator and pass it.

  • minimize_jerk – Minimize jerk as regularization during trajectory optimizaiton.

  • use_gradient_descent – Use gradient descent instead of L-BFGS for trajectory optimization. Default of False is recommended. Set to True for debugging gradients of new cost terms.

  • collision_cache – Cache of obstacles to create to load obstacles between planning calls. An example: {"obb": 10, "mesh": 10}, to create a cache of 10 cuboids and 10 meshes.

  • n_collision_envs – Number of collision environments to create for batched optimization across different environments. Only used for TrajOptSolver.solve_batch_env`and :py:meth:`TrajOptSolver.solve_batch_env_goalset.

  • n_collision_envs – Number of collision environments to create for batched planning across different environments. Only used for MotionGen.plan_batch_env and MotionGen.plan_batch_env_goalset.

  • use_es – Use Evolution Strategies for optimization. Default of None will use MPPI.

  • es_learning_rate – Learning rate to use for Evolution Strategies.

  • use_fixed_samples – Use fixed samples for MPPI. Setting to False will increase compute time as new samples are generated for each iteration of MPPI.

  • aux_rollout – Rollout instance to use for auxiliary rollouts.

  • evaluate_interpolated_trajectory – Evaluate interpolated trajectory after optimization. Default of True is recommended to ensure the optimized trajectory is not passing through very thin obstacles.

  • fixed_iters – Use fixed number of iterations of L-BFGS for trajectory optimization. Default of None will use the setting from the optimizer configuration. In most cases, fixed iterations of solvers are run as current solvers treat constraints as costs and there is no guarantee that the constraints will be satisfied. Instead of checking constraints between iterations of a solver and exiting, it’s computationally cheaper to run a fixed number of iterations. In addition, running fixed iterations of solvers is more robust to outlier problems.

  • store_debug – Store debugging information such as values of optimization variables in TrajOpt result. Setting this to True will set use_cuda_graph to False.

  • sync_cuda_time – Synchronize with host using torch.cuda.synchronize before measuring compute time.

  • collision_activation_distance – Distance in meters to activate collision cost. A good value to start with is 0.01 meters. Increase the distance if the robot needs to stay further away from obstacles.

  • trajopt_dt – Time step in seconds to use for trajectory optimization. A good value to start with is 0.15 seconds. This value is used to compute velocity, acceleration, and jerk values for waypoints through finite difference.

  • trim_steps – Trim waypoints from optimized trajectory. The optimized trajectory will contain the start state at index 0 and have the last two waypoints be the same as T-2 as trajectory optimization implicitly optimizes for zero acceleration and velocity at the last waypoint. An example: [1,-2] will trim the first waypoint and last 3 waypoints from the optimized trajectory.

  • store_debug_in_result – Store debugging information in MotionGenResult. This value is set to True if either store_ik_debug or store_trajopt_debug is set to True.

  • smooth_weight – Override smooth weight for trajectory optimization. It’s not recommended to set this value for most cases.

  • state_finite_difference_mode – Finite difference mode to use for computing velocity, acceleration, and jerk values. Default of None will use the setting from the optimizer configuration file. The default finite difference method is a five point stencil to compute the derivatives as this is accurate and provides faster convergence compared to backward or central difference methods.

  • filter_robot_command – Filter generated trajectory to remove finite difference artifacts. Default of True is recommended.

  • optimize_dt – Optimize dt during trajectory optimization. Default of True is recommended to find time-optimal trajectories. Setting this to False will use the provided trajopt_dt for trajectory optimization. Setting to False is required when optimizing from a non-static start state.

  • project_pose_to_goal_frame – Project pose to goal frame when calculating distance between reached and goal pose. Use this to constrain motion to specific axes either in the global frame or the goal frame.

Returns:

Trajectory optimization configuration.

Return type:

TrajOptSolverConfig

class TrajOptResult(
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,
optimized_seeds: Tensor | None = None,
)

Bases: Sequence

Trajectory optimization result.

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
optimized_seeds: Tensor | None = None
_abc_impl = <_abc._abc_data object>
_is_protocol = False
count(
value,
) → integer -- return number of occurrences of value
index(
value[,
start[,
stop,]]
) → integer -- return first index of value.

Raises ValueError if the value is not present.

Supporting start and stop arguments is optional, but recommended.

class TrajResult(
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,
optimized_seeds: Tensor | None = None,
)

Bases: TrajOptResult

Deprecated: Use TrajOptResult instead of TrajResult

_abc_impl = <_abc._abc_data object>
_is_protocol = False
count(
value,
) → integer -- return number of occurrences of value
cspace_error: Tensor | None = None
debug_info: Any | None = None
goalset_index: Tensor | None = None
index(
value[,
start[,
stop,]]
) → integer -- return first index of value.

Raises ValueError if the value is not present.

Supporting start and stop arguments is optional, but recommended.

interpolated_solution: JointState | None = None
metrics: RolloutMetrics | None = None
optimized_dt: Tensor | None = None
optimized_seeds: Tensor | None = None
path_buffer_last_tstep: List[int] | None = None
position_error: Tensor | None = None
raw_action: Tensor | None = None
raw_solution: JointState | None = None
rotation_error: Tensor | None = None
smooth_error: Tensor | None = None
smooth_label: Tensor | None = None
success: Tensor
goal: Goal
solution: JointState
seed: Tensor
solve_time: float
class TrajOptSolver(
config: TrajOptSolverConfig,
)

Bases: TrajOptSolverConfig

Trajectory Optimization Solver class for generating minimum-jerk trajectories.

Trajectory Optimization uses joint positions as optimization variables with cost terms for avoiding world collisions, self-collisions, and joint limits. The joint velocities, accelerations, and jerks are computed using five point stencil. A squared l2-norm cost term on joint accelerations and jerks is used to encourage smooth trajectories. A cost term for the terminal state to reach either a Cartesian pose or joint configuration is also used. Read Technical Report for more details.

Initialize TrajOptSolver with configuration parameters.

Parameters:

config – Configuration parameters for TrajOptSolver.

traj_evaluator: TrajEvaluator | None = None
get_all_rollout_instances() → List[RolloutBase]

Get all rollout instances in the solver.

Useful to update parameters across all rollout instances.

Returns:

List of all rollout instances.

Return type:

List[RolloutBase]

get_all_kinematics_instances() → List[CudaRobotModel]

Get all kinematics instances used across components in motion generation.

This is deprecated. Use TrajOptSolver.kinematics instead as TrajOptSolver now uses a shared kinematics instance across all components.

Returns:

Single kinematics instance, returned as a list for compatibility.

Return type:

List[CudaRobotModel]

attach_spheres_to_robot(
sphere_radius: float,
sphere_tensor: Tensor | None = None,
link_name: str = 'attached_object',
) → None

Attach spheres to robot for collision checking.

To fit spheres to an obstacle, see get_bounding_spheres

Parameters:

  • sphere_radius – Radius of the spheres. Set to None if sphere_tensor is provided.

  • sphere_tensor – Sphere x, y, z, r tensor.

  • link_name – Name of the link to attach the spheres to. Note that this link should already have pre-allocated spheres.

detach_spheres_from_robot(
link_name: str = 'attached_object',
) → None

Detach spheres from robot.

Parameters:

link_name – Name of the link to detach the spheres from.

_update_solve_state_and_goal_buffer(
solve_state: ReacherSolveState,
goal: Goal,
)

Update goal buffer and solve state of current trajectory optimization problem.

Parameters:

  • solve_state – New solve state.

  • goal – New goal buffer.

Returns:

Updated goal buffer with augmentations for new solve state.

Return type:

Goal

solve_any(
solve_type: ReacherSolveType,
goal: Goal,
seed_traj: JointState | None = None,
use_nn_seed: bool = False,
return_all_solutions: bool = False,
num_seeds: int | None = None,
seed_success: Tensor | None = None,
newton_iters: int | None = None,
) → TrajOptResult

Solve trajectory optimization problem with any solve type.

Parameters:

  • solve_type – Type of solve to perform.

  • goal – Goal to reach.

  • seed_traj – Seed trajectory to start optimization from. This should be of shape [num_seeds, batch_size, action_horizon, dof]. If None, linearly interpolated seeds from current state to goal state are used. If goal.goal_state is empty, random seeds are generated.

  • use_nn_seed – Use neural network seed for optimization. This is not implemented.

  • return_all_solutions – Return solutions for all seeds.

  • num_seeds – Number of seeds to use for optimization. This cannot be changed after the first call to solve as CUDA graph re-creation is currently not supported.

  • seed_success – Success of seeds. This is used to filter out successful seeds from seed_traj.

  • newton_iters – Number of iterations to run LBFGS optimization. If None, the number of iterations is set to the default value in TrajOptSolver.newton_iters.

Returns:

Result of the trajectory optimization.

Return type:

TrajOptResult

_solve_from_solve_state(
solve_state: ReacherSolveState,
goal: Goal,
seed_traj: JointState | None = None,
use_nn_seed: bool = False,
return_all_solutions: bool = False,
num_seeds: int | None = None,
seed_success: Tensor | None = None,
newton_iters: int | None = None,
)

Solve trajectory optimization problem with a given solve state.

Parameters:

  • solve_state – Solve state for the optimization problem.

  • goal – Goal object containing target pose or joint configuration.

  • seed_traj – Seed trajectory to start optimization from. This should be of shape [num_seeds, batch_size, action_horizon, dof]. If None, linearly interpolated seeds from current state to goal state are used. If goal.goal_state is empty, random seeds are generated.

  • use_nn_seed – Use neural network seed for optimization. This is not implemented.

  • return_all_solutions – Return solutions for all seeds.

  • num_seeds – Number of seeds to use for optimization. This cannot be changed after the first call to solve as CUDA graph re-creation is currently not supported.

  • seed_success – Success of seeds. This is used to filter out successful seeds from seed_traj.

  • newton_iters – Number of iterations to run LBFGS optimization. If None, the number of iterations is set to the default value in TrajOptSolver.newton_iters. This is the outer iterations, where each outer iteration will run 25 inner iterations of LBFGS optimization captured in a CUDA-Graph. Total number of optimization iterations is 25 * outer_iters. The number of inner iterations can be changed with curobo.opt.newton.lbfgs.LBFGSOptConfig.inner_iters.

Returns:

Result of the trajectory optimization.

Return type:

TrajOptResult

solve_single(
goal: Goal,
seed_traj: JointState | None = None,
use_nn_seed: bool = False,
return_all_solutions: bool = False,
num_seeds: int | None = None,
newton_iters: int | None = None,
) → TrajOptResult

Solve trajectory optimization problem for a single goal.

This will use multiple seeds internally and return the best solution.

Parameters:

  • goal – Goal to reach.

  • seed_traj – Seed trajectory to start optimization from. This should be of shape [num_seeds, 1, TrajOptSolver.action_horizon, TrajOptSolver.dof]. If None, linearly interpolated seeds from current state to goal state are used. If goal.goal_state is empty, random seeds are generated.

  • use_nn_seed – Use neural network seed for optimization. This is not implemented.

  • return_all_solutions – Return solutions for all seeds.

  • num_seeds – Number of seeds to use for optimization. If None, the number of seeds is set to TrajOptSolver.num_seeds.

  • newton_iters – Number of iterations to run LBFGS optimization. If None, the number of iterations is set to the default value in TrajOptSolver.newton_iters. This is the outer iterations, where each outer iteration will run 25 inner iterations of LBFGS optimization captured in a CUDA-Graph. Total number of optimization iterations is 25 * outer_iters. The number of inner iterations can be changed with curobo.opt.newton.lbfgs.LBFGSOptConfig.inner_iters.

Returns:

Result of the trajectory optimization.

Return type:

TrajOptResult

solve_goalset(
goal: Goal,
seed_traj: JointState | None = None,
use_nn_seed: bool = False,
return_all_solutions: bool = False,
num_seeds: int | None = None,
newton_iters: int | None = None,
) → TrajOptResult

Solve trajectory optimization problem that uses goalset to represent Pose target.

Parameters:

  • goal – Goal to reach.

  • seed_traj – Seed trajectory to start optimization from. This should be of shape [num_seeds, 1, TrajOptSolver.action_horizon, TrajOptSolver.dof]. If None, linearly interpolated seeds from current state to goal state are used. If goal.goal_state is empty, random seeds are generated.

  • use_nn_seed – Use neural network seed for optimization. This is not implemented.

  • return_all_solutions – Return solutions for all seeds.

  • num_seeds – Number of seeds to use for optimization. If None, the number of seeds is set to TrajOptSolver.num_seeds.

  • newton_iters – Number of iterations to run LBFGS optimization. If None, the number of iterations is set to the default value in TrajOptSolver.newton_iters. This is the outer iterations, where each outer iteration will run 25 inner iterations of LBFGS optimization captured in a CUDA-Graph. Total number of optimization iterations is 25 * outer_iters. The number of inner iterations can be changed with curobo.opt.newton.lbfgs.LBFGSOptConfig.inner_iters.

Returns:

Result of the trajectory optimization.

Return type:

TrajOptResult

solve_batch(
goal: Goal,
seed_traj: JointState | None = None,
use_nn_seed: bool = False,
return_all_solutions: bool = False,
num_seeds: int | None = None,
seed_success: Tensor | None = None,
newton_iters: int | None = None,
) → TrajOptResult

Solve trajectory optimization problem for a batch of goals.

Parameters:

  • goal – Batch of goals to reach, this includes batch of current states.

  • seed_traj – Seed trajectory to start optimization from. This should be of shape [num_seeds, goal.batch, TrajOptSolver.action_horizon]. If None, linearly interpolated seeds from current state to goal state are used. If goal.goal_state is empty, random seeds are generated.

  • use_nn_seed – Use neural network seed for optimization. This is not implemented.

  • return_all_solutions – Return solutions for all seeds.

  • num_seeds – Number of seeds to use for optimization. If None, the number of seeds is set to TrajOptSolver.num_seeds.

  • seed_success – Success of seeds. This is used to filter out successful seeds from seed_traj.

  • newton_iters – Number of iterations to run LBFGS optimization. If None, the number of iterations is set to the default value in TrajOptSolver.newton_iters. This is the outer iterations, where each outer iteration will run 25 inner iterations of LBFGS optimization captured in a CUDA-Graph. Total number of optimization iterations is 25 * outer_iters. The number of inner iterations can be changed with curobo.opt.newton.lbfgs.LBFGSOptConfig.inner_iters.

Returns:

Result of the trajectory optimization.

Return type:

TrajOptResult

solve_batch_goalset(
goal: Goal,
seed_traj: JointState | None = None,
use_nn_seed: bool = False,
return_all_solutions: bool = False,
num_seeds: int | None = None,
seed_success: Tensor | None = None,
newton_iters: int | None = None,
) → TrajOptResult

Solve trajectory optimization problem for a batch of Poses with goalset.

Parameters:

  • goal – Batch of goals to reach, this includes batch of current states.

  • seed_traj – Seed trajectory to start optimization from. This should be of shape [num_seeds, goal.batch, TrajOptSolver.action_horizon]. If None, linearly interpolated seeds from current state to goal state are used. If goal.goal_state is empty, random seeds are generated.

  • use_nn_seed – Use neural network seed for optimization. This is not implemented.

  • return_all_solutions – Return solutions for all seeds.

  • num_seeds – Number of seeds to use for optimization. If None, the number of seeds is set to TrajOptSolver.num_seeds.

  • seed_success – Success of seeds. This is used to filter out successful seeds from seed_traj.

  • newton_iters – Number of iterations to run LBFGS optimization. If None, the number of iterations is set to the default value in TrajOptSolver.newton_iters. This is the outer iterations, where each outer iteration will run 25 inner iterations of LBFGS optimization captured in a CUDA-Graph. Total number of optimization iterations is 25 * outer_iters. The number of inner iterations can be changed with curobo.opt.newton.lbfgs.LBFGSOptConfig.inner_iters.

Returns:

Result of the trajectory optimization.

Return type:

TrajOptResult

solve_batch_env(
goal: Goal,
seed_traj: JointState | None = None,
use_nn_seed: bool = False,
return_all_solutions: bool = False,
num_seeds: int | None = None,
seed_success: Tensor | None = None,
newton_iters: int | None = None,
) → TrajOptResult

Solve trajectory optimization problem in a batch of environments.

Parameters:

  • goal – Batch of goals to reach, this includes batch of current states.

  • seed_traj – Seed trajectory to start optimization from. This should be of shape [num_seeds, goal.batch, TrajOptSolver.action_horizon]. If None, linearly interpolated seeds from current state to goal state are used. If goal.goal_state is empty, random seeds are generated.

  • use_nn_seed – Use neural network seed for optimization. This is not implemented.

  • return_all_solutions – Return solutions for all seeds.

  • num_seeds – Number of seeds to use for optimization. If None, the number of seeds is set to TrajOptSolver.num_seeds.

  • seed_success – Success of seeds. This is used to filter out successful seeds from seed_traj.

  • newton_iters – Number of iterations to run LBFGS optimization. If None, the number of iterations is set to the default value in TrajOptSolver.newton_iters. This is the outer iterations, where each outer iteration will run 25 inner iterations of LBFGS optimization captured in a CUDA-Graph. Total number of optimization iterations is 25 * outer_iters. The number of inner iterations can be changed with curobo.opt.newton.lbfgs.LBFGSOptConfig.inner_iters.

Returns:

Result of the trajectory optimization.

Return type:

TrajOptResult

solve_batch_env_goalset(
goal: Goal,
seed_traj: JointState | None = None,
use_nn_seed: bool = False,
return_all_solutions: bool = False,
num_seeds: int | None = None,
seed_success: Tensor | None = None,
newton_iters: int | None = None,
) → TrajOptResult

Solve trajectory optimization problem in a batch of environments with goalset.

Parameters:

  • goal – Batch of goals to reach, this includes batch of current states.

  • seed_traj – Seed trajectory to start optimization from. This should be of shape [num_seeds, goal.batch, TrajOptSolver.action_horizon]. If None, linearly interpolated seeds from current state to goal state are used. If goal.goal_state is empty, random seeds are generated.

  • use_nn_seed – Use neural network seed for optimization. This is not implemented.

  • return_all_solutions – Return solutions for all seeds.

  • num_seeds – Number of seeds to use for optimization. If None, the number of seeds is set to TrajOptSolver.num_seeds.

  • seed_success – Success of seeds. This is used to filter out successful seeds from seed_traj.

  • newton_iters – Number of iterations to run LBFGS optimization. If None, the number of iterations is set to the default value in TrajOptSolver.newton_iters. This is the outer iterations, where each outer iteration will run 25 inner iterations of LBFGS optimization captured in a CUDA-Graph. Total number of optimization iterations is 25 * outer_iters. The number of inner iterations can be changed with curobo.opt.newton.lbfgs.LBFGSOptConfig.inner_iters.

Returns:

Result of the trajectory optimization.

Return type:

TrajOptResult

solve(
goal: Goal,
seed_traj: JointState | None = None,
use_nn_seed: bool = False,
return_all_solutions: bool = False,
num_seeds: int | None = None,
newton_iters: int | None = None,
) → TrajOptResult

Deprecated: Use TrajOptSolver.solve_single or others instead.

_get_result(
result: WrapResult,
return_all_solutions: bool,
goal: Goal,
seed_traj: JointState,
num_seeds: int,
batch_mode: bool = False,
)

Get result from the optimization problem.

Parameters:

  • result – _description_

  • return_all_solutions – _description_

  • goal – _description_

  • seed_traj – _description_

  • num_seeds – _description_

  • batch_mode – _description_

Raises:

  • log_error – _description_

  • ValueError – _description_

Returns:

_description_

batch_solve(
goal: Goal,
seed_traj: JointState | None = None,
seed_success: Tensor | None = None,
use_nn_seed: bool = False,
return_all_solutions: bool = False,
num_seeds: int | None = None,
) → TrajOptResult

Deprecated: Use TrajOptSolver.solve_batch or others instead.

get_linear_seed(
start_state,
goal_state,
) → Tensor

Get linearly interpolated seeds from start states to goal states.

Parameters:

  • start_state – start state of the robot.

  • goal_state – goal state of the robot.

Returns:

Linearly interpolated seeds.

Return type:

torch.Tensor

get_start_seed(
start_state,
) → Tensor

Get trajectory seeds with start state repeated.

Parameters:

start_state – start state of the robot.

Returns:

Trajectory seeds with start state repeated.

Return type:

torch.Tensor

_get_seed_numbers(
num_seeds: int,
) → Dict[str, int]

Get number of seeds for each seed type.

Parameters:

num_seeds – Total number of seeds to generate.

Returns:

Number of seeds for each seed type.

Return type:

Dict[str, int]

get_seed_set(
goal: Goal,
seed_traj: JointState | Tensor | None = None,
seed_success: Tensor | None = None,
num_seeds: int | None = None,
batch_mode: bool = False,
)

Get seed set for optimization.

Parameters:

  • goal – Goal object containing target pose or joint configuration.

  • batch – _description_

  • h – _description_

  • seed_traj – _description_

  • dofseed_success – _description_

  • n_seedsnum_seeds – _description_

  • batch_mode – _description_

Returns:

_description_

get_seeds(
start_state: JointState,
goal_state: JointState,
num_seeds: int | None = None,
) → Tensor

Get seed trajectories for optimization.

Parameters:

  • start_state – Start state of the robot.

  • goal_state – Goal state of the robot.

  • num_seeds – Number of seeds to generate. If None, the number of seeds is set to TrajOptSolver.num_seeds.

Returns:

Seed trajectories of shape [num_seeds, batch, action_horizon, dof]

Return type:

torch.Tensor

get_bias_seed(
start_state,
goal_state,
) → Tensor

Get seed trajectories that pass through the retract configuration at mid-waypoint.

Parameters:

  • start_state – start state of the robot.

  • goal_state – goal state of the robot.

Returns:

Seed trajectories of shape [num_seeds * batch, action_horizon, dof].

Return type:

torch.Tensor

get_interpolated_trajectory(
traj_state: JointState,
) → Tuple[JointState, Tensor]

Get interpolated trajectory from optimized trajectories.

This function will first find the optimal dt for each trajectory in the batch by scaling the trajectories to joint limits. Then it will interpolate the trajectory using the optimal dt.

Parameters:

traj_state – Optimized trajectories of shape [num_seeds * batch, action_horizon, dof].

Returns:

Interpolated trajectory, last time step

for each trajectory in batch, optimal dt for each trajectory in batch.

Return type:

Tuple[JointState, torch.Tensor, torch.Tensor]

calculate_trajectory_dt(
trajectory: JointState,
epsilon: float = 1e-06,
) → Tensor

Calculate the optimal dt for a given trajectory by scaling it to joint limits.

Parameters:

  • trajectory – Trajectory to calculate optimal dt for.

  • epsilon – Small value to improve numerical stability.

Returns:

Optimal trajectory dt.

Return type:

torch.Tensor

reset_seed()

Reset the seed for random number generators in MPPI and rollout functions.

reset_cuda_graph()

Clear all recorded CUDA graphs. This does not work.

reset_shape()

Reset the shape of the rollout function and the solver.

property kinematics: CudaRobotModel

Get the kinematics instance of the robot.

property retract_config: Tensor

Get the retract/home configuration of the robot.

Returns:

Retract configuration of the robot.

Return type:

torch.Tensor

fk(
q: Tensor,
) → CudaRobotModelState

Compute forward kinematics for the robot.

Parameters:

q – Joint configuration of the robot.

Returns:

Forward kinematics of the robot.

Return type:

CudaRobotModelState

property solver_dt: Tensor

Get the current trajectory dt for the solver.

Returns:

Trajectory dt for the solver.

Return type:

torch.Tensor

property solver_dt_tensor: Tensor

Get the current trajectory dt for the solver.

Returns:

Trajectory dt for the solver.

Return type:

torch.Tensor

property minimum_trajectory_dt: float

Get the minimum trajectory dt that is allowed, smaller dt will be clamped to this value.

Returns:

Minimum trajectory dt that is allowed.

Return type:

float

update_solver_dt(
dt: float | Tensor,
base_dt: float | None = None,
max_dt: float | None = None,
base_ratio: float | None = None,
)

Update the trajectory dt for the solver.

This dt is used to calculate the velocity, acceleration and jerk of the trajectory through five point stencil (finite difference).

Parameters:

  • dt – New trajectory dt.

  • base_dt – Base dt for the trajectory. This is not supported.

  • max_dt – Maximum dt for the trajectory. This is not supported.

  • base_ratio – Ratio in trajectory length to scale from base_dt to max_dt. This is not supported.

get_full_js(
active_js: JointState,
) → JointState

Get full joint state from controlled joint state, appending locked joints.

Parameters:

active_js – Controlled joint state

Returns:

Full joint state.

Return type:

JointState

get_active_js(
in_js: JointState,
)

Get controlled joint state from input joint state.

This is used to get the joint state for only joints that are optimization variables. This also re-orders the joints to match the order of optimization variables.

Parameters:

in_js – Input joint state.

Returns:

Active joint state.

Return type:

JointState

update_pose_cost_metric(
metric: PoseCostMetric,
)

Update the pose cost metric for Constrained Planning.

Only supports for the main end-effector. Does not support for multiple links that are specified with link_poses in planning methods.

Parameters:

  • metric – Type and parameters for pose constraint to add.

  • start_state – Start joint state for the constraint.

  • goal_pose – Goal pose for the constraint.

Returns:

True if the constraint can be added, False otherwise.

Return type:

bool

property newton_iters

Get the number of newton outer iterations during L-BFGS optimization.

Returns:

Number of newton outer iterations during L-BFGS optimization.

Return type:

int

property dof: int

Get the number of degrees of freedom of the robot.

Returns:

Number of degrees of freedom of the robot.

Return type:

int

property action_horizon: int

Get the action horizon of the trajectory optimization problem.

Number of actions in trajectory optimization can be smaller than number of waypoints as the first waypoint is the current state of the robot and the last two waypoints are the same as T-2 waypoint to implicitly enforce zero acceleration and zero velocity at T.

Returns:

Action horizon of the trajectory optimization problem.

Return type:

int

bias_node: Tensor | None = None
cspace_threshold: float = 0.1
evaluate_interpolated_trajectory: bool = True
interpolate_rollout: ArmReacher | None = None
interpolation_dt: float = 0.01
interpolation_steps: int = 1000
interpolation_type: InterpolateType = 'linear_cuda'
static load_from_robot_config(
robot_cfg: str | Dict | RobotConfig,
world_model: List[Dict] | List[WorldConfig] | Dict | WorldConfig | None = None,
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),
position_threshold: float = 0.005,
rotation_threshold: float = 0.05,
cspace_threshold: float = 0.05,
world_coll_checker=None,
base_cfg_file: str = 'base_cfg.yml',
particle_file: str = 'particle_trajopt.yml',
gradient_file: str = 'gradient_trajopt.yml',
traj_tsteps: int | None = None,
interpolation_type: InterpolateType = InterpolateType.LINEAR_CUDA,
interpolation_steps: int = 10000,
interpolation_dt: float = 0.01,
minimum_trajectory_dt: float | None = None,
use_cuda_graph: bool = True,
self_collision_check: bool = False,
self_collision_opt: bool = True,
grad_trajopt_iters: int | None = None,
num_seeds: int = 2,
seed_ratio: Dict[str, int] = {'bias': 0.0, 'end': 0.0, 'linear': 1.0, 'start': 0.0},
use_particle_opt: bool = True,
collision_checker_type: CollisionCheckerType | None = CollisionCheckerType.MESH,
traj_evaluator_config: TrajEvaluatorConfig | None = None,
traj_evaluator: TrajEvaluator | None = None,
minimize_jerk: bool = True,
use_gradient_descent: bool = False,
collision_cache: Dict[str, int] | None = None,
n_collision_envs: int | None = None,
use_es: bool | None = None,
es_learning_rate: float | None = 0.1,
use_fixed_samples: bool | None = None,
aux_rollout: ArmReacher | None = None,
evaluate_interpolated_trajectory: bool = True,
fixed_iters: bool | None = None,
store_debug: bool = False,
sync_cuda_time: bool = True,
collision_activation_distance: float | None = None,
trajopt_dt: float | None = None,
trim_steps: List[int] | None = None,
store_debug_in_result: bool = False,
smooth_weight: List[float] | None = None,
state_finite_difference_mode: str | None = None,
filter_robot_command: bool = False,
optimize_dt: bool = True,
project_pose_to_goal_frame: bool = True,
)

Load TrajOptSolver configuration from robot configuration.

Parameters:

  • robot_cfg – Robot configuration to use for motion generation. This can be a path to a yaml file, a dictionary, or an instance of RobotConfig. See Supported Robots for a list of available robots. You can also create a a configuration file for your robot using Configuring a New Robot.

  • world_model – World configuration to use for motion generation. This can be a path to a yaml file, a dictionary, or an instance of WorldConfig. See Collision World Representation for more details.

  • tensor_args – Numerical precision and compute device to use for motion generation.

  • position_threshold – Position threshold between target position and reached position in meters. 0.005 is a good value for most tasks (5mm).

  • rotation_threshold – Rotation threshold between target orientation and reached orientation. The metric is q^T * q, where q is the quaternion difference between target and reached orientation. The metric is not easy to interpret and a future release will provide a more intuitive metric. For now, use 0.05 as a good value.

  • cspace_threshold – Joint space threshold in radians for revolute joints and meters for linear joints between reached joint configuration and target joint configuration used to measure success. Default of 0.05 has been found to be a good value for most cases.

  • world_coll_checker – Instance of world collision checker to use for motion generation. Leaving this to None will create a new instance of world collision checker using the provided attr:world_model.

  • base_cfg_file – Base configuration file containing convergence and constraint criteria to measure success.

  • particle_file – Optimizer configuration file to use for particle-based optimization during trajectory optimization.

  • gradient_file – Optimizer configuration file to use for gradient-based optimization during trajectory optimization.

  • trajopt_tsteps – Number of waypoints to use for trajectory optimization. Default of 32 is found to be a good number for most cases.

  • interpolation_type – Interpolation type to use for generating dense waypoints from optimized trajectory. Default of curobo.util.trajectory.InterpolateType.LINEAR_CUDA is found to be a good choice for most cases. Other suitable options for real robot execution are curobo.util.trajectory.InterpolateType.QUINTIC and curobo.util.trajectory.InterpolateType.CUBIC.

  • interpolation_steps – Buffer size to use for storing interpolated trajectory. Default of 5000 is found to be a good number for most cases.

  • interpolation_dt – Time step in seconds to use for generating interpolated trajectory from optimized trajectory. Change this if you want to generate a trajectory with a fixed timestep between waypoints.

  • minimum_trajectory_dt – Minimum time step in seconds allowed for trajectory optimization.

  • use_cuda_graph – Record compute ops as cuda graphs and replay recorded graphs where implemented. This can speed up execution by upto 10x. Default of True is recommended. Enabling this will prevent changing solve type or batch size after the first call to the solver.

  • self_collision_check – Enable self collision checks for generated motions. Default of True is recommended. Set this to False to debug planning failures. Setting this to False will also set self_collision_opt to False.

  • self_collision_opt – Enable self collision cost during optimization (IK, TrajOpt). Default of True is recommended.

  • grad_trajopt_iters – Number of L-BFGS iterations to run trajectory optimization.

  • num_seeds – Number of seeds to use for trajectory optimization per problem.

  • seed_ratio – Ratio of linear and bias seeds to use for trajectory optimization. Linear seed will generate a linear interpolated trajectory from start state to IK solutions. Bias seed will add a mid-waypoint through the retract configuration. Default of 1.0 linear and 0.0 bias is recommended. This can be changed to 0.5 linear and 0.5 bias, along with changing num_seeds to 2.

  • trajopt_particle_opt – Enable particle-based optimization during trajectory optimization. Default of True is recommended as particle-based optimization moves the interpolated seeds away from bad local minima.

  • collision_checker_type – Type of collision checker to use for motion generation. Default of CollisionCheckerType.MESH supports world represented by Cuboids and Meshes. See Collision World Representation for more details.

  • traj_evaluator_config – Configuration for trajectory evaluator. Default of None will create a new instance of TrajEvaluatorConfig. After trajectory optimization across many seeds, the best trajectory is selected based on this configuration. This evaluator also checks if the optimized dt is within curobo.wrap.reacher.evaluator.TrajEvaluatorConfig.max_dt. This check is needed to measure smoothness of the optimized trajectory as bad trajectories can have very high dt to fit within velocity, acceleration, and jerk limits.

  • traj_evaluator – Instance of trajectory evaluator to use for trajectory optimization. Default of None will create a new instance of TrajEvaluator. In case you want to use a custom evaluator, you can create a child instance of TrajEvaluator and pass it.

  • minimize_jerk – Minimize jerk as regularization during trajectory optimizaiton.

  • use_gradient_descent – Use gradient descent instead of L-BFGS for trajectory optimization. Default of False is recommended. Set to True for debugging gradients of new cost terms.

  • collision_cache – Cache of obstacles to create to load obstacles between planning calls. An example: {"obb": 10, "mesh": 10}, to create a cache of 10 cuboids and 10 meshes.

  • n_collision_envs – Number of collision environments to create for batched optimization across different environments. Only used for TrajOptSolver.solve_batch_env`and :py:meth:`TrajOptSolver.solve_batch_env_goalset.

  • n_collision_envs – Number of collision environments to create for batched planning across different environments. Only used for MotionGen.plan_batch_env and MotionGen.plan_batch_env_goalset.

  • use_es – Use Evolution Strategies for optimization. Default of None will use MPPI.

  • es_learning_rate – Learning rate to use for Evolution Strategies.

  • use_fixed_samples – Use fixed samples for MPPI. Setting to False will increase compute time as new samples are generated for each iteration of MPPI.

  • aux_rollout – Rollout instance to use for auxiliary rollouts.

  • evaluate_interpolated_trajectory – Evaluate interpolated trajectory after optimization. Default of True is recommended to ensure the optimized trajectory is not passing through very thin obstacles.

  • fixed_iters – Use fixed number of iterations of L-BFGS for trajectory optimization. Default of None will use the setting from the optimizer configuration. In most cases, fixed iterations of solvers are run as current solvers treat constraints as costs and there is no guarantee that the constraints will be satisfied. Instead of checking constraints between iterations of a solver and exiting, it’s computationally cheaper to run a fixed number of iterations. In addition, running fixed iterations of solvers is more robust to outlier problems.

  • store_debug – Store debugging information such as values of optimization variables in TrajOpt result. Setting this to True will set use_cuda_graph to False.

  • sync_cuda_time – Synchronize with host using torch.cuda.synchronize before measuring compute time.

  • collision_activation_distance – Distance in meters to activate collision cost. A good value to start with is 0.01 meters. Increase the distance if the robot needs to stay further away from obstacles.

  • trajopt_dt – Time step in seconds to use for trajectory optimization. A good value to start with is 0.15 seconds. This value is used to compute velocity, acceleration, and jerk values for waypoints through finite difference.

  • trim_steps – Trim waypoints from optimized trajectory. The optimized trajectory will contain the start state at index 0 and have the last two waypoints be the same as T-2 as trajectory optimization implicitly optimizes for zero acceleration and velocity at the last waypoint. An example: [1,-2] will trim the first waypoint and last 3 waypoints from the optimized trajectory.

  • store_debug_in_result – Store debugging information in MotionGenResult. This value is set to True if either store_ik_debug or store_trajopt_debug is set to True.

  • smooth_weight – Override smooth weight for trajectory optimization. It’s not recommended to set this value for most cases.

  • state_finite_difference_mode – Finite difference mode to use for computing velocity, acceleration, and jerk values. Default of None will use the setting from the optimizer configuration file. The default finite difference method is a five point stencil to compute the derivatives as this is accurate and provides faster convergence compared to backward or central difference methods.

  • filter_robot_command – Filter generated trajectory to remove finite difference artifacts. Default of True is recommended.

  • optimize_dt – Optimize dt during trajectory optimization. Default of True is recommended to find time-optimal trajectories. Setting this to False will use the provided trajopt_dt for trajectory optimization. Setting to False is required when optimizing from a non-static start state.

  • project_pose_to_goal_frame – Project pose to goal frame when calculating distance between reached and goal pose. Use this to constrain motion to specific axes either in the global frame or the goal frame.

Returns:

Trajectory optimization configuration.

Return type:

TrajOptSolverConfig

num_seeds: int = 1
optimize_dt: bool = True
seed_ratio: Dict[str, int] | None = None
store_debug_in_result: bool = False
sync_cuda_time: bool = True
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)
traj_evaluator_config: TrajEvaluatorConfig | None = None
trim_steps: List[int] | None = None
use_cspace_seed: bool = True
use_cuda_graph: bool = True
use_cuda_graph_metrics: bool = False
world_coll_checker: WorldCollision | None = None
robot_config: RobotConfig
solver: WrapBase
rollout_fn: ArmReacher
position_threshold: float
rotation_threshold: float
traj_tsteps: int
jit_trajopt_best_select(
success,
smooth_label,
cspace_error: Tensor | None,
pose_error: Tensor | None,
position_error: Tensor | None,
rotation_error: Tensor | None,
goalset_index: Tensor | None,
cost,
smooth_cost,
batch_mode: bool,
batch: int,
num_seeds: int,
col,
opt_dt,
)

JIT function to select the best solution from optimized seeds.