fix finished trajectory for disabled dofs

pantor · Oct 12, 2021 · 715d74d · 715d74d
1 parent d74e394
commit 715d74d
Show file tree

Hide file tree

Showing 5 changed files with 34 additions and 7,009 deletions.
diff --git a/include/ruckig/trajectory.hpp b/include/ruckig/trajectory.hpp
@@ -201,11 +201,10 @@ class Trajectory {
 
         for (size_t dof = 0; dof < profiles.size(); ++dof) {
             auto& p = profiles[dof];
-            p.pf = inp.current_position[dof];
-            p.vf = inp.current_velocity[dof];
-            p.af = inp.current_acceleration[dof];
-
             if (!inp.enabled[dof]) {
+                p.pf = inp.current_position[dof];
+                p.vf = inp.current_velocity[dof];
+                p.af = inp.current_acceleration[dof];
                 p.t_sum[6] = 0.0;
                 continue;
             }
@@ -399,7 +398,8 @@ class Trajectory {
             // Keep constant acceleration
             new_section = 1;
             for (size_t dof = 0; dof < profiles.size(); ++dof) {
-                std::tie(new_position[dof], new_velocity[dof], new_acceleration[dof]) = Profile::integrate(time - duration, profiles[dof].pf, profiles[dof].vf, profiles[dof].af, 0);
+                const double t_diff = time - (profiles[dof].t_brake.value_or(0.0) + profiles[dof].t_sum[6]);
+                std::tie(new_position[dof], new_velocity[dof], new_acceleration[dof]) = Profile::integrate(t_diff, profiles[dof].pf, profiles[dof].vf, profiles[dof].af, 0);
             }
             return;
         }