Rust implementation of inverse and forward kinematic solutions for six-axis industrial robots with a parallel base and spherical wrist. Hardened against the J5 = 0° or ± 180° singularity and optimized for trajectory planning.

Intro

This work builds upon the 2014 paper titled An Analytical Solution of the Inverse Kinematics Problem of Industrial Serial Manipulators with an Ortho-parallel Basis and a Spherical Wrist, authored by Mathias Brandstötter, Arthur Angerer, and Michael Hofbaur. The paper is available in ResearchGate. Additionally, it draws inspiration from the similar C++ project, Jmeyer1292/opw_kinematics, which served as a reference implementation for generating data for the test suite. This documentation also incorporates the robot diagram from that project.

Features

• rs-opw-kinematics is written entirely in Rust (not a C++ binding) and deployable via Cargo.
• All returned solutions are valid, normalized, and cross-checked with forward kinematics.
• Joint angles can be checked against constraints, ensuring only compliant solutions are returned.
• Collision detection (with Parry) allows to exclude solutions where robot would collide with itself or environment objects.
• For kinematic singularity at J5 = 0° or J5 = ±180° positions this solver provides reasonable J4 and J6 values close to the previous positions of these joints (and not arbitrary that may result in a large jerk of the real robot)
• Respecting previous joint position helps against unwanted jumps.
• Jacobian, torgues and velocities
• The robot can be equipped with the tool and placed on the base, planning for the desired location and orientation of the tool center point (TCP) rather than any part of the robot.
• 5 DOF inverse kinematics.
• Visualization (with Bevy) allows quick check if the robot is properly configured.
• Easily integrates with path finding libraries like rrt and pathfinding, examples provided.

The solver currently uses 64-bit floats (Rust f64), providing the positional accuracy below 1µm for the two robots tested.

Parameters

This library uses seven kinematic parameters (a1, a2, b, c1, c2, c3, and c4). This solver assumes that the arm is at zero when all joints stick straight up in the air, as seen in the image below. It also assumes that all rotations are positive about the base axis of the robot. No other setup is required.

To use the library, fill out an opw_kinematics::Parameters data structure with the appropriate values for the 7 kinematic parameters and any joint offsets required to bring the paper's zero position (arm up in Z) to the manufacturer's position. Additionally, there are 6 "sign correction" parameters (-1 or 1) that should be specified if your robot's axes do not match the convention in the paper.

For example, the ABB IRB2400 has the following values:

let parameters = Parameters {
a1: 0.100, a2: - 0.135, b: 0.000, c1: 0.615, c2: 0.705, c3: 0.755, c4: 0.085,
offsets: [0.0, 0.0, -std::f64::consts::PI / 2.0, 0.0, 0.0, 0.0],
sign_corrections: [1; 6],
}

Note that the offset of the third joint is -90°, bringing the joint from the upright position to parallel with the ground at "zero."

Constraints

Since 1.1.0, it is possible to set constraints for the joints. Robot poses where any of the joints are outside the specified constraint range are not included into returned list of solutions. It is also possible to influence the sorting of the result list by giving some preference to the center of constraints.

Constraints are specified by providing two angles, from and to, for every joint. If from < to, the valid range spans between from and to. If from > to, the valid range spans over the 0°, wrapping around. For instance, if from = 5° and to = 15°, values 6°, 8°, and 11° are valid, while values like 90°, and 180° are not. If from = 15° and to = 5° (the opposite), values 16°, 17°, 100°, 180°, 359°, 0°, 1°, 3°, 4° are valid, while 6°, 8°, and 11° are not.

Constraints are tested for the range from -2π to 2π, but as angles repeat with period of 2π, the constraint from -π to π already permits free rotation, covering any angle.

Since 1.7.0, convenience method exists to specify constraints as ranges in degrees.

Jacobian: torgues and velocities

Since 1.3.2, it is possible to obtain the Jacobian that represents the relationship between the joint velocities and the end-effector velocities. The computed Jacobian object provides:

• Joint velocities required to achieve a desired end-effector velocity.
• Joint torques required to achieve a desired end-effector force/torque.

