Pose Inverse Kinematics Library

PoseIK is a relatively simple but efficient implementation of a Jacobian-based Newton-Raphson Inverse Kinematics Solver, using native MoveIt! types and algorithms (e.g. RobotState for Forward Kinematics). It works by:

1. Taking random joint configurations using a deterministic sequence. The solver will always return the same solution given the same inputs.
2. For each random joint configuration, measure the error to the target in Cartesian space, and do a gradient descent towards the target.
3. Gradient descent is done with a damped-least-squares Jacobian inverse, with low damping. This adds some amount of robustness against singularities during the gradient descent, without taking a big hit in convergence rates.
4. We avoid the computation of the full Jacobian inverse at each minimization step. Instead, we solve directly for the joint-space velocity that minimizes the Cartesian error.
5. We don't enforce position joint limits during the gradient descent step. Instead, we minimize without joint limits and try to 'unwind' the solution to within the limits as a final step.
6. We use an adaptive step size multiplier for the gradient descent for a faster convergence. The step multiplier becomes larger as the error to the target gets smaller.

The solver shows significant performance gains compared to KDL and TRAC_IK. In addition, it supports computing IK for multiple tips simultaneously. These are the average solve times, in milliseconds, for three common robots:

Solve times (ms)	KDL	TRAC_IK	PoseIK
panda	1.58	0.306	0.0421
fanuc	3.28	0.489	0.0416
Kinova Gen3	1.073	0.386	0.0321

These numbers were obtained using the ik_benchmarking package on a 12th Gen Intel(R) Core(TM) i7-12700H, 2.7 GHz, with the following benchmark config:

sample_size: 10000
random_seed: 12345
ik_timeout: 0.1
ik_iteration_display_step: 1000

All solvers were configured with a target tolerance of 0.001m.

Comparison with TRAC_IK

TRAC_IK was configured in Speed mode, with the following parameters:

kinematics_solver_timeout: 0.005
kinematics_solver_attempts: 3
epsilon: 0.001
solve_type: "Speed"

PoseIK was configured with the same target tolerance. The rest of parameters were left to the default values.

	Kinova Gen3	Panda	Fanuc
Solve time
Position Error
Success Rate

Success rate is 100% in all the cases. TRAC_IK exceeds the configured position/orientation tolerances in some cases, even if it's configured to stay under those thresholds. The PoseIK solver achieves much lower solve times (~10x faster), while keeping a much smaller target error.

Exposed libraries

This package exposes two libraries:

• pose_ik: This contains the actual IK solver. It depends on moveit_core and cartesian_planning (for math types and functions).
• pose_ik_plugin: This library implements MoveIt's KinematicsBase interface to integrate the solver as a MoveIt! kinematics plugin.

The pose_ik library exposes a single IK free function:

tl::expected<Eigen::VectorXd, std::string> solveIK(moveit::core::RobotState& robot_state,
                                                   const moveit::core::JointModelGroup& group,
                                                   const std::vector<PoseTarget>& targets, const Eigen::VectorXd& seed,
                                                   const rclcpp::Duration& timeout,
                                                   const IKValidationFunction& validation_fn, const Params& params);

This function will try to find a joint-space solution that reaches all targets. Each PoseTarget is a target pose that the solver will try to reach with a given robot link (e.g. multiple tips). seed will be used in the first gradient descent iteration, before starting taking random seeds. robot_state is used to have access to the robot Forward Kinematics. It is also modified internally while performing the search. If a solution is found, robot_state is returned as an Eigen::VectorXd, and robot_state will be updated to the solution.

MoveIt plugin

To use this solver with the MoveIt planning pipeline, update your config files (kinematics.yaml) to use this solver, e.g.:

manipulator:
  kinematics_solver: pose_ik_plugin/PoseIKPlugin
  target_tolerance: 0.001

See the Parameter Reference section below for a full list of accepted parameters and their description.

Solve modes

Two solve modes are implemented:

• first_found: In this mode the solver will return as soon as a solution is found. It won't try to optimize for any cost. Use this mode if you care about solve speed and don't have a preference on the solution.
• optimize_distance: In this mode the solver will always run for a given period of time, in order to find better solutions, even if one was found already. The solver will prefer solutions closer to the seed (initial joint configuration) in joint-space. Use this mode if your application can afford spending additional time in IK in exchange for better solutions.

The solve mode can be configured via the YAML config file used by the generate_parameters library. All the possible parameters and their description are listed in the next section.

Parameter reference

These are the parameters that can be configured in the PoseIK YAML configuration file:

pose_ik:
  solve_mode: {
    type: string,
    default_value: "first_found",
    description: "Use `first_found` to return the first solution found, or `optimize_distance` to run for a set period of time and minimize the squared distance to the seed in joint space.",
    validation: {
      one_of<>: [["first_found", "optimize_distance"]]
    }
  }
  target_tolerance: {
    type: double,
    default_value: 0.001,  # 1mm or 0.057 deg.
    description: "Position/orientation threshold for solving IK (norm of maximum allowed error).",
    validation: {
      gt<>: [0.0],
    }
  }
  gd_max_iters: {
    type: int,
    default_value: 100,
    description: "Maximum iterations for local gradient descent.",
    validation: {
      gt_eq<>: [1],
    }
  }
  gd_jacobian_damping: {
    type: double,
    default_value: 0.00001,
    description: "Damping coefficient for the Jacobian inverse.",
    validation: {
      bounds<>: [0.0, 1.0]
    }
  }
  gd_step_multiplier: {
    type: double,
    default_value: 0.5,
    description: "Multiplier to apply to the gradient descent step.",
    validation: {
      bounds<>: [0.001, 1.0]
    }
  }
  gd_adaptive_threshold: {
    type: double,
    default_value: 0.6,
    description: "When the distance to target is below this value, 'gd_step_multiplier' is gradually increased towards 1.0.",
    validation: {
      bounds<>: [0.001, 1.0]
    }
  }
  optimization_timeout: {
    type: double,
    default_value: 0.01, # seconds.
    description: "If solve_mode='optimize_distance', maximum time to spend searching for better solutions even if one has been found already.",
    validation: {
      gt<>: [0.0],
    }
  }
  optimization_distance_gain: {
    type: double,
    default_value: 0.1,
    description: "Gain to use for the distance-to-seed cost function, when solve_mode='optimize_distance'",
    validation: {
      gt_eq<>: [0.0],
    }
  }

Relevant headers

• pose_ik.hpp: The main header that exposes the solveIK function.
• pose_ik_plugin.hpp: The MoveIt plugin that integrates the solver with the MoveIt planning pipeline.