Rust 实现了具有平行基座和球形手腕的六轴工业机器人的逆运动学和正运动学解决方案。针对 J5 = 0° 或 ± 180° 的奇点进行了加固，并针对轨迹规划进行了优化。

简介

这项工作基于 Mathias Brandstötter、Arthur Angerer 和 Michael Hofbaur 撰写的 2014 年论文《具有正交平行基座和球形手腕的工业串联机械手的逆运动学问题的解析解》，该论文可在 ResearchGate 上找到。[链接](https://www.researchgate.net/profile/Mathias-Brandstoetter/publication/264212870_An_Analytical_Solution_of_the_Inverse_Kinematics_Problem_of_Industrial_Serial_Manipulators_with_an_Ortho-parallel_Basis_and_a_Spherical_Wrist/links/53d2417e0cf2a7fbb2e98b09/An-Analytical-Solution-of-the-Inverse-Kinematics-Problem-of-Industrial-Serial-Manipulators-with-an-Ortho-parallel-Basis-and-a-Spherical-Wrist.pdf)。此外，它还借鉴了类似的 C++ 项目 [Jmeyer1292/opw_kinematics](https://github.com/Jmeyer1292/opw_kinematics)，该项目作为测试套件的参考实现生成数据。该文档还包含了该项目的机器人图。

功能

• rs-opw-kinematics 完全用 Rust 编写（不是 C++ 绑定）并通过 Cargo 部署。
• 所有返回的解都是有效的、规范的，并与正运动学进行了交叉检查。
• 关节角度可以与约束条件进行核对，以确保只返回合规的解。
• 要生成机器人的轨迹（姿态序列），可以使用“上一个关节位置”作为额外的输入。
• 如果提供了上一个关节位置，则根据它们之间的接近程度对解进行排序（最近的首先）。还可以优先考虑接近约束中心的程度。
• 对于 J5 = 0° 或 J5 = ±180° 位置的运动学奇点，该求解器提供了接近这些关节上一个位置的合理的 J4 和 J6 值（而不是任意值，这可能导致真实机器人的较大冲击）
• 雅可比矩阵、扭矩和速度
• 机器人可以配备工具并放置在基座上，规划工具中心点（TCP）的期望位置和方向，而不是机器人的任何部分。
• 对从URDF中提取参数的实验性支持。

求解器目前使用64位浮点数（Rust f64），为测试的两个机器人提供低于1µm的定位精度。

参数

此库使用七个运动学参数（a1, a2, b, c1, c2, c3和c4）。此求解器假设当所有关节都直直地向上竖立时，手臂处于零位置，如以下图片所示。它还假设所有旋转都是围绕机器人的基轴的正旋转。无需其他设置。

要使用此库，请使用适当的7个运动学参数值和任何需要将论文的零位置（Z轴向上的手臂）调整到制造商位置的关节偏移值填写opw_kinematics::Parameters数据结构。此外，还有6个“符号校正”参数（-1或1），如果您的机器人的轴与论文中的约定不匹配，则应指定这些参数。

例如，ABB IRB2400具有以下值

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],
}

请注意，第三个关节的偏移量为-90°，将关节从直立位置移至与地面平行的“零”位置。

约束

自1.1.0版本以来，可以为关节设置约束。任何关节超出指定约束范围的机器人姿态都不会包含在返回的解决方案列表中。还可以通过给予约束中心一些优先级来影响结果列表的排序。

对于每个关节，通过提供两个角度从和到来指定约束。如果从 < 到，则有效范围在从和到之间。如果从 > 到，则有效范围在0°周围展开。例如，如果从 = 5°和到 = 15°，则6°、8°和11°是有效值，而90°和180°不是有效值。如果从 = 15°和到 = 5°（相反），则16°、17°、100°、180°、359°、0°、1°、3°、4°是有效值，而6°、8°和11°不是有效值。

约束在-2π到2π的范围内进行测试，但由于角度以2π的周期重复，因此从-π到π的约束已经允许自由旋转，覆盖任何角度。

雅可比：扭矩和速度

自1.3.2以来，可以获得表示关节速度和末端执行器速度之间关系的雅可比。计算的雅可比对象提供

• 实现所需末端执行器速度所需的关节速度。
• 实现所需末端执行器力/扭矩所需的关节扭矩。

重新使用相同的Joints结构，六个值现在表示每秒弧度或牛顿米的角速度或扭矩。对于末端执行器，可以使用nalgebra::Isometry3或Vector6，两者都定义了每秒m/s或旋转N m的速度。

这些值在为需要非常快速移动的机器人进行路径规划时很有用，以防止单个关节的超速或过扭矩。

工具和基座

自1.3.2以来，机器人可以配备工具，定义为nalgebra::Isometry3。工具等距定义了额外的平移和额外的旋转。“姿态”现在定义为工具中心点的姿态，而不是机器人的任何部分。机器人还可以放置在基座上，进一步支持更接近真实工业环境的条件。