The same Joints structure is reused, the six values now representing either angular velocities in radians per second or torgues in Newton meters. For the end effector, it is possible to use either nalgebra::Isometry3 or Vector6, both defining velocities in m/s or rotations in N m.

These values are useful when path planning for a robot that needs to move very swiftly, to prevent overspeed or overtorgue of individual joints.

The tool and the base

Since 1.3.2, robot can be equipped with the tool, defined as nalgebra::Isometry3. The tool isometry defines both additional translation and additional rotation. The "pose" as defined in forward and inverse kinematics now becomes the pose of the tool center point, not any part of the robot. The robot can also be placed on a base, further supporting the conditions much closer to the real industrial environment.

"Robot with the tool" and "Robot on the base" can be constructed around any Kinematics trait, and implement this trait themselves. It is possible to cascade them, constructing a robot on a base and with the tool (or two tools if the first is a receptacle of the tool changer).

The frame

Since 1.7.0 this package supports the frame transform that allows to transform the robot trajectory (in terms of joint angles) prepared for one location to make the same kind of movements in another location (translated and rotated). Frame in robotics is most commonly defined by the 3 pairs of points (to and from) if the transform includes also rotation, or just a single pair is enough if only shift (but not a rotation) is involved.

Once constructed by specifying original and transformed points, the Frame object can take "canonical" joint angles and calculated joint angles for the transformed (shifted and rotated) trajector. See the frame documentation for details.

Individual link positions

It is now possible to obtain positions of individual links in forward kinematics. This would be needed for collision avoidance and graphical rendering of the robot. See forward_with_joint_poses method.

5 DOF inverse kinematics

For tools that are not sensitive to axis rotation (such as welding torches or paint sprayers), inverse kinematics can be requested where the value of joint 6 (which typically controls this rotation) is either inherited from the previous position or explicitly specified.

The 5 DOF robot can stil be represented with the same diagram, and has the same parameters. However, joint 6 is assumed to be fixed. Such a robot still can bring the tool to the needed location, also following the generic orientation. but the rotation around the tool axis is not followed.

Support for 5 DOF robots is now included through an additional 'dof' field in the parameter data structure. 5 DOF inverse kinematics can also be requested for 6 DOF robots, particularly when the last joint is in constant motion (e.g., for drilling), or when maintaining precise tool rotation would cause the robot to exceed its constraints. This method is also faster to compute. If the robot is flagged as 5 DOF robot, the value of the joint 6 will normally be 0 and ignored.

Parallelogram

The parallelogram mechanism maintains the orientation of the end-effector in some robots. It introduces a geometric relationship between two joints, typically referred to as the driven joint (often J₂) and the coupled joint (often J₃), to ensure the end-effector remains stable in its orientation during motion.

In forward kinematics, J₃ is adjusted by subtracting the value of J₂ multiplied by a scaling factor s (often 1.0). The relationship can be written as:

J₃' = J₃ − s * J₂

This adjustment maintains the correct orientation of the end-effector as the robot moves through its workspace.

In inverse kinematics, the process is reversed. The value of J₂ is added back to J₃, ensuring accurate joint angle calculations for achieving the desired end-effector pose and orientation. This can be expressed as:

J₃' = J₃ + s * J₂

The scaling factor s determines how much influence J₂ has on J₃. A scaling factor of 1.0 is the common, as this value ensures the end-effector’s orientation remains unchanged if only J₃ and J₂ move.

See Parallelogram.

Collision avoidance

The new class KinematicsWithShape combines kinematics and collision checking. It implements the Kinematics trait, providing both forward and inverse kinematic solutions. During inverse kinematics, any colliding poses are excluded from the solution list.

For collision avoidance, you need to supply meshes for robot joints and, optionally, for the base, tool, and environment objects. The example below demonstrates how to create this structure, complete with tool, base and constraints:

