[JTC] cross chunk continuity for positions upsampling

joint_trajectory_controller/doc/trajectory.rst

@@ -52,7 +52,7 @@ The spline interpolator uses the following interpolation strategies depending on
   velocities. Enabling ``positions_upsampling`` avoids this for positions-only inputs: the controller
   pre-solves the knot velocities of a global cubic spline (``fill_cubic_spline_velocities``) and
   writes them into the trajectory, so the cubic strategy is used instead and the sampled motion is
-  continuous in acceleration across the trajectory's own waypoints. See :ref:`Ingesting
+  continuous in acceleration from the last commanded state onwards. See :ref:`Ingesting
   positions-only action chunks <joint_trajectory_controller_userdoc>`.
 
 Trajectories with velocity fields only, velocity and acceleration only, or acceleration fields only can be processed and are accepted, if ``allow_integration_in_goal_trajectories`` is true. Position (and velocity) is then integrated from velocity (or acceleration, respectively) by Heun's method.
@@ -72,7 +72,8 @@ To visualize the difference of the different interpolation methods and their inp
 
 .. note::
   The linear strategy reports no acceleration, so only the enabled series is drawn in the bottom
-  plot. Its step at ``t=0.5`` is the hand-off from the initial point, which is not part of the solve.
+  plot. The last commanded state joins the solve as a waypoint at ``t=0``, so the motion leaves the
+  initial point without a step in acceleration.
 
 .. image:: spline_position_upsampling.png
   :alt: Sampled trajectory with positions-only points, with and without positions_upsampling
@@ -198,4 +199,16 @@ In this case, the first waypoint is discarded and only the second one is realize
 
 |
 
+Internally, the controller assembles the merged trajectory from four regions, shown in the diagram below.
+*Prefix* waypoints are the old trajectory between the current playback position (*cursor*) and the handoff.
+The *bridge* point is sampled from the old trajectory at the handoff instant, providing velocity continuity into the new trajectory.
+*New points* are the waypoints from the incoming message.
+
+If the new trajectory is a partial goal (not all joints are commanded), the controller must reconcile the shared timeline.
+The *suffix* carries old waypoints that fall after the new trajectory ends and only exists for partial-joint goals:
+commanded joints are held at their final new value with zero velocity, while omitted joints keep their original waypoints so they can finish uninterrupted.
+
+.. image:: trajectory_blending.png
+  :alt: Structure of the merged trajectory message showing prefix, bridge, new points, and suffix regions.
+
 .. [#f1] Adolfo Rodriguez: `Understanding trajectory replacement <http://wiki.ros.org/joint_trajectory_controller/UnderstandingTrajectoryReplacement>`_

joint_trajectory_controller/doc/userdoc.rst

@@ -117,22 +117,20 @@ Ingesting positions-only action chunks
 Action policies (e.g. diffusion policy, ACT) emit *action chunks*: short trajectories of
 positions-only waypoints with no velocities and often no timing. Fed positions-only, the controller
 interpolates linearly (C0), yielding discontinuous velocities at every waypoint (see :ref:`trajectory
-representation <joint_trajectory_controller_trajectory_representation>`).
-
-When ``positions_upsampling.enable`` is true, incoming positions-only messages on ``~/joint_trajectory``
-are upsampled in place: the knot velocities of a global cubic spline (rest boundary conditions,
-``v0 = v_{N-1} = 0``) are solved and written into the trajectory, so the existing sampler reproduces a
-motion that is continuous in acceleration across the chunk's waypoints (see :ref:`the plotted
-comparison <joint_trajectory_controller_trajectory_representation>`). Messages that already carry
-velocities are passed through unchanged, so the feature
-is a strict superset of the default behaviour (it is off by default). It has no effect when
-``interpolation_method`` is ``none``.
-
-``positions_upsampling.policy_frequency`` (double, Hz) is used to synthesize
-``time_from_start = (i + 1) / policy_frequency`` for chunks that arrive without timing; when it is
-``0`` the chunks must carry their own strictly-increasing ``time_from_start``. The first waypoint is
-placed one policy step ahead rather than at ``0``, so the controller ramps into the chunk from the
-current state instead of stepping to it.
+representation <joint_trajectory_controller_trajectory_representation>` for the plotted comparison).
+
+``positions_upsampling.enable`` solves the knot velocities of a global cubic spline for incoming
+positions-only messages on ``~/joint_trajectory``, upgrading them from C0 to C2. The state the
+controller last commanded is prepended as an extra waypoint at ``time_from_start = 0``, so the
+segment into the first waypoint is part of the solve and a chunk arriving mid-motion continues it
+rather than braking to a stop. The spline ends at rest, and starts at rest as well until the
+controller has commanded anything. Messages that already carry velocities pass through unchanged;
+the feature is off by default and has no effect when ``interpolation_method`` is ``none``.
+
+``positions_upsampling.policy_frequency`` (double, Hz) synthesizes
+``time_from_start = (i + 1) / policy_frequency`` for chunks that arrive without timing, placing the
+first waypoint one policy step ahead. ``0`` means the chunks must carry their own
+strictly-increasing ``time_from_start``.
 
    .. code-block:: yaml
 
