curobo.wrap.reacher.motion_gen module

class curobo.wrap.reacher.motion_gen.MotionGenConfig(ik_seeds, graph_seeds, trajopt_seeds, noisy_trajopt_seeds, batch_ik_seeds, batch_trajopt_seeds, robot_cfg, ik_solver, graph_planner, trajopt_solver, js_trajopt_solver, finetune_trajopt_solver, interpolation_type, interpolation_steps, world_coll_checker, tensor_args, partial_ik_iters, graph_trajopt_iters=None, store_debug_in_result=False, interpolation_dt=0.01, finetune_dt_scale=0.9, use_cuda_graph=True)

Bases: object

Configuration dataclass for creating a motion generation instance.

Parameters:
  • 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_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) –

  • store_debug_in_result (bool) –

  • interpolation_dt (float) –

  • finetune_dt_scale (float) –

  • use_cuda_graph (bool) –

ik_seeds: int

number of IK seeds to run per query problem.

graph_seeds: int

number of graph planning seeds to use per query problem.

trajopt_seeds: int

number of trajectory optimization seeds to use per query problem.

noisy_trajopt_seeds: int

number of seeds to run trajectory optimization per trajopt_seed.

batch_ik_seeds: int

number of IK seeds to use for batched queries.

batch_trajopt_seeds: int

number of trajectory optimization seeds to use for batched queries.

robot_cfg: RobotConfig

instance of robot configuration shared across all solvers.

ik_solver: IKSolver

instance of IK solver to use for motion generation.

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_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.

interpolation_steps: int

maximum number of steps to interpolate

world_coll_checker: WorldCollision

instance of world collision checker.

tensor_args: TensorDeviceType

device to load motion generation.

partial_ik_iters: int

number of IK iterations to run for initializing trajectory optimization

graph_trajopt_iters: int | None = None

number of iterations to run trajectory optimization when seeded from a graph plan.

store_debug_in_result: bool = False

store debugging information in MotionGenResult

interpolation_dt: float = 0.01

interpolation dt to use for output trajectory.

finetune_dt_scale: float = 0.9

scale initial dt by this value to finetune trajectory optimization.

use_cuda_graph: bool = True

record compute ops as cuda graphs and replay recorded graphs for upto 10x faster execution.

static load_from_robot_config(robot_cfg, world_model=None, tensor_args=TensorDeviceType(device=device(type='cuda', index=0), dtype=torch.float32), num_ik_seeds=30, num_graph_seeds=1, num_trajopt_seeds=12, num_batch_ik_seeds=30, num_batch_trajopt_seeds=1, num_trajopt_noisy_seeds=1, position_threshold=0.005, rotation_threshold=0.05, cspace_threshold=0.05, world_coll_checker=None, base_cfg_file='base_cfg.yml', particle_ik_file='particle_ik.yml', gradient_ik_file='gradient_ik.yml', graph_file='graph.yml', particle_trajopt_file='particle_trajopt.yml', gradient_trajopt_file='gradient_trajopt.yml', finetune_trajopt_file=None, trajopt_tsteps=32, interpolation_steps=5000, interpolation_dt=0.02, interpolation_type=InterpolateType.LINEAR_CUDA, use_cuda_graph=True, self_collision_check=True, self_collision_opt=True, grad_trajopt_iters=None, trajopt_seed_ratio={'bias': 0.0, 'linear': 1.0}, ik_opt_iters=None, ik_particle_opt=True, collision_checker_type=CollisionCheckerType.MESH, sync_cuda_time=None, trajopt_particle_opt=True, traj_evaluator_config=None, traj_evaluator=None, minimize_jerk=True, filter_robot_command=True, use_gradient_descent=False, collision_cache=None, n_collision_envs=None, ee_link_name=None, use_es_ik=None, use_es_trajopt=None, es_ik_learning_rate=1.0, es_trajopt_learning_rate=1.0, use_ik_fixed_samples=None, use_trajopt_fixed_samples=None, evaluate_interpolated_trajectory=True, partial_ik_iters=2, fixed_iters_trajopt=None, store_ik_debug=False, store_trajopt_debug=False, graph_trajopt_iters=None, collision_max_outside_distance=None, collision_activation_distance=None, trajopt_dt=None, js_trajopt_dt=None, js_trajopt_tsteps=None, trim_steps=None, store_debug_in_result=False, finetune_trajopt_iters=None, smooth_weight=None, finetune_smooth_weight=None, state_finite_difference_mode=None, finetune_dt_scale=0.98, maximum_trajectory_time=None, maximum_trajectory_dt=0.1, velocity_scale=None, acceleration_scale=None, jerk_scale=None, optimize_dt=True, project_pose_to_goal_frame=True)