use std::ops::RangeInclusive;
use nalgebra::{Isometry3, Translation3, UnitQuaternion};
use rs_opw_kinematics::constraints::{Constraints, BY_PREV};
use rs_opw_kinematics::collisions::{BaseBody, CollisionBody, RobotBody};
use rs_opw_kinematics::kinematics_with_shape::KinematicsWithShape;
use rs_opw_kinematics::parameters::opw_kinematics::Parameters;
use rs_opw_kinematics::{utils, visualization};
use rs_opw_kinematics::read_trimesh::load_trimesh_from_stl;


/// Create the sample robot we will visualize. This function creates
/// Staubli RX160, using is parameter set.
/// It loads the joint meshes from .stl files bundles in the test folder
/// where they are shared under the rights of Apache license (ROS Industrial project).
/// Four environment objects and tool are also created.
pub fn create_rx160_robot() -> KinematicsWithShape {
    // Environment object to collide with.
    let monolith = load_trimesh_from_stl("src/tests/data/object.stl");

    let robot = KinematicsWithShape::new(
        // OPW parameters for Staubli RX 160
        Parameters {
            a1: 0.15,
            a2: 0.0,
            b: 0.0,
            c1: 0.55,
            c2: 0.825,
            c3: 0.625,
            c4: 0.11,
            ..Parameters::new()
        },

        Constraints::from_degrees(
          [
            -225.0..=225.0, -225.0..=225.0, -225.0..=225.0,
            -225.0..=225.0, -225.0..=225.0, -360.0..=360.0,
          ],
          BY_PREV, // Prioritize previous joint position
        ),

        // Joint meshes
        [
            // If your mesh if offset in .stl file, use Trimesh::transform_vertices,
            // you may also need Trimesh::scale on some extreme cases.
            // If your joints or tool consist of multiple meshes, combine these
            // with Trimesh::append

            load_trimesh_from_stl("src/tests/data/staubli/rx160/link_1.stl"),
            load_trimesh_from_stl("src/tests/data/staubli/rx160/link_2.stl"),
            load_trimesh_from_stl("src/tests/data/staubli/rx160/link_3.stl"),
            load_trimesh_from_stl("src/tests/data/staubli/rx160/link_4.stl"),
            load_trimesh_from_stl("src/tests/data/staubli/rx160/link_5.stl"),
            load_trimesh_from_stl("src/tests/data/staubli/rx160/link_6.stl")
        ],

        // Base link mesh
        load_trimesh_from_stl("src/tests/data/staubli/rx160/base_link.stl"),

        // Base transform, this is where the robot is standing
        Isometry3::from_parts(
            Translation3::new(0.4, 0.7, 0.0).into(),
            UnitQuaternion::identity(),
        ),

        // Tool mesh
        load_trimesh_from_stl("src/tests/data/flag.stl"),

        // Tool transform, tip (not base) of the tool. The point past this
        // transform is known as tool center point (TCP).
        Isometry3::from_parts(
            Translation3::new(0.0, 0.0, 0.8).into(),
            UnitQuaternion::identity(),
        ),

        // Objects arround the robot, with global transforms for them.
        vec![
            CollisionBody { mesh: monolith.clone(), pose: Isometry3::translation(1., 0., 0.) },
            CollisionBody { mesh: monolith.clone(), pose: Isometry3::translation(-1., 0., 0.) },
            CollisionBody { mesh: monolith.clone(), pose: Isometry3::translation(0., 1., 0.) },
            CollisionBody { mesh: monolith.clone(), pose: Isometry3::translation(0., -1., 0.) }
        ],
        true // First good pose only (true) or all poses (false)
    );

  // Let's play a bit with this robot now:
 
  let pose = Isometry3::from_parts(
    Translation3::new(0.0, 0.0, 1.5), // position 1.5 meter high above center of the world
    UnitQuaternion::identity()); // with robot tool pointing right upwards

  // Collision aware inverse kinematics (colliding solutions discarded)  
  let solutions = robot.inverse(&pose);
  dump_solutions(&solutions); // Print solutions in degrees
  
  // Collision check against given joint positions
  if robot.collides(&[173_f64.to_radians(), 0., -94_f64.to_radians(), 0., 0., 0.]) {
    println!("Collision detected");
  }  
  
  robot
}

