initial commit

.gitignore

@@ -0,0 +1,6 @@
+.vscode
+
+build
+reflexxes
+
+local.sh

CMakeLists.txt

@@ -0,0 +1,68 @@
+cmake_minimum_required(VERSION 3.11)
+
+
+project(ruckig VERSION 0.1.0 LANGUAGES CXX)
+list(INSERT CMAKE_MODULE_PATH 0 ${CMAKE_SOURCE_DIR}/cmake)
+
+
+option(BUILD_PYTHON_MODULE "Build python module" ON)
+option(BUILD_TESTS "Build tests" ON)
+
+
+find_package(Eigen3 3.3.7 REQUIRED NO_MODULE)
+find_package(Reflexxes)
+
+message("Found Eigen Version: ${Eigen3_VERSION}")
+
+if(Reflexxes)
+  set(REFLEXXES_TYPE "ReflexxesTypeII" CACHE STRING "Type of Reflexxes library") # or ReflexxesTypeIV
+
+  message("Found Reflexxes ${REFLEXXES_TYPE}")
+else()
+  message("Did not found Reflexxes.")
+endif()
+
+
+add_library(ruckig SHARED
+  src/ruckig.cpp
+)
+target_compile_features(ruckig PUBLIC cxx_std_17)
+target_include_directories(ruckig PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include)
+target_link_libraries(ruckig PUBLIC Eigen3::Eigen)
+
+
+if(Reflexxes)
+  target_compile_definitions(ruckig PUBLIC WITH_REFLEXXES)
+  target_link_libraries(ruckig PUBLIC Reflexxes::Reflexxes)
+endif()
+
+
+if(BUILD_PYTHON_MODULE)
+  # Check if pybind11 exists as a subdirectory
+  if(EXISTS pybind11)
+    add_subdirectory(pybind11)
+  else()
+    find_package(Python COMPONENTS Interpreter Development)
+    find_package(pybind11 2.6 REQUIRED)
+  endif()
+
+  pybind11_add_module(_ruckig src/python.cpp)
+  target_compile_features(_ruckig PUBLIC cxx_std_17)
+  target_link_libraries(_ruckig PUBLIC ruckig)
+endif()
+
+
+if(BUILD_TESTS)
+  enable_testing()
+
+  find_package(Catch2 REQUIRED)
+
+  foreach(test IN ITEMS otg-test)
+    add_executable(${test} "test/${test}.cpp")
+    if(Reflexxes)
+      target_compile_definitions(${test} PUBLIC WITH_REFLEXXES)
+    endif()
+    target_link_libraries(${test} PRIVATE ruckig Catch2::Catch2)
+    add_test(NAME ${test} COMMAND ${test})
+  endforeach()
+endif()

README.md

@@ -0,0 +1,3 @@
+# Ruckig
+
+Online Trajectory Generation. Real-time. Jerk-constrained.

cmake/FindReflexxes.cmake

@@ -0,0 +1,12 @@
+if(NOT TARGET Reflexxes::Reflexxes)
+  if(NOT ${Reflexxes_ROOT_DIR} STREQUAL "")
+    set(Reflexxes_LIB_DIR "${Reflexxes_ROOT_DIR}/MacOS/x64/release/lib/")
+    set(Reflexxes_INCLUDE_DIR "${Reflexxes_ROOT_DIR}/include")
+  endif()
+
+  find_library(Reflexxes ${REFLEXXES_TYPE} PATHS ${Reflexxes_LIB_DIR})
+
+  add_library(Reflexxes::Reflexxes INTERFACE IMPORTED)
+  target_include_directories(Reflexxes::Reflexxes INTERFACE ${Reflexxes_INCLUDE_DIR})
+  target_link_libraries(Reflexxes::Reflexxes INTERFACE ${Reflexxes})
+endif()

include/ruckig/parameter.hpp

