Introduction¶

Inverse kinematics (IK) is the problem of computing motions (in Pinker: velocities) that achieve a given set of tasks, such as putting a foot on a surface, moving the center of mass to a target location, etc.

This documentation assumes you are already familiar with task-based inverse kinematics. You can check out for instance this post on inverse kinematics for a general introduction.

Notations¶

In Pinker, we adopt the subscript right-to-left convention for transforms, and superscript notation to indicate the frame of a motion or force vector:

Quantity	Notation
Affine transform from frame \(A\) to frame \(B\)	\(T_{BA}\)
Body angular velocity of frame \(A\) in frame \(B\)	\({}^A \omega_{BA}\)
Position of frame \(B\) in frame \(A\)	\({}^A p_B\)
Rotation matrix from frame \(A\) to frame \(B\)	\(R_{BA}\)
Spatial angular velocity of frame \(A\) in frame \(B\)	\({}^B \omega_{BA}\)
World frame (inertial)	\(W\)

With these notations frame transforms can be read left to right, for example:

\[\begin{align} T_{CA} & = T_{CB} T_{BA} & {}^{B} \omega & = R_{BA} {}^{A} \omega & {}^B p_C & = R_{BA} {}^A p_C + {}^B p_A \end{align}\]

See also this spatial algebra cheat sheet.

Loading a robot¶

Load robot models from robot descriptions or from URDF files.

pinker.loaders.ROOT_JOINTS = {'free_flyer': <function JointModelFreeFlyer>, 'planar': <function JointModelPlanar>, 'spherical': <function JointModelSpherical>}¶: Root joints that can be named by string, for the root_joint argument.

pinker.loaders.load_robot_description(description_name, root_joint=None, commit=None)¶

Load a robot description with Pinker’s kinematics.

Parameters:

description_name (str) – Name of the robot description, for instance "ur3_official_description".
root_joint (Union[str, JointModel, None]) – Joint between the world and the root link of the robot, either by name ("free_flyer", "planar" or "spherical") or as a joint model. Defaults to a fixed root.
commit (Optional[str]) – If specified, check out that commit from the cloned robot description repository.

Return type:

RobotWrapper

Returns:

Robot model, bundled with its data and initial configuration.

Raises:

ModuleNotFoundError – if the robot_descriptions package is not installed, or if it has no such description.
ValueError – if the root joint is not a known name.

pinker.loaders.load_robot_urdf(urdf_path, root_joint=None, package_dirs=None)¶

Load a robot model from a URDF file.

The robot bundles a model with its data and initial configuration, which is what a Configuration is built from:

robot = pinker.load_robot_urdf("robot.urdf")
configuration = pinker.Configuration(
    robot.model, robot.data, robot.q0
)

Parameters:

urdf_path (Union[str, PathLike]) – Path to the URDF file.
root_joint (Union[str, JointModel, None]) – Joint between the world and the root link of the robot, either by name ("free_flyer", "planar" or "spherical") or as a joint model. Defaults to a fixed root.
package_dirs (Optional[List[str]]) – Directories where mesh files are looked up. Mesh files are only read when the robot is displayed, so this argument can be left out when it is not.

Return type:

RobotWrapper

Returns:

Robot model, bundled with its data and initial configuration.

Raises:

FileNotFoundError – if there is no URDF file at that path.
ValueError – if the root joint is not a known name.

Configuration¶

Configuration of a robot model.

Pinker uses pinker.kinematics for forward kinematics. A Configuration holds a robot model and data for this model where forward kinematics have been run. This means that the geometric state of the model has been computed, and quantities such as frame transforms and frame Jacobians are available.

class pinker.configuration.Configuration(model, data, q, copy_data=True, forward_kinematics=True, default_limits=None)¶

Type indicating that configuration-dependent quantities are available.

In Pinker, this type enables access to frame transforms and frame Jacobians. We rely on typing to make sure the proper forward kinematics functions have been called beforehand:

kin.compute_joint_jacobians(model, data, configuration)
kin.update_frame_placements(model, data)

The former computes the full model Jacobian into data.J. (It also computes forward kinematics, so there is no need to further call kin.forward_kinematics(model, data, configuration).) The latter updates frame placements.

data¶: Data corresponding to Configuration.model.

default_limits¶: Limits that solve_ik() enforces unless it is given its own.

model¶: Kinodynamic model.

q¶: Configuration vector for the robot model.

tangent¶: Tangent space of the model, with its characteristic matrices.

check_limits(tol=1e-06, safety_break=True)¶

Check that the current configuration is within limits.

Parameters:

tol (float) – Tolerance in radians.
safety_break (bool) – If True, stop execution and raise an exception if the current configuration is outside limits. If False, print a warning and continue execution.

Raises:

NotWithinConfigurationLimits – If the current configuration is outside limits.

Return type:

None

copy()¶

Copy this configuration.

The copy gets its own data, so that running forward kinematics on it leaves the original untouched. Attributes that don’t depend on the configuration vector (model, default limits and tangent space) are shared.

Return type:: Configuration
Returns:: New configuration initialized the same configuration vector.

get_frame_jacobian(frame)¶

Compute the Jacobian matrix of a frame velocity.

Denoting our frame by \(B\) and the world frame by \(W\), the Jacobian matrix \({}_B J_{WB}\) is related to the body velocity \({}_B v_{WB}\) by:

\[{}_B v_{WB} = {}_B J_{WB} \dot{q}\]