Path planning

There are currently few path planning libraries available in Rust. Instead of incorporating them directly into our project and writing the code arround, we decided to explore the complexity of integrating these libraries as external dependencies (referenced only in examples). This approach allowed us to identify key "pain points" that complicate the integration of external path planners.

We provide external support for two libraries, rrt and pathfinding. Although rs-opw-kinematics does not use either internally, we include examples demonstrating their usage. These two libraries are listed as development dependencies in Cargo.toml.

rrt

The Rapidly-Exploring Random Tree (RRT) library, rrt, is available under the Apache 2.0 license by Takashi Ogura and Mitsuharu Kojima. It can be used the following way:

use rrt::dual_rrt_connect;
fn plan_path(
  kinematics: &KinematicsWithShape,
  start: Joints, goal: Joints,
) -> Result<Vec<Vec<f64>>, String> {
  let collision_free = |joint_angles: &[f64]| -> bool {
    let joints = &<Joints>::try_from(joint_angles).expect("Cannot convert vector to array");
    !kinematics.collides(joints)
  };

  // Constraint compliant random joint configuration generator. 
  let random_joint_angles = || -> Vec<f64> {
    // RRT requires vector and we return array so convert
    return kinematics.constraints()
            .expect("Set joint ranges on kinematics").random_angles().to_vec();
  };

  // Plan the path with RRT
  dual_rrt_connect(
    &start, &goal, collision_free,
    random_joint_angles, 3_f64.to_radians(), // Step size in joint space
    2000,  // Max iterations
  )
}

This library required to produce random joint angles within constraints. We made constraints easily accessible from the instance of Kinematics, and provided random_angles() methods for them.

See the file examples/path_planning_rrt.rs for how to define the robot and other boilerplate code. The direct output will be a vector of vectors (not vector of Joints), each representing a step in the trajectory.

pathfinding

Pathfinding by Samuel Tardieu provides A*, IDA* and other comparable algorithms, under Apache 2 or MIT license. While these algorithms may also lift the robot hand over obstacle, they may struggle with very complex path. From the other side, the path quality is better. Supporting this group of libraries required to provide the list of neighouring joints that would be constraint-compliant and collision free that was done implementing non_colliding_offsets on KinematicsWithShape. You can find example of pathfinding integration in examples/path_planning_idastar.rs .

Visualization

KinematicsWithShape is also straightforward to visualize, as it fully integrates both the kinematics and 3D meshes representing the robot. To display it, simply pass this structure to the built-in function visualize_robot:

fn main() {
    // The robot itself (as per example above)
    let robot = create_rx160_robot();

    // In which position to show the robot on startup
    let intial_angles = [173., -8., -94., 6., 83., 207.];

    // Boundaries for XYZ drawbars in visualizaiton GUI
    let tcp_box: [RangeInclusive<f64>; 3] = [-2.0..=2.0, -2.0..=2.0, 1.0..=2.0];

    visualization::visualize_robot(robot, intial_angles, tcp_box);
}

Visualization primarily serves to verify that your parameters, meshes, tool, and base setup are correct. It is not intended as a production feature. Using Bevy, the visualization will display the robot (mounted on its base and equipped with the tool), various environment objects, and a selection of handles to manipulate the robot.

In the visualization window, you can adjust joint positions for forward kinematics or set the tool center point using Cartesian coordinates for inverse kinematics. Collision detection is active in both modes, but it functions differently: in inverse kinematics, movement is restricted to prevent collisions entirely (the robot will not move if a collision would occur). In forward kinematics, collisions are allowed, but colliding robot joints and environment objects will be highlighted.

When using inverse kinematics, you may observe unexpected large "jumps," or in some cases, no viable solution within the robot's reach, constraints, and collision boundaries. This simply reflects the inherent limitations and complexities of real-world robotic movement.

Example

Cargo.toml:

[dependencies]
rs-opw-kinematics = ">=1.7.0, <2.0.0"

Simple "hello world" demonstrating singularity evasion would look more or less like this:

use rs_opw_kinematics::kinematic_traits::{Joints, Kinematics, Pose, JOINTS_AT_ZERO};
use rs_opw_kinematics::kinematics_impl::OPWKinematics;
use rs_opw_kinematics::parameters::opw_kinematics::Parameters;
use rs_opw_kinematics::utils::{dump_joints, dump_solutions};

fn main() {
    // Create a robot with built-in parameter set. It is a simple Kinematics with no collision checks.
    let robot = OPWKinematics::new(Parameters::irb2400_10());
    let joints: Joints = [0.0, 0.1, 0.2, 0.3, 0.0, 0.5]; // Joints are alias of [f64; 6], given in radians here
    println!("\nInitial joints with singularity J5 = 0: ");
    dump_joints(&joints);

    println!("\nSolutions assuming we continue from somewhere close. No singularity effect.");
    // Some previous pose for picking the best solution when infinite number of these exist.
    let when_continuing_from: [f64; 6] = [0.0, 0.11, 0.22, 0.3, 0.1, 0.5]; 
    let solutions = robot.inverse_continuing(&pose, &when_continuing_from);
    dump_solutions(&solutions);
}

Starting from version 1.7.0, rs-opw-kinematics has evolved beyond being just a set of "useful building blocks." It now enables the creation of a complete robot setup, which includes mounting the robot on a base, equipping it with a tool, and integrating collision checking. See example complete_visible_robot.

Configuring the solver for your robot

The project contains built-in definitions for ABB IRB 2400/10, IRB 2600-12/1.65, IRB 4600-60/2.05; KUKA KR 6 R700 sixx, FANUC R-2000iB/200R; Stäubli TX40, TX2-140, TX2-160 and TX2-160L with various levels of testing. Robot manufacturers may provide such configurations for the robots they make. For instance, FANUC M10IA is described here. Many other robots are described in ros-industrial/fanuc repository. This project contains the code for reading such configurations directly, including support for deg(angle) function that sometimes occurs there:

  let parameters = Parameters::from_yaml_file(filename).expect("Failed to load parameters");
  println!("Reading:\n{}", &parameters.to_yaml());
  let robot = OPWKinematics::new(parameters);

Since version 1.2.0, parameters and constraints can also be directly extracted from URDF file:

  let robot = rs_opw_kinematics::urdf::from_urdf_file("/path/to/robot.urdf");
  println!("Reading:\n{}", &parameters.to_yaml());

There is also more advanced function rs_opw_kinematics::urdf::from_urdf that takes URDF string rather than the file, provides error handling and much more control over how the solver is constructed from the extracted values.

YAML reader supports additional 'dof' field that can be set to 6 (default) or 5 (5DOF robot, tool rotation not accounted for). The URDF reader has als been extended to support such robots, but joint names must aways be explicitly provided. Instead of specifying a name for joint 6, the name of the tool center point (TCP) must be given. Both YAML and URDF readers still try to obtain the parameter c4 that is now distance from J5 axis till TCP.

Important: The URDF reader assumes a robot with parallel base and spherical wrist and not an arbitrary robot. You can easily check this in the robot documentation or simply looking into the drawing. If the robot appears OPW compliant yet parameters are not extracted correctly, please submit a bug report, providing URDF file and expected values. Use visualization as explained before feeding the output to the physical robot.

Disabling filesystem

For security and performance, some users prefer smaller libraries with less dependencies. If YAML and URDF readers are not in use and meshes for collision detection are obtained from somewhere else ( or collision detection is not used), the filesystem access can be completely disabled in your Cargo.toml, importing the library like:

rs-opw-kinematics = { version = ">=1.7.0, <2.0.0", default-features = false }

In this case, import of URDF and YAML files will be unaccessible, visualization and collision detection will not work either, and used dependencies will be limited to the single nalgebra crate.

Testing

The code of this project is tested against the test set (cases.yaml, 2048 cases per robot) that is believed to be correct for the two robots, KUKA KR 6 R700 sixx and ABB IRB 2400/10. It has been produced using independent C++ implementation by Jmeyer1292/opw_kinematics. The testing suite checks if the solutions match.