split parameter files, calculate in trajectory

examples/position.cpp

@@ -1,6 +1,5 @@
 #include <iostream>
 
-#include <ruckig/parameter.hpp>
 #include <ruckig/ruckig.hpp>
 
 

include/ruckig/alternative/quintic.hpp

@@ -1,6 +1,7 @@
 #pragma once
 
-#include <ruckig/parameter.hpp>
+#include <ruckig/input_parameter.hpp>
+#include <ruckig/output_parameter.hpp>
 
 
 namespace ruckig {

include/ruckig/alternative/reflexxes.hpp

@@ -8,7 +8,8 @@
 #include <RMLPositionInputParameters.h>
 #include <RMLPositionOutputParameters.h>
 
-#include <ruckig/parameter.hpp>
+#include <ruckig/input_parameter.hpp>
+#include <ruckig/output_parameter.hpp>
 
 
 namespace ruckig {
@@ -55,7 +56,7 @@ class Reflexxes {
             }
 
             switch (input.interface) {
-            case InputParameter<DOFs>::Interface::Position: {
+            case Interface::Position: {
                 if (input.minimum_duration) {
                     input_parameters->SetMinimumSynchronizationTime(input.minimum_duration.value());
                 }
@@ -70,7 +71,7 @@ class Reflexxes {
                 input_parameters->SetMaxAccelerationVector(input.max_acceleration.data());
                 input_parameters->SetMaxJerkVector(input.max_jerk.data());
             } break;
-            case InputParameter<DOFs>::Interface::Velocity: {
+            case Interface::Velocity: {
                 if (input.minimum_duration) {
                     input_vel_parameters->SetMinimumSynchronizationTime(input.minimum_duration.value());
                 }
@@ -89,7 +90,7 @@ class Reflexxes {
         auto start = std::chrono::high_resolution_clock::now();
 
         switch (input.interface) {
-        case InputParameter<DOFs>::Interface::Position: {
+        case Interface::Position: {
             result_value = rml->RMLPosition(*input_parameters, output_parameters.get(), flags);
 
             for (size_t dof = 0; dof < DOFs; ++dof) {
@@ -100,7 +101,7 @@ class Reflexxes {
             output.trajectory.duration = output_parameters->GetSynchronizationTime();
             output.new_calculation = output_parameters->WasACompleteComputationPerformedDuringTheLastCycle();
         } break;
-        case InputParameter<DOFs>::Interface::Velocity: {
+        case Interface::Velocity: {
             result_value = rml->RMLVelocity(*input_vel_parameters, output_vel_parameters.get(), vel_flags);
 
             for (size_t dof = 0; dof < DOFs; ++dof) {
@@ -126,7 +127,7 @@ class Reflexxes {
 
     void at_time(double time, OutputParameter<DOFs>& output) {
         switch (current_input.interface) {
-        case InputParameter<DOFs>::Interface::Position: {
+        case Interface::Position: {
             rml->RMLPositionAtAGivenSampleTime(time, output_parameters.get());
 
             for (size_t dof = 0; dof < DOFs; dof += 1) {
@@ -135,7 +136,7 @@ class Reflexxes {
                 output.new_acceleration[dof] = output_parameters->NewAccelerationVector->VecData[dof];
             }
         } break;
-        case InputParameter<DOFs>::Interface::Velocity: {
+        case Interface::Velocity: {
             rml->RMLVelocityAtAGivenSampleTime(time, output_vel_parameters.get());
 
             for (size_t dof = 0; dof < DOFs; dof += 1) {

include/ruckig/alternative/smoothie.hpp

@@ -2,7 +2,8 @@
 
 #include <chrono>
 
-#include <ruckig/parameter.hpp>
+#include <ruckig/input_parameter.hpp>
+#include <ruckig/output_parameter.hpp>
 
 
 namespace ruckig {

include/ruckig/block.hpp

@@ -92,6 +92,67 @@ struct Block {
 
         return false;
     }
+
+    template<size_t DOFs>
+    static bool synchronize(const std::array<Block, DOFs>& blocks, std::optional<double> t_min, double& t_sync, int& limiting_dof, std::array<Profile, DOFs>& profiles, bool discrete_duration, double delta_time) {
+        if (DOFs == 1 && !t_min && !discrete_duration) {
+            limiting_dof = 0;
+            t_sync = blocks[0].t_min;
+            profiles[0] = blocks[0].p_min;
+            return true;
+        }
+
+        // Possible t_syncs are the start times of the intervals and optional t_min
+        std::array<double, 3*DOFs+1> possible_t_syncs;
+        std::array<int, 3*DOFs+1> idx;
+        for (size_t dof = 0; dof < DOFs; ++dof) {
+            possible_t_syncs[3 * dof] = blocks[dof].t_min;
+            possible_t_syncs[3 * dof + 1] = blocks[dof].a ? blocks[dof].a->right : std::numeric_limits<double>::infinity();
+            possible_t_syncs[3 * dof + 2] = blocks[dof].b ? blocks[dof].b->right : std::numeric_limits<double>::infinity();
+        }
+        possible_t_syncs[3 * DOFs] = t_min.value_or(std::numeric_limits<double>::infinity());
+
+        if (discrete_duration) {
+            for (size_t i = 0; i < 3*DOFs+1; ++i) {
+                possible_t_syncs[i] = std::ceil(possible_t_syncs[i] / delta_time) * delta_time;
+            }
+        }
+
+        // Test them in sorted order
+        std::iota(idx.begin(), idx.end(), 0);
+        std::sort(idx.begin(), idx.end(), [&possible_t_syncs](size_t i, size_t j) { return possible_t_syncs[i] < possible_t_syncs[j]; });
+
+        // Start at last tmin (or worse)
+        for (auto i = idx.begin() + DOFs - 1; i != idx.end(); ++i) {
+            const double possible_t_sync = possible_t_syncs[*i];
+            if (std::any_of(blocks.begin(), blocks.end(), [possible_t_sync](auto block){ return block.is_blocked(possible_t_sync); }) || possible_t_sync < t_min.value_or(0.0)) {
+                continue;
+            }
+
+            t_sync = possible_t_sync;
+            if (*i == 3*DOFs) { // Optional t_min
+                limiting_dof = -1;
+                return true;
+            }
+
+            const auto div = std::div(*i, 3);
+            limiting_dof = div.quot;
+            switch (div.rem) {
+                case 0: {
+                    profiles[limiting_dof] = blocks[limiting_dof].p_min;
+                } break;
+                case 1: {
+                    profiles[limiting_dof] = blocks[limiting_dof].a->profile;
+                } break;
+                case 2: {
+                    profiles[limiting_dof] = blocks[limiting_dof].b->profile;
+                } break;
+            }
+            return true;
+        }
+
+        return false;
+    }
 };
 
 } // namespace ruckig

include/ruckig/parameter.hpp → include/ruckig/input_parameter.hpp

@@ -5,20 +5,35 @@
 #include <optional>
 #include <sstream>
 
-#include <ruckig/trajectory.hpp>
-
 
 namespace ruckig {
 
 //! Result type of the OTGs update function
 enum Result {
-    Working = 0,
-    Finished = 1,
-    Error = -1,
-    ErrorInvalidInput = -100,
-    ErrorTrajectoryDuration = -101,
-    ErrorExecutionTimeCalculation = -110,
-    ErrorSynchronizationCalculation = -111,
+    Working = 0, ///< The trajectory is calculated normally
+    Finished = 1, ///< Trajectory has reached its final position
+    Error = -1, ///< Unclassified error
+    ErrorInvalidInput = -100, ///< Error in the input parameter
+    ErrorTrajectoryDuration = -101, ///< The trajectory duration exceed the numeral limits
+    ErrorExecutionTimeCalculation = -110, ///< Error during the extremel time calculation (Step 1)
+    ErrorSynchronizationCalculation = -111, ///< Error during the synchronization calculation (Step 2)
+};
+
+
+enum class Interface {
+    Position, ///< Position-control: Full control over the entire kinematic state (Default)
+    Velocity, ///< Velocity-control: Ignores the current position, target position, and velocity limits
+};
+
+enum class Synchronization {
+    Time, ///< Always synchronize the DoFs to reach the target at the same time (Default)
+    TimeIfNecessary, ///< Synchronize only when necessary (e.g. for non-zero target velocity or acceleration)
+    None, ///< Calculate every DoF independently
+};
+
+enum class DurationDiscretization {
+    Continuous, ///< Every trajectory duration is allowed (Default)
+    Discrete, ///< The trajectory duration must be a multiple of the control cycle
 };
 
 
@@ -38,22 +53,10 @@ class InputParameter {
 public:
     using Vector = std::array<double, DOFs>;
     static constexpr size_t degrees_of_freedom {DOFs};
-
-    enum class Interface {
-        Position, ///< Position-control: Full control over the entire kinematic state (Default)
-        Velocity, ///< Velocity-control: Ignores the current position, target position, and velocity limits
-    } interface {Interface::Position};
-
-    enum class Synchronization {
-        Time, ///< Always synchronize the DoFs to reach the target at the same time (Default)
-        TimeIfNecessary, ///< Synchronize only when necessary (e.g. for non-zero target velocity or acceleration)
-        None, ///< Calculate every DoF independently
-    } synchronization {Synchronization::Time};
-
-    enum class DurationDiscretization {
-        Continuous, ///< Every trajectory duration is allowed (Default)
-        Discrete, ///< The trajectory duration must be a multiple of the control cycle
-    } duration_discretization {DurationDiscretization::Continuous};
+
+    Interface interface {Interface::Position};
+    Synchronization synchronization {Synchronization::Time};
+    DurationDiscretization duration_discretization {DurationDiscretization::Continuous};
 
     Vector current_position, current_velocity {}, current_acceleration {};
     Vector target_position, target_velocity {}, target_acceleration {};
@@ -109,22 +112,4 @@ class InputParameter {
     }
 };
 
-
-//! Output type of the OTG
-template<size_t DOFs>
-struct OutputParameter {
-    static constexpr size_t degrees_of_freedom {DOFs};
-
-    std::array<double, DOFs> new_position, new_velocity, new_acceleration;
-
-    bool new_calculation {false};
-    double calculation_duration; // [µs]
-
-    // Current trajectory
-    Trajectory<DOFs> trajectory;
-
-    // Current time on trajectory
-    double time;
-};
-
 } // namespace ruckig

include/ruckig/output_parameter.hpp

@@ -0,0 +1,27 @@
+#pragma once
+
+#include <array>
+
+#include <ruckig/trajectory.hpp>
+
+
+namespace ruckig {
+
+//! Output type of the OTG
+template<size_t DOFs>
+struct OutputParameter {
+    static constexpr size_t degrees_of_freedom {DOFs};
+
+    std::array<double, DOFs> new_position, new_velocity, new_acceleration;
+
+    bool new_calculation {false};
+    double calculation_duration; // [µs]
+
+    // Current trajectory
+    Trajectory<DOFs> trajectory;
+
+    // Current time on trajectory
+    double time;
+};
+
+} // namespace ruckig