“带有工具的机器人”和“位于底座上的机器人”可以围绕任何运动学特质构建，并自行实现此特质。它们可以级联，构建一个带有工具的底座上的机器人（或者如果第一个是工具变换器的容器，则两个工具）。

框架

从1.4.0版本开始，此包支持框架变换，允许将一个位置为机器人轨迹（以关节角度表示）准备的变换应用于另一个位置（平移和旋转）。在机器人技术中，框架通常由包含旋转的3对点（去和来）定义，或者如果只涉及位移（但不是旋转），则只需要一对点。

一旦指定了原始点和变换点构建，框架对象就可以获取变换的（平移和旋转）轨迹的“规范”关节角度和计算出的关节角度。有关详细信息，请参阅框架文档。

示例

Cargo.toml

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

main.rs

use std::f64::consts::PI;
use std::sync::Arc;
use nalgebra::{Isometry3, Translation3, UnitQuaternion, Vector3};
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 prever 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);

    // This requires YAML library
    let parameters = Parameters::irb2400_10();
    println!("Reading:\n{}", &parameters.to_yaml());

    // Jacobian, velocities and forces:
    let jakobian = rs_opw_kinematics::jakobian::Jacobian::new(&robot, &joints, 1E-6);
    let desired_velocity_isometry =
        Isometry3::new(Vector3::new(0.0, 1.0, 0.0),
                       Vector3::new(0.0, 0.0, 1.0));
    let joint_velocities = jakobian.velocities(&desired_velocity_isometry);
    println!("Computed joint velocities: {:?}", joint_velocities.unwrap());

    let desired_force_torque =
        Isometry3::new(Vector3::new(0.0, 0.0, 0.0),
                       Vector3::new(0.0, 0.0, 1.234));

    let joint_torques = jakobian.torques(&desired_force_torque);
    println!("Computed joint torques: {:?}", joint_torques);

    // Robot with the tool, standing on a base:
    let robot_alone = OPWKinematics::new(Parameters::staubli_tx2_160l());

    // Half meter high pedestal
    let pedestal = 0.5;
    let base_translation = Isometry3::from_parts(
        Translation3::new(0.0, 0.0, pedestal).into(),
        UnitQuaternion::identity(),
    );

    let robot_with_base = rs_opw_kinematics::tool::Base {
        robot: Arc::new(robot_alone),
        base: base_translation,
    };

    // Tool extends 1 meter in the Z direction, envisioning something like sword
    let sword = 1.0;
    let tool_translation = Isometry3::from_parts(
        Translation3::new(0.0, 0.0, sword).into(),
        UnitQuaternion::identity(),
    );

    // Create the Tool instance with the transformation
    let robot_complete = rs_opw_kinematics::tool::Tool {
        robot: Arc::new(robot_with_base),
        tool: tool_translation,
    };

    let tcp_pose: Pose = robot_complete.forward(&joints);
    println!("The sword tip is at: {:?}", tcp_pose);
}

常量BY_PREV（= 0.0）和BY_CONSTRAINTS（= 1.0）仅用于方便。也可以指定中间值，如0.6，这将导致加权排序。

为您的机器人配置求解器

项目包含了对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和TX2-160L等机器人的内置定义，这些定义经过不同级别的测试。机器人制造商可能为其制造的机器人提供此类配置。例如，FANUC M10IA在这里描述：这里。许多其他机器人在ros-industrial/fanuc存储库中描述。此项目包含直接读取此类配置的代码，包括对deg(angle)函数的支持，该函数有时出现在其中。

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

从版本1.2.0开始，参数和约束也可以直接从URDF文件提取

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

还有一个更高级的函数rs_opw_kinematics::urdf::from_urdf，它接受URDF字符串而不是文件，提供错误处理以及更多对求解器构建的控制。

重要：URDF读取器假定具有平行底座和球形手腕的机器人，而不是任意机器人。您可以在机器人文档中轻松检查此信息，或者简单地查看图纸。如果机器人看起来符合OPW规范，但参数未正确提取，请提交错误报告，提供URDF文件和预期值。一般来说，在将任何软件的输出提供给物理机器人之前，始终在模拟器中进行测试。

禁用YAML和URDF读取器

出于安全和性能考虑，一些用户更喜欢更小的库，依赖性更少。如果不需要YAML和URDF读取器，可以在Cargo.toml中完全禁用文件系统访问，导入库如下：

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

在这种情况下，将无法导入URDF和YAML文件，使用的依赖项将仅限于单个nalgebracrate。

测试

此项目的代码针对测试集（cases.yaml，每个机器人2048个案例）进行了测试，该测试集被认为适用于两个机器人，KUKA KR 6 R700 sixx和ABB IRB 2400/10。它是通过Jmeyer1292/opw_kinematics的独立C++实现生成的。测试套件检查解决方案是否匹配。