Velocity and Path Kinematics Library

This library exposes an API to compute Velocity and Path Inverse Kinematics:

• Velocity Inverse Kinematics: Given a desired end-effector velocity, this library provides functions to compute the joint velocities that will achieve that Cartesian velocity.
• Path Inverse Kinematics: Given a desired end-effector path, this library provides functions to compute the joint-space path that maps to the given Cartesian path.

Jacobians

Inverse Kinematics rely heavily on the computation of Jacobian matrices. These relate joint-space velocities with Cartesian velocities. We have an efficient implementation of Jacobian computation both for kinematic chains and kinematic trees (e.g. dual arm systems).

Checkout the C++ API documentation in the jacobian.hpp header.

Velocity Inverse Kinematics

This is a C++ library that provides a simple interface to compute the joint velocities that will achieve a desired Cartesian velocity. The computation is done using a damped least squares Jacobian inverse, which is robust to singularities and fast to compute (about 5x faster than a truncated pseudo-inverse approach). We support the definition of additional joint-space tasks to be executed in the nullspace of the primary task. The functions are templated to allow their use with dynamic or static-sized Eigen types (i.e. suitable for real-time systems).

Checkout the C++ API documentation in the velocity_inverse_kinematics.hpp header.

Path Inverse Kinematics

A library that exposes functions to compute joint-space paths that correspond to Cartesian space paths:

• Supports multi-waypoint paths with changes of direction in translation and orientation.
• Implements a damped least squares Jacobian inverse for robustness to singularities.
• Implements Jacobian axis selection, which allows for relaxation of orientation constraints to solve only for translation constraints, if desired. Can be extended for partial orientation constraints.
• Adds a multi-waypoint path interpolation method via straight-line segments and quadratic Bezier curves at the corners. This adds support for arbitrarily long paths in 3D space.
• Computes the kinematics ‘incrementally’ over the densely interpolated path, following an MPC-like approach. This guarantees a smooth solution without jumps, and avoids calling a separate pose-based IK solver.
• The output path can be directly executed on hardware, after timing.
• The output path can be directly timed and converted into a trajectory, with user control on the maximum joint-space velocities and accelerations.
• Supports user-defined joint-space tasks to be executed in the nullspace of the primary task.

Checkout the C++ API documentation in the path_ik.hpp header.

Algorithm Overview

The pathIK solver converts a Cartesian reference path into a dense joint-space path using incremental inverse kinematics. The algorithm:

1. Densely interpolates the input Cartesian path at a fixed resolution, with corner blending controlled by the blending radius. These step sizes are not configurable through PathIKOptions, to internally guarantee a path dense enough for the algorithm requirements.
2. Solves IK incrementally at each interpolated waypoint using a damped least-squares Jacobian inverse, performing up to max_optimizer_iterations iterations per waypoint, to accommodate nullspace optimization.
3. Checks path deviation after each IK iteration. If the deviation exceeds the allowed tolerance, the solver immediately fails and returns the partial path solved so far. The solver does NOT automatically relax tolerances or retry — users must explicitly increase the deviation thresholds or relax the tip constraints (via setTipConstraint) if a more lenient solve is desired.
4. Enforces joint position limits at every step. If a joint limit causes the deviation to exceed the threshold, the error message will identify which joint hit its limit.

The output is a dense joint-space path without timing information. Before execution, the path must be timed via a trajectory timing algorithm (e.g., TOTG).

Dense Path Density

The density of the output joint-space path is determined by the interpolation step sizes, and may not be accurate at the blending points:

• Translational step: 0.001 m (1 mm)
• Angular step: 0.001 rad (~0.057 degrees)

For a straight-line translational path, the output will contain approximately 1000 waypoints per meter. After IK solving, the path can be downsampled using the downsamplePath function in path_utils.hpp.

Other related functions

Checkout these files for additional support functions:

• damped_least_squares_inverse.hpp: Implementation of a damped least squares Jacobian inverse.
• interpolate.hpp: Path interpolation functions.
• nullspace_tasks.hpp: Implementation of common nullspace tasks.
• path_utils.hpp: Utility functions for path computation (e.g. path downsampling, etc.).
• math.hpp: Various math functions.