### 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

**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.

# 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.

To use the library, fill out an

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.`opw_kinematics ::`Parameters

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