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.
• To generate a trajectory of the robot (sequence of poses), it is possible to use "previous joint positions" as additional input.
• If the previous joint positions are provided, the solutions are sorted by proximity to them (closest first). It is also possible to prioritize proximity to the center of constraints.
• 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)
• Use zeros to get the possible solution of singularity case with J4 and J6 close to zero rotation.

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.

Example

Cargo.toml:

[dependencies]
rs-opw-kinematics = "1.1.0"

main.rs:

use std::f64::consts::PI;
use rs_opw_kinematics::kinematic_traits::{Joints, Kinematics, Pose, 
                                          JOINTS_AT_ZERO, CONSTRAINT_CENTERED};
use rs_opw_kinematics::kinematics_impl::OPWKinematics;
use rs_opw_kinematics::parameters::opw_kinematics::Parameters;
use rs_opw_kinematics::utils::{dump_joints, dump_solutions};
use rs_opw_kinematics::constraints::{BY_CONSTRAINS, BY_PREV, Constraints};

fn main() {
  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]
  println!("Initial joints with singularity J5 = 0: ");
  dump_joints(&joints);

  println!("Solutions (original angle set is lacking due singularity there: ");
  let pose: Pose = robot.forward(&joints); // Pose is alias of nalgebra::Isometry3<f64>

  let solutions = robot.inverse(&pose); // Solutions is alias of Vec<Joints>
  dump_solutions(&solutions);

  println!("Solutions assuming we continue from somewhere close. The 'lost solution' returns");
  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);

  println!("Same pose, all J4+J6 rotation assumed to be previously concentrated on J4 only");
  let when_continuing_from_j6_0: [f64; 6] = [0.0, 0.11, 0.22, 0.8, 0.1, 0.0];
  let solutions = robot.inverse_continuing(&pose, &when_continuing_from_j6_0);
  dump_solutions(&solutions);

  println!("If we do not have the previous position, we can assume we want J4, J6 close to 0.0 \
    The solution appears and the needed rotation is now equally distributed between J4 and J6.");
  let solutions = robot.inverse_continuing(&pose, &JOINTS_AT_ZERO);
  dump_solutions(&solutions);

  let robot = OPWKinematics::new_with_constraints(
    Parameters::irb2400_10(), Constraints::new(
      [-0.1, 0.0, 0.0, 0.0, -PI, -PI],
      [ PI, PI, 2.0*PI, PI, PI, PI],
      BY_PREV,
    ));

  println!("If we do not have the previous pose yet, we can now ask to prefer the pose \
    closer to the center of constraints.");
  let solutions = robot.inverse_continuing(&pose, &CONSTRAINT_CENTERED);
  dump_solutions(&solutions);

  println!("With constraints, sorted by proximity to the previous pose");
  let solutions = robot.inverse_continuing(&pose, &when_continuing_from_j6_0);
  dump_solutions(&solutions);

  let robot = OPWKinematics::new_with_constraints(
    Parameters::irb2400_10(), Constraints::new(
      [-0.1, 0.0, 0.0, 0.0, -PI, -PI],
      [ PI, PI, 2.0*PI, PI, PI, PI],
      BY_CONSTRAINS,
    ));
  println!("With constraints, sorted by proximity to the center of constraints");
  let solutions = robot.inverse_continuing(&pose, &when_continuing_from_j6_0);
  dump_solutions(&solutions);
}

The constants BY_PREV ( = 0.0) and BY_CONSTRAINTS ( = 1.0) are for convenience only. Intermediate values like 0.6 can also be specified and result in weighted sorting.

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");
let robot = OPWKinematics::new(parameters);

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.