Parameters:: frame (str) – Name of the frame, typically a link name from the URDF.
Return type:: ndarray
Returns:: Jacobian \({}_B J_{WB}\) of the frame.

When the robot model includes a floating base (kin.JointModelFreeFlyer), the configuration vector \(q\) consists of:

q[0:3]: position in [m] of the floating base in the inertial frame, formatted as \([p_x, p_y, p_z]\).
q[3:7]: unit quaternion for the orientation of the floating base in the inertial frame, formatted as \([q_x, q_y, q_z, q_w]\).
q[7:]: joint angles in [rad].

get_transform(source, dest)¶

Get the pose of a frame with respect to another frame.

Parameters:

source (str) – Name of the frame to get the pose of.
dest (str) – Name of the frame to get the pose in.

Return type:

SE3

Returns:

Current transform from the source frame to the dest frame.

Raises:

FrameNotFound – if any frame name is not found in the model.

get_transform_frame_to_world(frame)¶

Get the pose of a frame in the current configuration.

Parameters:: frame (str) – Name of a frame, typically a link name from the URDF.
Return type:: SE3
Returns:: Current transform from the given frame to the world frame.
Raises:: FrameNotFound – if the frame name is not found in the robot model.

integrate(velocity, dt)¶

Integrate a velocity starting from the current configuration.

Parameters:

velocity – Velocity in tangent space.
dt – Integration duration in [s].

Return type:

Configuration

Returns:

New configuration after integration, sharing the default limits and tangent space of this one.

integrate_inplace(velocity, dt)¶

Integrate a velocity starting from the current configuration.

Parameters:

velocity – Velocity in tangent space.
dt – Integration duration in [s].

Return type:

None

update(q=None)¶

Update configuration to a new vector.

Calling this function runs forward kinematics.

Parameters:: q (Optional[ndarray]) – New configuration vector.
Return type:: None

Task formalism¶

The task function approach followed in Pinker, and many works before it, was originally formalized by Samson, Espiau and Le Borgne in [Samson1991].

In Pinker, all kinematic tasks derive from the Task base class.

The formalism used in the library is written down in this note on task-based inverse kinematics. As of February 2022, it hasn’t been updated with the proper dimensional analysis, but the core concepts and notations are there.

class pinker.tasks.task.Task(cost=None, gain=1.0, lm_damping=0.0)¶

Abstract base class for kinematic tasks.

cost¶: cost vector with the same dimension as the error of the task. Its units depends on the error as well.

gain¶: Task gain \(\alpha \in [0, 1]\) for additional low-pass filtering. Defaults to 1.0 (no filtering) for dead-beat control.

lm_damping¶: Unitless scale of the Levenberg-Marquardt (only when the error is large) regularization term, which helps when targets are unfeasible. Increase this value if the task is too jerky under unfeasible targets, but beware that a larger damping slows down the task.

abstractmethod compute_error(configuration)¶

Compute the task error function.

The error function \(e(q) \in \mathbb{R}^{k}\) is the quantity that the task aims to drive to zero (\(k\) is the dimension of the task). It appears in the first-order task dynamics:

\[J(q) \Delta q = -\alpha e(q)\]

The Jacobian matrix \(J(q) \in \mathbb{R}^{k \times n_v}\), with \(n_v\) the dimension of the robot’s tangent space, is the derivative of the task error \(e(q)\) with respect to the configuration \(q \in \mathbb{R}^{n_q}\). This Jacobian is implemented in Task.compute_jacobian(). Finally, the configuration displacement \(\Delta q\) is the output of inverse kinematics.

In the first-order task dynamics, the error \(e(q)\) is multiplied by the task gain \(\alpha \in [0, 1]\). This gain can be one for dead-beat control (i.e. converge as fast as possible, but might be unstable as it neglects our first-order approximation), but it can also be lower a slower task (similar to low-pass filtering).

Parameters:: configuration (Configuration) – Robot configuration \(q\).
Return type:: ndarray
Returns:: Task error vector \(e(q)\).

abstractmethod compute_jacobian(configuration)¶

Compute the task Jacobian at a given configuration.

The task Jacobian \(J(q) \in \mathbb{R}^{k \times n_v}\) is the first-order derivative of the error \(e(q) \in \mathbb{R}^{k}\) that defines the task, with \(k\) the dimension of the task and \(n_v\) the dimension of the robot’s tangent space.

Parameters:: configuration (Configuration) – Robot configuration \(q\).
Return type:: ndarray
Returns:: Task Jacobian \(J(q)\).

compute_qp_objective(configuration)¶

Compute the matrix-vector pair \((H, c)\) of the QP objective.

This pair is such that the contribution of the task to the QP objective of the IK is:

\[\frac{1}{2} \| J \Delta q + \alpha e \|_{W}^2 = \frac{1}{2} \Delta q^T H \Delta q + c^T q\]

The weight matrix \(W \in \mathbb{R}^{k \times k}\) weighs and normalizes task coordinates to the same unit. The unit of the overall contribution is [cost]^2. The configuration displacement \(\Delta q\) is the output of inverse kinematics (we divide it by \(\Delta t\) to get a commanded velocity).

Parameters:: configuration (Configuration) – Robot configuration \(q\).
Return type:: Tuple[ndarray, ndarray]
Returns:: Pair \((H(q), c(q))\) of Hessian matrix and linear vector of the QP objective.