Helper function to create configuration from robot and world configuration.

Parameters:
  • robot_cfg (str | Dict | RobotConfig) – Robot configuration to use for motion generation.

  • world_model (str | Dict | WorldConfig | None) – World model to use for motion generation. Use None to disable world model.

  • tensor_args (TensorDeviceType) – Tensor device to use for motion generation. Use to change cuda device id.

  • num_ik_seeds (int) – Number of seeds to use in inverse kinematics (IK) optimization.

  • num_graph_seeds (int) – Number of graph paths to use as seed in trajectory optimization.

  • num_trajopt_seeds (int) – Number of seeds to use in trajectory optimization.

  • num_batch_ik_seeds (int) – Number of seeds to use in batch planning modes for IK.

  • num_batch_trajopt_seeds (int) – Number of seeds to use in batch planning modes for trajopt.

  • num_trajopt_noisy_seeds (int) – Number of seeds to use for trajopt.

  • position_threshold (float) – _description_

  • rotation_threshold (float) – _description_

  • cspace_threshold (float) – _description_

  • world_coll_checker – _description_

  • base_cfg_file (str) – _description_

  • particle_ik_file (str) – _description_

  • gradient_ik_file (str) – _description_

  • graph_file (str) – _description_

  • particle_trajopt_file (str) – _description_

  • gradient_trajopt_file (str) – _description_

  • finetune_trajopt_file (str | None) – _description_

  • trajopt_tsteps (int) – _description_

  • interpolation_steps (int) – _description_

  • interpolation_dt (float) – _description_

  • interpolation_type (InterpolateType) – _description_

  • use_cuda_graph (bool) – _description_

  • self_collision_check (bool) – _description_

  • self_collision_opt (bool) – _description_

  • grad_trajopt_iters (int | None) – _description_

  • trajopt_seed_ratio (Dict[str, int]) – _description_

  • ik_opt_iters (int | None) – _description_

  • ik_particle_opt (bool) – _description_

  • collision_checker_type (CollisionCheckerType | None) – _description_

  • sync_cuda_time (bool | None) – _description_

  • trajopt_particle_opt (bool) – _description_

  • traj_evaluator_config (TrajEvaluatorConfig | None) – _description_

  • traj_evaluator (TrajEvaluator | None) – _description_

  • minimize_jerk (bool) – _description_

  • filter_robot_command (bool) – _description_

  • use_gradient_descent (bool) – _description_

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

  • n_collision_envs (int | None) – _description_

  • ee_link_name (str | None) – _description_

  • use_es_ik (bool | None) – _description_

  • use_es_trajopt (bool | None) – _description_

  • es_ik_learning_rate (float) – _description_

  • es_trajopt_learning_rate (float) – _description_

  • use_ik_fixed_samples (bool | None) – _description_

  • use_trajopt_fixed_samples (bool | None) – _description_

  • evaluate_interpolated_trajectory (bool) – _description_

  • partial_ik_iters (int) – _description_

  • fixed_iters_trajopt (bool | None) – _description_

  • store_ik_debug (bool) – _description_

  • store_trajopt_debug (bool) – _description_

  • graph_trajopt_iters (int | None) – _description_

  • collision_max_outside_distance (float | None) – _description_

  • collision_activation_distance (float | None) – _description_

  • trajopt_dt (float | None) – _description_

  • js_trajopt_dt (float | None) – _description_

  • js_trajopt_tsteps (int | None) – _description_

  • trim_steps (List[int] | None) – _description_

  • store_debug_in_result (bool) – _description_

  • finetune_trajopt_iters (int | None) – _description_

  • smooth_weight (List[float]) – _description_

  • finetune_smooth_weight (List[float] | None) – _description_

  • state_finite_difference_mode (str | None) – _description_

  • finetune_dt_scale (float) – _description_

  • maximum_trajectory_time (float | None) – _description_

  • maximum_trajectory_dt (float) – _description_

  • velocity_scale (List[float] | float | None) – _description_

  • acceleration_scale (List[float] | float | None) – _description_

  • jerk_scale (List[float] | float | None) – _description_

  • optimize_dt (bool) – _description_

  • project_pose_to_goal_frame (bool) –