@@ -0,0 +1,112 @@
+#pragma once
+
+#include <optional>
+
+#include <Eigen/Core>
+
+
+namespace ruckig {
+
+enum class Result {
+    Working,
+    Finished,
+    Error
+};
+
+
+template<size_t DOFs>
+struct InputParameter {
+    using Vector = Eigen::Matrix<double, DOFs, 1, Eigen::ColMajor>;
+    static constexpr size_t degrees_of_freedom {DOFs};
+
+    enum class Type {
+        Position,
+        Velocity,
+    } type {Type::Position};
+
+    Vector current_position;
+    Vector current_velocity {Vector::Zero()};
+    Vector current_acceleration {Vector::Zero()};
+
+    Vector target_position;
+    Vector target_velocity {Vector::Zero()};
+    Vector target_acceleration {Vector::Zero()};
+
+    Vector max_velocity;
+    Vector max_acceleration;
+    Vector max_jerk;
+
+    std::array<bool, DOFs> enabled;
+    std::optional<double> minimum_duration;
+
+    InputParameter() {
+        enabled.fill(true);
+    }
+
+    bool operator!=(const InputParameter<DOFs>& rhs) const {
+        return (
+            current_position != rhs.current_position
+            || current_velocity != rhs.current_velocity
+            || current_acceleration != rhs.current_acceleration
+            || target_position != rhs.target_position
+            || target_velocity != rhs.target_velocity
+            || target_acceleration != rhs.target_acceleration
+            || max_velocity != rhs.max_velocity
+            || max_acceleration != rhs.max_acceleration
+            || max_jerk != rhs.max_jerk
+            || enabled != rhs.enabled
+            || minimum_duration != rhs.minimum_duration
+            || type != rhs.type
+        );
+    }
+
+    std::string to_string(size_t dof) const {
+        std::stringstream ss;
+        ss << "p0: " << current_position[dof] << ", ";
+        ss << "v0: " << current_velocity[dof] << ", ";
+        ss << "a0: " << current_acceleration[dof] << ", ";
+        ss << "pf: " << target_position[dof] << ", ";
+        ss << "vf: " << target_velocity[dof] << ", ";
+        ss << "vMax: " << max_velocity[dof] << ", ";
+        ss << "aMax: " << max_acceleration[dof] << ", ";
+        ss << "jMax: " << max_jerk[dof];
+        return ss.str();
+    }
+
+    std::string to_string() const {
+        Eigen::IOFormat formatter(10, 0, ", ", "\n", "[", "]");
+
+        std::stringstream ss;
+        ss << "\ninp.current_position = " << current_position.transpose().format(formatter) << "\n";
+        ss << "inp.current_velocity = " << current_velocity.transpose().format(formatter) << "\n";
+        ss << "inp.current_acceleration = " << current_acceleration.transpose().format(formatter) << "\n";
+        ss << "inp.target_position = " << target_position.transpose().format(formatter) << "\n";
+        ss << "inp.target_velocity = " << target_velocity.transpose().format(formatter) << "\n";
+        ss << "inp.target_acceleration = " << target_acceleration.transpose().format(formatter) << "\n";
+        ss << "inp.max_velocity = " << max_velocity.transpose().format(formatter) << "\n";
+        ss << "inp.max_acceleration = " << max_acceleration.transpose().format(formatter) << "\n";
+        ss << "inp.max_jerk = " << max_jerk.transpose().format(formatter) << "\n";
+        return ss.str();
+    }
+};
+
+
+template<size_t DOFs>
+struct OutputParameter {
+    using Vector = Eigen::Matrix<double, DOFs, 1, Eigen::ColMajor>;
+    static constexpr size_t degrees_of_freedom {DOFs};
+
+    Vector new_position;
+    Vector new_velocity;
+    Vector new_acceleration;
+
+    double duration; // [s]
+    bool new_calculation {false};
+    double calculation_duration; // [µs]
+
+    // Vector independent_min_durations;
+    // Vector min_positions, max_positions;
+    // Vector time_min_positions, time_max_positions;
+};
+
+} // namespace ruckig

include/ruckig/quintic.hpp

