这是从 https://github.com/pantor/ruckig/ 仓库移植到 Rust 的版本。云客户端和专业功能尚未移植。示例使用 Gnuplot 来演示轨迹。

Ruckig 在线生成轨迹，使机器人和机器能够即时响应传感器输入。Ruckig 从任何受速度、加速度和加加速度约束的初始状态计算到目标点（具有位置、速度和加速度）的轨迹。除了目标状态，Ruckig 允许定义中间位置以跟随航点。对于状态到状态的运动，Ruckig 保证时间最优解。使用中间航点，Ruckig 联合计算路径及其时间参数化，从而比传统方法产生更快的轨迹。

更多信息可在 ruckig.com 和对应的论文 Jerk-limited Real-time Trajectory Generation with Arbitrary Target States 中找到，该论文被 2021 年的 Robotics: Science and Systems (RSS) 会议接受。

安装

cargo build --release

文档（不完整）

cargo doc --open

教程

让我们开始吧！

基于航点的轨迹生成

Ruckig 提供了三个主要接口类：Ruckig、InputParameter 和 OutputParameter 类。

首先，您需要使用 DOF 和 RuckigErrorHandler 作为模板参数创建一个 Ruckig 实例，并在构造函数中将 DOF 数量作为选项和控件周期（例如，以秒为单位）。

您可以选择使用模板参数来指定自由度（DOF）的数量，并使用堆栈分配，或者使用构造函数来指定DOF的数量，格式为 Option<usize>。如果您使用动态数量的DOF，则模板DOF参数必须设置为0以动态分配DOF。作为模板参数的任何大于0的数字都将导致省略动态DOF参数。在动态DOF分配的情况下，将构造函数的DOF参数设置为None是一种良好的做法。

use rsruckig::prelude::*;

// stack allocation using template parameter
let mut ruckig = Ruckig::<6, ThrowErrorHandler>::new(None, 0.01); // Number DoFs; control cycle in [s]

// dynamic allocation
let mut ruckig = Ruckig::<0, ThrowErrorHandler>::new(Some(6), 0.01); // Number DoFs; control cycle in [s]

已实现的错误处理器有

• ThrowErrorHandler - 如果输入无效，则抛出带有详细原因的错误。
• IgnoreErrorHandler - 忽略错误并返回 Ok(RuckigResult)。

要实现您自己的错误处理器，您需要实现 RuckigErrorHandler trait。

pub trait RuckigErrorHandler {
    fn handle_validation_error(message: &str) -> Result<bool, RuckigError>;
    fn handle_calculator_error(
        message: &str,
        result: RuckigResult,
    ) -> Result<RuckigResult, RuckigError>;
}

输入类型包含3块数据：当前状态、目标状态以及相应的运动学限制。

