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¶
- rollout_fn: ArmReacher¶
- interpolation_type: InterpolateType = 'linear_cuda'¶
- world_coll_checker: WorldCollision | None = None¶
- traj_evaluator_config: TrajEvaluatorConfig | None = None¶
- traj_evaluator: TrajEvaluator | 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)¶
- interpolate_rollout: ArmReacher | None = None¶
- 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 arecurobo.util.trajectory.InterpolateType.QUINTIC
andcurobo.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
andMotionGen.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:
- 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.
- solution: JointState¶
- metrics: RolloutMetrics | None = None¶
- interpolated_solution: JointState | None = None¶
- raw_solution: JointState | None = None¶
- _abc_impl = <_abc._abc_data object>¶
- _is_protocol = False¶
- count(
- value,
- index(
- value[,
- start[,
- stop,]]
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,
- index(
- value[,
- start[,
- stop,]]
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¶
- raw_solution: JointState | None = None¶
- solution: JointState¶
- 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( ) 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',
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:
- 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,
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:
- _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 withcurobo.opt.newton.lbfgs.LBFGSOptConfig.inner_iters
.
- Returns:
Result of the trajectory optimization.
- Return type:
- 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,
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 withcurobo.opt.newton.lbfgs.LBFGSOptConfig.inner_iters
.
- Returns:
Result of the trajectory optimization.
- Return type:
- 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,
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 withcurobo.opt.newton.lbfgs.LBFGSOptConfig.inner_iters
.
- Returns:
Result of the trajectory optimization.
- Return type:
- 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,
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 withcurobo.opt.newton.lbfgs.LBFGSOptConfig.inner_iters
.
- Returns:
Result of the trajectory optimization.
- Return type:
- 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,
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 withcurobo.opt.newton.lbfgs.LBFGSOptConfig.inner_iters
.
- Returns:
Result of the trajectory optimization.
- Return type:
- 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,
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 withcurobo.opt.newton.lbfgs.LBFGSOptConfig.inner_iters
.
- Returns:
Result of the trajectory optimization.
- Return type:
- 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,
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 withcurobo.opt.newton.lbfgs.LBFGSOptConfig.inner_iters
.
- Returns:
Result of the trajectory optimization.
- Return type:
- 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,
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 – Result of the optimization problem.
return_all_solutions – Return solutions for all seeds.
goal – Goal object containing convergence parameters.
seed_traj – Seed trajectory used for optimization.
num_seeds – Number of seeds used for optimization.
batch_mode – Batch mode for problems.
- Returns:
Result of the trajectory optimization.
- Return type:
- 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,
Deprecated: Use
TrajOptSolver.solve_batch
or others instead.
- get_linear_seed(
- start_state,
- goal_state,
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:
- get_start_seed(
- start_state,
Get trajectory seeds with start state repeated.
- Parameters:
start_state – start state of the robot.
- Returns:
Trajectory seeds with start state repeated.
- Return type:
- 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,
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:
- get_bias_seed(
- start_state,
- goal_state,
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:
- get_interpolated_trajectory(
- traj_state: JointState,
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,
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:
- 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:
- fk(
- q: Tensor,
Compute forward kinematics for the robot.
- Parameters:
q – Joint configuration of the robot.
- Returns:
Forward kinematics of the robot.
- Return type:
- property solver_dt: Tensor¶
Get the current trajectory dt for the solver.
- Returns:
Trajectory dt for the solver.
- Return type:
- property solver_dt_tensor: Tensor¶
Get the current trajectory dt for the solver.
- Returns:
Trajectory dt for the solver.
- Return type:
- 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:
- 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,
Get full joint state from controlled joint state, appending locked joints.
- Parameters:
active_js – Controlled joint state
- Returns:
Full joint state.
- Return type:
- 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:
- 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:
- 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:
- property dof: int¶
Get the number of degrees of freedom of the robot.
- Returns:
Number of degrees of freedom of the robot.
- Return type:
- 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:
- interpolate_rollout: ArmReacher | None = None¶
- 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 arecurobo.util.trajectory.InterpolateType.QUINTIC
andcurobo.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
andMotionGen.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:
- 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¶
- world_coll_checker: WorldCollision | None = None¶
- robot_config: RobotConfig¶
- rollout_fn: ArmReacher¶
- jit_feasible_success(
- feasible,
- position_error: Tensor | None,
- rotation_error: Tensor | None,
- cspace_error: Tensor | None,
- position_threshold: float,
- rotation_threshold: float,
- cspace_threshold: float,
JIT function to check if the optimization is successful.
- 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.