Returns:

_description_

class curobo.wrap.reacher.motion_gen.MotionGenResult(success=None, valid_query=True, optimized_plan=None, optimized_dt=None, position_error=None, rotation_error=None, cspace_error=None, solve_time=0.0, ik_time=0.0, graph_time=0.0, trajopt_time=0.0, finetune_time=0.0, total_time=0.0, interpolated_plan=None, interpolation_dt=0.02, path_buffer_last_tstep=None, debug_info=None, status=None, attempts=1, trajopt_attempts=0, used_graph=False, graph_plan=None, goalset_index=None)

Bases: object

Result obtained from motion generation.

Parameters:
  • success (Tensor | None) –

  • valid_query (bool) –

  • optimized_plan (JointState | None) –

  • optimized_dt (Tensor | None) –

  • position_error (Tensor | None) –

  • rotation_error (Tensor | None) –

  • cspace_error (Tensor | None) –

  • solve_time (float) –

  • ik_time (float) –

  • graph_time (float) –

  • trajopt_time (float) –

  • finetune_time (float) –

  • total_time (float) –

  • interpolated_plan (JointState | None) –

  • interpolation_dt (float) –

  • path_buffer_last_tstep (List[int] | None) –

  • debug_info (Any) –

  • status (str | None) –

  • attempts (int) –

  • trajopt_attempts (int) –

  • used_graph (bool) –

  • graph_plan (JointState | None) –

  • goalset_index (Tensor | None) –

success: Tensor | None = None

success tensor with index referring to the batch index.

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

optimized_dt: Tensor | None = None

dt between steps in the optimized plan

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.

solve_time: float = 0.0

seconds taken in the optimizer for solving the motion generation problem.

ik_time: float = 0.0

seconds taken to solve IK.

graph_time: float = 0.0

seconds taken to find graph plan.

trajopt_time: float = 0.0

seconds taken in trajectory optimization.

finetune_time: float = 0.0

seconds to run finetune trajectory optimization.

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.

interpolation_dt: float = 0.02

dt between steps in interpolated plan

path_buffer_last_tstep: List[int] | None = None

last timestep in interpolated plan per batch index. Only used for batched queries

debug_info: Any = None

Debug information

status: str | None = None

status of motion generation query. returns [IK Fail, Graph Fail, TrajOpt Fail].

attempts: int = 1

number of attempts used before returning a solution.

trajopt_attempts: int = 0

number of trajectory optimization attempts used per attempt.

used_graph: bool = False

returns true when a graph plan was used to seed trajectory optimization.

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()
static _check_none_and_copy_idx(current_tensor, source_tensor, idx)
Parameters:
  • current_tensor (Tensor | JointState | None) –

  • source_tensor (Tensor | JointState | None) –

  • idx (int) –

copy_idx(idx, source_result)
Parameters:
get_paths()
Return type:

List[JointState]

get_successful_paths()
Return type:

List[Tensor]

get_interpolated_plan()
Return type:

JointState

