.. _program_listing_file_include_ruckig_trajectory.hpp:

Program Listing for File trajectory.hpp
=======================================

|exhale_lsh| :ref:`Return to documentation for file <file_include_ruckig_trajectory.hpp>` (``include/ruckig/trajectory.hpp``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   #pragma once
   
   #include <array>
   #include <functional>
   #include <tuple>
   #include <vector>
   
   #include <ruckig/error.hpp>
   #include <ruckig/profile.hpp>
   
   
   namespace ruckig {
   
   template<size_t, template<class, size_t> class> class TargetCalculator;
   template<size_t, template<class, size_t> class> class WaypointsCalculator;
   
   
   template<size_t DOFs, template<class, size_t> class CustomVector = StandardVector>
   class Trajectory {
   #if defined WITH_CLOUD_CLIENT
       template<class T> using Container = std::vector<T>;
   #else
       template<class T> using Container = std::array<T, 1>;
   #endif
   
       template<class T> using Vector = CustomVector<T, DOFs>;
   
       friend class TargetCalculator<DOFs, CustomVector>;
       friend class WaypointsCalculator<DOFs, CustomVector>;
   
       Container<Vector<Profile>> profiles;
   
       double duration {0.0};
       Container<double> cumulative_times;
   
       Vector<double> independent_min_durations;
       Vector<Bound> position_extrema;
   
       size_t continue_calculation_counter {0};
   
   #if defined WITH_CLOUD_CLIENT
       template<size_t D = DOFs, typename std::enable_if<(D >= 1), int>::type = 0>
       void resize(size_t max_number_of_waypoints) {
           profiles.resize(max_number_of_waypoints + 1);
           cumulative_times.resize(max_number_of_waypoints + 1);
       }
   
       template<size_t D = DOFs, typename std::enable_if<(D == 0), int>::type = 0>
       void resize(size_t max_number_of_waypoints) {
           resize<1>(max_number_of_waypoints); // Also call resize method above
   
           for (auto& p: profiles) {
               p.resize(degrees_of_freedom);
           }
       }
   #endif
   
       template<typename Func>
       void state_to_integrate_from(double time, size_t& new_section, Func&& set_integrate) const {
           if (time >= duration) {
               // Keep constant acceleration
               new_section = profiles.size();
               const auto& profiles_dof = profiles.back();
               for (size_t dof = 0; dof < degrees_of_freedom; ++dof) {
                   const double t_pre = (profiles.size() > 1) ? cumulative_times[cumulative_times.size() - 2] : profiles_dof[dof].brake.duration;
                   const double t_diff = time - (t_pre + profiles_dof[dof].t_sum.back());
                   set_integrate(dof, t_diff, profiles_dof[dof].p.back(), profiles_dof[dof].v.back(), profiles_dof[dof].a.back(), 0.0);
               }
               return;
           }
   
           const auto new_section_ptr = std::upper_bound(cumulative_times.begin(), cumulative_times.end(), time);
           new_section = std::distance(cumulative_times.begin(), new_section_ptr);
           double t_diff = time;
           if (new_section > 0) {
               t_diff -= cumulative_times[new_section - 1];
           }
   
           for (size_t dof = 0; dof < degrees_of_freedom; ++dof) {
               const Profile& p = profiles[new_section][dof];
               double t_diff_dof = t_diff;
   
               // Brake pre-trajectory
               if (new_section == 0 && p.brake.duration > 0) {
                   if (t_diff_dof < p.brake.duration) {
                       const size_t index = (t_diff_dof < p.brake.t[0]) ? 0 : 1;
                       if (index > 0) {
                           t_diff_dof -= p.brake.t[index - 1];
                       }
   
                       set_integrate(dof, t_diff_dof, p.brake.p[index], p.brake.v[index], p.brake.a[index], p.brake.j[index]);
                       continue;
                   } else {
                       t_diff_dof -= p.brake.duration;
                   }
               }
   
               // Accel post-trajectory
               // if (new_section == profiles.size() - 1 && p.accel.duration > 0) {
               //     if (t_diff_dof > p.t_sum.back()) {
               //         const size_t index = (t_diff_dof < p.accel.t[1]) ? 1 : 0;
               //         if (index > 0) {
               //             t_diff_dof -= p.accel.t[index - 1];
               //         }
   
               //         t_diff_dof -= p.t_sum.back();
   
               //         if (t_diff_dof < p.accel.t[1]) {
               //             set_integrate(dof, t_diff_dof, p.p.back(), p.v.back(), p.a.back(), p.accel.j[1]);
               //             continue;
               //         }
   
               //         t_diff_dof -= p.accel.t[1];
   
               //         const size_t index = 1;
               //         set_integrate(dof, t_diff_dof, p.accel.p[index], p.accel.v[index], p.accel.a[index], p.accel.j[index]);
               //         continue;
               //     }
               // }
   
               // Non-time synchronization
               if (t_diff_dof >= p.t_sum.back()) {
                   // Keep constant acceleration
                   set_integrate(dof, t_diff_dof - p.t_sum.back(), p.p.back(), p.v.back(), p.a.back(), 0.0);
                   continue;
               }
   
               const auto index_dof_ptr = std::upper_bound(p.t_sum.begin(), p.t_sum.end(), t_diff_dof);
               const size_t index_dof = std::distance(p.t_sum.begin(), index_dof_ptr);
   
               if (index_dof > 0) {
                   t_diff_dof -= p.t_sum[index_dof - 1];
               }
   
               set_integrate(dof, t_diff_dof, p.p[index_dof], p.v[index_dof], p.a[index_dof], p.j[index_dof]);
           }
       }
   
   public:
       size_t degrees_of_freedom;
   
       template<size_t D = DOFs, typename std::enable_if<(D >= 1), int>::type = 0>
       Trajectory(): degrees_of_freedom(DOFs) {
   #if defined WITH_CLOUD_CLIENT
           resize(0);
   #endif
       }
   
       template<size_t D = DOFs, typename std::enable_if<(D == 0), int>::type = 0>
       Trajectory(size_t dofs): degrees_of_freedom(dofs) {
   #if defined WITH_CLOUD_CLIENT
           resize(0);
   #endif
   
           profiles[0].resize(dofs);
           independent_min_durations.resize(dofs);
           position_extrema.resize(dofs);
       }
   
   #if defined WITH_CLOUD_CLIENT
       template<size_t D = DOFs, typename std::enable_if<(D >= 1), int>::type = 0>
       Trajectory(size_t max_number_of_waypoints): degrees_of_freedom(DOFs) {
           resize(max_number_of_waypoints);
       }
   
       template<size_t D = DOFs, typename std::enable_if<(D == 0), int>::type = 0>
       Trajectory(size_t dofs, size_t max_number_of_waypoints): degrees_of_freedom(dofs) {
           resize(max_number_of_waypoints);
   
           independent_min_durations.resize(dofs);
           position_extrema.resize(dofs);
       }
   #endif
   
       void at_time(double time, Vector<double>& new_position, Vector<double>& new_velocity, Vector<double>& new_acceleration, Vector<double>& new_jerk, size_t& new_section) const {
           if constexpr (DOFs == 0) {
               if (degrees_of_freedom != new_position.size() || degrees_of_freedom != new_velocity.size() || degrees_of_freedom != new_acceleration.size() || degrees_of_freedom != new_jerk.size()) {
                   throw RuckigError("mismatch in degrees of freedom (vector size).");
               }
           }
   
           state_to_integrate_from(time, new_section, [&](size_t dof, double t, double p, double v, double a, double j) {
               std::tie(new_position[dof], new_velocity[dof], new_acceleration[dof]) = integrate(t, p, v, a, j);
               new_jerk[dof] = j;
           });
       }
   
       void at_time(double time, Vector<double>& new_position, Vector<double>& new_velocity, Vector<double>& new_acceleration) const {
           if constexpr (DOFs == 0) {
               if (degrees_of_freedom != new_position.size() || degrees_of_freedom != new_velocity.size() || degrees_of_freedom != new_acceleration.size()) {
                   throw RuckigError("mismatch in degrees of freedom (vector size).");
               }
           }
   
           size_t new_section;
           state_to_integrate_from(time, new_section, [&](size_t dof, double t, double p, double v, double a, double j) {
               std::tie(new_position[dof], new_velocity[dof], new_acceleration[dof]) = integrate(t, p, v, a, j);
           });
       }
   
       void at_time(double time, Vector<double>& new_position) const {
           if constexpr (DOFs == 0) {
               if (degrees_of_freedom != new_position.size()) {
                   throw RuckigError("mismatch in degrees of freedom (vector size).");
               }
           }
   
           size_t new_section;
           state_to_integrate_from(time, new_section, [&](size_t dof, double t, double p, double v, double a, double j) {
               std::tie(new_position[dof], std::ignore, std::ignore) = integrate(t, p, v, a, j);
           });
       }
   
       template<size_t D = DOFs, typename std::enable_if<(D == 1), int>::type = 0>
       void at_time(double time, double& new_position, double& new_velocity, double& new_acceleration, double& new_jerk, size_t& new_section) const {
           state_to_integrate_from(time, new_section, [&](size_t, double t, double p, double v, double a, double j) {
               std::tie(new_position, new_velocity, new_acceleration) = integrate(t, p, v, a, j);
               new_jerk = j;
           });
       }
   
       template<size_t D = DOFs, typename std::enable_if<(D == 1), int>::type = 0>
       void at_time(double time, double& new_position, double& new_velocity, double& new_acceleration) const {
           size_t new_section;
           state_to_integrate_from(time, new_section, [&](size_t, double t, double p, double v, double a, double j) {
               std::tie(new_position, new_velocity, new_acceleration) = integrate(t, p, v, a, j);
           });
       }
   
       template<size_t D = DOFs, typename std::enable_if<(D == 1), int>::type = 0>
       void at_time(double time, double& new_position) const {
           size_t new_section;
           state_to_integrate_from(time, new_section, [&](size_t, double t, double p, double v, double a, double j) {
               std::tie(new_position, std::ignore, std::ignore) = integrate(t, p, v, a, j);
           });
       }
   
   
       Container<Vector<Profile>> get_profiles() const {
           return profiles;
       }
   
       double get_duration() const {
           return duration;
       }
   
       Container<double> get_intermediate_durations() const {
           return cumulative_times;
       }
   
       Vector<double> get_independent_min_durations() const {
           return independent_min_durations;
       }
   
       Vector<Bound> get_position_extrema() {
           for (size_t dof = 0; dof < degrees_of_freedom; ++dof) {
               position_extrema[dof] = profiles[0][dof].get_position_extrema();
           }
   
           for (size_t i = 1; i < profiles.size(); ++i) {
               for (size_t dof = 0; dof < degrees_of_freedom; ++dof) {
                   auto section_position_extrema = profiles[i][dof].get_position_extrema();
                   if (section_position_extrema.max > position_extrema[dof].max) {
                       position_extrema[dof].max = section_position_extrema.max;
                       position_extrema[dof].t_max = section_position_extrema.t_max;
                   }
                   if (section_position_extrema.min < position_extrema[dof].min) {
                       position_extrema[dof].min = section_position_extrema.min;
                       position_extrema[dof].t_min = section_position_extrema.t_min;
                   }
               }
           }
   
           return position_extrema;
       }
   
       std::optional<double> get_first_time_at_position(size_t dof, double position, double time_after=0.0) const {
           if (dof >= degrees_of_freedom) {
               return std::nullopt;
           }
   
           double time;
           for (size_t i = 0; i < profiles.size(); ++i) {
               if (profiles[i][dof].get_first_state_at_position(position, time, time_after)) {
                   const double section_time = (i > 0) ? cumulative_times[i-1] : 0.0;
                   return section_time + time;
               }
           }
           return std::nullopt;
       }
   };
   
   } // namespace ruckig