curobo.wrap.reacher.ik_solver module
This module contains IKSolver
to solve inverse kinematics, along with
IKSolverConfig
to configure the solver, and IKResult
to store the result. A minimal
example to solve IK is available at Inverse Kinematics.
This demo is available in Reachability using Batched Collision-Free Inverse Kinematics.
- class IKSolverConfig(
- robot_config: RobotConfig,
- solver: WrapBase,
- num_seeds: int,
- position_threshold: float,
- rotation_threshold: float,
- rollout_fn: ArmReacher,
- ik_nn_seeder: str | None = None,
- world_coll_checker: WorldCollision | None = None,
- sample_rejection_ratio: int = 50,
- 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),
- use_cuda_graph: bool = True,
- seed: int = 1531,
Bases:
object
Configuration for Inverse Kinematics Solver.
A helper function to load from robot configuration is provided as
IKSolverConfig.load_from_robot_config
.- robot_config: RobotConfig
representation of robot, includes kinematic model, link geometry and joint limits.
- solver: WrapBase
Wrapped solver which has many instances of ArmReacher and two optimization solvers (MPPI, LBFGS) as default.
- rotation_threshold: float
Rotation convergence threshold to use to compute success. Currently this measures the geodesic distance between reached quaternion and target quaternion. A good accuracy threshold is 0.05. Check pose_distance_kernel.cu for the exact implementation.
- rollout_fn: ArmReacher
Reference to an instance of ArmReacher rollout class to use for auxillary functions.
- world_coll_checker: WorldCollision | None = None
Reference to world collision checker to use for collision avoidance.
- sample_rejection_ratio: int = 50
Rejection ratio for sampling collision-free configurations. These samples are not used as seeds for solving IK as starting from collision-free seeds did not improve success.
- 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)
Device and floating precision to use for IKSolver.
- use_cuda_graph: bool = True
Flag to enable capturing solver iterations as a cuda graph to reduce kernel launch overhead. Setting this to True can give upto 10x speedup while limiting the IKSolver to solving fixed batch size of problems.
- static load_from_robot_config(
- robot_cfg: str | Dict | RobotConfig,
- world_model: List[Dict] | List[WorldConfig] | Dict | WorldConfig | None = None,
- tensor_args: TensorDeviceType = TensorDeviceType(device=device(type='cuda', index=0), dtype=torch.float32, collision_geometry_dtype=torch.float32, collision_gradient_dtype=torch.float32, collision_distance_dtype=torch.float32),
- num_seeds: int = 100,
- position_threshold: float = 0.005,
- rotation_threshold: float = 0.05,
- world_coll_checker=None,
- base_cfg_file: str = 'base_cfg.yml',
- particle_file: str = 'particle_ik.yml',
- gradient_file: str = 'gradient_ik.yml',
- use_cuda_graph: bool = True,
- self_collision_check: bool = True,
- self_collision_opt: bool = True,
- grad_iters: int | None = None,
- use_particle_opt: bool = True,
- collision_checker_type: CollisionCheckerType | None = CollisionCheckerType.MESH,
- sync_cuda_time: bool | None = None,
- 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: bool | None = None,
- es_learning_rate: float | None = 0.1,
- use_fixed_samples: bool | None = None,
- store_debug: bool = False,
- regularization: bool = True,
- collision_activation_distance: float | None = None,
- high_precision: bool = False,
- project_pose_to_goal_frame: bool = True,
- seed: int = 1531,
Helper function to load IKSolver configuration from a robot file and world file.
Use this function to create an instance of IKSolverConfig and load the config into IKSolver.
- Parameters:
robot_cfg – representation of robot, includes kinematic model, link geometry, and joint limits. This can take as input a cuRobo robot configuration yaml file path or a loaded dictionary of the robot configuration file or an instance of RobotConfig. Configuration files for some common robots is available at Supported Robots. For other robots, follow Configuring a New Robot tutorial to create a configuration file.
world_model – representation of the world for obtaining collision-free IK solutions. The world can be represented as cuboids, meshes, from depth camera with nvblox, and as an Euclidean Signed Distance Grid (ESDF). This world model can be loaded from a dictionary (from disk through yaml) or from
curobo.geom.types.WorldConfig
. In an application, if collision computations are being used in more than one instance, it’s memory efficient to create one instance ofcurobo.geom.sdf.world.WorldCollision
and share across these class. For example, if an instance of IKSolver and MotionGen exists in an application, acurobo.geom.sdf.world.WorldCollision
object can be created in IKSolver and then when creatingcurobo.wrap.reacher.motion_gen.MotionGenConfig
, thisworld_coll_checker
can be passed as reference. Collision World Representation discusses types of obstacles in more detail.tensor_args – Device and floating precision to use for IKSolver.
num_seeds – Number of seeds to optimize per IK problem in parallel.
position_threshold – Position convergence threshold in meters to use to compute success.
rotation_threshold – Rotation convergence threshold to use to compute success. See
IKSolverConfig.rotation_threshold
for more details.world_coll_checker – Reference to world collision checker to use for collision avoidance.
base_cfg_file – Base configuration file for IKSolver. This configuration file is used to measure convergence and collision-free check after optimization is complete.
particle_file – Configuration file for particle optimization (uses MPPI as optimizer).
gradient_file – Configuration file for gradient optimization (uses LBFGS as optimizer).
use_cuda_graph – Flag to enable capturing solver iterations as a cuda graph to reduce kernel launch overhead. Setting this to True can give upto 10x speedup while limiting the IKSolver to solving fixed batch size of problems.
self_collision_check – Flag to enable self-collision checking.
self_collision_opt – Flag to enable self-collision cost during optimization.
grad_iters – Number of iterations for gradient optimization.
use_particle_opt – Flag to enable particle optimization.
collision_checker_type – Type of collision checker to use for collision checking.
sync_cuda_time – Flag to enable synchronization of cuda device with host using
torch.cuda.synchronize
before measuring compute time. If you set this to False, then measured time will not include completion of any launched CUDA kernels.use_gradient_descent – Flag to enable gradient descent optimization instead of LBFGS.
collision_cache – Number of objects to cache for collision checking.
n_collision_envs – Number of collision environments to use for IK. This is useful when solving IK for multiple robots in different environments in parallel.
ee_link_name – Name of end-effector link to use for IK.
use_es – Flag to enable Evolution Strategies optimization instead of MPPI.
es_learning_rate – Learning rate for Evolution Strategies optimization.
use_fixed_samples – Flag to enable fixed samples for MPPI optimization.
store_debug – Flag to enable storing debug information during optimization. Enabling this will store solution and cost at every iteration. This will significantly slow down the optimization as CUDA graph capture is disabled. Only use this for debugging.
regularization – Flag to enable regularization during optimization.
collision_activation_distance – Distance from obstacle at which to activate collision cost. A good value is 0.01 (1cm).
high_precision – Flag to solve IK at higher pose accuracy. This will increase the number of LBFGS iterations from 100 to 200. This flag is set to True when position_threshold is less than or equal to 1mm (0.001).
project_pose_to_goal_frame – Flag to project pose to goal frame when computing distance.
seed – Seed to use in pseudorandom generator used for creating IK seeds.
- class IKResult(
- js_solution: JointState,
- goal_pose: Pose,
- solution: Tensor,
- seed: Tensor,
- success: Tensor,
- position_error: Tensor,
- rotation_error: Tensor,
- error: Tensor,
- solve_time: float,
- debug_info: Any | None = None,
- goalset_index: Tensor | None = None,
Bases:
Sequence
Solution of Inverse Kinematics problem.
- js_solution: JointState
Joint state solution for the IK problem.
- seed: Tensor
Seed that was selected as the starting point for optimization. This is currently not available in the result and is set to None.
- success: Tensor
Success tensor for IK problem. If planning for batch, use this to filter js_solution. If planning for single problem, use this to check if the problem was solved successfully.
- position_error: Tensor
Position error between solved pose and goal pose in meters, computed with l-2 norm.
- rotation_error: Tensor
Rotation error between solved pose and goal pose, computed with geodesic distance. Roughly sqrt(q_des^T * q). A good accuracy threshold is 0.05.
- debug_info: Any | None = None
Debug information from solver. This can be used to debug solver convergence and tune weights between cost terms.
- goalset_index: Tensor | None = None
Index of goal in goalset that the IK solver reached. This is useful when solving with
IKSolver.solve_goalset
(alsoIKSolver.solve_batch_goalset
,IKSolver.solve_batch_env_goalset
) where the task is to find an IK solution that reaches 1 pose in set of poses. This index is used to identify which pose was reached by the solution.
- get_unique_solution(
- roundoff_decimals: int = 2,
Get unique solutions from many feasible solutions for the same problem.
Use this after solving IK with
IKSolver.solve_single
with return_seeds > 1. This function will return unique solutions from the set of feasible solutions, filering out solutions that are within roundoff_decimals of each other.- Parameters:
roundoff_decimals – Number of decimal places to round off the solution to measure uniqueness.
- Returns:
Unique solutions from the set of feasible solutions.
- get_batch_unique_solution(
- roundoff_decimals: int = 2,
Get unique solutions from many feasible solutions for the same problem in batch.
Use this after solving IK with
IKSolver.solve_batch
with return_seeds > 1. This function will return unique solutions from the set of feasible solutions, filering out solutions that are within roundoff_decimals of each other. Current implementation is not efficient as it will run a for loop over the batch as each problem in the batch can have different number of unique solutions.- Parameters:
roundoff_decimals – Number of decimal places to round off the solution to measure uniqueness.
- Returns:
List of unique solutions from the set of feasible solutions.
- _abc_impl = <_abc._abc_data object>
- _is_protocol = False
- count(
- value,
- index(
- value[,
- start[,
- stop,]]
Raises ValueError if the value is not present.
Supporting start and stop arguments is optional, but recommended.
- class IKSolver(
- config: IKSolverConfig,
Bases:
IKSolverConfig
Inverse Kinematics Solver for reaching Cartesian Pose with end-effector.
This also supports reaching poses for multiple links of a robot (e.g., a bimanual robot). This solver creates memory buffers on the GPU, captures CUDAGraphs of the operations during the very first call to any of the solve functions and reuses them for solving subsequent IK problems. As such, changing the number of problems, number of seeds, number of seeds or type of IKProblem to solve will cause existing CUDAGraphs to be invalidated, which currently leads to an exit with error. Either use multiple instances of IKSolver to solve different types of IKProblems or disable use_cuda_graph to avoid this issue. Disabling use_cuda_graph can lead to a 10x slowdown in solving IK problems.
Initializer for IK Solver.
- Parameters:
config – Configuration for Inverse Kinematics Solver.
- _update_goal_buffer(
- solve_state: ReacherSolveState,
- goal_pose: Pose,
- retract_config: Tensor | None = None,
- link_poses: Dict[str, Pose] | None = None,
Update goal buffer with new goal pose and retract configuration.
- Parameters:
solve_state – Current IK problem parameters.
goal_pose – New goal pose to solve for IK.
retract_config – Retract configuration to use for IK. If None, the retract configuration is set to the retract_config in
curobo.types.robot.RobotConfig
.link_poses – New poses for other links to use for IK. This is useful when solving for bimanual robots or when solving for multiple links of the same robot.
- Returns:
Updated goal buffer. This also updates the internal goal_buffer of IKSolver.
- solve_single(
- 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,
- link_poses: Dict[str, Pose] | None = None,
Solve single IK problem.
To get the closest IK solution from current joint configuration useful for IK servoing, set retract_config and seed_config to current joint configuration, also make sure IKSolverConfig was created with regularization enabled. If the solution is still not sufficiently close to the current joint configuration, increase the weight for null_space_cfg in convergence of base_cfg.yml which will select a solution that is closer to the current joint configuration after optimization. You can also increase the weight of null_space_weight in bound_cfg of gradient_ik.yml to encourage the optimization to stay near the current joint configuration during iterations.
- Parameters:
goal_pose – Pose to reach with end-effector.
retract_config – Retract configuration to use as regularization for IK. For this to work,
IKSolverConfig.load_from_robot_config
should have regularization enabled. This should be of shape (1, dof), where dof is the number of degrees of freedom in the robot. The order of joints should matchIKSolver.joint_names
.seed_config – Initial seed configuration to use for optimization. If None, a random seed is generated. The n seeds passed should be of shape (n, 1, dof), where dof is the number of degrees of freedom in the robot. The number of seeds do not have to match the number of seeds in the IKSolver. The remaining seeds are generated randomly. The order of joints should match
IKSolver.joint_names
.return_seeds – Number of solutions to return for the IK problem.
num_seeds – Number of seeds to optimize per IK problem in parallel. Changing number of seeds is not allowed when use_cuda_graph is enabled.
use_nn_seed – Flag to use neural network as seed for IK. This is not implemented yet.
newton_iters – Number of iterations to run LBFGS optimization. If None, the number of iterations is set to the default value in the configuration (gradient_ik.yml).
link_poses – Poses for other links to use for IK. This is useful when solving for bimanual robots or when solving for multiple links of the same robot. The link_poses should be a dictionary with link name as key and pose as value.
- Returns:
IKResult
object with solution to the IK problem. UseIKResult.success
to check if the problem was solved successfully.
- solve_goalset(
- 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,
- link_poses: Dict[str, Pose] | None = None,
Solve IK problem to reach one pose in a set of poses.
To get the closest IK solution from current joint configuration useful for IK servoing, set retract_config and seed_config to current joint configuration, also make sure IKSolverConfig was created with regularization enabled. If the solution is still not sufficiently close to the current joint configuration, increase the weight for null_space_cfg in convergence of base_cfg.yml which will select a solution that is closer to the current joint configuration after optimization. You can also increase the weight of null_space_weight in bound_cfg of gradient_ik.yml to encourage the optimization to stay near the current joint configuration during iterations.
- Parameters:
goal_pose – Pose to reach with end-effector.
retract_config – Retract configuration to use as regularization for IK. For this to work,
IKSolverConfig.load_from_robot_config
should have regularization enabled. This should be of shape (1, dof), where dof is the number of degrees of freedom in the robot. The order of joints should matchIKSolver.joint_names
.seed_config – Initial seed configuration to use for optimization. If None, a random seed is generated. The n seeds passed should be of shape (n, 1, dof), where dof is the number of degrees of freedom in the robot. The number of seeds do not have to match the number of seeds in the IKSolver. The remaining seeds are generated randomly. The order of joints should match
IKSolver.joint_names
.return_seeds – Number of solutions to return for the IK problem.
num_seeds – Number of seeds to optimize per IK problem in parallel. Changing number of seeds is not allowed when use_cuda_graph is enabled.
use_nn_seed – Flag to use neural network as seed for IK. This is not implemented yet.
newton_iters – Number of iterations to run LBFGS optimization. If None, the number of iterations is set to the default value in the configuration (gradient_ik.yml).
link_poses – Poses for other links to use for IK. This is useful when solving for bimanual robots or when solving for multiple links of the same robot. The link_poses should be a dictionary with link name as key and pose as value.
- Returns:
IKResult
object with solution to the IK problem. CheckIKResult.goalset_index
to identify which pose was reached by the solution. UseIKResult.success
to check if the problem was solved successfully.
- solve_batch(
- 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,
- link_poses: Dict[str, Pose] | None = None,
Solve batch of IK problems.
Changing number of problems (batch size) is not allowed when use_cuda_graph is enabled. The number of problems is determined during the first call to this function.
- Parameters:
goal_pose – Pose to reach with end-effector.
retract_config – Retract configuration to use as regularization for IK. For this to work,
IKSolverConfig.load_from_robot_config
should have regularization enabled. This should be of shape (batch, dof), where dof is the number of degrees of freedom in the robot. The order of joints should matchIKSolver.joint_names
.seed_config – Initial seed configuration to use for optimization. If None, a random seed is generated. The n seeds passed should be of shape (n, batch, dof), where dof is the number of degrees of freedom in the robot, and batch is number of problems in batch. The number of seeds do not have to match the number of seeds in the IKSolver. The remaining seeds are generated randomly. The order of joints should match
IKSolver.joint_names
.return_seeds – Number of solutions to return per problem in batch.
num_seeds – Number of seeds to optimize per IK problem in parallel. Changing number of seeds is not allowed when use_cuda_graph is enabled.
use_nn_seed – Flag to use neural network as seed for IK. This is not implemented yet.
newton_iters – Number of iterations to run LBFGS optimization. If None, the number of iterations is set to the default value in the configuration (gradient_ik.yml).
link_poses – Poses for other links to use for IK. This is useful when solving for bimanual robots or when solving for multiple links of the same robot. The link_poses should be a dictionary with link name as key and pose as value.
- Returns:
IKResult
object with solution to the batch of IK problems. UseIKResult.success
to filter successful solutions.
- solve_batch_goalset(
- 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,
- link_poses: Dict[str, Pose] | None = None,
Solve batch of IK problems to reach one pose in a set of poses for a batch of problems.
- Parameters:
goal_pose – Pose to reach with end-effector.
retract_config – Retract configuration to use as regularization for IK. For this to work,
IKSolverConfig.load_from_robot_config
should have regularization enabled. This should be of shape (batch, dof), where dof is the number of degrees of freedom in the robot. The order of joints should matchIKSolver.joint_names
.seed_config – Initial seed configuration to use for optimization. If None, a random seed is generated. The n seeds passed should be of shape (n, batch, dof), where dof is the number of degrees of freedom in the robot, and batch is number of problems in batch. The number of seeds do not have to match the number of seeds in the IKSolver. The remaining seeds are generated randomly. The order of joints should match
IKSolver.joint_names
.return_seeds – Number of solutions to return per problem in batch.
num_seeds – Number of seeds to optimize per IK problem in parallel. Changing number of seeds is not allowed when use_cuda_graph is enabled.
use_nn_seed – Flag to use neural network as seed for IK. This is not implemented yet.
newton_iters – Number of iterations to run LBFGS optimization. If None, the number of iterations is set to the default value in the configuration (gradient_ik.yml).
link_poses – Poses for other links to use for IK. This is useful when solving for bimanual robots or when solving for multiple links of the same robot. The link_poses should be a dictionary with link name as key and pose as value.
- Returns:
IKResult
object with solution to the batch of IK problems. UseIKResult.success
to filter successful solutions. UseIKResult.goalset_index
to identify which pose was reached by each solution.
- solve_batch_env(
- 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,
- link_poses: Dict[str, Pose] | None = None,
Solve batch of IK problems with each problem in different world environments.
- Parameters:
goal_pose – Pose to reach with end-effector.
retract_config – Retract configuration to use as regularization for IK. For this to work,
IKSolverConfig.load_from_robot_config
should have regularization enabled. This should be of shape (batch, dof), where dof is the number of degrees of freedom in the robot. The order of joints should matchIKSolver.joint_names
.seed_config – Initial seed configuration to use for optimization. If None, a random seed is generated. The n seeds passed should be of shape (n, batch, dof), where dof is the number of degrees of freedom in the robot, and batch is number of problems in batch. The number of seeds do not have to match the number of seeds in the IKSolver. The remaining seeds are generated randomly. The order of joints should match
IKSolver.joint_names
.return_seeds – Number of solutions to return per problem in batch.
num_seeds – Number of seeds to optimize per IK problem in parallel. Changing number of seeds is not allowed when use_cuda_graph is enabled.
use_nn_seed – Flag to use neural network as seed for IK. This is not implemented yet.
newton_iters – Number of iterations to run LBFGS optimization. If None, the number of iterations is set to the default value in the configuration (gradient_ik.yml).
link_poses – Poses for other links to use for IK. This is useful when solving for bimanual robots or when solving for multiple links of the same robot. The link_poses should be a dictionary with link name as key and pose as value.
- Returns:
IKResult
object with solution to the batch of IK problems. UseIKResult.success
to filter successful solutions.
- solve_batch_env_goalset(
- 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,
- link_poses: Dict[str, Pose] | None = None,
Solve batch of goalset IK problems with each problem in different world environments.
- Parameters:
goal_pose – Pose to reach with end-effector.
retract_config – Retract configuration to use as regularization for IK. For this to work,
IKSolverConfig.load_from_robot_config
should have regularization enabled. This should be of shape (batch, dof), where dof is the number of degrees of freedom in the robot. The order of joints should matchIKSolver.joint_names
.seed_config – Initial seed configuration to use for optimization. If None, a random seed is generated. The n seeds passed should be of shape (n, batch, dof), where dof is the number of degrees of freedom in the robot, and batch is number of problems in batch. The number of seeds do not have to match the number of seeds in the IKSolver. The remaining seeds are generated randomly. The order of joints should match
IKSolver.joint_names
.return_seeds – Number of solutions to return per problem in batch.
num_seeds – Number of seeds to optimize per IK problem in parallel. Changing number of seeds is not allowed when use_cuda_graph is enabled.
use_nn_seed – Flag to use neural network as seed for IK. This is not implemented yet.
newton_iters – Number of iterations to run LBFGS optimization. If None, the number of iterations is set to the default value in the configuration (gradient_ik.yml).
link_poses – Poses for other links to use for IK. This is useful when solving for bimanual robots or when solving for multiple links of the same robot. The link_poses should be a dictionary with link name as key and pose as value.
- Returns:
IKResult
object with solution to the batch of IK problems. UseIKResult.success
to filter successful solutions. UseIKResult.goalset_index
to identify which pose was reached by each solution.
- _solve_from_solve_state(
- solve_state: ReacherSolveState,
- goal_pose: Pose,
- num_seeds: int,
- retract_config: Tensor | None = None,
- seed_config: Tensor | None = None,
- return_seeds: int = 1,
- use_nn_seed: bool = True,
- newton_iters: int | None = None,
- link_poses: Dict[str, Pose] | None = None,
Solve IK problem from ReacherSolveState. Called by all solve functions.
- Parameters:
solve_state – ReacherSolveState with information about the type of IK problem to solve.
goal_pose – Pose to reach with end-effector.
num_seeds – Number of seeds to optimize per IK problem in parallel.
retract_config – Retract configuration to use as regularization for IK. For this to work,
IKSolverConfig.load_from_robot_config
should have regularization enabled. Shape of retract_config depends on type of IK problem being solved.seed_config – Initial seed configuration to use for optimization. If None, a random seed is generated. The n seeds passed should be of shape (n, batch, dof), where dof is the number of degrees of freedom in the robot, and batch is number of problems in batch. The number of seeds do not have to match the number of seeds in the IKSolver. The remaining seeds are generated randomly. The order of joints should match
IKSolver.joint_names
. When solving for single IK problem types, batch==1.return_seeds – Number of solutions to return per problem in batch.
use_nn_seed – Flag to use neural network as seed for IK. This is not implemented yet.
newton_iters – Number of iterations to run LBFGS optimization. If None, the number of iterations is set to the default value in the configuration (gradient_ik.yml).
link_poses – Poses for other links to use for IK. This is useful when solving for bimanual robots or when solving for multiple links of the same robot. The link_poses should be a dictionary with link name as key and pose as value.
- Returns:
IKResult
object with solution to the IK problem. UseIKResult.success
to check if the problem was solved successfully.
- _get_result(
- num_seeds: int,
- result: WrapResult,
- goal_pose: Pose,
- return_seeds: int,
Get IKResult from WrapResult after optimization.
- Parameters:
num_seeds – number of seeds used for optimization.
result – result from optimization.
goal_pose – goal poses used for IK problems.
return_seeds – number of seeds to return per problem.
- Returns:
IKResult object with solutions to the IK problems.
- get_seed( ) Tensor
Get seed joint configurations for optimization.
- Parameters:
num_seeds – number of seeds to generate.
goal_pose – goal poses for IK problems. This is used to generate seeds with a neural network. Not implemented yet.
use_nn_seed – flag to use neural network as seed for IK. This is not implemented yet.
seed_config – seed configuration to use for optimization. If None, random seeds are generated. seed config should be of shape (batch, n, dof), where n can be lower or equal to num_seeds. The order of joints should match
IKSolver.joint_names
.
- Returns:
seed joint configurations for optimization.
- solve_any(
- solve_type: ReacherSolveType,
- 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,
- link_poses: Dict[str, Pose] | None = None,
Solve IK problem with any solve type.
- solve(
- 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,
Deprecated API for solving single or batch problems.
- batch_env_solve(
- 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,
Deprecated API, use solve_batch_env() or solve_batch_env_goalset() instead.
- _get_success(
- metrics: RolloutMetrics,
- num_seeds: int,
Get success of IK optimization.
- Parameters:
metrics – RolloutMetrics with feasibility, pose error, and other costs.
num_seeds – Number of seeds used for IK optimization.
- Returns:
Success of IK optimization as a boolean tensor of shape (batch, num_seeds).
- generate_seed( ) Tensor
Generate seeds for IK optimization.
- Parameters:
num_seeds – Number of seeds to generate using pseudo-random generator.
batch – Number of problems in batch.
use_nn_seed – Flag to use neural network as seed for IK. This is not implemented yet.
pose – Pose to use for generating seeds. This is not implemented yet.
- Returns:
Seed configurations for IK optimization.
- update_world(
- world: WorldConfig,
Update world in IKSolver.
If the new world configuration has more obstacles than initial cache, the collision cache will be recreated, breaking existing cuda graphs. This will lead to an exit with error if use_cuda_graph is enabled.
- Parameters:
world – World configuration to update in IKSolver.
- check_constraints(
- q: JointState,
Check constraints for joint state.
- Parameters:
q – Joint state to check constraints for.
- Returns:
RolloutMetrics with feasibility of joint state.
- sample_configs(
- n: int,
- use_batch_env=False,
- sample_from_ik_seeder: bool = False,
- rejection_ratio: int | None = None,
Sample n feasible joint configurations. Only samples with environment=0.
- Parameters:
n – Number of joint configurations to sample.
use_batch_env – Flag to sample from batch environments. This is not implemented yet.
sample_from_ik_seeder – Flag to sample from IK seeder. This is not implemented yet.
rejection_ratio – Ratio of samples to generate to get n feasible samples. If None, the rejection ratio is set to the default value meth:IKSolver.sample_rejection_ratio.
- Returns:
Joint configurations sampled from the IK seeder of shape (n, dof).
- property kinematics: CudaRobotModel
Get kinematics instance in IKSolver.
- get_all_rollout_instances() List[ArmReacher]
Get all rollout instances in IKSolver.
- Returns:
List of all rollout instances in IKSolver.
- get_all_kinematics_instances() List[CudaRobotModel]
Deprecated API, use kinematics instead.
- fk(
- q: Tensor,
Forward kinematics for the robot.
- Parameters:
q – Joint configuration of the robot, with joint values in order of
joint_names
.- Returns:
CudaRobotModelState
with link poses, and link spheres for the robot.
- reset_cuda_graph() None
Reset the cuda graph for all rollout instances in IKSolver. Does not work currently.
- reset_shape()
Reset the shape of the rollout function and solver to the original shape.
- attach_object_to_robot( ) None
Attach object to robot for collision checking.
- Parameters:
sphere_radius – Radius of the sphere to attach to robot.
sphere_tensor – Tensor of shape (n, 4) to attach to robot, where n is the number of spheres for that link. If None, only radius is updated for the existing spheres.
link_name – Name of the link to attach object spheres to.
- detach_object_from_robot(
- link_name: str = 'attached_object',
Detach all attached objects from robot.
This currently will reset the spheres for link_name to -10, disabling collision checking for that link.
- Parameters:
link_name – Name of the link to detach object from.
- update_pose_cost_metric(
- metric: PoseCostMetric,
Update pose cost metric for all rollout instances in IKSolver.
- Parameters:
metric – New values for Pose cost metric to update.
- get_full_js(
- active_js: JointState,
Get full joint state from controlled joint state, appending locked joints.
- Parameters:
active_js – Controlled joint state
- Returns:
Full joint state.
- Return type:
- get_active_js(
- in_js: JointState,
Get controlled joint state from input joint state.
This is used to get the joint state for only joints that are optimization variables. This also re-orders the joints to match the order of optimization variables.
- Parameters:
in_js – Input joint state.
- Returns:
Active joint state.
- Return type:
- property joint_names: List[str]
Get ordered names of all joints used in optimization with IKSolver.
- static load_from_robot_config(
- robot_cfg: str | Dict | RobotConfig,
- world_model: List[Dict] | List[WorldConfig] | Dict | WorldConfig | None = None,
- tensor_args: TensorDeviceType = TensorDeviceType(device=device(type='cuda', index=0), dtype=torch.float32, collision_geometry_dtype=torch.float32, collision_gradient_dtype=torch.float32, collision_distance_dtype=torch.float32),
- num_seeds: int = 100,
- position_threshold: float = 0.005,
- rotation_threshold: float = 0.05,
- world_coll_checker=None,
- base_cfg_file: str = 'base_cfg.yml',
- particle_file: str = 'particle_ik.yml',
- gradient_file: str = 'gradient_ik.yml',
- use_cuda_graph: bool = True,
- self_collision_check: bool = True,
- self_collision_opt: bool = True,
- grad_iters: int | None = None,
- use_particle_opt: bool = True,
- collision_checker_type: CollisionCheckerType | None = CollisionCheckerType.MESH,
- sync_cuda_time: bool | None = None,
- 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: bool | None = None,
- es_learning_rate: float | None = 0.1,
- use_fixed_samples: bool | None = None,
- store_debug: bool = False,
- regularization: bool = True,
- collision_activation_distance: float | None = None,
- high_precision: bool = False,
- project_pose_to_goal_frame: bool = True,
- seed: int = 1531,
Helper function to load IKSolver configuration from a robot file and world file.
Use this function to create an instance of IKSolverConfig and load the config into IKSolver.
- Parameters:
robot_cfg – representation of robot, includes kinematic model, link geometry, and joint limits. This can take as input a cuRobo robot configuration yaml file path or a loaded dictionary of the robot configuration file or an instance of RobotConfig. Configuration files for some common robots is available at Supported Robots. For other robots, follow Configuring a New Robot tutorial to create a configuration file.
world_model – representation of the world for obtaining collision-free IK solutions. The world can be represented as cuboids, meshes, from depth camera with nvblox, and as an Euclidean Signed Distance Grid (ESDF). This world model can be loaded from a dictionary (from disk through yaml) or from
curobo.geom.types.WorldConfig
. In an application, if collision computations are being used in more than one instance, it’s memory efficient to create one instance ofcurobo.geom.sdf.world.WorldCollision
and share across these class. For example, if an instance of IKSolver and MotionGen exists in an application, acurobo.geom.sdf.world.WorldCollision
object can be created in IKSolver and then when creatingcurobo.wrap.reacher.motion_gen.MotionGenConfig
, thisworld_coll_checker
can be passed as reference. Collision World Representation discusses types of obstacles in more detail.tensor_args – Device and floating precision to use for IKSolver.
num_seeds – Number of seeds to optimize per IK problem in parallel.
position_threshold – Position convergence threshold in meters to use to compute success.
rotation_threshold – Rotation convergence threshold to use to compute success. See
IKSolverConfig.rotation_threshold
for more details.world_coll_checker – Reference to world collision checker to use for collision avoidance.
base_cfg_file – Base configuration file for IKSolver. This configuration file is used to measure convergence and collision-free check after optimization is complete.
particle_file – Configuration file for particle optimization (uses MPPI as optimizer).
gradient_file – Configuration file for gradient optimization (uses LBFGS as optimizer).
use_cuda_graph – Flag to enable capturing solver iterations as a cuda graph to reduce kernel launch overhead. Setting this to True can give upto 10x speedup while limiting the IKSolver to solving fixed batch size of problems.
self_collision_check – Flag to enable self-collision checking.
self_collision_opt – Flag to enable self-collision cost during optimization.
grad_iters – Number of iterations for gradient optimization.
use_particle_opt – Flag to enable particle optimization.
collision_checker_type – Type of collision checker to use for collision checking.
sync_cuda_time – Flag to enable synchronization of cuda device with host using
torch.cuda.synchronize
before measuring compute time. If you set this to False, then measured time will not include completion of any launched CUDA kernels.use_gradient_descent – Flag to enable gradient descent optimization instead of LBFGS.
collision_cache – Number of objects to cache for collision checking.
n_collision_envs – Number of collision environments to use for IK. This is useful when solving IK for multiple robots in different environments in parallel.
ee_link_name – Name of end-effector link to use for IK.
use_es – Flag to enable Evolution Strategies optimization instead of MPPI.
es_learning_rate – Learning rate for Evolution Strategies optimization.
use_fixed_samples – Flag to enable fixed samples for MPPI optimization.
store_debug – Flag to enable storing debug information during optimization. Enabling this will store solution and cost at every iteration. This will significantly slow down the optimization as CUDA graph capture is disabled. Only use this for debugging.
regularization – Flag to enable regularization during optimization.
collision_activation_distance – Distance from obstacle at which to activate collision cost. A good value is 0.01 (1cm).
high_precision – Flag to solve IK at higher pose accuracy. This will increase the number of LBFGS iterations from 100 to 200. This flag is set to True when position_threshold is less than or equal to 1mm (0.001).
project_pose_to_goal_frame – Flag to project pose to goal frame when computing distance.
seed – Seed to use in pseudorandom generator used for creating IK seeds.
- sample_rejection_ratio: int = 50
Rejection ratio for sampling collision-free configurations. These samples are not used as seeds for solving IK as starting from collision-free seeds did not improve success.
- 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)
Device and floating precision to use for IKSolver.
- use_cuda_graph: bool = True
Flag to enable capturing solver iterations as a cuda graph to reduce kernel launch overhead. Setting this to True can give upto 10x speedup while limiting the IKSolver to solving fixed batch size of problems.
- world_coll_checker: WorldCollision | None = None
Reference to world collision checker to use for collision avoidance.
- robot_config: RobotConfig
representation of robot, includes kinematic model, link geometry and joint limits.
- solver: WrapBase
Wrapped solver which has many instances of ArmReacher and two optimization solvers (MPPI, LBFGS) as default.
- rotation_threshold: float
Rotation convergence threshold to use to compute success. Currently this measures the geodesic distance between reached quaternion and target quaternion. A good accuracy threshold is 0.05. Check pose_distance_kernel.cu for the exact implementation.
- rollout_fn: ArmReacher
Reference to an instance of ArmReacher rollout class to use for auxillary functions.