property motion_time
class curobo.wrap.reacher.motion_gen.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: 'Optional[int]' = 3, disable_graph_attempt: 'Optional[int]' = None, ik_fail_return: 'Optional[int]' = None, partial_ik_opt: 'bool' = False, num_ik_seeds: 'Optional[int]' = None, num_graph_seeds: 'Optional[int]' = None, num_trajopt_seeds: 'Optional[int]' = None, success_ratio: 'float' = 1, fail_on_invalid_query: 'bool' = True, enable_finetune_trajopt: 'bool' = True, parallel_finetune: 'bool' = True, use_start_state_as_retract: 'bool' = True, pose_cost_metric: 'Optional[PoseCostMetric]' = None)

Bases: object

Parameters:
  • enable_graph (bool) –

  • enable_opt (bool) –

  • use_nn_ik_seed (bool) –

  • need_graph_success (bool) –

  • max_attempts (int) –

  • timeout (float) –

  • enable_graph_attempt (int | None) –

  • disable_graph_attempt (int | None) –

  • ik_fail_return (int | None) –

  • partial_ik_opt (bool) –

  • num_ik_seeds (int | None) –

  • num_graph_seeds (int | None) –

  • num_trajopt_seeds (int | None) –

  • success_ratio (float) –

  • fail_on_invalid_query (bool) –

  • enable_finetune_trajopt (bool) –

  • parallel_finetune (bool) –

  • use_start_state_as_retract (bool) –

  • pose_cost_metric (PoseCostMetric | None) –

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
enable_finetune_trajopt: bool = True

enables retiming trajectory optimization, useful for getting low jerk trajectories.

parallel_finetune: bool = True
use_start_state_as_retract: bool = True

use start config as regularization

pose_cost_metric: PoseCostMetric | None = None
clone()
Return type:

MotionGenPlanConfig

class curobo.wrap.reacher.motion_gen.MotionGen(config)

Bases: MotionGenConfig

Parameters:

config (MotionGenConfig) –

update_batch_size(seeds=10, batch=1)
solve_ik(goal_pose, retract_config=None, seed_config=None, return_seeds=1, num_seeds=None, use_nn_seed=True, newton_iters=None)
Parameters:
  • goal_pose (Pose) –

  • retract_config (Tensor | None) –

  • seed_config (Tensor | None) –

  • return_seeds (int) –

  • num_seeds (int | None) –

  • use_nn_seed (bool) –

  • newton_iters (int | None) –

Return type:

IKResult

_solve_ik_from_solve_state(goal_pose, solve_state, start_state, use_nn_seed, partial_ik_opt, link_poses=None)
Parameters:
  • goal_pose (Pose) –

  • solve_state (ReacherSolveState) –

  • start_state (JointState) –

  • use_nn_seed (bool) –

  • partial_ik_opt (bool) –

  • link_poses (Dict[str, Pose] | None) –

Return type:

IKResult

_solve_trajopt_from_solve_state(goal, solve_state, act_seed=None, use_nn_seed=False, return_all_solutions=False, seed_success=None, newton_iters=None, trajopt_instance=None, num_seeds_override=None)
Parameters:
  • goal (Goal) –

  • solve_state (ReacherSolveState) –

  • act_seed (Optional[JointState]) –

  • use_nn_seed (bool) –

  • return_all_solutions (bool) –

  • seed_success (Optional[torch.Tensor]) –

  • newton_iters (Optional[int]) –

  • trajopt_instance (Optional[TrajOptSolver]) –

  • num_seeds_override (Optional[int]) –

Return type:

TrajOptResult

Parameters:
  • start_config (Tensor) –

  • goal_config (Tensor) –

  • interpolation_steps (int | None) –

Return type:

GraphResult

_get_solve_state(solve_type, plan_config, goal_pose, start_state)
Parameters:
_plan_attempts(solve_state, start_state, goal_pose, plan_config=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, enable_finetune_trajopt=True, parallel_finetune=True, use_start_state_as_retract=True, pose_cost_metric=None), link_poses=None)
Parameters:
_plan_batch_attempts(solve_state, start_state, goal_pose, plan_config=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, enable_finetune_trajopt=True, parallel_finetune=True, use_start_state_as_retract=True, pose_cost_metric=None))
Parameters:
plan_single(start_state, goal_pose, plan_config=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, enable_finetune_trajopt=True, parallel_finetune=True, use_start_state_as_retract=True, pose_cost_metric=None), link_poses=None)
Parameters:
Return type:

