#kinematics #ik #path-planning #opw

bin+lib rs-opw-kinematics

Inverse and forward kinematics for 6 axis robots with a parallel base and spherical wrist

7 releases (stable)

new 1.2.0 May 21, 2024
1.1.2-alpha.1 May 16, 2024
1.0.2 Apr 21, 2024

#4 in Robotics

Download history 291/week @ 2024-04-17 39/week @ 2024-04-24 257/week @ 2024-05-01 99/week @ 2024-05-08 175/week @ 2024-05-15

585 downloads per month

BSD-3-Clause

600KB
2.5K SLoC

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.

GitHub crates.io GitHub Workflow Status crates.io crates.io docs.rs

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)
  • Experimental support for parameter extraction from URDF.

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.

OPW Diagram

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.1, <2.0.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");
  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.

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. In general, always test in simulator before feeding the output of any software to the physical robot.

Disabling YAML and URDF readers

For security and performance, some users prefer smaller libraries with less dependencies. If YAML and URDF readers are not in use, the filesystem access can be completely disabled in your Cargo.toml, importing the library like:

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

In this case, import of URDF and YAML files will be unaccessible, 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.

Dependencies

~2.6–5MB
~95K SLoC