@@ -150,9 +148,8 @@ current state instead of stepping to it.
             policy_frequency: 30.0
 
    .. note::
-      Only the topic interface (``~/joint_trajectory``) is upsampled. Each chunk uses rest boundary
-      conditions, so streaming chunks decelerate to a stop at each chunk boundary; cross-chunk C2
-      continuity and ``FollowJointTrajectory`` action-goal upsampling are future work.
+      Only the topic interface (``~/joint_trajectory``) is upsampled. ``FollowJointTrajectory``
+      action goals bypass this path and are not upsampled.
 
 
 Preemption policy [#f1]_

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

@@ -220,6 +220,9 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   bool is_positions_only(const trajectory_msgs::msg::JointTrajectory & traj) const;
   // fill time_from_start from positions_upsampling.policy_frequency when timing is absent
   void synthesize_timing(trajectory_msgs::msg::JointTrajectory & traj) const;
+  // prepends the last commanded state as knot 0 at t=0; false if there is none usable yet
+  bool prepend_commanded_state(
+    trajectory_msgs::msg::JointTrajectory & traj, std::vector<double> & start_velocity) const;
 
   // callbacks for action_server_
   rclcpp_action::GoalResponse goal_received_callback(

joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp

@@ -215,14 +215,19 @@ void wraparound_joint(
  * For each joint independently, solves the knot velocities that make the
  * acceleration continuous across every interior knot -- an O(n) tridiagonal
  * (Thomas) solve over the waypoint positions and their ``time_from_start`` --
- * using rest boundary conditions ``v[0] = v[n-1] = 0``, and writes the result
- * into ``points[i].velocities``. Once velocities are present, the per-segment
+ * and writes the result into ``points[i].velocities``. The end is always at
+ * rest (``v[n-1] = 0``); the start is at rest too unless \p start_velocity is
+ * given, which clamps it instead. Once velocities are present, the per-segment
  * cubic-Hermite sampling in \ref Trajectory::sample reproduces the global cubic
  * spline, so a positions-only trajectory is upgraded from linear (C0) to C2.
  *
  * \param[in,out] traj Trajectory whose points carry positions; velocities are filled.
+ * \param[in] start_velocity Velocity at the first knot, one entry per joint. Empty (the default)
+ *   keeps the rest condition ``v[0] = 0``; passing the current velocity clamps the start instead,
+ *   so a streamed chunk continues the ongoing motion with acceleration still continuous.
  * \return true if velocities were written; false (trajectory left untouched) if it has
- *   fewer than two points, inconsistent widths, or non-strictly-increasing timing.
+ *   fewer than two points, inconsistent widths, non-strictly-increasing timing, or a
+ *   start_velocity whose size does not match the joint count.
  *
  * \code
  *   trajectory_msgs::msg::JointTrajectory msg;  // positions-only waypoints, with time_from_start
@@ -231,7 +236,8 @@ void wraparound_joint(
  *   traj.sample(t, interpolation_methods::DEFAULT_INTERPOLATION, out, start, end);
  * \endcode
  */
-bool fill_cubic_spline_velocities(trajectory_msgs::msg::JointTrajectory & traj);
+bool fill_cubic_spline_velocities(
+  trajectory_msgs::msg::JointTrajectory & traj, const std::vector<double> & start_velocity = {});
 
 }  // namespace joint_trajectory_controller
 

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -15,7 +15,9 @@
 #include "joint_trajectory_controller/joint_trajectory_controller.hpp"
 
 #include <chrono>
+#include <cmath>
 #include <functional>
+#include <limits>
 #include <memory>
 #include <numeric>
 #include <stdexcept>
@@ -1425,7 +1427,58 @@ void JointTrajectoryController::preprocess_incoming_trajectory(
     return;
   }
   synthesize_timing(msg);
-  fill_cubic_spline_velocities(msg);
+  // with nothing to anchor on start_velocity stays empty, which is the rest boundary
+  std::vector<double> start_velocity;
+  prepend_commanded_state(msg, start_velocity);
+  fill_cubic_spline_velocities(msg, start_velocity);
+}
+
+bool JointTrajectoryController::prepend_commanded_state(
+  trajectory_msgs::msg::JointTrajectory & traj, std::vector<double> & start_velocity) const
+{
+  start_velocity.clear();
+  // only a zero stamp puts the trajectory's t=0 at "now", where the commanded state belongs
+  if (traj.header.stamp.sec != 0 || traj.header.stamp.nanosec != 0u)
+  {
+    return false;
+  }
+  // the anchor needs room ahead of the first waypoint
+  if (
+    traj.points.empty() ||
+    rclcpp::Duration(traj.points.front().time_from_start) <= rclcpp::Duration(0, 0))
+  {
+    return false;
+  }
+  const auto commanded = rt_last_commanded_state_.get();
+  if (commanded.positions.size() != dof_ || commanded.velocities.size() != dof_)
+  {
+    return false;
+  }
+  // not sorted into the controller's joint order until install, so index by the sender's order
+  const auto joint_map = mapping(traj.joint_names, params_.joints);
+  if (joint_map.size() != traj.joint_names.size())
+  {
+    return false;
+  }
+  trajectory_msgs::msg::JointTrajectoryPoint anchor;
+  anchor.positions.resize(joint_map.size());
+  std::vector<double> anchor_velocities(joint_map.size());
+  for (size_t i = 0; i < joint_map.size(); ++i)
+  {
+    // NaN before the first update(), and while the hardware exposes no velocity state
+    if (
+      !std::isfinite(commanded.positions[joint_map[i]]) ||
+      !std::isfinite(commanded.velocities[joint_map[i]]))
+    {
+      return false;
+    }
+    anchor.positions[i] = commanded.positions[joint_map[i]];
+    anchor_velocities[i] = commanded.velocities[joint_map[i]];
+  }
+  anchor.time_from_start = rclcpp::Duration(0, 0);
+  traj.points.insert(traj.points.begin(), std::move(anchor));
+  start_velocity = std::move(anchor_velocities);
+  return true;
 }
 
 bool JointTrajectoryController::is_positions_only(

joint_trajectory_controller/src/trajectory.cpp

@@ -98,7 +98,8 @@ void wraparound_joint(
   }
 }
 
-bool fill_cubic_spline_velocities(trajectory_msgs::msg::JointTrajectory & traj)
+bool fill_cubic_spline_velocities(
+  trajectory_msgs::msg::JointTrajectory & traj, const std::vector<double> & start_velocity)
 {
   const size_t n = traj.points.size();
   if (n < 2)
@@ -110,6 +111,10 @@ bool fill_cubic_spline_velocities(trajectory_msgs::msg::JointTrajectory & traj)
   {
     return false;
   }
+  if (!start_velocity.empty() && start_velocity.size() != n_joints)
+  {
+    return false;
+  }
   for (const auto & point : traj.points)
   {
     if (point.positions.size() != n_joints)
@@ -144,12 +149,10 @@ bool fill_cubic_spline_velocities(trajectory_msgs::msg::JointTrajectory & traj)
 
   for (size_t joint = 0; joint < n_joints; ++joint)
   {
-    // Rest boundary conditions: velocity is zero at both ends.
-    // TODO(vedh1234): accept a non-zero start velocity to stitch chunks (cross-chunk C2
-    // continuity).
+    // Clamped start when a start velocity is given, otherwise at rest.
     diagonal[0] = 1.0;
     upper_diagonal[0] = 0.0;
-    rhs[0] = 0.0;
+    rhs[0] = start_velocity.empty() ? 0.0 : start_velocity[joint];
     lower_diagonal[n - 1] = 0.0;
     diagonal[n - 1] = 1.0;
     rhs[n - 1] = 0.0;

joint_trajectory_controller/test/test_trajectory.cpp

@@ -1299,6 +1299,48 @@ TEST(TestTrajectory, sample_after_fill_is_smooth_not_staircase)
     << "unfilled positions-only samples to a C0 staircase with far larger accel spikes";
 }
 
+// A start velocity clamps the first knot and is solved into the interior knots, so acceleration
+// stays continuous across a chunk seam.
+TEST(TestTrajectory, fill_cubic_spline_velocities_clamps_start_velocity)
+{
+  const std::vector<std::vector<double>> positions{{{0.0}}, {{0.1}}, {{0.2}}, {{0.3}}};
+  const double dt = 0.1;
+  const double start_velocity = 0.8;
+
+  auto rest = make_positions_chunk(positions, dt);
+  ASSERT_TRUE(joint_trajectory_controller::fill_cubic_spline_velocities(rest));
+  EXPECT_NEAR(rest.points.front().velocities[0], 0.0, EPS) << "default is still the rest boundary";
+
+  auto clamped = make_positions_chunk(positions, dt);
+  ASSERT_TRUE(joint_trajectory_controller::fill_cubic_spline_velocities(clamped, {start_velocity}));
+  EXPECT_NEAR(clamped.points.front().velocities[0], start_velocity, EPS);
+  EXPECT_NEAR(clamped.points.back().velocities[0], 0.0, EPS) << "the end stays at rest";
+
+  // Overwriting velocities[0] after the solve would leave the interior untouched.
+  EXPECT_GT(std::abs(clamped.points[1].velocities[0] - rest.points[1].velocities[0]), 1e-6)
+    << "interior knots were not re-solved for the clamped start";
+
+  auto segment_acceleration =
+    [&](const trajectory_msgs::msg::JointTrajectory & traj, size_t i, bool leaving)
+  {
+    const double h = dt;
+    const double p0 = traj.points[i].positions[0];
+    const double v0 = traj.points[i].velocities[0];
+    const double p1 = traj.points[i + 1].positions[0];
+    const double v1 = traj.points[i + 1].velocities[0];
+    const double c2 = (-3.0 * p0 + 3.0 * p1 - 2.0 * v0 * h - v1 * h) / (h * h);
+    const double c3 = (2.0 * p0 - 2.0 * p1 + v0 * h + v1 * h) / (h * h * h);
+    return leaving ? 2.0 * c2 : 2.0 * c2 + 6.0 * c3 * h;
+  };
+  for (size_t knot = 1; knot + 1 < clamped.points.size(); ++knot)
+  {
+    EXPECT_NEAR(
+      segment_acceleration(clamped, knot - 1, false), segment_acceleration(clamped, knot, true),
+      EPS)
+      << "acceleration is discontinuous at interior knot " << knot;
+  }
+}
+
 // Edge cases: a two-point chunk is a valid single segment (rest-to-rest);
 // trajectories with < 2 points or inconsistent widths are a safe no-op; and
 // back-to-back chunks of different sizes both fill correctly.