MotionGenResult

plan_goalset(start_state, goal_pose, plan_config=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, enable_finetune_trajopt=True, parallel_finetune=True, use_start_state_as_retract=True, pose_cost_metric=None), link_poses=None)
Parameters:
Return type:

MotionGenResult

plan_batch(start_state, goal_pose, plan_config=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, enable_finetune_trajopt=True, parallel_finetune=True, use_start_state_as_retract=True, pose_cost_metric=None))
Parameters:
Return type:

MotionGenResult

plan_batch_goalset(start_state, goal_pose, plan_config=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, enable_finetune_trajopt=True, parallel_finetune=True, use_start_state_as_retract=True, pose_cost_metric=None))
Parameters:
Return type:

MotionGenResult

plan_batch_env_goalset(start_state, goal_pose, plan_config=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, enable_finetune_trajopt=True, parallel_finetune=True, use_start_state_as_retract=True, pose_cost_metric=None))
Parameters:
Return type:

MotionGenResult

plan_batch_env(start_state, goal_pose, plan_config=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, enable_finetune_trajopt=True, parallel_finetune=True, use_start_state_as_retract=True, pose_cost_metric=None))
Parameters:
Return type:

MotionGenResult

_plan_from_solve_state(solve_state, start_state, goal_pose, plan_config=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, enable_finetune_trajopt=True, parallel_finetune=True, use_start_state_as_retract=True, pose_cost_metric=None), link_poses=None)
Parameters:
Return type:

MotionGenResult

_plan_js_from_solve_state(solve_state, start_state, goal_state, plan_config=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, enable_finetune_trajopt=True, parallel_finetune=True, use_start_state_as_retract=True, pose_cost_metric=None))
Parameters:
Return type:

MotionGenResult

_plan_from_solve_state_batch(solve_state, start_state, goal_pose, plan_config=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, enable_finetune_trajopt=True, parallel_finetune=True, use_start_state_as_retract=True, pose_cost_metric=None))
Parameters:
Return type:

MotionGenResult

compute_kinematics(state)
Parameters:

state (JointState) –

Return type:

KinematicModelState

property kinematics
property dof
check_constraints(state)
Parameters:

state (JointState) –

Return type:

RolloutMetrics

update_world(world)
Parameters:

world (WorldConfig) –

clear_world_cache()
reset(reset_seed=True)
reset_seed()
get_retract_config()
warmup(enable_graph=True, batch=None, warmup_js_trajopt=True, batch_env_mode=False, parallel_finetune=True, n_goalset=-1, warmup_joint_index=0, warmup_joint_delta=0.1)
Parameters:
  • enable_graph (bool) –

  • batch (int | None) –

  • warmup_js_trajopt (bool) –

  • batch_env_mode (bool) –

  • parallel_finetune (bool) –

  • n_goalset (int) –

  • warmup_joint_index (int) –

  • warmup_joint_delta (float) –

plan_single_js(start_state, goal_state, plan_config=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, enable_finetune_trajopt=True, parallel_finetune=True, use_start_state_as_retract=True, pose_cost_metric=None))
Parameters:
Return type:

MotionGenResult

