per dof control interface and synchronization #53

README.md

@@ -139,6 +139,9 @@ std::optional<double> interrupt_calculation_duration; // [µs], only in Ruckig P
 ControlInterface control_interface; // The default position interface controls the full kinematic state.
 Synchronization synchronization; // Synchronization behavior of multiple DoFs
 DurationDiscretization duration_discretization; // Whether the duration should be a discrete multiple of the control cycle (off by default)
+
+std::optional<Vector<ControlInterface>> per_dof_control_interface; // Sets the control interface for each DoF individually, overwrites global control_interface
+std::optional<Vector<Synchronization>> per_dof_synchronization; // Sets the synchronization for each DoF individually, overwrites global synchronization
 ```
 
 On top of the current state, target state, and constraints, Ruckig allows for a few more advanced settings:

include/ruckig/input_parameter.hpp

@@ -35,8 +35,8 @@ enum class Synchronization {
 };
 
 enum class DurationDiscretization {
-    Continuous, ///< Every trajectory duration is allowed (Default)
-    Discrete, ///< The trajectory duration must be a multiple of the control cycle
+    Continuous, ///< Every trajectory synchronization duration is allowed (Default)
+    Discrete, ///< The trajectory synchronization duration must be a multiple of the control cycle
 };
 
 
@@ -85,11 +85,11 @@ class InputParameter {
     //! Is the DoF considered for calculation?
     Vector<bool> enabled;
 
-    //! Per-DoF control_interface (overwrites global synchronization, not yet used)
-    // std::optional<Vector<ControlInterface>> per_dof_control_interface;
+    //! Per-DoF control_interface (overwrites global control_interface)
+    std::optional<Vector<ControlInterface>> per_dof_control_interface;
 
-    //! Per-DoF synchronization (overwrites global synchronization, not yet used)
-    // std::optional<Vector<Synchronization>> per_dof_synchronization;
+    //! Per-DoF synchronization (overwrites global synchronization)
+    std::optional<Vector<Synchronization>> per_dof_synchronization;
 
     //! Optional minimum trajectory duration
     std::optional<double> minimum_duration;
@@ -137,6 +137,8 @@ class InputParameter {
             || control_interface != rhs.control_interface
             || synchronization != rhs.synchronization
             || duration_discretization != rhs.duration_discretization
+            || per_dof_control_interface != rhs.per_dof_control_interface
+            || per_dof_synchronization != rhs.per_dof_synchronization
         );
     }
 

include/ruckig/trajectory.hpp

@@ -47,6 +47,9 @@ class Trajectory {
     Vector<double> p0s, v0s, a0s; // Starting point of profiles without brake trajectory
     Vector<double> inp_min_velocity, inp_min_acceleration;
 
+    Vector<ControlInterface> inp_per_dof_control_interface;
+    Vector<Synchronization> inp_per_dof_synchronization;
+
     Vector<double> new_max_velocity, new_min_velocity, new_max_acceleration, new_min_acceleration, new_max_jerk; // For phase synchronization
 
     Vector<PositionExtrema> position_extrema;
@@ -55,20 +58,21 @@ class Trajectory {
     bool is_phase_synchronizable(
         const InputParameter<DOFs>& inp,
         const Vector<double>& vMax, const Vector<double>& vMin,
-        const Vector<double>& aMax, const Vector<double>& aMin,
-        const Vector<double>& jMax,
-        Profile::Direction limiting_direction,
-        size_t limiting_dof,
+        const Vector<double>& aMax, const Vector<double>& aMin, const Vector<double>& jMax,
+        Profile::Direction limiting_direction, size_t limiting_dof,
         Vector<double>& new_max_velocity, Vector<double>& new_min_velocity,
-        Vector<double>& new_max_acceleration, Vector<double>& new_min_acceleration,
-        Vector<double>& new_max_jerk
+        Vector<double>& new_max_acceleration, Vector<double>& new_min_acceleration, Vector<double>& new_max_jerk
     ) {
         using Direction = Profile::Direction;
 
         // Get scaling factor of first DoF
         bool pd_found_nonzero {false};
         double v0_scale, a0_scale, vf_scale, af_scale;
         for (size_t dof = 0; dof < pd.size(); ++dof) {
+            if (inp_per_dof_synchronization[dof] != Synchronization::Phase) {
+                continue;
+            }
+
             pd[dof] = inp.target_position[dof] - inp.current_position[dof];
 
             if (!pd_found_nonzero && std::abs(pd[dof]) > eps) {
@@ -84,19 +88,20 @@ class Trajectory {
             return false;
         }
 
-        const double max_jerk_limiting = (limiting_direction == Direction::UP) ? jMax[limiting_dof] : -jMax[limiting_dof];
-        const double max_vel_limiting = (limiting_direction == Direction::UP) ? vMax[limiting_dof] : vMin[limiting_dof];
-        const double min_vel_limiting = (limiting_direction == Direction::UP) ? vMin[limiting_dof] : vMax[limiting_dof];
-        const double max_acc_limiting = (limiting_direction == Direction::UP) ? aMax[limiting_dof] : aMin[limiting_dof];
-        const double min_acc_limiting = (limiting_direction == Direction::UP) ? aMin[limiting_dof] : aMax[limiting_dof];
-
+        const bool is_direction_up = (limiting_direction == Direction::UP);
+        const double max_vel_limiting = is_direction_up ? vMax[limiting_dof] : vMin[limiting_dof];
+        const double min_vel_limiting = is_direction_up ? vMin[limiting_dof] : vMax[limiting_dof];
+        const double max_acc_limiting = is_direction_up ? aMax[limiting_dof] : aMin[limiting_dof];
+        const double min_acc_limiting = is_direction_up ? aMin[limiting_dof] : aMax[limiting_dof];
+        const double max_jerk_limiting = is_direction_up ? jMax[limiting_dof] : -jMax[limiting_dof];
+
         for (size_t dof = 0; dof < pd.size(); ++dof) {
-            if (dof == limiting_dof) {
+            if (dof == limiting_dof || inp_per_dof_synchronization[dof] != Synchronization::Phase) {
                 continue;
             }
 
             const double scale = pd[dof] / pd[limiting_dof];
-            const double eps_colinear {10 * eps};
+            constexpr double eps_colinear {10 * eps};
 
             // Are the vectors colinear?
             if (
@@ -110,12 +115,12 @@ class Trajectory {
             }
 
             // Are the old kinematic limits met?
-            const Direction new_direction = ((limiting_direction == Direction::UP && scale >= 0.0) || (limiting_direction == Direction::DOWN && scale <= 0.0)) ? Direction::UP : Direction::DOWN;
-            const double old_max_jerk = (new_direction == Direction::UP) ? jMax[dof] : -jMax[dof];
-            const double old_max_vel = (new_direction == Direction::UP) ? vMax[dof] : vMin[dof];
-            const double old_min_vel = (new_direction == Direction::UP) ? vMin[dof] : vMax[dof];
-            const double old_max_acc = (new_direction == Direction::UP) ? aMax[dof] : aMin[dof];
-            const double old_min_acc = (new_direction == Direction::UP) ? aMin[dof] : aMax[dof];
+            const bool is_new_direction_up = ((limiting_direction == Direction::UP && scale >= 0.0) || (limiting_direction == Direction::DOWN && scale <= 0.0));
+            const double old_max_vel = is_new_direction_up ? vMax[dof] : vMin[dof];
+            const double old_min_vel = is_new_direction_up ? vMin[dof] : vMax[dof];
+            const double old_max_acc = is_new_direction_up ? aMax[dof] : aMin[dof];
+            const double old_min_acc = is_new_direction_up ? aMin[dof] : aMax[dof];
+            const double old_max_jerk = is_new_direction_up ? jMax[dof] : -jMax[dof];
 
             new_max_velocity[dof] = scale * max_vel_limiting;
             new_min_velocity[dof] = scale * min_vel_limiting;
@@ -213,6 +218,8 @@ class Trajectory {
         a0s.resize(dofs);
         inp_min_velocity.resize(dofs);
         inp_min_acceleration.resize(dofs);
+        inp_per_dof_control_interface.resize(dofs);
+        inp_per_dof_synchronization.resize(dofs);
         profiles.resize(dofs);
         independent_min_durations.resize(dofs);
         pd.resize(dofs);
@@ -247,9 +254,11 @@ class Trajectory {
 
             inp_min_velocity[dof] = inp.min_velocity ? inp.min_velocity.value()[dof] : -inp.max_velocity[dof];
             inp_min_acceleration[dof] = inp.min_acceleration ? inp.min_acceleration.value()[dof] : -inp.max_acceleration[dof];
+            inp_per_dof_control_interface[dof] = inp.per_dof_control_interface ? inp.per_dof_control_interface.value()[dof] : inp.control_interface;
+            inp_per_dof_synchronization[dof] = inp.per_dof_synchronization ? inp.per_dof_synchronization.value()[dof] : inp.synchronization;
 
             // Calculate brake (if input exceeds or will exceed limits)
-            switch (inp.control_interface) {
+            switch (inp_per_dof_control_interface[dof]) {
                 case ControlInterface::Position: {
                     Brake::get_position_brake_trajectory(inp.current_velocity[dof], inp.current_acceleration[dof], inp.max_velocity[dof], inp_min_velocity[dof], inp.max_acceleration[dof], inp_min_acceleration[dof], inp.max_jerk[dof], p.t_brakes, p.j_brakes);
                 } break;
@@ -272,7 +281,7 @@ class Trajectory {
             }
 
             bool found_profile;
-            switch (inp.control_interface) {
+            switch (inp_per_dof_control_interface[dof]) {
                 case ControlInterface::Position: {
                     PositionStep1 step1 {p0s[dof], v0s[dof], a0s[dof], inp.target_position[dof], inp.target_velocity[dof], inp.target_acceleration[dof], inp.max_velocity[dof], inp_min_velocity[dof], inp.max_acceleration[dof], inp_min_acceleration[dof], inp.max_jerk[dof]};
                     found_profile = step1.get_profile(p, blocks[dof]);
@@ -314,22 +323,22 @@ class Trajectory {
             return Result::Working;
         }
 
-        if (inp.synchronization == Synchronization::None) {
-            for (size_t dof = 0; dof < blocks.size(); ++dof) {
-                if (!inp.enabled[dof] || dof == limiting_dof) {
-                    continue;
-                }
-
+        // None Synchronization
+        for (size_t dof = 0; dof < blocks.size(); ++dof) {
+            if (inp.enabled[dof] && dof != limiting_dof && inp_per_dof_synchronization[dof] == Synchronization::None) {
                 profiles[dof] = blocks[dof].p_min;
             }
+        }
+        if (std::all_of(inp_per_dof_synchronization.begin(), inp_per_dof_synchronization.end(), [](auto s){ return s == Synchronization::None; })) {
             return Result::Working;
         }
 
-        if (inp.synchronization == Synchronization::Phase && inp.control_interface == ControlInterface::Position) {
+        // Phase Synchronization
+        if (std::any_of(inp_per_dof_synchronization.begin(), inp_per_dof_synchronization.end(), [](auto s){ return s == Synchronization::Phase; }) && std::all_of(inp_per_dof_control_interface.begin(), inp_per_dof_control_interface.end(), [](auto s){ return s == ControlInterface::Position; })) {
             if (is_phase_synchronizable(inp, inp.max_velocity, inp_min_velocity, inp.max_acceleration, inp_min_acceleration, inp.max_jerk, profiles[limiting_dof].direction, limiting_dof, new_max_velocity, new_min_velocity, new_max_acceleration, new_min_acceleration, new_max_jerk)) {
                 bool found_time_synchronization {true};
                 for (size_t dof = 0; dof < profiles.size(); ++dof) {
-                    if (!inp.enabled[dof] || dof == limiting_dof) {
+                    if (!inp.enabled[dof] || dof == limiting_dof || inp_per_dof_synchronization[dof] != Synchronization::Phase) {
                         continue;
                     }
 
@@ -357,22 +366,22 @@ class Trajectory {
                     p.limits = profiles[limiting_dof].limits; // After check method call to set correct limits
                 }
 
-                if (found_time_synchronization) {
+                if (found_time_synchronization && std::all_of(inp_per_dof_synchronization.begin(), inp_per_dof_synchronization.end(), [](auto s){ return s == Synchronization::Phase || s == Synchronization::None; })) {
                     return Result::Working;
                 }
             }
         }
 
-        // The general case
+        // Time Synchronization
         for (size_t dof = 0; dof < profiles.size(); ++dof) {
-            if (!inp.enabled[dof] || dof == limiting_dof) {
+            if (!inp.enabled[dof] || dof == limiting_dof || inp_per_dof_synchronization[dof] == Synchronization::None) {
                 continue;
             }
 
             Profile& p = profiles[dof];
             const double t_profile = duration - p.t_brake.value_or(0.0);
 
-            if (inp.synchronization == Synchronization::TimeIfNecessary && std::abs(inp.target_velocity[dof]) < eps && std::abs(inp.target_acceleration[dof]) < eps) {
+            if (inp_per_dof_synchronization[dof] == Synchronization::TimeIfNecessary && std::abs(inp.target_velocity[dof]) < eps && std::abs(inp.target_acceleration[dof]) < eps) {
                 p = blocks[dof].p_min;
                 continue;
             }
@@ -390,7 +399,7 @@ class Trajectory {
             }
 
             bool found_time_synchronization;
-            switch (inp.control_interface) {
+            switch (inp_per_dof_control_interface[dof]) {
                 case ControlInterface::Position: {
                     PositionStep2 step2 {t_profile, p0s[dof], v0s[dof], a0s[dof], inp.target_position[dof], inp.target_velocity[dof], inp.target_acceleration[dof], inp.max_velocity[dof], inp_min_velocity[dof], inp.max_acceleration[dof], inp_min_acceleration[dof], inp.max_jerk[dof]};
                     found_time_synchronization = step2.get_profile(p);

src/python.cpp

@@ -100,6 +100,8 @@ limited by velocity, acceleration, and jerk constraints.";
         .def_readwrite("control_interface", &InputParameter<DynamicDOFs>::control_interface)
         .def_readwrite("synchronization", &InputParameter<DynamicDOFs>::synchronization)
         .def_readwrite("duration_discretization", &InputParameter<DynamicDOFs>::duration_discretization)
+        .def_readwrite("per_dof_control_interface", &InputParameter<DynamicDOFs>::per_dof_control_interface)
+        .def_readwrite("per_dof_synchronization", &InputParameter<DynamicDOFs>::per_dof_synchronization)
         .def_readwrite("minimum_duration", &InputParameter<DynamicDOFs>::minimum_duration)
         .def_readwrite("interrupt_calculation_duration", &InputParameter<DynamicDOFs>::interrupt_calculation_duration)
         .def(py::self != py::self)

test/otg-test.cpp

@@ -242,6 +242,82 @@ TEST_CASE("secondary" * doctest::description("Secondary Features")) {
     CHECK( output.trajectory.get_duration() == doctest::Approx(12.0) );
 }
 
+TEST_CASE("per-dof-setting" * doctest::description("Per DoF Settings")) {
+    Ruckig<3, true> otg {0.005};
+    InputParameter<3> input;
+    Trajectory<3> traj;
+
+    input.current_position = {0.0, -2.0, 0.0};
+    input.current_velocity = {0.0, 0.0, 0.0};
+    input.current_acceleration = {0.0, 0.0, 0.0};
+    input.target_position = {1.0, -3.0, 2.0};
+    input.target_velocity = {0.0, 0.3, 0.0};
+    input.target_acceleration = {0.0, 0.0, 0.0};
+    input.max_velocity = {1.0, 1.0, 1.0};
+    input.max_acceleration = {1.0, 1.0, 1.0};
+    input.max_jerk = {1.0, 1.0, 1.0};
+
+    auto result = otg.calculate(input, traj);
+    CHECK( result == Result::Working );
+    CHECK( traj.get_duration() == doctest::Approx(4.0) );
+
+    std::array<double, 3> new_position, new_velocity, new_acceleration;
+    traj.at_time(2.0, new_position, new_velocity, new_acceleration);
+    check_array(new_position, {0.5, -2.6871268303, 1.0});
+
+
+    input.control_interface = ControlInterface::Velocity;
+
+    result = otg.calculate(input, traj);
+    CHECK( result == Result::Working );
+    CHECK( traj.get_duration() == doctest::Approx(1.095445115) );
+
+    traj.at_time(1.0, new_position, new_velocity, new_acceleration);
+    check_array(new_position, {0.0, -1.8641718534, 0.0});
+
+
+    input.per_dof_control_interface = {ControlInterface::Position, ControlInterface::Velocity, ControlInterface::Position};
+
+    result = otg.calculate(input, traj);
+    CHECK( result == Result::Working );
+    CHECK( traj.get_duration() == doctest::Approx(4.0) );
+
+    traj.at_time(2.0, new_position, new_velocity, new_acceleration);
+    check_array(new_position, {0.5, -1.8528486838, 1.0});
+
+
+    input.per_dof_synchronization = {Synchronization::Time, Synchronization::None, Synchronization::Time};
+
+    result = otg.calculate(input, traj);
+    CHECK( result == Result::Working );
+    CHECK( traj.get_duration() == doctest::Approx(4.0) );
+
+    traj.at_time(2.0, new_position, new_velocity, new_acceleration);
+    check_array(new_position, {0.5, -1.5643167673, 1.0});
+
+
+    input.control_interface = ControlInterface::Position;
+    input.per_dof_control_interface = std::nullopt;
+    input.per_dof_synchronization = {Synchronization::None, Synchronization::Time, Synchronization::Time};
+
+
+    result = otg.calculate(input, traj);
+    CHECK( result == Result::Working );
+    CHECK( traj.get_duration() == doctest::Approx(4.0) );
+
+    traj.at_time(2.0, new_position, new_velocity, new_acceleration);
+    check_array(new_position, {0.7482143874, -2.6871268303, 1.0});
+
+
+    auto independent_min_durations = traj.get_independent_min_durations();
+    traj.at_time(independent_min_durations[0], new_position, new_velocity, new_acceleration);
+    CHECK( new_position[0] == doctest::Approx(input.target_position[0]) );
+    traj.at_time(independent_min_durations[1], new_position, new_velocity, new_acceleration);
+    CHECK( new_position[1] == doctest::Approx(-3.0890156397) );
+    traj.at_time(independent_min_durations[2], new_position, new_velocity, new_acceleration);
+    CHECK( new_position[2] == doctest::Approx(input.target_position[2]) );
+}
+
 TEST_CASE("dynamic-dofs" * doctest::description("Dynamic DoFs")) {
     Ruckig<DynamicDOFs, true> otg {3, 0.005};
     InputParameter<DynamicDOFs> input {3};

test/plot_target.py

@@ -7,7 +7,7 @@
 path.insert(0, str(Path(__file__).parent.absolute().parent / 'build'))
 
 from ruckig import InputParameter, OutputParameter, Result, Ruckig, Synchronization, ControlInterface, DurationDiscretization
-from ruckig import Reflexxes
+# from ruckig import Reflexxes
 
 
 def walk_through_trajectory(otg, inp):
@@ -29,21 +29,24 @@ def walk_through_trajectory(otg, inp):
 
 if __name__ == '__main__':
     inp = InputParameter(3)
-    # # inp.control_interface = ControlInterface.Velocity
+    # inp.control_interface = ControlInterface.Velocity
     # inp.synchronization = Synchronization.No
     # inp.duration_discretization = DurationDiscretization.Discrete
 
-    inp.current_position = [0.0, 0.0, 0.5]
-    inp.current_velocity = [0.0, -2.2, -0.5]
-    inp.current_acceleration = [0.0, 2.5, -0.5]
+    # inp.per_dof_control_interface = [ControlInterface.Position, ControlInterface.Velocity, ControlInterface.Position]
+    # inp.per_dof_synchronization = [Synchronization.Phase, Synchronization.Time, Synchronization.Phase]
 
-    inp.target_position = [-5.0, -2.0, -3.5]
-    inp.target_velocity = [0.0, -0.5, -2.0]
-    inp.target_acceleration = [0.0, 0.0, 0.5]
+    inp.current_position = [0.0, -2.0, 0.0]
+    inp.current_velocity = [0.0, 0.0, 0.0]
+    inp.current_acceleration = [0.0, 0.0, 0.0]
 
-    inp.max_velocity = [3.0, 1.0, 3.0]
-    inp.max_acceleration = [3.0, 2.0, 1.0]
-    inp.max_jerk = [4.0, 3.0, 2.0]
+    inp.target_position = [1.0, -3.0, 2.0]
+    inp.target_velocity = [0.0, 0.0, 0.0]
+    inp.target_acceleration = [0.0, 0.0, 0.0]
+
+    inp.max_velocity = [1.0, 1.0, 1.0]
+    inp.max_acceleration = [1.0, 1.0, 1.0]
+    inp.max_jerk = [1.0, 1.0, 1.0]
 
     # inp.minimum_duration = 5.0
 
@@ -54,6 +57,7 @@ def walk_through_trajectory(otg, inp):
     out_list = walk_through_trajectory(otg, inp)
 
     # print(out_list[0].trajectory.position_extrema)
+    # print(out_list[0].trajectory.independent_min_durations)
 
     print(f'Calculation duration: {out_list[0].calculation_duration:0.1f} [µs]')
     print(f'Trajectory duration: {out_list[0].trajectory.duration:0.4f} [s]')