@@ -0,0 +1,91 @@
+#pragma once
+
+#include <Eigen/Core>
+
+#include <ruckig/parameter.hpp>
+
+
+namespace ruckig {
+
+template<size_t DOFs>
+class Quintic {
+    using Vector = Eigen::Matrix<double, DOFs, 1, Eigen::ColMajor>;
+
+    // Trajectory
+    Vector a, b, c, d, e, f;
+    double t, tf;
+    InputParameter<DOFs> current_input;
+
+    bool calculate(const InputParameter<DOFs>& input, OutputParameter<DOFs>& output) {
+        current_input = input;
+
+        const Vector& x0 = input.current_position;
+        const Vector& v0 = input.current_velocity;
+        const Vector& a0 = input.current_acceleration;
+        const Vector& xf = input.target_position;
+        const Vector& vf = input.target_velocity;
+        const Vector& af = input.target_acceleration;
+        const Vector& v_max = input.max_velocity;
+        const Vector& a_max = input.max_acceleration;
+        const Vector& j_max = input.max_jerk;
+
+        // Check input
+        if ((v_max.array() <= 0.0).any() || (a_max.array() <= 0.0).any() || (j_max.array() <= 0.0).any()) {
+            return false;
+        }
+
+        // Approximations for v0 == 0, vf == 0, a0 == 0, af == 0
+        Vector v_max_tfs = (15 * (x0 - xf).array().abs()) / (8 * v_max).array();
+        Vector a_max_tfs = (std::sqrt(10) * (x0.array().pow(2) - 2 * x0.array() * xf.array() + xf.array().pow(2)).pow(1./4)) / (std::pow(3, 1./4) * a_max.array().sqrt());
+        Vector j_max_tfs = ((60 * (x0 - xf).array().abs()) / j_max.array()).pow(1./3); // Also solvable for v0 != 0
+
+        tf = std::max<double>({v_max_tfs.maxCoeff(), a_max_tfs.maxCoeff(), j_max_tfs.maxCoeff()});
+        if (input.minimum_duration.has_value()) {
+            tf = std::max<double>({tf, input.minimum_duration.value()});
+        }
+
+        a = -((a0 - af) * std::pow(tf, 2) + 6 * tf * (v0 + vf) + 12 * (x0 - xf)) / (2 * std::pow(tf, 5));
+        b = -((2 * af - 3 * a0) * std::pow(tf, 2) - 16 * tf * v0 - 14 * tf * vf - 30 * (x0 - xf)) / (2 * std::pow(tf, 4));
+        c = -((3 * a0 - af) * std::pow(tf, 2) + 12 * tf * v0 + 8 * tf * vf + 20 * (x0 - xf)) / (2 * std::pow(tf, 3));
+        d = a0 / 2;
+        e = v0;
+        f = x0;
+
+        t = 0.0;
+        output.duration = tf;
+        output.new_calculation = true;
+        return true;
+    }
+
+public:
+    double delta_time;
+
+    explicit Quintic(double delta_time): delta_time(delta_time) { }
+
+    Result update(const InputParameter<DOFs>& input, OutputParameter<DOFs>& output) {
+        t += delta_time;
+        output.new_calculation = false;
+
+        if (input != current_input && !calculate(input, output)) {
+            return Result::Error;
+        }
+
+        if (t >= tf) {
+            output.new_position = input.target_position;
+            output.new_velocity = input.target_velocity;
+            output.new_acceleration = input.target_acceleration;
+            return Result::Finished;
+        }
+
+        output.new_position = f + t * (e + t * (d + t * (c + t * (b + a * t))));
+        output.new_velocity = e + t * (2 * d + t * (3 * c + t * (4 * b + 5 * a * t)));
+        output.new_acceleration = 2 * d + t * (6 * c + t * (12 * b + t * (20 * a)));
+
+        current_input.current_position = output.new_position;
+        current_input.current_velocity = output.new_velocity;
+        current_input.current_acceleration = output.new_acceleration;
+        return Result::Working;
+    }
+};
+
+} // namespace ruckig