curobo.wrap.reacher.motion_gen module¶
This module contains MotionGen
class that provides a high-level interface for motion
generation. Motion Generation can take goals either as joint configurations
MotionGen.plan_single_js
or as Cartesian poses MotionGen.plan_single
. When Cartesian
pose is given as target, inverse kinematics is first done to generate seeds for trajectory
optimization. Motion generation fallback to using a graph planner when linear interpolated
trajectory optimization seeds are not successful. Reaching one Cartesian pose in a goalset is also
supported using MotionGen.plan_goalset
. Batched planning in the same world environment (
MotionGen.plan_batch
) and different world environments (MotionGen.plan_batch_env
)
is also provided.
A motion generation request can be configured using MotionGenPlanConfig
. The result
of motion generation is returned as a MotionGenResult
. A minimal example is availble at
Motion Generation.
- class MotionGenConfig(
- ik_seeds: int,
- graph_seeds: int,
- trajopt_seeds: int,
- noisy_trajopt_seeds: int,
- batch_ik_seeds: int,
- batch_trajopt_seeds: int,
- robot_cfg: RobotConfig,
- ik_solver: IKSolver,
- graph_planner: GraphPlanBase,
- trajopt_solver: TrajOptSolver,
- js_trajopt_solver: TrajOptSolver,
- finetune_js_trajopt_solver: TrajOptSolver,
- finetune_trajopt_solver: TrajOptSolver,
- interpolation_type: InterpolateType,
- interpolation_steps: int,
- world_coll_checker: WorldCollision,
- tensor_args: TensorDeviceType,
- partial_ik_iters: int,
- graph_trajopt_iters: int | None = None,
- store_debug_in_result: bool = False,
- interpolation_dt: float = 0.01,
- finetune_dt_scale: float = 0.9,
- use_cuda_graph: bool = True,
- optimize_dt: bool = True,
Bases:
object
Configuration dataclass for creating a motion generation instance.
- robot_cfg: RobotConfig¶
instance of robot configuration shared across all solvers.
- graph_planner: GraphPlanBase¶
instance of graph planner to use.
- trajopt_solver: TrajOptSolver¶
instance of trajectory optimization solver to use for reaching Cartesian poses.
- js_trajopt_solver: TrajOptSolver¶
instance of trajectory optimization solver to use for reaching joint space targets.
- finetune_js_trajopt_solver: TrajOptSolver¶
instance of trajectory optimization solver for final fine tuning for joint space targets.
- finetune_trajopt_solver: TrajOptSolver¶
instance of trajectory optimization solver for final fine tuning.
- interpolation_type: InterpolateType¶
interpolation to use for getting dense waypoints from optimized solution.
- world_coll_checker: WorldCollision¶
instance of world collision checker.
- tensor_args: TensorDeviceType¶
device to load motion generation.
- graph_trajopt_iters: int | None = None¶
number of iterations to run trajectory optimization when seeded from a graph plan.
- use_cuda_graph: bool = True¶
record compute ops as cuda graphs and replay recorded graphs for upto 10x faster execution.
- optimize_dt: bool = True¶
After 100 iterations of trajectory optimization, a new dt is computed that pushes the velocity, acceleration, or jerk limits to the maximum. This new dt is then used with a reduction
MotionGenPlanConfig.finetune_dt_scale
to run 200+ iterations of trajectory optimization. If trajectory optimization fails with the new dt, the new dt is increased and tried again untilMotionGenPlanConfig.finetune_attempts
.
- static load_from_robot_config(
- robot_cfg: str | Dict | RobotConfig,
- world_model: str | 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),
- num_ik_seeds: int = 32,
- num_graph_seeds: int = 4,
- num_trajopt_seeds: int = 4,
- num_batch_ik_seeds: int = 32,
- num_batch_trajopt_seeds: int = 1,
- num_trajopt_noisy_seeds: int = 1,
- 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_ik_file: str = 'particle_ik.yml',
- gradient_ik_file: str = 'gradient_ik.yml',
- graph_file: str = 'graph.yml',
- particle_trajopt_file: str = 'particle_trajopt.yml',
- gradient_trajopt_file: str = 'gradient_trajopt.yml',
- finetune_trajopt_file: str | None = None,
- trajopt_tsteps: int = 32,
- interpolation_steps: int = 5000,
- interpolation_dt: float = 0.02,
- interpolation_type: InterpolateType = InterpolateType.LINEAR_CUDA,
- use_cuda_graph: bool = True,
- self_collision_check: bool = True,
- self_collision_opt: bool = True,
- grad_trajopt_iters: int | None = None,
- trajopt_seed_ratio: Dict[str, int] = {'bias': 0.0, 'linear': 1.0},
- ik_opt_iters: int | None = None,
- ik_particle_opt: bool = True,
- collision_checker_type: CollisionCheckerType | None = CollisionCheckerType.MESH,
- sync_cuda_time: bool | None = None,
- trajopt_particle_opt: bool = True,
- traj_evaluator_config: TrajEvaluatorConfig | None = None,
- traj_evaluator: TrajEvaluator | None = None,
- minimize_jerk: bool = True,
- filter_robot_command: bool = True,
- use_gradient_descent: bool = False,
- collision_cache: Dict[str, int] | None = None,
- n_collision_envs: int | None = None,
- ee_link_name: str | None = None,
- use_es_ik: bool | None = None,
- use_es_trajopt: bool | None = None,
- es_ik_learning_rate: float = 1.0,
- es_trajopt_learning_rate: float = 1.0,
- use_ik_fixed_samples: bool | None = None,
- use_trajopt_fixed_samples: bool | None = None,
- evaluate_interpolated_trajectory: bool = True,
- partial_ik_iters: int = 2,
- fixed_iters_trajopt: bool | None = None,
- store_ik_debug: bool = False,
- store_trajopt_debug: bool = False,
- graph_trajopt_iters: int | None = None,
- collision_max_outside_distance: float | None = None,
- collision_activation_distance: float | None = None,
- trajopt_dt: float | None = None,
- js_trajopt_dt: float | None = None,
- js_trajopt_tsteps: int | None = None,
- trim_steps: List[int] | None = None,
- store_debug_in_result: bool = False,
- finetune_trajopt_iters: int | None = None,
- smooth_weight: List[float] | None = None,
- finetune_smooth_weight: List[float] | None = None,
- state_finite_difference_mode: str | None = None,
- finetune_dt_scale: float = 0.9,
- minimum_trajectory_dt: float | None = None,
- maximum_trajectory_time: float | None = None,
- maximum_trajectory_dt: float | None = None,
- velocity_scale: List[float] | float | None = None,
- acceleration_scale: List[float] | float | None = None,
- jerk_scale: List[float] | float | None = None,
- optimize_dt: bool = True,
- project_pose_to_goal_frame: bool = True,
- ik_seed: int = 1531,
- graph_seed: int = 1531,
- high_precision: bool = False,
Create a motion generation configuration from robot and world 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.
num_ik_seeds – Number of seeds to use for solving inverse kinematics. Default of 32 is found to be a good number for most cases. In sparse environments, a lower number of 16 can also be used.
num_graph_seeds – Number of seeds to use for graph planner per problem query. When graph planning is used to generate seeds for trajectory optimization, graph planner will attempt to find collision-free paths from the start state to the many inverse kinematics solutions.
num_trajopt_seeds – Number of seeds to use for trajectory optimization per problem query. Default of 4 is found to be a good number for most cases. Increasing this will increase memory usage.
num_batch_ik_seeds – Number of seeds to use for inverse kinematics during batched planning. Default of 32 is found to be a good number for most cases.
num_batch_trajopt_seeds – Number of seeds to use for trajectory optimization during batched planning. Using more than 1 will disable graph planning for batched planning.
num_trajopt_noisy_seeds – Number of augmented trajectories to use per trajectory seed. The augmentation is done by adding random noise to the trajectory. This augmentation has not been found to be useful and it’s recommended to keep this to 1. The noisy seeds can also be used in conjunction with the trajopt_seed_ratio to generate seeds that go through a bias point.
position_threshold – Position threshold in meters between reached position and target position used to measure success.
rotation_threshold – Rotation threshold between reached orientation and target orientation used to measure success. 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_ik_file – Optimizer configuration file to use for particle-based optimization during inverse kinematics.
gradient_ik_file – Optimizer configuration file to use for gradient-based optimization during inverse kinematics.
graph_file – Configuration file to use for graph planner.
particle_trajopt_file – Optimizer configuration file to use for particle-based optimization during trajectory optimization.
gradient_trajopt_file – Optimizer configuration file to use for gradient-based optimization during trajectory optimization.
finetune_trajopt_file – Optimizer configuration file to use for finetuning 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_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.
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
.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 problem 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 iterations to run trajectory optimization.
trajopt_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 trajopt_noisy_seeds to 2.
ik_opt_iters – Number of iterations to run inverse kinematics.
ik_particle_opt – Enable particle-based optimization during inverse kinematics. Default of True is recommended as particle-based optimization moves the random seeds to a regions of 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.
sync_cuda_time – Synchronize with host using
torch.cuda.synchronize
before measuring compute time.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.
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.
filter_robot_command – Filter generated trajectory to remove finite difference artifacts. Default of True is recommended.
use_gradient_descent – Use gradient descent instead of L-BFGS for trajectory optimization.
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 planning across different environments. Only used for
MotionGen.plan_batch_env
andMotionGen.plan_batch_env_goalset
.ee_link_name – End effector link/frame to use for reaching Cartesian poses. Default of None will use the end effector link from the robot configuration. This cannot be changed after creating the robot configuration.
use_es_ik – Use evolutionary strategy for as the particle-based optimizer for inverse kinematics. Default of None will use MPPI as the optimization algorithm. ES is not recommended as it’s unstable and provided optimization parameters were not tuned.
use_es_trajopt – Use evolutionary strategy as the particle-based optimizer for trajectory optimization. Default of None will use MPPI as the optimization algorithm. ES is not recommended as it’s unstable and provided optimization parameters were not tuned.
es_ik_learning_rate – Learning rate to use for evolutionary strategy in IK.
es_trajopt_learning_rate – Learning rate to use for evolutionary strategy in TrajOpt.
use_ik_fixed_samples – Use fixed samples of noise during particle-based optimization of IK. Default of None will use the setting from the optimizer configuration file (
particle_ik.yml
).use_trajopt_fixed_samples – Use fixed samples of noise during particle-based optimization of trajectory optimization. Default of None will use the setting from the optimizer configuration file (
particle_trajopt.yml
).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.
partial_ik_iters – Number of iterations of L-BFGS to run inverse kinematics when only partial IK is needed.
fixed_iters_trajopt – 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_ik_debug – Store debugging information such as values of optimization variables at every iteration in IK result. Setting this to True will set
use_cuda_graph
to False.store_trajopt_debug – Store debugging information such as values of optimization variables in TrajOpt result. Setting this to True will set
use_cuda_graph
to False.graph_trajopt_iters – Number of iterations to run trajectory optimization when seeded from a graph plan. Default of None will use the same number of iterations as linear seeded trajectory optimization. This can be set to a higher value of 200 in case where trajectories obtained are not of requird quality.
collision_max_outside_distance – Maximum distance to check for collision outside a obstacle. Increasing this value will slow down collision checks with Meshes as closest point queries will be run up to this distance outside an obstacle.
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.
js_trajopt_dt – Time step in seconds to use for trajectory optimization when reaching joint space targets. 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.
js_trajopt_tsteps – Number of waypoints to use for trajectory optimization when reaching joint space targets. Default of None will use the same number of waypoints as Cartesian trajectory optimization.
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.
finetune_trajopt_iters – Number of iterations to run trajectory optimization for finetuning after an initial collision-free trajectory is obtained.
smooth_weight – Override smooth weight for trajectory optimization. It’s not recommended to set this value for most cases.
finetune_smooth_weight – Override smooth weight for finetuning 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.
finetune_dt_scale – Scale initial estimated dt by this value to finetune trajectory optimization. This is deprecated and will be removed in future releases. Use
MotionGenPlanConfig.finetune_dt_scale
instead.minimum_trajectory_dt – Minimum time step in seconds allowed for trajectory optimization.
maximum_trajectory_time – Maximum time in seconds allowed for trajectory optimization.
maximum_trajectory_dt – Maximum time step in seconds allowed for trajectory optimization.
velocity_scale – Scale velocity limits by this value. Default of None will not scale the velocity limits. To generate slower trajectories, use
MotionGenPlanConfig.time_dilation_factor
< 1.0 instead. Changing this value is not recommended as it changes the scale of cost terms and they would require retuning.acceleration_scale – Scale acceleration limits by this value. Default of None will not scale the acceleration limits. To generate slower trajectories, use
MotionGenPlanConfig.time_dilation_factor
< 1.0 instead. Changing this value is not recommended as it changes the scale of cost terms and they would require retuning.jerk_scale – Scale jerk limits by this value. Default of None will not scale the jerk limits. To generate slower trajectories, use
MotionGenPlanConfig.time_dilation_factor
< 1.0 instead. Changing this value is not recommended as it changes the scale of cost terms and they would require retuning.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.
ik_seed – Random seed to use for inverse kinematics.
graph_seed – Random seed to use for graph planner.
high_precision – Use high precision settings for motion generation. This will increase the number of iterations for optimization solvers and reduce the thresholds for position to 1mm and rotation to 0.025. Default of False is recommended for most cases as standard motion generation settings reach within 0.5mm on most problems.
- Returns:
Instance of motion generation configuration.
- Return type:
- class MotionGenPlanConfig(
- enable_graph: bool = False,
- enable_opt: bool = True,
- use_nn_ik_seed: bool = False,
- need_graph_success: bool = False,
- max_attempts: int = 60,
- timeout: float = 10.0,
- enable_graph_attempt: int | None = 3,
- disable_graph_attempt: int | None = None,
- ik_fail_return: int | None = None,
- partial_ik_opt: bool = False,
- num_ik_seeds: int | None = None,
- num_graph_seeds: int | None = None,
- num_trajopt_seeds: int | None = None,
- success_ratio: float = 1,
- fail_on_invalid_query: bool = True,
- use_start_state_as_retract: bool = True,
- pose_cost_metric: PoseCostMetric | None = None,
- enable_finetune_trajopt: bool = True,
- parallel_finetune: bool = True,
- finetune_dt_scale: float | None = 0.85,
- finetune_attempts: int = 5,
- finetune_dt_decay: float = 1.025,
- time_dilation_factor: float | None = None,
- check_start_validity: bool = True,
- finetune_js_dt_scale: float | None = 1.1,
Bases:
object
Configuration for querying motion generation.
- enable_graph: bool = False¶
Use graph planner to generate collision-free seed for trajectory optimization.
- use_nn_ik_seed: bool = False¶
Use neural network IK seed for solving inverse kinematics. Not implemented.
- enable_graph_attempt: int | None = 3¶
Number of failed attempts at which to fallback to a graph planner for obtaining trajectory seeds.
- disable_graph_attempt: int | None = None¶
Number of failed attempts at which to disable graph planner. This has not been found to be useful.
- ik_fail_return: int | None = None¶
Number of IK attempts allowed before returning a failure. Set this to a low value (5) to save compute time when an unreachable goal is given.
- partial_ik_opt: bool = False¶
Full IK solving takes 10% of the planning time. Setting this to True will run only 50 iterations of IK instead of 100 and then run trajecrtory optimization without checking if IK is successful. This can reduce the planning time by 5% but generated solutions can have larger motion time and path length. Leave this to False for most cases.
- num_ik_seeds: int | None = None¶
Number of seeds to use for solving inverse kinematics. Chanigng this will cause the existing CUDAGraphs to be invalidated. Instead, set num_ik_seeds when creating
MotionGenConfig
.
- num_graph_seeds: int | None = None¶
Number of seeds to use for graph planner. We found 4 to be a good number for most cases. The default value is set when creating
MotionGenConfig
so leave this to None.
- num_trajopt_seeds: int | None = None¶
Number of seeds to use for trajectory optimization. We found 12 to be a good number for most cases. The default value is set when creating
MotionGenConfig
so leave this to None.
- success_ratio: float = 1¶
Ratio of successful motion generation queries to total queries in batch planning mode. Motion generation is queries upto
MotionGenPlanConfig.max_attempts
until this ratio is met.
- fail_on_invalid_query: bool = True¶
Return a failure if the query is invalid. Set this to False to debug a query that is invalid.
- use_start_state_as_retract: bool = True¶
use start config as regularization for IK instead of
curobo.types.robot.RobotConfig.kinematics.kinematics_config.retract_config
- pose_cost_metric: PoseCostMetric | None = None¶
Use a custom pose cost metric for trajectory optimization. This is useful for adding additional constraints to motion generation, such as constraining the end-effector’s motion to a plane or a line or hold orientation while moving. This is also useful for only reaching partial pose (e.g., only position). See
curobo.rollout.cost.pose_cost.PoseCostMetric
for more details.
- enable_finetune_trajopt: bool = True¶
Run finetuning trajectory optimization after running 100 iterations of trajectory optimization. This will provide shorter and smoother trajectories. When
MotionGenConfig.optimize_dt
is True, this flag will also scale the trajectory optimization by a new dt. Leave this to True for most cases. If you are not interested in finding time-optimal solutions and only want to use motion generation as a feasibility check, set this to False. Note that when set to False, the resulting trajectory is only guaranteed to be collision-free and within joint limits. When False, it’s not guaranteed to be smooth and might not execute on a real robot.
- parallel_finetune: bool = True¶
Run finetuning trajectory optimization across all trajectory optimization seeds. If this is set to False, then only 1 successful seed per query is selected and finetuned. When False, we have observed that the resulting trajectory is not as optimal as when set to True.
- finetune_dt_scale: float | None = 0.85¶
Scale dt by this value before running finetuning trajectory optimization. This enables trajectory optimization to find shorter paths and also account for smoothness over variable length trajectories. This is only used when
MotionGenConfig.optimize_dt
is True.
- finetune_attempts: int = 5¶
Number of attempts to run finetuning trajectory optimization. Every attempt will increase the
MotionGenPlanConfig.finetune_dt_scale
byMotionGenPlanConfig.finetune_dt_decay
as a path couldn’t be found with the previous smaller dt.
- finetune_dt_decay: float = 1.025¶
Decay factor used to increase
MotionGenPlanConfig.finetune_dt_scale
when optimization fails to find a solution. This is only used whenMotionGenConfig.optimize_dt
is True.
- time_dilation_factor: float | None = None¶
Slow down optimized trajectory by re-timing with a dilation factor. This is useful to execute trajectories at a slower speed for debugging. Use this to generate slower trajectories instead of reducing
MotionGenConfig.velocity_scale
orMotionGenConfig.acceleration_scale
as those parameters will require re-tuning of the cost terms whileMotionGenPlanConfig.time_dilation_factor
will only post-process the trajectory.
- check_start_validity: bool = True¶
Check if the start state is valid before runnning any steps in motion generation. This will check for joint limits, self-collision, and collision with the world.
- clone() MotionGenPlanConfig ¶
Clone the current planning configuration.
- class MotionGenStatus(value)¶
Bases:
Enum
Status of motion generation query.
- IK_FAIL = 'IK Fail'¶
Inverse kinematics failed to find a solution.
- GRAPH_FAIL = 'Graph Fail'¶
Graph planner failed to find a solution.
- TRAJOPT_FAIL = 'TrajOpt Fail'¶
Trajectory optimization failed to find a solution.
- FINETUNE_TRAJOPT_FAIL = 'Finetune TrajOpt Fail'¶
Finetune Trajectory optimization failed to find a solution.
- DT_EXCEPTION = 'dt exceeded maximum allowed trajectory dt'¶
Optimized dt is greater than the maximum allowed dt. Set maximum_trajectory_dt to a higher value.
- INVALID_QUERY = 'Invalid Query'¶
Invalid query was given. The start state is either out of joint limits, in collision with world, or in self-collision. In the future, this will also check for reachability of goal pose/ joint target in joint limits.
- INVALID_START_STATE_UNKNOWN_ISSUE = 'Invalid Start State, unknown issue'¶
Invalid start state was given. Unknown reason.
- INVALID_START_STATE_WORLD_COLLISION = 'Start state is colliding with world'¶
Invalid start state was given. The start state is in world collision.
- INVALID_START_STATE_SELF_COLLISION = 'Start state is in self-collision'¶
Invalid start state was given. The start state is in self-collision.
- INVALID_START_STATE_JOINT_LIMITS = 'Start state is out of joint limits'¶
Invalid start state was given. The start state is out of joint limits.
- INVALID_PARTIAL_POSE_COST_METRIC = 'Invalid partial pose metric'¶
Invalid partial pose target.
- SUCCESS = 'Success'¶
Motion generation query was successful.
- NOT_ATTEMPTED = 'Not Attempted'¶
Motion generation was not attempted.
- class MotionGenResult(
- success: Tensor | None = None,
- valid_query: bool = True,
- optimized_plan: JointState | None = None,
- optimized_dt: Tensor | None = None,
- position_error: Tensor | None = None,
- rotation_error: Tensor | None = None,
- cspace_error: Tensor | None = None,
- solve_time: float = 0.0,
- ik_time: float = 0.0,
- graph_time: float = 0.0,
- trajopt_time: float = 0.0,
- finetune_time: float = 0.0,
- total_time: float = 0.0,
- interpolated_plan: JointState | None = None,
- interpolation_dt: float = 0.02,
- path_buffer_last_tstep: List[int] | None = None,
- debug_info: Any | None = None,
- status: MotionGenStatus | str | None = None,
- attempts: int = 1,
- trajopt_attempts: int = 0,
- used_graph: bool = False,
- graph_plan: JointState | None = None,
- goalset_index: Tensor | None = None,
Bases:
object
Result obtained from motion generation.
- valid_query: bool = True¶
returns true if the start state is not satisfying constraints (e.g., in collision)
- optimized_plan: JointState | None = None¶
optimized trajectory
- position_error: Tensor | None = None¶
Cartesian position error at final timestep, returning l2 distance.
- rotation_error: Tensor | None = None¶
Cartesian rotation error at final timestep, computed as q^(-1) * q_g
- cspace_error: Tensor | None = None¶
Error in joint configuration, when planning in joint space, returning l2 distance.
- total_time: float = 0.0¶
sum of ik_time, graph_time, and trajopt_time. This also includes any processing between calling the different modules.
- interpolated_plan: JointState | None = None¶
interpolated solution, useful for visualization.
- path_buffer_last_tstep: List[int] | None = None¶
last timestep in interpolated plan per batch index. Only used for batched queries
- status: MotionGenStatus | str | None = None¶
status of motion generation query.
- graph_plan: JointState | None = None¶
stores graph plan.
- goalset_index: Tensor | None = None¶
stores the index of the goal pose reached when planning for a goalset.
- clone()¶
Clone the current result.
- copy_idx(
- idx: Tensor,
- source_result: MotionGenResult,
Copy data from source result to current result at index.
- Parameters:
idx – index to copy data at.
source_result – source result to copy data from.
- Returns:
copied result.
- Return type:
- get_paths() List[JointState] ¶
Get interpolated trajectories from the result. Use for batched queries.
This will return unsuccessful trajectories as well. Use
MotionGenResult.get_successful_paths
to get only successful trajectories.- Returns:
- Interpolated trajectories. Check
MotionGenResult.interpolation_dt
for the time between steps.
- Return type:
List[JointState]
- get_successful_paths() List[Tensor] ¶
Get successful interpolated trajectories from the result. Use for batched queries.
- Returns:
- Interpolated trajectories. Check
MotionGenResult.interpolation_dt
for the time between steps.
- Return type:
List[JointState]
- get_interpolated_plan() JointState ¶
Get interpolated trajectory from the result.
- Returns:
Interpolated trajectory. Check
MotionGenResult.interpolation_dt
for the time between steps.- Return type:
- retime_trajectory(
- time_dilation_factor: float,
- interpolate_trajectory: bool = True,
- interpolation_dt: float | None = None,
- interpolation_kind: InterpolateType = InterpolateType.LINEAR_CUDA,
- create_interpolation_buffer: bool = True,
Retime the optimized trajectory by a dilation factor.
- Parameters:
time_dilation_factor – Time factor to slow down the trajectory. Should be less than 1.0.
interpolate_trajectory – Interpolate the trajectory after retiming.
interpolation_dt – Time between steps in the interpolated trajectory. If None,
MotionGenResult.interpolation_dt
is used.interpolation_kind – Interpolation type to use.
create_interpolation_buffer – Create a new buffer for interpolated trajectory. Set this to True if existing buffer is not large enough to store new trajectory.
- static _check_none_and_copy_idx(
- current_tensor: Tensor | JointState | None,
- source_tensor: Tensor | JointState | None,
- idx: int,
Helper function to copy data from source tensor to current tensor at index.
Also supports copying from JointState to JointState.
- Parameters:
current_tensor – tensor to copy data to.
source_tensor – tensor to copy data from.
idx – index to copy data at.
- Returns:
copied tensor.
- Return type:
Union[torch.Tensor, JointState]
- class MotionGen(
- config: MotionGenConfig,
Bases:
MotionGenConfig
Motion generation wrapper for generating collision-free trajectories.
This module provides an interface to generate collision-free trajectories for manipulators. It supports Cartesian Pose for end-effectors and joint space goals. When a Cartesian Pose is used as target, it uses batched inverse kinematics to find multiple joint configuration solutions for the Cartesian Pose and then runs trajectory optimization with a linear interpolation from start configuration to each of the IK solutions. When trajectory optimization fails, it uses a graph planner to find collision-free paths to the IK solutions and then uses these paths as seeds for trajectory optimization. The module also supports batched queries for motion generation. Use this module as the high-level API for generating collision-free trajectories.
Initializes the motion generation module.
- Parameters:
config – Configuration for motion generation.
- update_batch_size(
- seeds=10,
- batch=1,
Update the batch size for motion generation.
- Parameters:
seeds – Number of seeds for trajectory optimization and graph planner.
batch – Number of queries to run in batch mode.
- solve_ik(
- goal_pose: Pose,
- retract_config: Tensor | None = None,
- seed_config: Tensor | None = None,
- return_seeds: int = 1,
- num_seeds: int | None = None,
- use_nn_seed: bool = True,
- newton_iters: int | None = None,
Solve inverse kinematics for a given Cartesian Pose or a batch of Poses.
Use this only if your problem size is same as when using motion generation. If you want to solve IK for a different problem size, create an instance of
curobo.wrap.reacher.ik_solver.IKSolver
.- Parameters:
goal_pose – Goal Pose for the end-effector.
retract_config – Retract configuration for the end-effector.
seed_config – Seed configuration for inverse kinematics.
return_seeds – Number of solutions to return per problem query.
num_seeds – Number of seeds to use for solving inverse kinematics.
use_nn_seed – Use neural network seed for solving inverse kinematics. This is not implemented.
newton_iters – Number of Newton iterations to run for solving inverse kinematics. This is useful to override the default number of iterations.
- Returns:
Result of inverse kinematics.
- Return type:
- graph_search( ) GraphResult ¶
Run graph search to find collision-free paths between start and goal configurations.
- Parameters:
start_config – Start joint configurations of the robot.
goal_config – Goal joint configurations of the robot.
interpolation_steps – Number of interpolation steps to interpolate obtained solutions.
- Returns:
Result of graph search.
- Return type:
- plan_single(
- start_state: JointState,
- goal_pose: Pose,
- plan_config: MotionGenPlanConfig = MotionGenPlanConfig(enable_graph=False, enable_opt=True, use_nn_ik_seed=False, need_graph_success=False, max_attempts=60, timeout=10.0, enable_graph_attempt=3, disable_graph_attempt=None, ik_fail_return=None, partial_ik_opt=False, num_ik_seeds=None, num_graph_seeds=None, num_trajopt_seeds=None, success_ratio=1, fail_on_invalid_query=True, use_start_state_as_retract=True, pose_cost_metric=None, enable_finetune_trajopt=True, parallel_finetune=True, finetune_dt_scale=0.85, finetune_attempts=5, finetune_dt_decay=1.025, time_dilation_factor=None, check_start_validity=True, finetune_js_dt_scale=1.1),
- link_poses: List[Pose] | None = None,
Plan a single motion to reach a goal pose from a start joint state.
This also supports reaching target poses for different links in the robot by providing goal poses for each link in the link_poses argument. The link_poses argument should contain a pose for each link specified in
MotionGen.kinematics
. Constrained planning is supported by callingMotionGen.update_pose_cost_metric
before calling this method. See Constrained Planning for more details.- Parameters:
start_state – Start joint state of the robot. When planning from a non-static state, i.e, when velocity or acceleration is non-zero, set
MotionGen.optimize_dt
to False.goal_pose – Goal pose for the end-effector.
plan_config – Planning parameters for motion generation.
link_poses – Goal poses for each link in the robot when planning for multiple links.
- Returns:
- Result of motion generation. Check
MotionGenResult.success
attribute to see if the query was successful.
- Result of motion generation. Check
- Return type:
- plan_goalset(
- start_state: JointState,
- goal_pose: Pose,
- plan_config: MotionGenPlanConfig = MotionGenPlanConfig(enable_graph=False, enable_opt=True, use_nn_ik_seed=False, need_graph_success=False, max_attempts=60, timeout=10.0, enable_graph_attempt=3, disable_graph_attempt=None, ik_fail_return=None, partial_ik_opt=False, num_ik_seeds=None, num_graph_seeds=None, num_trajopt_seeds=None, success_ratio=1, fail_on_invalid_query=True, use_start_state_as_retract=True, pose_cost_metric=None, enable_finetune_trajopt=True, parallel_finetune=True, finetune_dt_scale=0.85, finetune_attempts=5, finetune_dt_decay=1.025, time_dilation_factor=None, check_start_validity=True, finetune_js_dt_scale=1.1),
- link_poses: List[Pose] | None = None,
Plan a single motion to reach a goal from set of poses, from a start joint state.
Use this when planning to reach a grasp from a set of possible grasps. This method will try to reach the closest goal pose from the set of goal poses at every iteration of optimization during IK and trajectory optimization. This method only supports goalset for main end-effector (i.e., goal_pose). Use single Pose target for links in link_poses.
- Parameters:
start_state – Start joint state of the robot. When planning from a non-static state, i.e, when velocity or acceleration is non-zero, set
MotionGen.optimize_dt
to False.goal_pose – Goal pose for the end-effector.
plan_config – Planning parameters for motion generation.
link_poses – Goal poses for each link in the robot when planning for multiple links.
- Returns:
- Result of motion generation. Check
MotionGenResult.success
attribute to see if the query was successful.
- Result of motion generation. Check
- Return type:
- plan_batch(
- start_state: JointState,
- goal_pose: Pose,
- plan_config: MotionGenPlanConfig = MotionGenPlanConfig(enable_graph=False, enable_opt=True, use_nn_ik_seed=False, need_graph_success=False, max_attempts=60, timeout=10.0, enable_graph_attempt=3, disable_graph_attempt=None, ik_fail_return=None, partial_ik_opt=False, num_ik_seeds=None, num_graph_seeds=None, num_trajopt_seeds=None, success_ratio=1, fail_on_invalid_query=True, use_start_state_as_retract=True, pose_cost_metric=None, enable_finetune_trajopt=True, parallel_finetune=True, finetune_dt_scale=0.85, finetune_attempts=5, finetune_dt_decay=1.025, time_dilation_factor=None, check_start_validity=True, finetune_js_dt_scale=1.1),
- link_poses: Dict[str, List[Pose]] | None = None,
Plan motions to reach a batch of goal poses from a batch of start joint states.
- Parameters:
start_state – Start joint states of the robot. When planning from a non-static state, i.e, when velocity or acceleration is non-zero, set
MotionGen.optimize_dt
to False.goal_pose – Goal poses for the end-effector.
plan_config – Planning parameters for motion generation.
link_poses – Goal poses for each link in the robot when planning for multiple links.
- Returns:
- Result of motion generation. Check
MotionGenResult.success
attribute to check which indices of the batch were successful.
- Result of motion generation. Check
- Return type:
- plan_batch_goalset(
- start_state: JointState,
- goal_pose: Pose,
- plan_config: MotionGenPlanConfig = MotionGenPlanConfig(enable_graph=False, enable_opt=True, use_nn_ik_seed=False, need_graph_success=False, max_attempts=60, timeout=10.0, enable_graph_attempt=3, disable_graph_attempt=None, ik_fail_return=None, partial_ik_opt=False, num_ik_seeds=None, num_graph_seeds=None, num_trajopt_seeds=None, success_ratio=1, fail_on_invalid_query=True, use_start_state_as_retract=True, pose_cost_metric=None, enable_finetune_trajopt=True, parallel_finetune=True, finetune_dt_scale=0.85, finetune_attempts=5, finetune_dt_decay=1.025, time_dilation_factor=None, check_start_validity=True, finetune_js_dt_scale=1.1),
- link_poses: Dict[str, List[Pose]] | None = None,
Plan motions to reach a batch of poses (goalset) from a batch of start joint states.
- Parameters:
start_state – Start joint states of the robot. When planning from a non-static state, i.e, when velocity or acceleration is non-zero, set
MotionGen.optimize_dt
to False.goal_pose – Goal poses for the end-effector.
plan_config – Planning parameters for motion generation.
link_poses – Goal poses for each link in the robot when planning for multiple links.
- Returns:
- Result of motion generation. Check
MotionGenResult.success
attribute to check which indices of the batch were successful.
- Result of motion generation. Check
- Return type:
- plan_batch_env(
- start_state: JointState,
- goal_pose: Pose,
- plan_config: MotionGenPlanConfig = MotionGenPlanConfig(enable_graph=False, enable_opt=True, use_nn_ik_seed=False, need_graph_success=False, max_attempts=60, timeout=10.0, enable_graph_attempt=3, disable_graph_attempt=None, ik_fail_return=None, partial_ik_opt=False, num_ik_seeds=None, num_graph_seeds=None, num_trajopt_seeds=None, success_ratio=1, fail_on_invalid_query=True, use_start_state_as_retract=True, pose_cost_metric=None, enable_finetune_trajopt=True, parallel_finetune=True, finetune_dt_scale=0.85, finetune_attempts=5, finetune_dt_decay=1.025, time_dilation_factor=None, check_start_validity=True, finetune_js_dt_scale=1.1),
- link_poses: Dict[str, List[Pose]] | None = None,
Plan motions to reach (batch) poses in different collision environments.
- Parameters:
start_state – Start joint states of the robot. When planning from a non-static state, i.e, when velocity or acceleration is non-zero, set
MotionGen.optimize_dt
to False.goal_pose – Goal poses for the end-effector.
plan_config – Planning parameters for motion generation.
link_poses – Goal poses for each link in the robot when planning for multiple links.
- Returns:
- Result of motion generation. Check
MotionGenResult.success
attribute to check which indices of the batch were successful.
- Result of motion generation. Check
- Return type:
- plan_batch_env_goalset(
- start_state: JointState,
- goal_pose: Pose,
- plan_config: MotionGenPlanConfig = MotionGenPlanConfig(enable_graph=False, enable_opt=True, use_nn_ik_seed=False, need_graph_success=False, max_attempts=60, timeout=10.0, enable_graph_attempt=3, disable_graph_attempt=None, ik_fail_return=None, partial_ik_opt=False, num_ik_seeds=None, num_graph_seeds=None, num_trajopt_seeds=None, success_ratio=1, fail_on_invalid_query=True, use_start_state_as_retract=True, pose_cost_metric=None, enable_finetune_trajopt=True, parallel_finetune=True, finetune_dt_scale=0.85, finetune_attempts=5, finetune_dt_decay=1.025, time_dilation_factor=None, check_start_validity=True, finetune_js_dt_scale=1.1),
- link_poses: Dict[str, List[Pose]] | None = None,
Plan motions to reach (batch) goalset poses in different collision environments.
- Parameters:
start_state – Start joint states of the robot. When planning from a non-static state, i.e, when velocity or acceleration is non-zero, set
MotionGen.optimize_dt
to False.goal_pose – Goal poses for the end-effector.
plan_config – Planning parameters for motion generation.
link_poses – Goal poses for each link in the robot when planning for multiple links.
- Returns:
- Result of motion generation. Check
MotionGenResult.success
attribute to check which indices of the batch were successful.
- Result of motion generation. Check
- Return type:
- compute_kinematics(
- state: JointState,
Compute kinematics for a given joint state.
- Parameters:
state – Joint state of the robot. Only
JointState.position
is used.- Returns:
Kinematic state of the robot.
- Return type:
- property kinematics: CudaRobotModel¶
Returns the shared kinematics model of the robot.
- property collision_cache: Dict[str, int]¶
Returns the collision cache created by the world collision checker.
- check_constraints(
- state: JointState,
Compute IK constraints for a given joint state.
- Parameters:
state – Joint state of the robot.
- Returns:
Metrics for the joint state.
- Return type:
- update_world(
- world: WorldConfig,
Update the world representation for collision checking.
This allows for updating the world representation as long as the new world representation does not have a larger number of obstacles than the
MotionGen.collision_cache
as created during initialization ofMotionGenConfig
. Updating the world also invalidates the cached roadmaps in the graph planner. See Collision World Representation for more details.- Parameters:
world – New world configuration for collision checking.
- clear_world_cache()¶
Remove all collision objects from collision cache.
- reset(reset_seed=True)¶
Reset the motion generation module.
- Parameters:
reset_seed – Reset the random seed generator. Resetting this can be time consuming, if deterministic results are not required, set this to False.
- reset_seed()¶
Reset the random seed generators in all sub-modules of motion generation.
- warmup(
- enable_graph: bool = True,
- batch: int | None = None,
- warmup_js_trajopt: bool = True,
- batch_env_mode: bool = False,
- parallel_finetune: bool = True,
- n_goalset: int = -1,
- warmup_joint_index: int = 0,
- warmup_joint_delta: float = 0.1,
Warmup planning methods for motion generation.
- Parameters:
enable_graph – Enable graph search for warmup.
batch – Number of problem queries for batch mode. If None, single query is run.
warmup_js_trajopt – Warmup joint space planning in addition to Cartesian planning.
batch_env_mode – Enable batch world environment mode for warmup. Only used when batch is not None.
parallel_finetune – Run finetuning trajectory optimization in parallel for warmup. Leave this to True for most cases.
n_goalset – Number of goal poses to use for warmup. If -1, single goal pose is used. Set this to the largest number of goals you plan to use with
MotionGen.plan_goalset
. After warmup, you can use smaller number of goals and the method will internally pad the extra goals with the first goal.warmup_joint_index – Index of the joint to perturb for warmup.
warmup_joint_delta – Delta to perturb the joint for warmup.
- plan_single_js(
- start_state: JointState,
- goal_state: JointState,
- plan_config: MotionGenPlanConfig = MotionGenPlanConfig(enable_graph=False, enable_opt=True, use_nn_ik_seed=False, need_graph_success=False, max_attempts=60, timeout=10.0, enable_graph_attempt=3, disable_graph_attempt=None, ik_fail_return=None, partial_ik_opt=False, num_ik_seeds=None, num_graph_seeds=None, num_trajopt_seeds=None, success_ratio=1, fail_on_invalid_query=True, use_start_state_as_retract=True, pose_cost_metric=None, enable_finetune_trajopt=True, parallel_finetune=True, finetune_dt_scale=0.85, finetune_attempts=5, finetune_dt_decay=1.025, time_dilation_factor=None, check_start_validity=True, finetune_js_dt_scale=1.1),
Plan a single motion to reach a goal joint state from a start joint state.
This method uses trajectory optimization to find a collision-free path between the start and goal joint states. If trajectory optimization fails, it uses a graph planner to find a collision-free path to the goal joint state. The graph plan is then used as a seed for trajectory optimization.
- Parameters:
start_state – Start joint state of the robot.
goal_state – Goal joint state of the robot.
plan_config – Planning parameters for motion generation.
- Returns:
- Result of motion generation. Check
MotionGenResult.success
attribute to see if the query was successful.
- Result of motion generation. Check
- Return type:
- solve_trajopt( )¶
Solve trajectory optimization for a given goal.
- Parameters:
goal – Goal for trajectory optimization.
act_seed – Seed for trajectory optimization.
return_all_solutions – Return results for all seeds in trajectory optimization.
num_seeds – Override number of seeds to use for trajectory optimization.
- Returns:
Result of trajectory optimization.
- 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,
- start_state: JointState | None = None,
- goal_pose: Pose | None = None,
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:
- get_all_rollout_instances() List[RolloutBase] ¶
Get all rollout instances used across components in motion generation.
- get_all_solver_rollout_instances() List[RolloutBase] ¶
Get all rollout instances in solvers (IK, TrajOpt).
- get_all_pose_solver_rollout_instances() List[RolloutBase] ¶
Get all rollout instances in solvers (IK, TrajOpt) that support Cartesian cost terms.
- get_all_pose_rollout_instances() List[RolloutBase] ¶
Get all rollout instances used across components in motion generation.
- get_all_kinematics_instances() List[CudaRobotModel] ¶
Get all kinematics instances used across components in motion generation.
This is deprecated. Use
MotionGen.kinematics
instead as MotionGen now uses a shared kinematics instance across all components.- Returns:
Single kinematics instance, returned as a list for compatibility.
- Return type:
List[CudaRobotModel]
- attach_objects_to_robot(
- joint_state: JointState,
- object_names: List[str],
- surface_sphere_radius: float = 0.001,
- link_name: str = 'attached_object',
- sphere_fit_type: SphereFitType = SphereFitType.VOXEL_VOLUME_SAMPLE_SURFACE,
- voxelize_method: str = 'ray',
- world_objects_pose_offset: Pose | None = None,
- remove_obstacles_from_world_config: bool = False,
Attach an object or objects from world to a robot’s link.
This method assumes that the objects exist in the world configuration. If attaching objects that are not in world, use
MotionGen.attach_external_objects_to_robot
.- Parameters:
joint_state – Joint state of the robot.
object_names – Names of objects in the world to attach to the robot.
surface_sphere_radius – Radius (in meters) to use for points sampled on surface of the object. A smaller radius will allow for generating motions very close to obstacles.
link_name – Name of the link (frame) to attach the objects to. The assumption is that this link does not have any geometry and all spheres of this link represent attached objects.
sphere_fit_type – Sphere fit algorithm to use. See Geometry Approximation to Spheres for more details. The default method
SphereFitType.VOXEL_VOLUME_SAMPLE_SURFACE
voxelizes the volume of the objects and adds spheres representing the voxels, then samples points on the surface of the object, addssurface_sphere_radius
to these points. This should be used for most cases.voxelize_method – Method to use for voxelization, passed to
trimesh.voxel.creation.voxelize
.world_objects_pose_offset – Offset to apply to the object poses before attaching to the robot. This is useful when attaching an object that’s in contact with the world. The offset is applied in the world frame before attaching to the robot.
remove_obstacles_from_world_config – Remove the obstacles from the world cache after attaching to the robot to reduce memory usage. Note that when an object is attached to the robot, it’s disabled in the world collision checker. This flag when enabled, also removes the object from world cache. For most cases, this should be set to False.
- attach_external_objects_to_robot(
- joint_state: JointState,
- external_objects: List[Obstacle],
- surface_sphere_radius: float = 0.001,
- link_name: str = 'attached_object',
- sphere_fit_type: SphereFitType = SphereFitType.VOXEL_VOLUME_SAMPLE_SURFACE,
- voxelize_method: str = 'ray',
- world_objects_pose_offset: Pose | None = None,
Attach external objects (not in world model) to a robot’s link.
- Parameters:
joint_state – Joint state of the robot.
external_objects – List of external objects to attach to the robot.
surface_sphere_radius – Radius (in meters) to use for points sampled on surface of the object. A smaller radius will allow for generating motions very close to obstacles.
link_name – Name of the link (frame) to attach the objects to. The assumption is that this link does not have any geometry and all spheres of this link represent attached objects.
sphere_fit_type – Sphere fit algorithm to use. See Geometry Approximation to Spheres for more details. The default method
SphereFitType.VOXEL_VOLUME_SAMPLE_SURFACE
voxelizes the volume of the objects and adds spheres representing the voxels, then samples points on the surface of the object, addssurface_sphere_radius
to these points. This should be used for most cases.voxelize_method – Method to use for voxelization, passed to
trimesh.voxel.creation.voxelize
.world_objects_pose_offset – Offset to apply to the object poses before attaching to the robot. This is useful when attaching an object that’s in contact with the world. The offset is applied in the world frame before attaching to the robot.
- add_camera_frame(
- camera_observation: CameraObservation,
- obstacle_name: str,
Add camera frame to the world collision checker.
Only supported by
WorldBloxCollision
.- Parameters:
camera_observation – Camera observation to add to the world collision checker.
obstacle_name – Name of the obstacle/layer to add the camera frame to.
- process_camera_frames( )¶
Process camera frames for collision checking.
Only supported by
WorldBloxCollision
.- Parameters:
obstacle_name – Name of the obstacle/layer to process the camera frames for. If None, processes camera frames on all supported obstacles.
process_aux – Process auxillary information such as mesh integration in nvblox. This is not required for collision checking and is only needed for debugging. Default is False to reduce computation time.
- attach_bounding_box_from_blox_to_robot(
- joint_state: JointState,
- bounding_box: Cuboid,
- blox_layer_name: str | None = None,
- surface_sphere_radius: float = 0.001,
- link_name: str = 'attached_object',
- sphere_fit_type: SphereFitType = SphereFitType.VOXEL_VOLUME_SAMPLE_SURFACE,
- voxelize_method: str = 'ray',
- world_objects_pose_offset: Pose | None = None,
Attach the voxels in a blox layer to robot’s link.
Note
This is not currently implemented.
- attach_new_object_to_robot(
- joint_state: JointState,
- obstacle: Obstacle,
- surface_sphere_radius: float = 0.001,
- link_name: str = 'attached_object',
- sphere_fit_type: SphereFitType = SphereFitType.VOXEL_VOLUME_SAMPLE_SURFACE,
- voxelize_method: str = 'ray',
- world_objects_pose_offset: Pose | None = None,
Attach an object to robot’s link. The object to be attached is not in the world model.
Deprecated. Use
MotionGen.attach_external_objects_to_robot
instead.
- detach_object_from_robot(
- link_name: str = 'attached_object',
Detach object from robot’s link.
- Parameters:
link_name – Name of the link.
- attach_spheres_to_robot(
- sphere_radius: float | None = None,
- sphere_tensor: Tensor | None = None,
- link_name: str = 'attached_object',
Attach spheres to robot’s link.
- 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 a robot’s link.
- Parameters:
link_name – Name of the link.
- 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:
- property world_model: WorldConfig¶
Get the world model used for collision checking.
- property world_collision: WorldCollision¶
Get the shared instance of world collision checker.
- property project_pose_to_goal_frame: bool¶
Check if the pose cost metric is projected to goal frame.
- update_interpolation_type(
- interpolation_type: InterpolateType,
- update_graph: bool = True,
- update_trajopt: bool = True,
Update interpolation type for motion generation.
- Parameters:
interpolation_type – Type of interpolation to use.
update_graph – Update graph planner with the new interpolation type.
update_trajopt – Update trajectory optimization solvers with the new interpolation type.
- update_locked_joints( )¶
Update locked joints in the robot configuration.
Use this function to update the joint values of currently locked joints between planning calls. This function can also be used to change which joints are locked, however this is only supported when the number of locked joints is the same as the original robot configuration as the kinematics tensors are pre-allocated.
- Parameters:
lock_joints – Dictionary of joint names and values to lock.
robot_config_dict – Robot configuration dictionary or path to robot configuration file.
- check_start_state(
- start_state: JointState,
Check if the start state is valid for motion generation.
- Parameters:
start_state – Start joint state of the robot.
- Returns:
- Tuple containing True if the start state is valid and
the status of the start state.
- Return type:
Tuple[bool, MotionGenStatus]
- _solve_ik_from_solve_state(
- goal_pose: Pose,
- solve_state: ReacherSolveState,
- start_state: JointState,
- use_nn_seed: bool,
- partial_ik_opt: bool,
- link_poses: Dict[str, Pose] | None = None,
Solve inverse kinematics from solve state, used by motion generation planning call.
- Parameters:
goal_pose – Goal Pose for the end-effector.
solve_state – Solve state for motion generation.
start_state – Start joint configuration of the robot.
use_nn_seed – Use seed from a neural network. Not implemented.
partial_ik_opt – Only run 50 iterations of inverse kinematics.
link_poses – Goal Poses of any other link in the robot that was specified in
curobo.types.robot.RobotConfig.kinematics.link_names
.
- Returns:
Result of inverse kinematics.
- Return type:
- _solve_trajopt_from_solve_state(
- goal: Goal,
- solve_state: ReacherSolveState,
- act_seed: JointState | None = None,
- use_nn_seed: bool = False,
- return_all_solutions: bool = False,
- seed_success: Tensor | None = None,
- newton_iters: int | None = None,
- trajopt_instance: TrajOptSolver | None = None,
- num_seeds_override: int | None = None,
Solve trajectory optimization from solve state, used by motion generation planning call.
- Parameters:
goal – Goal containing Pose/Joint targets, current robot state and any other information.
solve_state – Solve state for motion generation.
act_seed – Seed to run trajectory optimization.
use_nn_seed – Use neural network seed for solving trajectory optimization. This is not implemented.
return_all_solutions – Return all solutions found by trajectory optimization.
seed_success – Success tensor for seeds.
newton_iters – Override Newton iterations to run for solving trajectory optimization.
trajopt_instance – Instance of TrajOptSolver to use for solving trajectory optimization.
num_seeds_override – Override number of seeds to use for solving trajectory optimization.
- Returns:
Result of trajectory optimization.
- Return type:
- _get_solve_state(
- solve_type: ReacherSolveType,
- plan_config: MotionGenPlanConfig,
- goal_pose: Pose,
- start_state: JointState,
Generate solve state for motion generation based on planning type and configuration.
MotionGen creates a
ReacherSolveState
for every planning call to keep track of planning parameters such as number of seeds, batch size, solve type, etc. This solve state is then compared with existing solve state to determine if solvers (IK, TrajOpt) need to be re-initialized. Note that changing solve state is not supported whenMotionGen.use_cuda_graph
is enabled.- Parameters:
solve_type – Type of reacher problem to solve.
plan_config – Planning configuration for motion generation.
goal_pose – Goal Pose for the end-effector.
start_state – Start joint configuration of the robot.
- Raises:
ValueError – If the solve type is not supported.
- Returns:
Solve state for motion generation.
- Return type:
- _plan_attempts(
- solve_state: ReacherSolveState,
- start_state: JointState,
- goal_pose: Pose,
- plan_config: MotionGenPlanConfig = MotionGenPlanConfig(enable_graph=False, enable_opt=True, use_nn_ik_seed=False, need_graph_success=False, max_attempts=60, timeout=10.0, enable_graph_attempt=3, disable_graph_attempt=None, ik_fail_return=None, partial_ik_opt=False, num_ik_seeds=None, num_graph_seeds=None, num_trajopt_seeds=None, success_ratio=1, fail_on_invalid_query=True, use_start_state_as_retract=True, pose_cost_metric=None, enable_finetune_trajopt=True, parallel_finetune=True, finetune_dt_scale=0.85, finetune_attempts=5, finetune_dt_decay=1.025, time_dilation_factor=None, check_start_validity=True, finetune_js_dt_scale=1.1),
- link_poses: List[Pose] | None = None,
Call many planning attempts for a given reacher solve state.
- Parameters:
solve_state – Reacher solve state for planning.
start_state – Start joint state for planning.
goal_pose – Goal pose to reach for end-effector.
plan_config – Planning parameters for motion generation.
link_poses – Goal poses for other links in the robot.
- Returns:
Result of planning.
- Return type:
- _plan_batch_attempts(
- solve_state: ReacherSolveState,
- start_state: JointState,
- goal_pose: Pose,
- plan_config: MotionGenPlanConfig = MotionGenPlanConfig(enable_graph=False, enable_opt=True, use_nn_ik_seed=False, need_graph_success=False, max_attempts=60, timeout=10.0, enable_graph_attempt=3, disable_graph_attempt=None, ik_fail_return=None, partial_ik_opt=False, num_ik_seeds=None, num_graph_seeds=None, num_trajopt_seeds=None, success_ratio=1, fail_on_invalid_query=True, use_start_state_as_retract=True, pose_cost_metric=None, enable_finetune_trajopt=True, parallel_finetune=True, finetune_dt_scale=0.85, finetune_attempts=5, finetune_dt_decay=1.025, time_dilation_factor=None, check_start_validity=True, finetune_js_dt_scale=1.1),
- link_poses: Dict[str, Pose] | None = None,
Plan batch attempts for a given reacher solve state.
- Parameters:
solve_state – Reacher solve state for planning.
start_state – Start joint state for planning.
goal_pose – Goal pose to reach for end-effector.
plan_config – Planning parameters for motion generation.
link_poses – Goal poses for other links in the robot.
- Returns:
Result of batched planning.
- Return type:
- _plan_from_solve_state(
- solve_state: ReacherSolveState,
- start_state: JointState,
- goal_pose: Pose,
- plan_config: MotionGenPlanConfig = MotionGenPlanConfig(enable_graph=False, enable_opt=True, use_nn_ik_seed=False, need_graph_success=False, max_attempts=60, timeout=10.0, enable_graph_attempt=3, disable_graph_attempt=None, ik_fail_return=None, partial_ik_opt=False, num_ik_seeds=None, num_graph_seeds=None, num_trajopt_seeds=None, success_ratio=1, fail_on_invalid_query=True, use_start_state_as_retract=True, pose_cost_metric=None, enable_finetune_trajopt=True, parallel_finetune=True, finetune_dt_scale=0.85, finetune_attempts=5, finetune_dt_decay=1.025, time_dilation_factor=None, check_start_validity=True, finetune_js_dt_scale=1.1),
- link_poses: Dict[str, Pose] | None = None,
Plan from a given reacher solve state.
- Parameters:
solve_state – Reacher solve state for planning.
start_state – Start joint state for planning.
goal_pose – Goal pose to reach for end-effector.
plan_config – Planning parameters for motion generation.
link_poses – Goal poses for other links in the robot.
- Returns:
Result of planning.
- Return type:
- _plan_js_from_solve_state(
- solve_state: ReacherSolveState,
- start_state: JointState,
- goal_state: JointState,
- plan_config: MotionGenPlanConfig = MotionGenPlanConfig(enable_graph=False, enable_opt=True, use_nn_ik_seed=False, need_graph_success=False, max_attempts=60, timeout=10.0, enable_graph_attempt=3, disable_graph_attempt=None, ik_fail_return=None, partial_ik_opt=False, num_ik_seeds=None, num_graph_seeds=None, num_trajopt_seeds=None, success_ratio=1, fail_on_invalid_query=True, use_start_state_as_retract=True, pose_cost_metric=None, enable_finetune_trajopt=True, parallel_finetune=True, finetune_dt_scale=0.85, finetune_attempts=5, finetune_dt_decay=1.025, time_dilation_factor=None, check_start_validity=True, finetune_js_dt_scale=1.1),
Plan from a given reacher solve state for joint state.
- Parameters:
solve_state – Reacher solve state for planning.
start_state – Start joint state for planning.
goal_state – Goal joint state to reach.
plan_config – Planning parameters for motion generation.
- Returns:
Result of planning.
- Return type:
- _plan_from_solve_state_batch(
- solve_state: ReacherSolveState,
- start_state: JointState,
- goal_pose: Pose,
- plan_config: MotionGenPlanConfig = MotionGenPlanConfig(enable_graph=False, enable_opt=True, use_nn_ik_seed=False, need_graph_success=False, max_attempts=60, timeout=10.0, enable_graph_attempt=3, disable_graph_attempt=None, ik_fail_return=None, partial_ik_opt=False, num_ik_seeds=None, num_graph_seeds=None, num_trajopt_seeds=None, success_ratio=1, fail_on_invalid_query=True, use_start_state_as_retract=True, pose_cost_metric=None, enable_finetune_trajopt=True, parallel_finetune=True, finetune_dt_scale=0.85, finetune_attempts=5, finetune_dt_decay=1.025, time_dilation_factor=None, check_start_validity=True, finetune_js_dt_scale=1.1),
- link_poses: Dict[str, Pose] | None = None,
Plan from a given reacher solve state in batch mode.
- Parameters:
solve_state – Reacher solve state for planning.
start_state – Start joint state for planning.
goal_pose – Goal poses to reach for end-effector.
plan_config – Planning parameters for motion generation.
link_poses – Goal poses for other links in the robot.
- Returns:
Result of planning.
- Return type:
- plan(
- start_state: JointState,
- goal_pose: Pose,
- enable_graph: bool = True,
- enable_opt: bool = True,
- use_nn_ik_seed: bool = False,
- need_graph_success: bool = False,
- max_attempts: int = 100,
- timeout: float = 10.0,
- enable_graph_attempt: int | None = None,
- disable_graph_attempt: int | None = None,
- trajopt_attempts: int = 1,
- ik_fail_return: int | None = None,
- partial_ik_opt: bool = False,
- num_ik_seeds: int | None = None,
- num_graph_seeds: int | None = None,
- num_trajopt_seeds: int | None = None,
Deprecated method. Use
MotionGen.plan_single
or others instead.
- batch_plan(
- start_state: JointState,
- goal_pose: Pose,
- enable_graph: bool = True,
- enable_opt: bool = True,
- use_nn_ik_seed: bool = False,
- need_graph_success: bool = False,
- max_attempts: int = 1,
- timeout: float = 10.0,
- enable_graph_attempt: int | None = None,
- disable_graph_attempt: int | None = None,
- success_ratio: float = 1.0,
- ik_fail_return: int | None = None,
- fail_on_invalid_query: bool = False,
- partial_ik_opt: bool = False,
- num_ik_seeds: int | None = None,
- num_graph_seeds: int | None = None,
- num_trajopt_seeds: int | None = None,
Deprecated method. Use
MotionGen.plan_batch
or others instead.
- graph_trajopt_iters: int | None = None¶
number of iterations to run trajectory optimization when seeded from a graph plan.
- static load_from_robot_config(
- robot_cfg: str | Dict | RobotConfig,
- world_model: str | 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),
- num_ik_seeds: int = 32,
- num_graph_seeds: int = 4,
- num_trajopt_seeds: int = 4,
- num_batch_ik_seeds: int = 32,
- num_batch_trajopt_seeds: int = 1,
- num_trajopt_noisy_seeds: int = 1,
- 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_ik_file: str = 'particle_ik.yml',
- gradient_ik_file: str = 'gradient_ik.yml',
- graph_file: str = 'graph.yml',
- particle_trajopt_file: str = 'particle_trajopt.yml',
- gradient_trajopt_file: str = 'gradient_trajopt.yml',
- finetune_trajopt_file: str | None = None,
- trajopt_tsteps: int = 32,
- interpolation_steps: int = 5000,
- interpolation_dt: float = 0.02,
- interpolation_type: InterpolateType = InterpolateType.LINEAR_CUDA,
- use_cuda_graph: bool = True,
- self_collision_check: bool = True,
- self_collision_opt: bool = True,
- grad_trajopt_iters: int | None = None,
- trajopt_seed_ratio: Dict[str, int] = {'bias': 0.0, 'linear': 1.0},
- ik_opt_iters: int | None = None,
- ik_particle_opt: bool = True,
- collision_checker_type: CollisionCheckerType | None = CollisionCheckerType.MESH,
- sync_cuda_time: bool | None = None,
- trajopt_particle_opt: bool = True,
- traj_evaluator_config: TrajEvaluatorConfig | None = None,
- traj_evaluator: TrajEvaluator | None = None,
- minimize_jerk: bool = True,
- filter_robot_command: bool = True,
- use_gradient_descent: bool = False,
- collision_cache: Dict[str, int] | None = None,
- n_collision_envs: int | None = None,
- ee_link_name: str | None = None,
- use_es_ik: bool | None = None,
- use_es_trajopt: bool | None = None,
- es_ik_learning_rate: float = 1.0,
- es_trajopt_learning_rate: float = 1.0,
- use_ik_fixed_samples: bool | None = None,
- use_trajopt_fixed_samples: bool | None = None,
- evaluate_interpolated_trajectory: bool = True,
- partial_ik_iters: int = 2,
- fixed_iters_trajopt: bool | None = None,
- store_ik_debug: bool = False,
- store_trajopt_debug: bool = False,
- graph_trajopt_iters: int | None = None,
- collision_max_outside_distance: float | None = None,
- collision_activation_distance: float | None = None,
- trajopt_dt: float | None = None,
- js_trajopt_dt: float | None = None,
- js_trajopt_tsteps: int | None = None,
- trim_steps: List[int] | None = None,
- store_debug_in_result: bool = False,
- finetune_trajopt_iters: int | None = None,
- smooth_weight: List[float] | None = None,
- finetune_smooth_weight: List[float] | None = None,
- state_finite_difference_mode: str | None = None,
- finetune_dt_scale: float = 0.9,
- minimum_trajectory_dt: float | None = None,
- maximum_trajectory_time: float | None = None,
- maximum_trajectory_dt: float | None = None,
- velocity_scale: List[float] | float | None = None,
- acceleration_scale: List[float] | float | None = None,
- jerk_scale: List[float] | float | None = None,
- optimize_dt: bool = True,
- project_pose_to_goal_frame: bool = True,
- ik_seed: int = 1531,
- graph_seed: int = 1531,
- high_precision: bool = False,
Create a motion generation configuration from robot and world 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.
num_ik_seeds – Number of seeds to use for solving inverse kinematics. Default of 32 is found to be a good number for most cases. In sparse environments, a lower number of 16 can also be used.
num_graph_seeds – Number of seeds to use for graph planner per problem query. When graph planning is used to generate seeds for trajectory optimization, graph planner will attempt to find collision-free paths from the start state to the many inverse kinematics solutions.
num_trajopt_seeds – Number of seeds to use for trajectory optimization per problem query. Default of 4 is found to be a good number for most cases. Increasing this will increase memory usage.
num_batch_ik_seeds – Number of seeds to use for inverse kinematics during batched planning. Default of 32 is found to be a good number for most cases.
num_batch_trajopt_seeds – Number of seeds to use for trajectory optimization during batched planning. Using more than 1 will disable graph planning for batched planning.
num_trajopt_noisy_seeds – Number of augmented trajectories to use per trajectory seed. The augmentation is done by adding random noise to the trajectory. This augmentation has not been found to be useful and it’s recommended to keep this to 1. The noisy seeds can also be used in conjunction with the trajopt_seed_ratio to generate seeds that go through a bias point.
position_threshold – Position threshold in meters between reached position and target position used to measure success.
rotation_threshold – Rotation threshold between reached orientation and target orientation used to measure success. 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_ik_file – Optimizer configuration file to use for particle-based optimization during inverse kinematics.
gradient_ik_file – Optimizer configuration file to use for gradient-based optimization during inverse kinematics.
graph_file – Configuration file to use for graph planner.
particle_trajopt_file – Optimizer configuration file to use for particle-based optimization during trajectory optimization.
gradient_trajopt_file – Optimizer configuration file to use for gradient-based optimization during trajectory optimization.
finetune_trajopt_file – Optimizer configuration file to use for finetuning 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_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.
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
.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 problem 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 iterations to run trajectory optimization.
trajopt_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 trajopt_noisy_seeds to 2.
ik_opt_iters – Number of iterations to run inverse kinematics.
ik_particle_opt – Enable particle-based optimization during inverse kinematics. Default of True is recommended as particle-based optimization moves the random seeds to a regions of 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.
sync_cuda_time – Synchronize with host using
torch.cuda.synchronize
before measuring compute time.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.
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.
filter_robot_command – Filter generated trajectory to remove finite difference artifacts. Default of True is recommended.
use_gradient_descent – Use gradient descent instead of L-BFGS for trajectory optimization.
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 planning across different environments. Only used for
MotionGen.plan_batch_env
andMotionGen.plan_batch_env_goalset
.ee_link_name – End effector link/frame to use for reaching Cartesian poses. Default of None will use the end effector link from the robot configuration. This cannot be changed after creating the robot configuration.
use_es_ik – Use evolutionary strategy for as the particle-based optimizer for inverse kinematics. Default of None will use MPPI as the optimization algorithm. ES is not recommended as it’s unstable and provided optimization parameters were not tuned.
use_es_trajopt – Use evolutionary strategy as the particle-based optimizer for trajectory optimization. Default of None will use MPPI as the optimization algorithm. ES is not recommended as it’s unstable and provided optimization parameters were not tuned.
es_ik_learning_rate – Learning rate to use for evolutionary strategy in IK.
es_trajopt_learning_rate – Learning rate to use for evolutionary strategy in TrajOpt.
use_ik_fixed_samples – Use fixed samples of noise during particle-based optimization of IK. Default of None will use the setting from the optimizer configuration file (
particle_ik.yml
).use_trajopt_fixed_samples – Use fixed samples of noise during particle-based optimization of trajectory optimization. Default of None will use the setting from the optimizer configuration file (
particle_trajopt.yml
).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.
partial_ik_iters – Number of iterations of L-BFGS to run inverse kinematics when only partial IK is needed.
fixed_iters_trajopt – 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_ik_debug – Store debugging information such as values of optimization variables at every iteration in IK result. Setting this to True will set
use_cuda_graph
to False.store_trajopt_debug – Store debugging information such as values of optimization variables in TrajOpt result. Setting this to True will set
use_cuda_graph
to False.graph_trajopt_iters – Number of iterations to run trajectory optimization when seeded from a graph plan. Default of None will use the same number of iterations as linear seeded trajectory optimization. This can be set to a higher value of 200 in case where trajectories obtained are not of requird quality.
collision_max_outside_distance – Maximum distance to check for collision outside a obstacle. Increasing this value will slow down collision checks with Meshes as closest point queries will be run up to this distance outside an obstacle.
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.
js_trajopt_dt – Time step in seconds to use for trajectory optimization when reaching joint space targets. 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.
js_trajopt_tsteps – Number of waypoints to use for trajectory optimization when reaching joint space targets. Default of None will use the same number of waypoints as Cartesian trajectory optimization.
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.
finetune_trajopt_iters – Number of iterations to run trajectory optimization for finetuning after an initial collision-free trajectory is obtained.
smooth_weight – Override smooth weight for trajectory optimization. It’s not recommended to set this value for most cases.
finetune_smooth_weight – Override smooth weight for finetuning 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.
finetune_dt_scale – Scale initial estimated dt by this value to finetune trajectory optimization. This is deprecated and will be removed in future releases. Use
MotionGenPlanConfig.finetune_dt_scale
instead.minimum_trajectory_dt – Minimum time step in seconds allowed for trajectory optimization.
maximum_trajectory_time – Maximum time in seconds allowed for trajectory optimization.
maximum_trajectory_dt – Maximum time step in seconds allowed for trajectory optimization.
velocity_scale – Scale velocity limits by this value. Default of None will not scale the velocity limits. To generate slower trajectories, use
MotionGenPlanConfig.time_dilation_factor
< 1.0 instead. Changing this value is not recommended as it changes the scale of cost terms and they would require retuning.acceleration_scale – Scale acceleration limits by this value. Default of None will not scale the acceleration limits. To generate slower trajectories, use
MotionGenPlanConfig.time_dilation_factor
< 1.0 instead. Changing this value is not recommended as it changes the scale of cost terms and they would require retuning.jerk_scale – Scale jerk limits by this value. Default of None will not scale the jerk limits. To generate slower trajectories, use
MotionGenPlanConfig.time_dilation_factor
< 1.0 instead. Changing this value is not recommended as it changes the scale of cost terms and they would require retuning.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.
ik_seed – Random seed to use for inverse kinematics.
graph_seed – Random seed to use for graph planner.
high_precision – Use high precision settings for motion generation. This will increase the number of iterations for optimization solvers and reduce the thresholds for position to 1mm and rotation to 0.025. Default of False is recommended for most cases as standard motion generation settings reach within 0.5mm on most problems.
- Returns:
Instance of motion generation configuration.
- Return type:
- optimize_dt: bool = True¶
After 100 iterations of trajectory optimization, a new dt is computed that pushes the velocity, acceleration, or jerk limits to the maximum. This new dt is then used with a reduction
MotionGenPlanConfig.finetune_dt_scale
to run 200+ iterations of trajectory optimization. If trajectory optimization fails with the new dt, the new dt is increased and tried again untilMotionGenPlanConfig.finetune_attempts
.
- use_cuda_graph: bool = True¶
record compute ops as cuda graphs and replay recorded graphs for upto 10x faster execution.
- robot_cfg: RobotConfig¶
instance of robot configuration shared across all solvers.
- graph_planner: GraphPlanBase¶
instance of graph planner to use.
- trajopt_solver: TrajOptSolver¶
instance of trajectory optimization solver to use for reaching Cartesian poses.
- js_trajopt_solver: TrajOptSolver¶
instance of trajectory optimization solver to use for reaching joint space targets.
- finetune_js_trajopt_solver: TrajOptSolver¶
instance of trajectory optimization solver for final fine tuning for joint space targets.
- finetune_trajopt_solver: TrajOptSolver¶
instance of trajectory optimization solver for final fine tuning.
- interpolation_type: InterpolateType¶
interpolation to use for getting dense waypoints from optimized solution.
- world_coll_checker: WorldCollision¶
instance of world collision checker.
- tensor_args: TensorDeviceType¶
device to load motion generation.