plan(start_state, goal_pose, enable_graph=True, enable_opt=True, use_nn_ik_seed=False, need_graph_success=False, max_attempts=100, timeout=10.0, enable_graph_attempt=None, disable_graph_attempt=None, trajopt_attempts=1, ik_fail_return=None, partial_ik_opt=False, num_ik_seeds=None, num_graph_seeds=None, num_trajopt_seeds=None)
Parameters:
  • start_state (JointState) –

  • goal_pose (Pose) –

  • enable_graph (bool) –

  • enable_opt (bool) –

  • use_nn_ik_seed (bool) –

  • need_graph_success (bool) –

  • max_attempts (int) –

  • timeout (float) –

  • enable_graph_attempt (int | None) –

  • disable_graph_attempt (int | None) –

  • trajopt_attempts (int) –

  • ik_fail_return (int | None) –

  • partial_ik_opt (bool) –

  • num_ik_seeds (int | None) –

  • num_graph_seeds (int | None) –

  • num_trajopt_seeds (int | None) –

batch_plan(start_state, goal_pose, enable_graph=True, enable_opt=True, use_nn_ik_seed=False, need_graph_success=False, max_attempts=1, timeout=10.0, enable_graph_attempt=None, disable_graph_attempt=None, success_ratio=1.0, ik_fail_return=None, fail_on_invalid_query=False, partial_ik_opt=False, num_ik_seeds=None, num_graph_seeds=None, num_trajopt_seeds=None)
Parameters:
  • start_state (JointState) –

  • goal_pose (Pose) –

  • enable_graph (bool) –

  • enable_opt (bool) –

  • use_nn_ik_seed (bool) –

  • need_graph_success (bool) –

  • max_attempts (int) –

  • timeout (float) –

  • enable_graph_attempt (int | None) –

  • disable_graph_attempt (int | None) –

  • success_ratio (float) –

  • ik_fail_return (int | None) –

  • fail_on_invalid_query (bool) –

  • partial_ik_opt (bool) –

  • num_ik_seeds (int | None) –

  • num_graph_seeds (int | None) –

  • num_trajopt_seeds (int | None) –

solve_trajopt(goal, act_seed, return_all_solutions=False, num_seeds=None)
Parameters:
  • goal (Goal) –

  • return_all_solutions (bool) –

  • num_seeds (int | None) –

get_active_js(in_js)
Parameters:

in_js (JointState) –

update_pose_cost_metric(metric, start_state=None, goal_pose=None)
Parameters:
Return type:

bool

get_all_rollout_instances()
Return type:

List[RolloutBase]

get_all_solver_rollout_instances()
Return type:

List[RolloutBase]

get_all_pose_solver_rollout_instances()
Return type:

List[RolloutBase]

get_all_kinematics_instances()
Return type:

List[CudaRobotModel]

attach_objects_to_robot(joint_state, object_names, surface_sphere_radius=0.001, link_name='attached_object', sphere_fit_type=SphereFitType.VOXEL_VOLUME_SAMPLE_SURFACE, voxelize_method='ray', world_objects_pose_offset=None, remove_obstacles_from_world_config=False)

Attach an object from world to robot’s link.

Parameters:
  • surface_sphere_radius (float) – _description_. Defaults to None.

  • sphere_tensor – _description_. Defaults to None.

  • link_name (str) – _description_. Defaults to “attached_object”.

  • joint_state (JointState) –

  • object_names (List[str]) –

  • sphere_fit_type (SphereFitType) –

  • voxelize_method (str) –

  • world_objects_pose_offset (Pose | None) –

  • remove_obstacles_from_world_config (bool) –

Return type:

None

attach_external_objects_to_robot(joint_state, external_objects, surface_sphere_radius=0.001, link_name='attached_object', sphere_fit_type=SphereFitType.VOXEL_VOLUME_SAMPLE_SURFACE, voxelize_method='ray', world_objects_pose_offset=None)

Attach an object from world to robot’s link.

Parameters:
  • surface_sphere_radius (float) – _description_. Defaults to None.

  • sphere_tensor – _description_. Defaults to None.

  • link_name (str) – _description_. Defaults to “attached_object”.

  • joint_state (JointState) –

  • external_objects (List[Obstacle]) –

  • sphere_fit_type (SphereFitType) –

  • voxelize_method (str) –

  • world_objects_pose_offset (Pose | None) –

Return type:

None

