#[cxx::bridge]
mod ffi {
unsafe extern "C++" {
include!("rust.hpp");
type Trajectory;
type InputParameter;
type OutputParameter;
type Ruckig;
fn traj_new_direct(dofs: usize) -> UniquePtr<Trajectory>;
fn traj_new(dofs: usize, waypoints: usize) -> UniquePtr<Trajectory>;
fn traj_get_dofs(traj: &Trajectory) -> usize;
fn traj_get_duration(traj: &Trajectory) -> f64;
unsafe fn traj_at_time(traj: &Trajectory, time: f64, pos: *mut f64, vel: *mut f64, acc: *mut f64, len: usize);
fn input_new(dofs: usize) -> UniquePtr<InputParameter>;
fn input_get_control_interface(input: &InputParameter) -> i32;
fn input_set_control_interface(input: Pin<&mut InputParameter>, v: i32);
fn input_get_synchronization(input: &InputParameter) -> i32;
fn input_set_synchronization(input: Pin<&mut InputParameter>, v: i32);
unsafe fn input_set_current_position(input: Pin<&mut InputParameter>, data: *const f64, len: usize);
fn input_get_current_position(input: &InputParameter) -> &CxxVector<f64>;
unsafe fn input_set_current_velocity(input: Pin<&mut InputParameter>, data: *const f64, len: usize);
fn input_get_current_velocity(input: &InputParameter) -> &CxxVector<f64>;
unsafe fn input_set_current_acceleration(input: Pin<&mut InputParameter>, data: *const f64, len: usize);
fn input_get_current_acceleration(input: &InputParameter) -> &CxxVector<f64>;
unsafe fn input_set_target_position(input: Pin<&mut InputParameter>, data: *const f64, len: usize);
fn input_get_target_position(input: &InputParameter) -> &CxxVector<f64>;
unsafe fn input_set_target_velocity(input: Pin<&mut InputParameter>, data: *const f64, len: usize);
fn input_get_target_velocity(input: &InputParameter) -> &CxxVector<f64>;
unsafe fn input_set_target_acceleration(input: Pin<&mut InputParameter>, data: *const f64, len: usize);
fn input_get_target_acceleration(input: &InputParameter) -> &CxxVector<f64>;
unsafe fn input_set_max_velocity(input: Pin<&mut InputParameter>, data: *const f64, len: usize);
fn input_get_max_velocity(input: &InputParameter) -> &CxxVector<f64>;
unsafe fn input_set_max_acceleration(input: Pin<&mut InputParameter>, data: *const f64, len: usize);
fn input_get_max_acceleration(input: &InputParameter) -> &CxxVector<f64>;
unsafe fn input_set_max_jerk(input: Pin<&mut InputParameter>, data: *const f64, len: usize);
fn input_get_max_jerk(input: &InputParameter) -> &CxxVector<f64>;
unsafe fn input_set_intermediate_positions(input: Pin<&mut InputParameter>, data: *const f64, dofs: usize, count: usize);
fn input_clear_intermediate_positions(input: Pin<&mut InputParameter>);
unsafe fn input_get_intermediate_positions(input: &InputParameter, buf: *mut f64, capacity: usize) -> usize;
fn output_new_direct(dofs: usize) -> UniquePtr<OutputParameter>;
fn output_new(dofs: usize, waypoints: usize) -> UniquePtr<OutputParameter>;
fn output_get_trajectory(output: &OutputParameter) -> &Trajectory;
fn output_get_new_position(output: &OutputParameter) -> &CxxVector<f64>;
fn output_get_new_velocity(output: &OutputParameter) -> &CxxVector<f64>;
fn output_get_new_acceleration(output: &OutputParameter) -> &CxxVector<f64>;
fn output_get_time(output: &OutputParameter) -> f64;
fn output_get_new_section(output: &OutputParameter) -> usize;
fn output_get_new_calculation(output: &OutputParameter) -> bool;
fn output_get_calculation_duration(output: &OutputParameter) -> f64;
fn output_pass_to_input(output: &OutputParameter, input: Pin<&mut InputParameter>);
fn ruckig_new_direct_and_offline(dofs: usize) -> UniquePtr<Ruckig>;
fn ruckig_new_direct(dofs: usize, delta_time: f64) -> UniquePtr<Ruckig>;
fn ruckig_new_offline(dofs: usize, waypoints: usize) -> UniquePtr<Ruckig>;
fn ruckig_new(dofs: usize, delta_time: f64, waypoints: usize) -> UniquePtr<Ruckig>;
fn ruckig_update(ruckig: Pin<&mut Ruckig>, input: &InputParameter, output: Pin<&mut OutputParameter>) -> i32;
fn ruckig_calculate(ruckig: Pin<&mut Ruckig>, input: &InputParameter, trajectory: Pin<&mut Trajectory>) -> i32;
fn ruckig_reset(ruckig: Pin<&mut Ruckig>);
}
}
use cxx::UniquePtr;
pub struct Trajectory(UniquePtr<ffi::Trajectory>);
pub struct TrajectoryRef<'a> {
inner: &'a ffi::Trajectory,
}
fn traj_at_time_impl(inner: &ffi::Trajectory, time: f64) -> (Vec<f64>, Vec<f64>, Vec<f64>) {
let dofs = ffi::traj_get_dofs(inner);
let mut pos = vec![0.0f64; dofs];
let mut vel = vec![0.0f64; dofs];
let mut acc = vec![0.0f64; dofs];
unsafe {
ffi::traj_at_time(inner, time, pos.as_mut_ptr(), vel.as_mut_ptr(), acc.as_mut_ptr(), dofs);
}
(pos, vel, acc)
}
impl Trajectory {
pub fn new_direct(dofs: usize) -> Self {
Self(ffi::traj_new_direct(dofs))
}
pub fn new(dofs: usize, waypoints: usize) -> Self {
Self(ffi::traj_new(dofs, waypoints))
}
pub fn get_duration(&self) -> f64 {
ffi::traj_get_duration(&self.0)
}
pub fn at_time(&self, time: f64) -> (Vec<f64>, Vec<f64>, Vec<f64>) {
traj_at_time_impl(&self.0, time)
}
}
impl<'a> TrajectoryRef<'a> {
pub fn get_duration(&self) -> f64 {
ffi::traj_get_duration(self.inner)
}
pub fn at_time(&self, time: f64) -> (Vec<f64>, Vec<f64>, Vec<f64>) {
traj_at_time_impl(self.inner, time)
}
}
#[derive(Debug, Clone, Copy, PartialEq, Eq, Default)]
#[repr(i32)]
pub enum ControlInterface {
#[default]
Position = 0,
Velocity = 1,
}
#[derive(Debug, Clone, Copy, PartialEq, Eq, Default)]
#[repr(i32)]
pub enum Synchronization {
#[default]
Time = 0,
TimeIfNecessary = 1,
Phase = 2,
None = 3,
}
#[repr(i32)]
#[derive(Debug, PartialEq, Eq)]
pub enum Result {
Working = 0,
Finished = 1,
Error = -1,
ErrorInvalidInput = -100,
ErrorTrajectoryDuration = -101,
ErrorPositionalLimits = -102,
ErrorZeroLimits = -104,
ErrorExecutionTimeCalculation = -110,
ErrorSynchronizationCalculation = -111,
}
impl From<i32> for Result {
fn from(v: i32) -> Self {
match v {
0 => Result::Working,
1 => Result::Finished,
-100 => Result::ErrorInvalidInput,
-101 => Result::ErrorTrajectoryDuration,
-102 => Result::ErrorPositionalLimits,
-104 => Result::ErrorZeroLimits,
-110 => Result::ErrorExecutionTimeCalculation,
-111 => Result::ErrorSynchronizationCalculation,
_ => Result::Error,
}
}
}
pub struct InputParameter {
pub degrees_of_freedom: usize,
pub control_interface: ControlInterface,
pub synchronization: Synchronization,
pub current_position: Vec<f64>,
pub current_velocity: Vec<f64>,
pub current_acceleration: Vec<f64>,
pub target_position: Vec<f64>,
pub target_velocity: Vec<f64>,
pub target_acceleration: Vec<f64>,
pub max_velocity: Vec<f64>,
pub max_acceleration: Vec<f64>,
pub max_jerk: Vec<f64>,
pub intermediate_positions: Vec<Vec<f64>>,
inner: UniquePtr<ffi::InputParameter>,
}
impl InputParameter {
pub fn new(dofs: usize) -> Self {
Self {
degrees_of_freedom: dofs,
control_interface: ControlInterface::Position,
synchronization: Synchronization::Time,
current_position: vec![0.0; dofs],
current_velocity: vec![0.0; dofs],
current_acceleration: vec![0.0; dofs],
target_position: vec![0.0; dofs],
target_velocity: vec![0.0; dofs],
target_acceleration: vec![0.0; dofs],
max_velocity: vec![0.0; dofs],
max_acceleration: vec![0.0; dofs],
max_jerk: vec![0.0; dofs],
intermediate_positions: Vec::new(),
inner: ffi::input_new(dofs),
}
}
fn sync(&mut self) {
self.control_interface = unsafe { std::mem::transmute(ffi::input_get_control_interface(&self.inner)) };
self.synchronization = unsafe { std::mem::transmute(ffi::input_get_synchronization(&self.inner)) };
self.current_position = ffi::input_get_current_position(&self.inner).iter().cloned().collect();
self.current_velocity = ffi::input_get_current_velocity(&self.inner).iter().cloned().collect();
self.current_acceleration = ffi::input_get_current_acceleration(&self.inner).iter().cloned().collect();
self.target_position = ffi::input_get_target_position(&self.inner).iter().cloned().collect();
self.target_velocity = ffi::input_get_target_velocity(&self.inner).iter().cloned().collect();
self.target_acceleration = ffi::input_get_target_acceleration(&self.inner).iter().cloned().collect();
self.max_velocity = ffi::input_get_max_velocity(&self.inner).iter().cloned().collect();
self.max_acceleration = ffi::input_get_max_acceleration(&self.inner).iter().cloned().collect();
self.max_jerk = ffi::input_get_max_jerk(&self.inner).iter().cloned().collect();
let cap = self.degrees_of_freedom * 256;
let mut flat = vec![0.0f64; cap];
let written = unsafe { ffi::input_get_intermediate_positions(&self.inner, flat.as_mut_ptr(), cap) };
self.intermediate_positions = if self.degrees_of_freedom > 0 && written > 0 {
flat[..written].chunks(self.degrees_of_freedom).map(|c| c.to_vec()).collect()
} else {
Vec::new()
};
}
fn apply(&mut self) {
ffi::input_set_control_interface(self.inner.pin_mut(), self.control_interface as i32);
ffi::input_set_synchronization(self.inner.pin_mut(), self.synchronization as i32);
unsafe {
ffi::input_set_current_position(self.inner.pin_mut(), self.current_position.as_ptr(), self.current_position.len());
ffi::input_set_current_velocity(self.inner.pin_mut(), self.current_velocity.as_ptr(), self.current_velocity.len());
ffi::input_set_current_acceleration(self.inner.pin_mut(), self.current_acceleration.as_ptr(), self.current_acceleration.len());
ffi::input_set_target_position(self.inner.pin_mut(), self.target_position.as_ptr(), self.target_position.len());
ffi::input_set_target_velocity(self.inner.pin_mut(), self.target_velocity.as_ptr(), self.target_velocity.len());
ffi::input_set_target_acceleration(self.inner.pin_mut(), self.target_acceleration.as_ptr(), self.target_acceleration.len());
ffi::input_set_max_velocity(self.inner.pin_mut(), self.max_velocity.as_ptr(), self.max_velocity.len());
ffi::input_set_max_acceleration(self.inner.pin_mut(), self.max_acceleration.as_ptr(), self.max_acceleration.len());
ffi::input_set_max_jerk(self.inner.pin_mut(), self.max_jerk.as_ptr(), self.max_jerk.len());
}
if self.intermediate_positions.is_empty() {
ffi::input_clear_intermediate_positions(self.inner.pin_mut());
} else {
let dofs = self.intermediate_positions[0].len();
let flat: Vec<f64> = self.intermediate_positions.iter().flat_map(|r| r.iter().cloned()).collect();
unsafe { ffi::input_set_intermediate_positions(self.inner.pin_mut(), flat.as_ptr(), dofs, self.intermediate_positions.len()); }
}
}
}
pub struct OutputParameter {
pub degrees_of_freedom: usize,
pub new_position: Vec<f64>,
pub new_velocity: Vec<f64>,
pub new_acceleration: Vec<f64>,
pub time: f64,
pub new_section: usize,
pub new_calculation: bool,
pub calculation_duration: f64,
inner: UniquePtr<ffi::OutputParameter>,
}
impl OutputParameter {
pub fn new_direct(dofs: usize) -> Self {
Self {
degrees_of_freedom: dofs,
new_position: vec![0.0; dofs],
new_velocity: vec![0.0; dofs],
new_acceleration: vec![0.0; dofs],
time: 0.0,
new_section: 0,
new_calculation: false,
calculation_duration: 0.0,
inner: ffi::output_new_direct(dofs),
}
}
pub fn new(dofs: usize, waypoints: usize) -> Self {
Self {
degrees_of_freedom: dofs,
new_position: vec![0.0; dofs],
new_velocity: vec![0.0; dofs],
new_acceleration: vec![0.0; dofs],
time: 0.0,
new_section: 0,
new_calculation: false,
calculation_duration: 0.0,
inner: ffi::output_new(dofs, waypoints),
}
}
pub fn pass_to_input(&self, input: &mut InputParameter) {
ffi::output_pass_to_input(&self.inner, input.inner.pin_mut());
input.sync();
}
fn sync(&mut self) {
self.new_position = ffi::output_get_new_position(&self.inner).iter().cloned().collect();
self.new_velocity = ffi::output_get_new_velocity(&self.inner).iter().cloned().collect();
self.new_acceleration = ffi::output_get_new_acceleration(&self.inner).iter().cloned().collect();
self.time = ffi::output_get_time(&self.inner);
self.new_section = ffi::output_get_new_section(&self.inner);
self.new_calculation = ffi::output_get_new_calculation(&self.inner);
self.calculation_duration = ffi::output_get_calculation_duration(&self.inner);
}
pub fn trajectory(&self) -> TrajectoryRef<'_> {
TrajectoryRef { inner: ffi::output_get_trajectory(&self.inner) }
}
}
pub struct Ruckig(UniquePtr<ffi::Ruckig>);
impl Ruckig {
pub fn new_direct_and_offline(dofs: usize) -> Self {
Self(ffi::ruckig_new_direct_and_offline(dofs))
}
pub fn new_direct(dofs: usize, delta_time: f64) -> Self {
Self(ffi::ruckig_new_direct(dofs, delta_time))
}
pub fn new_offline(dofs: usize, waypoints: usize) -> Self {
Self(ffi::ruckig_new_offline(dofs, waypoints))
}
pub fn new(dofs: usize, delta_time: f64, waypoints: usize) -> Self {
Self(ffi::ruckig_new(dofs, delta_time, waypoints))
}
pub fn reset(&mut self) {
ffi::ruckig_reset(self.0.pin_mut());
}
pub fn update(&mut self, input: &mut InputParameter, output: &mut OutputParameter) -> Result {
input.apply();
let raw = ffi::ruckig_update(self.0.pin_mut(), &input.inner, output.inner.pin_mut());
output.sync();
Result::from(raw)
}
pub fn calculate(&mut self, input: &mut InputParameter, trajectory: &mut Trajectory) -> Result {
input.apply();
let raw = ffi::ruckig_calculate(self.0.pin_mut(), &input.inner, trajectory.0.pin_mut());
Result::from(raw)
}
}