diff --git a/joint_trajectory_controller/doc/spline_position_upsampling.png b/joint_trajectory_controller/doc/spline_position_upsampling.png index 850ef1f8fd..ab32e18017 100644 Binary files a/joint_trajectory_controller/doc/spline_position_upsampling.png and b/joint_trajectory_controller/doc/spline_position_upsampling.png differ diff --git a/joint_trajectory_controller/doc/trajectory.rst b/joint_trajectory_controller/doc/trajectory.rst index df2c38852a..9f1eb217e0 100644 --- a/joint_trajectory_controller/doc/trajectory.rst +++ b/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 `. 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 `_ diff --git a/joint_trajectory_controller/doc/trajectory_blending.png b/joint_trajectory_controller/doc/trajectory_blending.png new file mode 100644 index 0000000000..b152edb9e6 Binary files /dev/null and b/joint_trajectory_controller/doc/trajectory_blending.png differ diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index d28808b902..48e173605a 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/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 `). - -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 `). 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 ` 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]_ diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index 2d7c628d61..be0f888e78 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/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 & start_velocity) const; // callbacks for action_server_ rclcpp_action::GoalResponse goal_received_callback( diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp index d405810739..6b615c04ef 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp +++ b/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 & start_velocity = {}); } // namespace joint_trajectory_controller diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 18642f59d8..2ba7a99cee 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -15,7 +15,9 @@ #include "joint_trajectory_controller/joint_trajectory_controller.hpp" #include +#include #include +#include #include #include #include @@ -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 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 & 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 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( diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index b91ead55a9..58f7e478e7 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/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 & 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; diff --git a/joint_trajectory_controller/test/test_trajectory.cpp b/joint_trajectory_controller/test/test_trajectory.cpp index 18e20a5058..6f5b1f1a0f 100644 --- a/joint_trajectory_controller/test/test_trajectory.cpp +++ b/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> 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. diff --git a/joint_trajectory_controller/test/test_trajectory_controller_upsampling.cpp b/joint_trajectory_controller/test/test_trajectory_controller_upsampling.cpp index 39c2a80697..34f5f8062f 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_upsampling.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_upsampling.cpp @@ -14,7 +14,9 @@ #include +#include #include +#include #include #include "test_trajectory_controller_utils.hpp" @@ -35,6 +37,7 @@ class UpsamplingHelpers : public joint_trajectory_controller::JointTrajectoryCon { public: using JointTrajectoryController::is_positions_only; + using JointTrajectoryController::prepend_commanded_state; using JointTrajectoryController::preprocess_incoming_trajectory; using JointTrajectoryController::synthesize_timing; @@ -43,6 +46,19 @@ class UpsamplingHelpers : public joint_trajectory_controller::JointTrajectoryCon params_.positions_upsampling.enable = enabled; params_.positions_upsampling.policy_frequency = policy_frequency; } + + // Stand in for an active controller that has already commanded a state. + void configure_commanded_state( + const std::vector & joints, const std::vector & positions, + const std::vector & velocities) + { + params_.joints = joints; + dof_ = joints.size(); + trajectory_msgs::msg::JointTrajectoryPoint commanded; + commanded.positions = positions; + commanded.velocities = velocities; + rt_last_commanded_state_.set(commanded); + } }; trajectory_msgs::msg::JointTrajectory make_positions_chunk( @@ -66,6 +82,77 @@ double time_at(const trajectory_msgs::msg::JointTrajectory & traj, size_t i) } } // namespace +// The start velocity follows the sender's joint order: the message is not sorted until install. +trajectory_msgs::msg::JointTrajectory anchorable_chunk(const std::vector & joints) +{ + trajectory_msgs::msg::JointTrajectory traj; + traj.joint_names = joints; + trajectory_msgs::msg::JointTrajectoryPoint point; + point.positions.assign(joints.size(), 0.0); + point.time_from_start = rclcpp::Duration::from_seconds(0.1); + traj.points.push_back(point); + return traj; +} + +// a mismatch between the two would pair a joint with another joint's state +TEST(JtcUpsamplingHelpers, anchor_follows_the_message_joint_order) +{ + UpsamplingHelpers c; + c.configure_commanded_state({"joint1", "joint2", "joint3"}, {1.0, 2.0, 3.0}, {0.1, 0.2, 0.3}); + + auto traj = anchorable_chunk({"joint3", "joint1", "joint2"}); // reordered + std::vector start_velocity; + ASSERT_TRUE(c.prepend_commanded_state(traj, start_velocity)); + + ASSERT_EQ(start_velocity.size(), 3u); + EXPECT_NEAR(start_velocity[0], 0.3, 1e-12); + EXPECT_NEAR(start_velocity[1], 0.1, 1e-12); + EXPECT_NEAR(start_velocity[2], 0.2, 1e-12); + + ASSERT_EQ(traj.points.size(), 2u); + EXPECT_DOUBLE_EQ(time_at(traj, 0), 0.0); + ASSERT_EQ(traj.points[0].positions.size(), 3u); + EXPECT_NEAR(traj.points[0].positions[0], 3.0, 1e-12); + EXPECT_NEAR(traj.points[0].positions[1], 1.0, 1e-12); + EXPECT_NEAR(traj.points[0].positions[2], 2.0, 1e-12); +} + +TEST(JtcUpsamplingHelpers, anchor_skipped_when_commanded_state_is_not_finite) +{ + UpsamplingHelpers c; + c.configure_commanded_state( + {"joint1", "joint2"}, {1.0, 2.0}, {0.1, std::numeric_limits::quiet_NaN()}); + + auto traj = anchorable_chunk({"joint1", "joint2"}); + std::vector start_velocity; + EXPECT_FALSE(c.prepend_commanded_state(traj, start_velocity)); + EXPECT_EQ(traj.points.size(), 1u); +} + +TEST(JtcUpsamplingHelpers, anchor_skipped_when_the_stamp_is_not_zero) +{ + UpsamplingHelpers c; + c.configure_commanded_state({"joint1"}, {1.0}, {0.1}); + + auto traj = anchorable_chunk({"joint1"}); + traj.header.stamp = rclcpp::Time(5, 0, RCL_ROS_TIME); + std::vector start_velocity; + EXPECT_FALSE(c.prepend_commanded_state(traj, start_velocity)); + EXPECT_EQ(traj.points.size(), 1u); +} + +TEST(JtcUpsamplingHelpers, anchor_skipped_when_the_first_waypoint_is_at_zero) +{ + UpsamplingHelpers c; + c.configure_commanded_state({"joint1"}, {1.0}, {0.1}); + + auto traj = anchorable_chunk({"joint1"}); + traj.points[0].time_from_start = rclcpp::Duration(0, 0); + std::vector start_velocity; + EXPECT_FALSE(c.prepend_commanded_state(traj, start_velocity)); + EXPECT_EQ(traj.points.size(), 1u); +} + // Untimed chunks get time_from_start = (i + 1) / policy_frequency. TEST(JtcUpsamplingHelpers, synthesizes_timing_from_policy_frequency) { @@ -230,10 +317,13 @@ TEST_F(TrajectoryControllerTest, upsampling_enabled_accepts_untimed_positions_on const auto state_reference = traj_controller_->get_state_reference(); EXPECT_EQ(state_reference.velocities.size(), joint_names_.size()); - // a second chunk blends: the bridge survives only if the first waypoint is at dt, not 0 + // a second chunk gets the anchor prepended: one point more than published, at t=0 publish(zero_delay, points, rclcpp::Time(0, 0, RCL_STEADY_TIME)); traj_controller_->wait_for_trajectory(executor); updateController(rclcpp::Duration::from_seconds(0.02)); - EXPECT_GT(traj_controller_->get_blend_prefix_size(), 0u); + const auto installed = traj_controller_->get_installed_trajectory_msg(); + ASSERT_NE(installed, nullptr); + EXPECT_EQ(installed->points.size(), points.size() + 1); + EXPECT_DOUBLE_EQ(rclcpp::Duration(installed->points.front().time_from_start).seconds(), 0.0); executor.cancel(); } diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 753a502d6c..8f5b09a9be 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -200,6 +200,11 @@ class TestableJointTrajectoryController size_t get_blend_prefix_size() const { return blend_prefix_size_; } + std::shared_ptr get_installed_trajectory_msg() const + { + return has_active_trajectory() ? current_trajectory_->get_trajectory_msg() : nullptr; + } + double get_cmd_timeout() { return cmd_timeout_; } void set_node_options(const rclcpp::NodeOptions & node_options) { node_options_ = node_options; }