add_camera_frame(camera_observation, obstacle_name)
Parameters:
process_camera_frames(obstacle_name=None, process_aux=False)
Parameters:
  • obstacle_name (str | None) –

  • process_aux (bool) –

attach_bounding_box_from_blox_to_robot(joint_state, bounding_box, blox_layer_name=None, surface_sphere_radius=0.001, link_name='attached_object', sphere_fit_type=SphereFitType.VOXEL_VOLUME_SAMPLE_SURFACE, voxelize_method='ray', world_objects_pose_offset=None)

Attach the voxels in a blox layer to robot’s link.

NOTE: This is not currently implemented.

Parameters:
  • joint_state (JointState) – _description_

  • bounding_box (Cuboid) – _description_

  • blox_layer_name (Optional[str], optional) – _description_. Defaults to None.

  • surface_sphere_radius (float, optional) – _description_. Defaults to 0.001.

  • link_name (str, optional) – _description_. Defaults to “attached_object”.

  • sphere_fit_type (SphereFitType, optional) – _description_. Defaults to SphereFitType.VOXEL_VOLUME_SAMPLE_SURFACE.

  • voxelize_method (str, optional) – _description_. Defaults to “ray”.

  • world_objects_pose_offset (Optional[Pose], optional) – _description_. Defaults to None.

attach_new_object_to_robot(joint_state, obstacle, surface_sphere_radius=0.001, link_name='attached_object', sphere_fit_type=SphereFitType.VOXEL_VOLUME_SAMPLE_SURFACE, voxelize_method='ray', world_objects_pose_offset=None)

Attach an object to robot’s link. The object to be attached is not in the world model.

Parameters:
  • joint_state (JointState) – _description_

  • obstacle (Obstacle) – _description_

  • surface_sphere_radius (float, optional) – _description_. Defaults to 0.001.

  • link_name (str, optional) – _description_. Defaults to “attached_object”.

  • sphere_fit_type (SphereFitType, optional) – _description_. Defaults to SphereFitType.VOXEL_VOLUME_SAMPLE_SURFACE.

  • voxelize_method (str, optional) – _description_. Defaults to “ray”.

  • world_objects_pose_offset (Optional[Pose], optional) – _description_. Defaults to None.

detach_object_from_robot(link_name='attached_object')
Parameters:

link_name (str) –

Return type:

None

attach_spheres_to_robot(sphere_radius=None, sphere_tensor=None, link_name='attached_object')

Attach spheres to robot’s link.

Parameters:
  • sphere_radius (float | None) – _description_. Defaults to None.

  • sphere_tensor (Tensor | None) – _description_. Defaults to None.

  • link_name (str) – _description_. Defaults to “attached_object”.

Return type:

None

detach_spheres_from_robot(link_name='attached_object')
Parameters:

link_name (str) –

Return type:

None

get_full_js(active_js)
Parameters:

active_js (JointState) –

Return type:

JointState

add_running_pose_constraint(lock_x=False, lock_y=False, lock_z=False, lock_rx=False, lock_ry=False, lock_rz=False)
Parameters:
  • lock_x (bool) –

  • lock_y (bool) –

  • lock_z (bool) –

  • lock_rx (bool) –

  • lock_ry (bool) –

  • lock_rz (bool) –

remove_running_pose_constraint()
run_finetune_trajopt(start_state, goal_pose, traj_solution, traj_dt, max_attempts=1)
Parameters:
  • start_state (JointState) –

  • goal_pose (Pose) –

  • traj_solution (JointState) –

  • traj_dt (float | Tensor | None) –

  • max_attempts (int) –

property world_model: WorldConfig
property world_collision: WorldCollision
property project_pose_to_goal_frame: bool
update_interpolation_type(interpolation_type, update_graph=True, update_trajopt=True)
Parameters:
  • interpolation_type (InterpolateType) –

  • update_graph (bool) –

  • update_trajopt (bool) –

update_locked_joints(lock_joints, robot_config_dict)
Parameters:
  • lock_joints (Dict[str, float]) –

  • robot_config_dict (Union[str, Dict[Any]]) –