// Stack DOF allocation
let mut input = InputParameter::new(None); // Number DoFs
input.current_position = daov_stack![[0.2, ...];
input.current_velocity = daov_stack![0.1, ...];
input.current_acceleration = daov_stack![0.1, ...];
input.target_position = daov_stack![0.5, ...];
input.target_velocity = daov_stack![-0.1, ...];
input.target_acceleration = daov_stack![0.2, ...];
input.max_velocity = daov_stack![0.4, ...];
input.max_acceleration = daov_stack![1.0, ...];
input.max_jerk = daov_stack![4.0, ...];

let mut output: OutputParameter = OutputParameter::new(None);

// Dynamic DOF allocation
let mut input = InputParameter::new(Some(6)); // Number DoFs
input.current_position = daov_heap![0.2, ...];
input.current_velocity = daov_heap![0.1, ...];
input.current_acceleration = daov_heap![0.1, ...];
input.target_position = daov_heap![0.5, ...];
input.target_velocity = daov_heap![-0.1, ...];
input.target_acceleration = daov_heap![0.2, ...];
input.max_velocity = daov_heap![0.4, ...];
input.max_acceleration = daov_heap![1.0, ...];
input.max_jerk = daov_heap![4.0, ...];

let mut output: OutputParameter = OutputParameter::new(Some(6)); // Number DoFs

如果您只想有一个加速度限制的轨迹，您也可以省略 max_jerk 以及 current 和 target_acceleration 的值。给定所有输入和输出资源，我们可以在每个离散时间步迭代轨迹。对于大多数应用，此循环必须在实时线程中运行并控制实际硬件。

while ruckig.update(&input, &mut output).unwrap() == RuckigResult::Working {
  // Make use of the new state here!
  // e.g. robo.setJointPositions(output.new_position);

  output.pass_to_input(&mut input); // Don't forget this!
}

在控制循环中，您需要根据计算出的轨迹更新输入参数的当前状态。因此，pass_to_input 方法将输出的新运动学状态复制到输入参数的当前运动学状态。如果在下一个步骤中当前状态不是预期的、预先计算出的轨迹，Ruckig将根据新的输入计算新的轨迹。当轨迹达到目标状态时，update 函数将返回 Result::Finished。

DataArrayOrVec

DataArrayOrVec 类型是一个固定大小数组或向量的包装器。它主要用于存储运动学状态。用户可以在堆栈和堆分配之间进行选择。堆栈分配更快，但DOF的数量必须在编译时已知。堆分配较慢，但DOF的数量可以在运行时设置。

pub enum DataArrayOrVec<T, const N: usize>
where T: std::fmt::Debug {
Stack([T; N]),
Heap(Vec<T>),
}    

DataArrayOrVec 有两个关联的宏，以简化的形式简化实例化过程

• daov_stack!
• daov_heap!

使用示例

// For stack allocation, using template parameter and array under the hood
let mut data = daov_stack![0.2, 0.3, 0.4];
// For heap allocation, using Vec under the hood
let mut dynamic_data = daov_heap![0.2, 0.3, 0.4];

data[0] = 0.5;
data[1] = 0.6;
data[2] = 0.7;

dynamic_data[0] = 0.5;
dynamic_data[1] = 0.6;
dynamic_data[2] = 0.7;

### Input Parameter

To go into more detail, the *InputParameter* type has following members:

current_position: DataArrayOrVec<f64, DOF>;
current_velocity: DataArrayOrVec<f64, DOF>; // Initialized to zero
current_acceleration: DataArrayOrVec<f64, DOF>; // Initialized to zero

target_position: DataArrayOrVec<f64, DOF>;
target_velocity: VDataArrayOrVec<f64, DOF>; // Initialized to zero
target_acceleration: DataArrayOrVec<f64, DOF>; // Initialized to zero

max_velocity: DataArrayOrVec<f64, DOF>;
max_acceleration: DataArrayOrVec<f64, DOF>;
max_jerk: DataArrayOrVec<f64, DOF>; // Initialized to infinity

min_velocity: Option<DataArrayOrVec<f64, DOF>>; // If not given, the negative maximum velocity will be used.
min_acceleration: Option<DataArrayOrVec<f64, DOF>>; // If not given, the negative maximum acceleration will be used.

enabled: Vec<bool>; // Initialized to true
minimum_duration: Option<f64>;

control_interface: ControlInterface; // The default position interface controls the full kinematic state.
synchronization: Synchronization; // Synchronization behavior of multiple DoFs
duration_discretization: DurationDiscretization; // Whether the duration should be a discrete multiple of the control cycle (off by default)

per_dof_control_interface: Option<Vec<ControlInterface>>; // Sets the control interface for each DoF individually, overwrites global control_interface
per_dof_synchronization: Option<Vec<Synchronization>>; // Sets the synchronization for each DoF individually, overwrites global synchronization

在当前状态、目标状态和约束的基础上，Ruckig还允许一些更高级的设置

• 可以指定 最小速度 和 加速度 - 这些应该是负数。如果没有提供，则将使用负的最大速度或加速度（类似于加速度限制）。例如，这在人机协作设置中可能很有用，其中对人类的速度限制不同。或者，当在不同移动坐标系之间切换时，如从传送带上取货。
• 您可以通过使用例如 per_section_max_velocity 来覆盖全局运动学限制，以分别指定两个航点之间每个部分的限制。
• 如果DoF是 未启用 的，则在计算中将被忽略。Ruckig将为这些DoF输出一个具有恒定加速度的轨迹。
• 可以可选地给出一个 最小持续时间。请注意，Ruckig不能保证轨迹的精确持续时间，而只能保证最小持续时间。
• 可以轻松切换控制接口（位置或速度控制）。例如，可以使用速度接口轻松实现停止轨迹或视觉伺服。
• 实现了不同的同步行为（例如，相位、时间或无同步）。相位同步会导致直线运动。
• 轨迹持续时间可能被限制为控制周期的倍数。这样，在控制循环执行时，可以到达精确的状态。

输入验证

为了检查Ruckig在实际计算步骤之前能否生成轨迹，

ruckig.validate_input<E: RuckigErrorHandler>(input, check_current_state_within_limits, check_target_state_within_limits);
// returns Result<bool, RuckigError>. If RuckigErrorHandler is ThrowErrorHandler, it returns Err(RuckigError::ValidationError) in case of error.
// If the error handler is IgnoreErrorHandler, it returns Ok(true) if the input is valid, and Ok(false) if the input is invalid.

如果输入无效，则抛出带有详细原因的错误。您必须通过例如以下方式设置模板参数ruckig.validate_input<ThrowErrorHandler>(...) 。这两个布尔参数检查当前状态或目标状态是否在限制范围内。检查包括一个典型的抛出异常：当当前状态处于最大速度时，任何正加速度都不可避免地会在未来的时间步中导致速度违规。通常，这种条件满足时

Abs(acceleration) <= Sqrt(2 * max_jerk * (max_velocity - Abs(velocity))).

如果两个参数都设置为true，则计算出的轨迹在整个持续期间保证在运动学限制内。此外，请注意，由于数值原因，输入存在范围限制，下面将提供更多详细信息。

RuckigResult 类型

Ruckig类的 update 函数返回一个Result类型，指示算法的当前状态。这可以是工作、如果轨迹已完成，则为完成，或者在计算过程中出现错误时为错误类型。结果类型可以像标准整数一样进行比较。

输出参数

输出类包括新的运动学状态和整体轨迹。

new_position: DataArrayOrVec<f64, DOF>;
new_velocity: DataArrayOrVec<f64, DOF>;
new_acceleration: DataArrayOrVec<f64, DOF>;

trajectory: Trajectory; // The current trajectory
time: f64; // The current, auto-incremented time. Reset to 0 at a new calculation.

new_section: usize; // Index of the section between two (possibly filtered) intermediate positions.
did_section_change: bool; // Was a new section reached in the last cycle?

new_calculation: bool; // Whether a new calculation was performed in the last cycle
was_calculation_interrupted: bool; // Was the trajectory calculation interrupted? (only in Pro Version)
calculation_duration: f64; // Duration of the calculation in the last cycle [µs]

此外，轨迹结构体还有一系列有用的参数和方法。

duration: f64; // Duration of the trajectory
independent_min_durations: DataArrayOrVec<f64, DOF>; // Time-optimal profile for each independent DoF

<...> pub fn at_time(
        &self,
        time: f64,
        new_position: &mut Option<&mut DataArrayOrVec<f64, DOF>>,
        new_velocity: &mut Option<&mut DataArrayOrVec<f64, DOF>>,
        new_acceleration: &mut Option<&mut DataArrayOrVec<f64, DOF>>,
        new_jerk: &mut Option<&mut DataArrayOrVec<f64, DOF>>,
        new_section: &mut Option<usize>,
    ); // Get the kinematic state of the trajectory at a given time
<...> get_position_extrema(); // Returns information about the position extrema and their times

有关确切签名的详细信息，请参阅API文档。（仅限C++版本）

离线计算

Ruckig还支持离线方法来计算轨迹

result = ruckig.calculate(input, trajectory); // Returns  Result<RuckigResult, RuckigError>

当只使用此方法时，Ruckig 构造函数不需要控制周期（delta_time）作为参数。但是，如果提供了，Ruckig支持使用以下方式遍历轨迹

while (ruckig.update(&input, &mut output).unwrap() == RuckigResult::Working {
  // Make use of the new state here!
  // e.g. robot.setJointPositions(output.new_position);
}

测试和数值稳定性

当前的测试套件验证了超过5,000,000,000个随机轨迹以及许多其他边缘情况。测试数值精确度以确保最终位置和最终速度在1e-8内，最终加速度在1e-10内，速度、加速度和加加速度限制在1e-12的数值误差内。这些都是绝对值 - 我们建议调整您的输入，使其与系统的所需精度相对应。例如，对于大多数现实世界系统，我们建议使用[m]（例如，而不是例如[mm]），因为1e-8m对于实际轨迹生成足够精确。此外，所有运动学限制应低于1e12。最大支持的轨迹持续时间是7e3。请注意，Ruckig还会输出此范围之外的价值，但无法保证正确性。Ruckig Pro版本具有额外的工具来增加数值范围并提高可靠性。

开发

原始的Ruckig是用C++17编写的。它持续在以下版本上进行测试：ubuntu-latest，macos-latest，和windows-latest

Rust版本是原始C++版本的移植，不包括专业功能和云客户端。

Rust移植待办事项

• 添加更多测试
• 将RuckigError转换为Rust枚举
• 添加更多示例
• 添加更多文档
• 进一步优化性能

引用

@article{berscheid2021jerk,
  title={Jerk-limited Real-time Trajectory Generation with Arbitrary Target States},
  author={Berscheid, Lars and Kr{\"o}ger, Torsten},
  journal={Robotics: Science and Systems XVII},
  year={2021}
}