+
+namespace
+{
+inline constexpr auto kDescriptionPlanJointSplineThroughPoses = R"(
+
+ Plans a Cartesian path as a single joint-space trajectory:
+ each waypoint is solved for inverse kinematics once, warm-started from the
+ previous solution, and the resulting joint knots are joined by a clamped
+ cubic spline. The arm eases from rest, passes through every waypoint exactly
+ without stopping, and arrives at the last one at rest.
+
+
+ Interpolating in joint space between same-branch solutions is what keeps a
+ redundant arm's posture stable. Solving inverse kinematics per interpolated
+ waypoint instead, as a Cartesian planner does, lets the null space drift
+ between waypoints and can swing the wrist through a different posture on a
+ path whose endpoints both look correct.
+
+
+ The duration is the slower of covering the path at
+ cartesian_speed and holding every joint under its model velocity
+ limit scaled by joint_velocity_scale, so the tip holds a roughly
+ constant speed instead of a constant joint rate.
+
+
+ Feed path from ComputeTopDownKeyposes,
+ seed_joint_state from GetRobotJointState, and the
+ output to ExecuteTrajectory.
+
+ )";
+
+constexpr auto kPortIDPath = "path";
+constexpr auto kPortIDSeedJointState = "seed_joint_state";
+constexpr auto kPortIDPlanningGroupName = "planning_group_name";
+constexpr auto kPortIDTipLink = "tip_link";
+constexpr auto kPortIDTipOffset = "tip_offset";
+constexpr auto kPortIDCartesianSpeed = "cartesian_speed";
+constexpr auto kPortIDJointVelocityScale = "joint_velocity_scale";
+constexpr auto kPortIDSamplingRate = "sampling_rate";
+constexpr auto kPortIDJointTrajectory = "joint_trajectory_msg";
+
+// Peak-to-mean speed ratio used only to size each segment's nominal duration, which sets how
+// much of the spline parameter it gets. The spline's true peak is measured afterwards, so this
+// is a weighting, not a limit; it is the oracle's figure, from the smootherstep profile that
+// preceded the spline.
+constexpr double kNominalPeakSpeedRatio = 1.875;
+
+// A trajectory long enough to be worth executing, and a ceiling so an unreachable waypoint
+// produces a failure rather than an arm that creeps for minutes.
+constexpr double kMinimumDuration = 0.2;
+constexpr double kMaximumDuration = 60.0;
+} // namespace
+
+namespace kinova_vla_test_sim_behaviors
+{
+JointSpline::JointSpline(const std::vector& parameters, const std::vector& knots)
+ : parameters_(parameters), knots_(knots)
+{
+ const std::size_t count = knots.size();
+ if (count < 2)
+ {
+ throw std::invalid_argument(fmt::format("A spline needs at least 2 knots, got {}.", count));
+ }
+ if (parameters.size() != count)
+ {
+ throw std::invalid_argument(
+ fmt::format("Got {} knots but {} parameters; they must correspond.", count, parameters.size()));
+ }
+ const Eigen::Index width = knots.front().size();
+ for (std::size_t i = 0; i < count; ++i)
+ {
+ if (knots[i].size() != width)
+ {
+ throw std::invalid_argument(
+ fmt::format("Knot {} has width {}, but knot 0 has width {}.", i, knots[i].size(), width));
+ }
+ if (i > 0 && !(parameters[i] > parameters[i - 1]))
+ {
+ throw std::invalid_argument(fmt::format("Parameters must strictly increase, but parameter {} is {} and {} is {}.",
+ i - 1, parameters[i - 1], i, parameters[i]));
+ }
+ }
+
+ // Second derivatives at the knots, from the standard tridiagonal moment formulation with
+ // both ends clamped to zero first derivative.
+ Eigen::MatrixXd system = Eigen::MatrixXd::Zero(count, count);
+ Eigen::MatrixXd right_hand_side = Eigen::MatrixXd::Zero(count, width);
+ const auto span = [&](std::size_t i) { return parameters_[i + 1] - parameters_[i]; };
+ const auto slope = [&](std::size_t i) -> Eigen::RowVectorXd {
+ return (knots_[i + 1] - knots_[i]).transpose() / span(i);
+ };
+
+ for (std::size_t i = 1; i + 1 < count; ++i)
+ {
+ system(i, i - 1) = span(i - 1);
+ system(i, i) = 2.0 * (span(i - 1) + span(i));
+ system(i, i + 1) = span(i);
+ right_hand_side.row(i) = 6.0 * (slope(i) - slope(i - 1));
+ }
+ system(0, 0) = 2.0 * span(0);
+ system(0, 1) = span(0);
+ right_hand_side.row(0) = 6.0 * slope(0);
+ system(count - 1, count - 1) = 2.0 * span(count - 2);
+ system(count - 1, count - 2) = span(count - 2);
+ right_hand_side.row(count - 1) = -6.0 * slope(count - 2);
+
+ const Eigen::MatrixXd moments = system.colPivHouseholderQr().solve(right_hand_side);
+ moments_.reserve(count);
+ for (std::size_t i = 0; i < count; ++i)
+ {
+ moments_.push_back(moments.row(i).transpose());
+ }
+}
+
+JointSpline::Segment JointSpline::locate(double s) const
+{
+ // Saturating rather than extrapolating: a cubic continued past its last knot diverges fast.
+ s = std::clamp(s, parameters_.front(), parameters_.back());
+ const auto upper = std::upper_bound(parameters_.begin(), parameters_.end(), s);
+ const auto found = static_cast(std::distance(parameters_.begin(), upper));
+ const std::size_t index = std::clamp(found == 0 ? 0 : found - 1, 0, parameters_.size() - 2);
+ const double width = parameters_[index + 1] - parameters_[index];
+ return { index, width, (parameters_[index + 1] - s) / width, (s - parameters_[index]) / width };
+}
+
+Eigen::VectorXd JointSpline::position(double s) const
+{
+ const auto [i, h, a, b] = locate(s);
+ return a * knots_[i] + b * knots_[i + 1] +
+ ((a * a * a - a) * moments_[i] + (b * b * b - b) * moments_[i + 1]) * (h * h / 6.0);
+}
+
+Eigen::VectorXd JointSpline::velocity(double s) const
+{
+ const auto [i, h, a, b] = locate(s);
+ return (knots_[i + 1] - knots_[i]) / h +
+ ((1.0 - 3.0 * a * a) * moments_[i] + (3.0 * b * b - 1.0) * moments_[i + 1]) * (h / 6.0);
+}
+
+Eigen::VectorXd JointSpline::acceleration(double s) const
+{
+ const auto [i, h, a, b] = locate(s);
+ return a * moments_[i] + b * moments_[i + 1];
+}
+
+Eigen::VectorXd JointSpline::peakSpeed() const
+{
+ Eigen::VectorXd peak = Eigen::VectorXd::Zero(knots_.front().size());
+ for (std::size_t i = 0; i + 1 < parameters_.size(); ++i)
+ {
+ // Speed is quadratic within a segment, so its extremes are at the ends or where the
+ // acceleration crosses zero.
+ std::vector candidates = { parameters_[i], parameters_[i + 1] };
+ for (Eigen::Index j = 0; j < peak.size(); ++j)
+ {
+ const double denominator = moments_[i + 1][j] - moments_[i][j];
+ if (std::abs(denominator) > std::numeric_limits::epsilon())
+ {
+ const double a = moments_[i + 1][j] / denominator;
+ if (a >= 0.0 && a <= 1.0)
+ {
+ candidates.push_back(parameters_[i + 1] - a * (parameters_[i + 1] - parameters_[i]));
+ }
+ }
+ }
+ for (const double s : candidates)
+ {
+ peak = peak.cwiseMax(velocity(s).cwiseAbs());
+ }
+ }
+ return peak;
+}
+
+double segmentDuration(double cartesian_length, const Eigen::VectorXd& joint_delta, double cartesian_speed,
+ const Eigen::VectorXd& joint_velocity_cap)
+{
+ const double cartesian = cartesian_speed > 0.0 ? cartesian_length / cartesian_speed : 0.0;
+ double joint = 0.0;
+ for (Eigen::Index j = 0; j < joint_delta.size() && j < joint_velocity_cap.size(); ++j)
+ {
+ if (joint_velocity_cap[j] > 0.0)
+ {
+ joint = std::max(joint, kNominalPeakSpeedRatio * std::abs(joint_delta[j]) / joint_velocity_cap[j]);
+ }
+ }
+ return std::max(cartesian, joint);
+}
+
+std::vector splineKnotParameters(const std::vector& segment_durations)
+{
+ std::vector parameters{ 0.0 };
+ parameters.reserve(segment_durations.size() + 1);
+ // A zero-length segment would collapse two knots onto one parameter, which the spline
+ // rejects, so give every segment a positive share.
+ const double floor = std::numeric_limits::epsilon();
+ for (const double duration : segment_durations)
+ {
+ parameters.push_back(parameters.back() + std::max(duration, floor));
+ }
+ const double total = parameters.back();
+ for (double& parameter : parameters)
+ {
+ parameter /= total;
+ }
+ return parameters;
+}
+
+double splineDuration(const JointSpline& spline, double cartesian_length, double cartesian_speed,
+ const Eigen::VectorXd& joint_velocity_cap)
+{
+ const double cartesian = cartesian_speed > 0.0 ? cartesian_length / cartesian_speed : 0.0;
+ const Eigen::VectorXd peak = spline.peakSpeed();
+ double joint = 0.0;
+ for (Eigen::Index j = 0; j < peak.size() && j < joint_velocity_cap.size(); ++j)
+ {
+ if (joint_velocity_cap[j] > 0.0)
+ {
+ joint = std::max(joint, peak[j] / joint_velocity_cap[j]);
+ }
+ }
+ return std::clamp(std::max(cartesian, joint), kMinimumDuration, kMaximumDuration);
+}
+
+PlanJointSplineThroughPoses::PlanJointSplineThroughPoses(
+ const std::string& name, const BT::NodeConfiguration& config,
+ const std::shared_ptr& shared_resources)
+ : SharedResourcesNode(name, config, shared_resources)
+{
+}
+
+BT::PortsList PlanJointSplineThroughPoses::providedPorts()
+{
+ return {
+ BT::InputPort>(kPortIDPath, "{path}",
+ "Waypoints to pass through, in order."),
+ BT::InputPort(kPortIDSeedJointState, "{seed_joint_state}",
+ "Joint positions the trajectory starts from, from "
+ "GetRobotJointState. Also seeds the first inverse kinematics "
+ "solve, which fixes the branch the whole path stays on."),
+ BT::InputPort(kPortIDPlanningGroupName, "manipulator", "SRDF joint group to plan for."),
+ BT::InputPort(kPortIDTipLink, "grasp_link", "Link the waypoints are solved for."),
+ BT::InputPort>(kPortIDTipOffset, "0;0;0",
+ "Translation from tip_link to the frame the waypoints actually position, "
+ "in the tip_link frame, semicolon separated."),
+ BT::InputPort(kPortIDCartesianSpeed, "0.065", "Tip speed budget along the path, in meters per second."),
+ BT::InputPort(kPortIDJointVelocityScale, "0.5",
+ "Fraction of each joint's model velocity limit the trajectory may reach. Binds only "
+ "where the path is short enough that cartesian_speed would exceed it."),
+ BT::InputPort(kPortIDSamplingRate, "100", "Output trajectory sampling rate, in Hz."),
+ BT::OutputPort(kPortIDJointTrajectory, "{joint_trajectory_msg}",
+ "Timed trajectory for ExecuteTrajectory."),
+ };
+}
+
+BT::KeyValueVector PlanJointSplineThroughPoses::metadata()
+{
+ return { { moveit_pro::behaviors::kSubcategoryMetadataKey, "Color-Cube Stacking" },
+ { moveit_pro::behaviors::kDescriptionMetadataKey, kDescriptionPlanJointSplineThroughPoses } };
+}
+
+BT::NodeStatus PlanJointSplineThroughPoses::tick()
+{
+ const auto ports = moveit_pro::behaviors::getRequiredInputs(
+ getInput>(kPortIDPath),
+ getInput(kPortIDSeedJointState), getInput(kPortIDPlanningGroupName),
+ getInput(kPortIDTipLink), getInput>(kPortIDTipOffset),
+ getInput(kPortIDCartesianSpeed), getInput(kPortIDJointVelocityScale),
+ getInput(kPortIDSamplingRate));
+ if (!ports.has_value())
+ {
+ getBehaviorContext()->logger->publishFailureMessage(
+ name(), "Failed to get required values from input data ports: " + ports.error());
+ return BT::NodeStatus::FAILURE;
+ }
+ const auto& [path, seed_joint_state, planning_group_name, tip_link, tip_offset, cartesian_speed, joint_velocity_scale,
+ sampling_rate] = ports.value();
+
+ if (path.empty())
+ {
+ getBehaviorContext()->logger->publishFailureMessage(name(), "path is empty, so there is nothing to plan.");
+ return BT::NodeStatus::FAILURE;
+ }
+ if (tip_offset.size() != 3)
+ {
+ getBehaviorContext()->logger->publishFailureMessage(name(), fmt::format("tip_offset needs 3 values, got {}.",
+ tip_offset.size()));
+ return BT::NodeStatus::FAILURE;
+ }
+ if (sampling_rate == 0)
+ {
+ getBehaviorContext()->logger->publishFailureMessage(name(), "sampling_rate must be positive.");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ const auto& robot_model = getBehaviorContext()->robot_model;
+ const auto* joint_group = robot_model ? robot_model->getJointModelGroup(planning_group_name) : nullptr;
+ if (joint_group == nullptr)
+ {
+ getBehaviorContext()->logger->publishFailureMessage(
+ name(), fmt::format("No planning group '{}' in the robot model.", planning_group_name));
+ return BT::NodeStatus::FAILURE;
+ }
+
+ moveit_pro::base::RobotState state(robot_model);
+ state.setToDefaultValues();
+ for (std::size_t i = 0; i < seed_joint_state.name.size() && i < seed_joint_state.position.size(); ++i)
+ {
+ if (robot_model->hasJointModel(seed_joint_state.name[i]))
+ {
+ state.setJointPositions(seed_joint_state.name[i], { seed_joint_state.position[i] });
+ }
+ }
+ state.update();
+
+ // The waypoints position a frame offset from tip_link, so the link itself has to arrive
+ // the same offset short of each one.
+ const Eigen::Translation3d offset(-tip_offset[0], -tip_offset[1], -tip_offset[2]);
+
+ std::vector knots;
+ std::vector tip_positions;
+ {
+ std::vector positions;
+ state.copyJointGroupPositions(joint_group, positions);
+ knots.push_back(Eigen::Map(positions.data(), static_cast(positions.size())));
+ tip_positions.push_back((state.getGlobalLinkTransform(tip_link) * offset.inverse()).translation());
+ }
+
+ for (std::size_t i = 0; i < path.size(); ++i)
+ {
+ Eigen::Isometry3d waypoint;
+ tf2::fromMsg(path[i].pose, waypoint);
+ if (!state.setFromIK(joint_group, waypoint * offset, tip_link))
+ {
+ getBehaviorContext()->logger->publishFailureMessage(
+ name(), fmt::format("Inverse kinematics failed for waypoint {} of {}.", i + 1, path.size()));
+ return BT::NodeStatus::FAILURE;
+ }
+ std::vector positions;
+ state.copyJointGroupPositions(joint_group, positions);
+ knots.push_back(Eigen::Map(positions.data(), static_cast(positions.size())));
+ tip_positions.push_back(waypoint.translation());
+ }
+
+ Eigen::VectorXd velocity_cap(knots.front().size());
+ const auto& bounds = joint_group->getActiveJointModelsBounds();
+ for (Eigen::Index j = 0; j < velocity_cap.size(); ++j)
+ {
+ const auto index = static_cast(j);
+ const bool bounded = index < bounds.size() && !bounds[index]->empty() && bounds[index]->front().velocity_bounded_;
+ velocity_cap[j] =
+ bounded ? bounds[index]->front().max_velocity_ * joint_velocity_scale : std::numeric_limits::infinity();
+ }
+
+ std::vector segment_durations;
+ double cartesian_length = 0.0;
+ segment_durations.reserve(knots.size() - 1);
+ for (std::size_t i = 0; i + 1 < knots.size(); ++i)
+ {
+ const double length = (tip_positions[i + 1] - tip_positions[i]).norm();
+ cartesian_length += length;
+ segment_durations.push_back(segmentDuration(length, knots[i + 1] - knots[i], cartesian_speed, velocity_cap));
+ }
+
+ std::unique_ptr spline;
+ try
+ {
+ spline = std::make_unique(splineKnotParameters(segment_durations), knots);
+ }
+ catch (const std::invalid_argument& exception)
+ {
+ getBehaviorContext()->logger->publishFailureMessage(name(), fmt::format("Could not fit a spline through the "
+ "inverse kinematics solutions: {}",
+ exception.what()));
+ return BT::NodeStatus::FAILURE;
+ }
+
+ const double duration = splineDuration(*spline, cartesian_length, cartesian_speed, velocity_cap);
+ const auto steps = static_cast(std::ceil(duration * sampling_rate));
+
+ trajectory_msgs::msg::JointTrajectory trajectory;
+ trajectory.header = path.front().header;
+ trajectory.joint_names = joint_group->getVariableNames();
+ trajectory.points.reserve(steps + 1);
+ for (std::size_t step = 0; step <= steps; ++step)
+ {
+ const double s = static_cast(step) / static_cast(steps);
+ const Eigen::VectorXd position = spline->position(s);
+ const Eigen::VectorXd velocity = spline->velocity(s) / duration;
+ const Eigen::VectorXd acceleration = spline->acceleration(s) / (duration * duration);
+
+ trajectory_msgs::msg::JointTrajectoryPoint point;
+ point.positions.assign(position.data(), position.data() + position.size());
+ point.velocities.assign(velocity.data(), velocity.data() + velocity.size());
+ point.accelerations.assign(acceleration.data(), acceleration.data() + acceleration.size());
+ point.time_from_start = rclcpp::Duration::from_seconds(s * duration);
+ trajectory.points.push_back(std::move(point));
+ }
+ setOutput(kPortIDJointTrajectory, trajectory);
+
+ return BT::NodeStatus::SUCCESS;
+}
+} // namespace kinova_vla_test_sim_behaviors
diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim_behaviors/src/register_behaviors.cpp b/src/moveit_pro_kinova_configs/kinova_vla_test_sim_behaviors/src/register_behaviors.cpp
new file mode 100644
index 000000000..a59e0844c
--- /dev/null
+++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim_behaviors/src/register_behaviors.cpp
@@ -0,0 +1,36 @@
+// Copyright 2026 PickNik Inc.
+// All rights reserved.
+//
+// Unauthorized copying of this code base via any medium is strictly prohibited.
+// Proprietary and confidential.
+
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+
+#include
+
+namespace kinova_vla_test_sim_behaviors
+{
+class KinovaVlaTestSimBehaviorsLoader : public moveit_pro::behaviors::SharedResourcesNodeLoaderBase
+{
+public:
+ void registerBehaviors(BT::BehaviorTreeFactory& factory,
+ const std::shared_ptr& shared_resources) override
+ {
+ moveit_pro::behaviors::registerBehavior(factory, "ComputeTopDownKeyposes", shared_resources);
+ moveit_pro::behaviors::registerBehavior(factory, "PlanJointSplineThroughPoses",
+ shared_resources);
+ moveit_pro::behaviors::registerBehavior(factory, "SendGripperCommand", shared_resources);
+ moveit_pro::behaviors::registerBehavior(factory, "WaitForEpisodeStart", shared_resources);
+ }
+};
+} // namespace kinova_vla_test_sim_behaviors
+
+PLUGINLIB_EXPORT_CLASS(kinova_vla_test_sim_behaviors::KinovaVlaTestSimBehaviorsLoader,
+ moveit_pro::behaviors::SharedResourcesNodeLoaderBase);
diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim_behaviors/src/send_gripper_command.cpp b/src/moveit_pro_kinova_configs/kinova_vla_test_sim_behaviors/src/send_gripper_command.cpp
new file mode 100644
index 000000000..5ecdfaff6
--- /dev/null
+++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim_behaviors/src/send_gripper_command.cpp
@@ -0,0 +1,110 @@
+// Copyright 2026 PickNik Inc.
+// All rights reserved.
+//
+// Unauthorized copying of this code base via any medium is strictly prohibited.
+// Proprietary and confidential.
+
+#include
+
+#include
+
+#include
+#include
+#include
+
+namespace
+{
+inline constexpr auto kDescriptionSendGripperCommand = R"(
+
+ Sends the gripper a position goal and succeeds as soon as the server accepts
+ it, without waiting for the jaws to arrive.
+
+
+ This is how ExecutePolicy drives the gripper at deploy time: one
+ goal per change of target, never awaited and never cancelled, while the arm
+ keeps moving. Collecting demonstrations any other way records a grip the
+ policy cannot reproduce. MoveGripperAction blocks instead, and
+ closing onto an object never reaches the commanded position, so it returns
+ only once the jaws stop compressing it, seconds after the grip is solid.
+ Cancelling that wait is not a fix: this controller rewrites its target to
+ wherever the jaws are, which pins a weaker grip than a live goal would hold.
+
+
+ Pair with a WaitForDuration to hold the arm still while the jaws
+ travel.
+
+ )";
+
+constexpr auto kPortIDActionName = "gripper_command_action_name";
+constexpr auto kPortIDPosition = "position";
+constexpr auto kPortIDMaxEffort = "max_effort";
+constexpr auto kPortIDServerTimeout = "wait_for_server_timeout";
+} // namespace
+
+namespace kinova_vla_test_sim_behaviors
+{
+SendGripperCommand::SendGripperCommand(const std::string& name, const BT::NodeConfiguration& config,
+ const std::shared_ptr& shared_resources)
+ : SharedResourcesNode(name, config, shared_resources)
+{
+}
+
+BT::PortsList SendGripperCommand::providedPorts()
+{
+ return {
+ BT::InputPort(kPortIDActionName, "/robotiq_gripper_controller/gripper_cmd",
+ "GripperCommand action that actuates the gripper."),
+ BT::InputPort(kPortIDPosition, "Gripper joint target position."),
+ BT::InputPort(kPortIDMaxEffort, "0.0", "Effort ceiling; 0 leaves it to the controller."),
+ BT::InputPort(kPortIDServerTimeout, "3.0", "Seconds to wait for the action server to appear."),
+ };
+}
+
+BT::KeyValueVector SendGripperCommand::metadata()
+{
+ return { { moveit_pro::behaviors::kSubcategoryMetadataKey, "Color-Cube Stacking" },
+ { moveit_pro::behaviors::kDescriptionMetadataKey, kDescriptionSendGripperCommand } };
+}
+
+BT::NodeStatus SendGripperCommand::tick()
+{
+ const auto ports =
+ moveit_pro::behaviors::getRequiredInputs(getInput(kPortIDActionName),
+ getInput(kPortIDPosition), getInput(kPortIDMaxEffort),
+ getInput(kPortIDServerTimeout));
+ if (!ports.has_value())
+ {
+ getBehaviorContext()->logger->publishFailureMessage(
+ name(), "Failed to get required values from input data ports: " + ports.error());
+ return BT::NodeStatus::FAILURE;
+ }
+ const auto& [action_name, position, max_effort, server_timeout] = ports.value();
+
+ // The client is kept between ticks: it outlives the goal request either way, and the
+ // shared node's executor is what delivers it.
+ if (client_ == nullptr || action_name_ != action_name)
+ {
+ client_ =
+ rclcpp_action::create_client(getBehaviorContext()->node, action_name);
+ action_name_ = action_name;
+ }
+
+ const auto timeout = std::chrono::duration(server_timeout);
+ if (!client_->wait_for_action_server(std::chrono::duration_cast(timeout)))
+ {
+ getBehaviorContext()->logger->publishFailureMessage(name(), fmt::format("No GripperCommand action server on '{}'.",
+ action_name));
+ return BT::NodeStatus::FAILURE;
+ }
+
+ control_msgs::action::GripperCommand::Goal goal;
+ goal.command.position = position;
+ goal.command.max_effort = max_effort;
+ // The goal handle is deliberately dropped: waiting on it is the blocking this Behavior
+ // exists to avoid, so a rejected goal shows up as a gripper that did not move rather
+ // than a FAILURE here.
+ client_->async_send_goal(goal);
+
+ return BT::NodeStatus::SUCCESS;
+}
+} // namespace kinova_vla_test_sim_behaviors
diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim_behaviors/src/wait_for_episode_start.cpp b/src/moveit_pro_kinova_configs/kinova_vla_test_sim_behaviors/src/wait_for_episode_start.cpp
new file mode 100644
index 000000000..df5b41193
--- /dev/null
+++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim_behaviors/src/wait_for_episode_start.cpp
@@ -0,0 +1,151 @@
+// Copyright 2026 PickNik Inc.
+// All rights reserved.
+//
+// Unauthorized copying of this code base via any medium is strictly prohibited.
+// Proprietary and confidential.
+
+#include
+
+#include
+#include
+
+#include
+#include
+#include
+#include
+
+namespace
+{
+inline constexpr auto kDescriptionWaitForEpisodeStart = R"(
+
+ Blocks until the active Trainer recording session reports that it has opened
+ an episode, then succeeds.
+
+
+ RecordEpisode returns as soon as the recorder process has been
+ spawned, but the episode does not begin until the Trainer publishes its start
+ marker, seconds later. Motion before that marker is silently discarded at
+ conversion: the converter segments on markers and drops any segment holding
+ none, treating it as recorder spillover. Demonstrations then begin partway
+ through the motion, with no error to notice. Tick this between
+ RecordEpisode and the demonstrated motion.
+
+
+ This polls the session state rather than watching for the marker, so it cannot
+ miss the transition by subscribing a moment too late. It is still worth pairing
+ with a short WaitForDuration, which keeps the arm still across the
+ few frames straddling the marker; the converter's idle trimming removes them.
+
+
+ Fails when no episode opens within timeout, naming the last state
+ seen.
+
+ )";
+
+constexpr auto kPortIDServiceName = "service_name";
+constexpr auto kPortIDTimeout = "timeout";
+
+constexpr auto kDefaultServiceName = "/trainer/active_recording";
+
+// The RecordingState value that means the episode's start marker has been published.
+constexpr auto kRecordingState = "recording";
+
+constexpr std::chrono::milliseconds kPollPeriod{ 50 };
+constexpr std::chrono::duration kServerTimeout{ 5.0 };
+constexpr std::chrono::duration kResponseTimeout{ 5.0 };
+
+/** @brief The session's state, or "" when no session is active or the payload has no state. */
+std::string stateOf(const std::string& session_json)
+{
+ if (session_json.empty())
+ {
+ return "";
+ }
+ const auto session = nlohmann::json::parse(session_json, nullptr, false);
+ if (session.is_discarded() || !session.contains("state") || !session["state"].is_string())
+ {
+ return "";
+ }
+ return session["state"].get();
+}
+} // namespace
+
+namespace kinova_vla_test_sim_behaviors
+{
+WaitForEpisodeStart::WaitForEpisodeStart(const std::string& name, const BT::NodeConfiguration& config,
+ const std::shared_ptr& shared_resources)
+ : AsyncBehaviorBase(name, config, shared_resources)
+ , client_(std::make_unique>(shared_resources))
+{
+}
+
+BT::PortsList WaitForEpisodeStart::providedPorts()
+{
+ return {
+ BT::InputPort(kPortIDServiceName, kDefaultServiceName, "Name of the Trainer active_recording service."),
+ BT::InputPort(kPortIDTimeout, "30.0", "Seconds to wait for an episode to open."),
+ };
+}
+
+BT::KeyValueVector WaitForEpisodeStart::metadata()
+{
+ return { { moveit_pro::behaviors::kSubcategoryMetadataKey, "Color-Cube Stacking" },
+ { moveit_pro::behaviors::kDescriptionMetadataKey, kDescriptionWaitForEpisodeStart } };
+}
+
+tl::expected WaitForEpisodeStart::doWork()
+{
+ const auto ports = moveit_pro::behaviors::getRequiredInputs(getInput(kPortIDServiceName),
+ getInput(kPortIDTimeout));
+ if (!ports.has_value())
+ {
+ return tl::make_unexpected("Failed to get required values from input data ports: " + ports.error());
+ }
+ const auto& [service_name, timeout] = ports.value();
+
+ halted_ = false;
+ client_->initialize(service_name, kServerTimeout, kResponseTimeout);
+ if (!client_->waitForServiceServer())
+ {
+ return tl::make_unexpected(fmt::format("No Trainer active_recording service on '{}'.", service_name));
+ }
+ // Nothing here holds a goal, so a halt may interrupt the poll as soon as it arrives.
+ notifyCanHalt();
+
+ const auto deadline = std::chrono::steady_clock::now() + std::chrono::duration(timeout);
+ std::string last_state;
+ while (!halted_)
+ {
+ const auto response = client_->syncSendRequest(GetActiveRecordingSrv::Request{});
+ if (!response.has_value())
+ {
+ return tl::make_unexpected("Failed to read the active recording: " + response.error());
+ }
+ if (!response.value().status.success)
+ {
+ return tl::make_unexpected("The Trainer refused to report the active recording: " +
+ response.value().status.error_message);
+ }
+ last_state = stateOf(response.value().session_json);
+ if (last_state == kRecordingState)
+ {
+ return true;
+ }
+ if (std::chrono::steady_clock::now() >= deadline)
+ {
+ return tl::make_unexpected(
+ fmt::format("No recording episode opened within {:g}s; the session's last state was '{}'.", timeout,
+ last_state.empty() ? "no active session" : last_state));
+ }
+ std::this_thread::sleep_for(kPollPeriod);
+ }
+ return false;
+}
+
+tl::expected WaitForEpisodeStart::doHalt()
+{
+ halted_ = true;
+ client_->cancelRequest();
+ return {};
+}
+} // namespace kinova_vla_test_sim_behaviors
diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim_behaviors/test/CMakeLists.txt b/src/moveit_pro_kinova_configs/kinova_vla_test_sim_behaviors/test/CMakeLists.txt
new file mode 100644
index 000000000..fe9ea895b
--- /dev/null
+++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim_behaviors/test/CMakeLists.txt
@@ -0,0 +1,24 @@
+find_package(ament_cmake_gtest REQUIRED)
+
+# rclcpp::init wedges on discovery when it shares a ROS domain with a live backend,
+# so every test that brings up a node gets a domain of its own -- one each, since
+# colcon runs them in parallel and they would otherwise discover each other. The
+# domain is read while the rmw library loads, too early for the test's own main().
+ament_add_gtest(test_behavior_plugins test_behavior_plugins.cpp ENV "ROS_DOMAIN_ID=89")
+ament_target_dependencies(test_behavior_plugins ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+ament_add_gtest(test_compute_top_down_keyposes test_compute_top_down_keyposes.cpp)
+target_link_libraries(test_compute_top_down_keyposes kinova_vla_test_sim_behaviors)
+ament_target_dependencies(test_compute_top_down_keyposes ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+ament_add_gtest(test_plan_joint_spline_through_poses test_plan_joint_spline_through_poses.cpp)
+target_link_libraries(test_plan_joint_spline_through_poses kinova_vla_test_sim_behaviors)
+ament_target_dependencies(test_plan_joint_spline_through_poses ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+ament_add_gtest(test_send_gripper_command test_send_gripper_command.cpp ENV "ROS_DOMAIN_ID=90")
+target_link_libraries(test_send_gripper_command kinova_vla_test_sim_behaviors)
+ament_target_dependencies(test_send_gripper_command ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+ament_add_gtest(test_wait_for_episode_start test_wait_for_episode_start.cpp ENV "ROS_DOMAIN_ID=91")
+target_link_libraries(test_wait_for_episode_start kinova_vla_test_sim_behaviors)
+ament_target_dependencies(test_wait_for_episode_start ${THIS_PACKAGE_INCLUDE_DEPENDS})
diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim_behaviors/test/test_behavior_plugins.cpp b/src/moveit_pro_kinova_configs/kinova_vla_test_sim_behaviors/test/test_behavior_plugins.cpp
new file mode 100644
index 000000000..68d27dcd1
--- /dev/null
+++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim_behaviors/test/test_behavior_plugins.cpp
@@ -0,0 +1,47 @@
+// Copyright 2026 PickNik Inc.
+// All rights reserved.
+//
+// Unauthorized copying of this code base via any medium is strictly prohibited.
+// Proprietary and confidential.
+
+#include
+
+#include
+#include
+#include
+#include
+
+/**
+ * @brief This test makes sure that the Behaviors provided in this package can be successfully registered and
+ * instantiated by the behavior tree factory.
+ */
+TEST(BehaviorTests, test_load_behavior_plugins)
+{
+ pluginlib::ClassLoader class_loader(
+ "moveit_pro_behavior_interface", "moveit_pro::behaviors::SharedResourcesNodeLoaderBase");
+
+ auto node = std::make_shared("test_node");
+ auto shared_resources = std::make_shared(node);
+
+ BT::BehaviorTreeFactory factory;
+ {
+ auto plugin_instance =
+ class_loader.createUniqueInstance("kinova_vla_test_sim_behaviors::KinovaVlaTestSimBehaviorsLoader");
+ ASSERT_NO_THROW(plugin_instance->registerBehaviors(factory, shared_resources));
+ }
+ // Test that ClassLoader is able to find and instantiate each Behavior using the package's plugin description info.
+ for (const auto& behavior_name :
+ { "ComputeTopDownKeyposes", "PlanJointSplineThroughPoses", "SendGripperCommand", "WaitForEpisodeStart" })
+ {
+ EXPECT_NO_THROW((void)factory.instantiateTreeNode("test_behavior_name", behavior_name, BT::NodeConfiguration()))
+ << "Behavior '" << behavior_name << "' is registered but could not be instantiated.";
+ }
+}
+
+int main(int argc, char** argv)
+{
+ rclcpp::init(argc, argv);
+
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim_behaviors/test/test_compute_top_down_keyposes.cpp b/src/moveit_pro_kinova_configs/kinova_vla_test_sim_behaviors/test/test_compute_top_down_keyposes.cpp
new file mode 100644
index 000000000..f348ea743
--- /dev/null
+++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim_behaviors/test/test_compute_top_down_keyposes.cpp
@@ -0,0 +1,229 @@
+// Copyright 2026 PickNik Inc.
+// All rights reserved.
+//
+// Unauthorized copying of this code base via any medium is strictly prohibited.
+// Proprietary and confidential.
+
+#include
+
+#include
+#include
+#include
+
+#include
+
+namespace
+{
+using kinova_vla_test_sim_behaviors::chooseTopDownYawByCost;
+using kinova_vla_test_sim_behaviors::computeTopDownKeyposes;
+using kinova_vla_test_sim_behaviors::jointDistanceCost;
+using kinova_vla_test_sim_behaviors::topDownGraspOrientation;
+using kinova_vla_test_sim_behaviors::yawOf;
+
+constexpr double kEpsilon = 1e-9;
+constexpr double kQuarterTurn = M_PI / 2.0;
+
+Eigen::Isometry3d makePose(const Eigen::Vector3d& position, double yaw)
+{
+ Eigen::Isometry3d pose(Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()));
+ pose.translation() = position;
+ return pose;
+}
+
+// The oracle's Q_TOPDOWN, in the (x, y, z, w) order Eigen's constructor takes last.
+Eigen::Quaterniond oracleTopDown()
+{
+ return Eigen::Quaterniond(0.0, 1.0, 0.0, 0.0);
+}
+} // namespace
+
+TEST(TopDownGraspOrientation, ZeroYawMatchesTheOracleBaseOrientation)
+{
+ // mujoco_ik.py pins Q_TOPDOWN = (w, x, y, z) = (0, 1, 0, 0). A different sign convention or
+ // axis here would still point the jaws down but spin them 90 degrees off the cube.
+ EXPECT_NEAR(std::abs(topDownGraspOrientation(0.0).dot(oracleTopDown())), 1.0, kEpsilon);
+}
+
+TEST(TopDownGraspOrientation, ApproachAxisPointsDown)
+{
+ for (const double yaw : { -2.0, 0.0, 0.7, 3.0 })
+ {
+ const Eigen::Vector3d approach = topDownGraspOrientation(yaw) * Eigen::Vector3d::UnitZ();
+ EXPECT_NEAR(approach.z(), -1.0, kEpsilon) << "yaw " << yaw;
+ }
+}
+
+TEST(TopDownGraspOrientation, SpinsTheJawAxisByYaw)
+{
+ // The jaw axis is the tip's +Y. Spinning by yaw must rotate it in the horizontal plane, or the
+ // grasp does not line up with the cube's faces.
+ // Rx(pi) flips +Y to -Y, then Rz(pi/2) carries that onto +X.
+ const Eigen::Vector3d jaw = topDownGraspOrientation(kQuarterTurn) * Eigen::Vector3d::UnitY();
+ EXPECT_NEAR(jaw.x(), 1.0, kEpsilon);
+ EXPECT_NEAR(jaw.z(), 0.0, kEpsilon);
+}
+
+TEST(YawOf, RoundTripsTopDownGraspOrientation)
+{
+ for (const double yaw : { -1.5, -0.2, 0.0, 0.9 })
+ {
+ EXPECT_NEAR(yawOf(topDownGraspOrientation(yaw)), yaw, kEpsilon) << "yaw " << yaw;
+ }
+}
+
+TEST(JointDistanceCost, IsZeroForTheSamePose)
+{
+ const std::vector pose{ 0.1, -0.2, 0.3 };
+ EXPECT_NEAR(jointDistanceCost(pose, pose), 0.0, kEpsilon);
+}
+
+TEST(JointDistanceCost, SumsTheSquaredPerJointDifferences)
+{
+ // The oracle's score, so a joint that moves twice as far counts four times as much and one
+ // big wrist swing outweighs several small arm adjustments.
+ EXPECT_NEAR(jointDistanceCost({ 0.0, 0.0 }, { 3.0, 4.0 }), 25.0, kEpsilon);
+}
+
+TEST(ChooseTopDownYawByCost, ReturnsACubeYawPlusAWholeNumberOfQuarterTurns)
+{
+ // Every candidate must be a symmetry of the cube. A yaw that is not one grasps a corner.
+ const double chosen =
+ chooseTopDownYawByCost(0.3, [](double yaw) { return std::optional(std::abs(yaw)); }).value();
+ const double turns = (chosen - 0.3) / kQuarterTurn;
+ EXPECT_NEAR(turns, std::round(turns), kEpsilon);
+}
+
+TEST(ChooseTopDownYawByCost, PicksTheCheapestCandidateRatherThanTheNearest)
+{
+ // The whole point of scoring by IK: on eval_0 the oracle takes a candidate 180 degrees from
+ // the wrist's current yaw because it holds joint_5 still. A nearest-yaw rule cannot do that.
+ const double cube_yaw = 0.0;
+ const auto cost = [](double yaw) -> std::optional {
+ // cheapest at two quarter turns, i.e. the candidate a half turn away
+ return std::abs(std::remainder(yaw - M_PI, 2.0 * M_PI));
+ };
+ EXPECT_NEAR(chooseTopDownYawByCost(cube_yaw, cost).value(), M_PI, kEpsilon);
+}
+
+TEST(ChooseTopDownYawByCost, SkipsUnreachableCandidates)
+{
+ // IK fails on candidates that would put the wrist past a limit; those must not be chosen
+ // even when a reachable one scores worse.
+ const auto cost = [](double yaw) -> std::optional {
+ if (std::abs(std::remainder(yaw, 2.0 * M_PI)) < kEpsilon)
+ {
+ return 0.0; // cheapest, but pretend it is the only reachable one below
+ }
+ return std::nullopt;
+ };
+ EXPECT_NEAR(chooseTopDownYawByCost(0.0, cost).value(), 0.0, kEpsilon);
+}
+
+TEST(ChooseTopDownYawByCost, ReturnsNulloptWhenNothingIsReachable)
+{
+ // The segment must fail loudly rather than plan a path the arm cannot follow.
+ EXPECT_FALSE(chooseTopDownYawByCost(0.4, [](double) { return std::nullopt; }).has_value());
+}
+
+TEST(ChooseTopDownYawByCost, IsUnchangedByCubeYawsAQuarterTurnApart)
+{
+ // The cube's own yaw is only known modulo a quarter turn, so equivalent readings of the same
+ // physical cube must produce the same grasp.
+ const auto cost = [](double yaw) -> std::optional { return std::abs(std::remainder(yaw - 0.9, 2.0 * M_PI)); };
+ const double base = chooseTopDownYawByCost(0.2, cost).value();
+ EXPECT_NEAR(std::remainder(chooseTopDownYawByCost(0.2 + kQuarterTurn, cost).value() - base, 2.0 * M_PI), 0.0,
+ kEpsilon);
+ EXPECT_NEAR(std::remainder(chooseTopDownYawByCost(0.2 - kQuarterTurn, cost).value() - base, 2.0 * M_PI), 0.0,
+ kEpsilon);
+}
+
+TEST(ComputeTopDownKeyposes, StacksOneWaypointPerHeightAboveTheAimPose)
+{
+ const auto keyposes = computeTopDownKeyposes(makePose({ 0.5, -0.1, 0.115 }, 0.0), topDownGraspOrientation(0.0),
+ Eigen::Vector3d::Zero(), { 0.12, 0.0 });
+
+ ASSERT_EQ(keyposes.size(), 2u);
+ EXPECT_NEAR(keyposes[0].translation().z(), 0.235, kEpsilon);
+ EXPECT_NEAR(keyposes[1].translation().z(), 0.115, kEpsilon);
+ for (const auto& keypose : keyposes)
+ {
+ EXPECT_NEAR(keypose.translation().x(), 0.5, kEpsilon);
+ EXPECT_NEAR(keypose.translation().y(), -0.1, kEpsilon);
+ }
+}
+
+TEST(ComputeTopDownKeyposes, GivesEveryWaypointTheChosenOrientation)
+{
+ // The oracle descends straight down onto the cube. Re-deriving the orientation per waypoint
+ // would let the wrist rotate mid-descent and shear the grasp.
+ const Eigen::Quaterniond orientation = topDownGraspOrientation(0.4);
+ const auto keyposes = computeTopDownKeyposes(makePose({ 0.5, 0.0, 0.115 }, 0.4), orientation, Eigen::Vector3d::Zero(),
+ { 0.12, 0.06, 0.0 });
+
+ ASSERT_EQ(keyposes.size(), 3u);
+ for (const auto& keypose : keyposes)
+ {
+ EXPECT_NEAR(std::abs(Eigen::Quaterniond(keypose.rotation()).dot(orientation)), 1.0, kEpsilon);
+ }
+}
+
+TEST(ComputeTopDownKeyposes, PlacesTheHeldObjectRatherThanTheTipWhenOffsetIsSet)
+{
+ // The place segment aims the carried cube at the target, so the tip must land offset by
+ // exactly the grip, rotated into the world.
+ const Eigen::Vector3d grip_offset(0.0, 0.0, 0.02);
+ const auto keyposes =
+ computeTopDownKeyposes(makePose({ 0.4, 0.2, 0.115 }, 0.0), topDownGraspOrientation(0.0), grip_offset, { 0.031 });
+
+ ASSERT_EQ(keyposes.size(), 1u);
+ // The tip's +Z points down, so an object 20 mm along +Z sits 20 mm below the tip: the tip goes
+ // that much higher for the object to land on the aim point.
+ EXPECT_NEAR(keyposes[0].translation().z(), 0.115 + 0.031 + 0.02, kEpsilon);
+ EXPECT_NEAR(keyposes[0].translation().x(), 0.4, kEpsilon);
+ EXPECT_NEAR(keyposes[0].translation().y(), 0.2, kEpsilon);
+}
+
+TEST(ComputeTopDownKeyposes, RotatesALateralHeldObjectOffsetIntoTheWorld)
+{
+ // A grip that is off-center laterally must be corrected in the direction the wrist is actually
+ // pointing. Ignoring the rotation would put the cube down on the wrong side of the target.
+ const Eigen::Vector3d grip_offset(0.01, 0.0, 0.0);
+ const auto keyposes = computeTopDownKeyposes(makePose({ 0.4, 0.2, 0.115 }, kQuarterTurn),
+ topDownGraspOrientation(kQuarterTurn), grip_offset, { 0.0 });
+
+ ASSERT_EQ(keyposes.size(), 1u);
+ // Rz(pi/2) * Rx(pi) maps the tip's +X onto world +Y, so the tip shifts back along -Y.
+ EXPECT_NEAR(keyposes[0].translation().x(), 0.4, kEpsilon);
+ EXPECT_NEAR(keyposes[0].translation().y(), 0.19, kEpsilon);
+}
+
+TEST(ComputeTopDownKeyposes, ReturnsAnEmptyPathForNoHeights)
+{
+ EXPECT_TRUE(computeTopDownKeyposes(makePose({ 0.5, 0.0, 0.115 }, 0.0), topDownGraspOrientation(0.0),
+ Eigen::Vector3d::Zero(), {})
+ .empty());
+}
+
+TEST(ComputeTopDownKeyposes, TakesOnlyThePositionFromATiltedAimPose)
+{
+ // Cube poses come from live physics and are never exactly level. The waypoint must still sit
+ // straight above the cube, since the arm approaches vertically.
+ Eigen::Isometry3d tilted(Eigen::AngleAxisd(0.3, Eigen::Vector3d::UnitZ()) *
+ Eigen::AngleAxisd(0.05, Eigen::Vector3d::UnitX()));
+ tilted.translation() = Eigen::Vector3d(0.5, 0.0, 0.115);
+
+ const auto keyposes = computeTopDownKeyposes(tilted, topDownGraspOrientation(0.3), Eigen::Vector3d::Zero(), { 0.12 });
+
+ ASSERT_EQ(keyposes.size(), 1u);
+ EXPECT_NEAR(keyposes[0].translation().x(), 0.5, kEpsilon);
+ EXPECT_NEAR(keyposes[0].translation().y(), 0.0, kEpsilon);
+ EXPECT_NEAR(keyposes[0].translation().z(), 0.235, kEpsilon);
+ const Eigen::Vector3d approach = keyposes[0].rotation() * Eigen::Vector3d::UnitZ();
+ EXPECT_NEAR(approach.z(), -1.0, kEpsilon);
+}
+
+int main(int argc, char** argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim_behaviors/test/test_plan_joint_spline_through_poses.cpp b/src/moveit_pro_kinova_configs/kinova_vla_test_sim_behaviors/test/test_plan_joint_spline_through_poses.cpp
new file mode 100644
index 000000000..3c8c7b7ee
--- /dev/null
+++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim_behaviors/test/test_plan_joint_spline_through_poses.cpp
@@ -0,0 +1,181 @@
+// Copyright 2026 PickNik Inc.
+// All rights reserved.
+//
+// Unauthorized copying of this code base via any medium is strictly prohibited.
+// Proprietary and confidential.
+
+#include
+#include
+
+#include
+
+#include
+
+namespace
+{
+using kinova_vla_test_sim_behaviors::JointSpline;
+using kinova_vla_test_sim_behaviors::segmentDuration;
+using kinova_vla_test_sim_behaviors::splineDuration;
+using kinova_vla_test_sim_behaviors::splineKnotParameters;
+
+Eigen::VectorXd vec(std::initializer_list values)
+{
+ Eigen::VectorXd result(static_cast(values.size()));
+ Eigen::Index i = 0;
+ for (const double value : values)
+ {
+ result[i++] = value;
+ }
+ return result;
+}
+
+/** Two knots one unit apart on a single joint, over the full parameter span. */
+JointSpline twoKnotSpline()
+{
+ return JointSpline({ 0.0, 1.0 }, { vec({ 0.0 }), vec({ 1.0 }) });
+}
+
+/** Three knots, the interior one deliberately off the midpoint so corner-cutting shows. */
+JointSpline threeKnotSpline()
+{
+ return JointSpline({ 0.0, 0.25, 1.0 }, { vec({ 0.0, 0.0 }), vec({ 0.5, -1.0 }), vec({ 2.0, 1.0 }) });
+}
+} // namespace
+
+TEST(JointSpline, RejectsFewerThanTwoKnots)
+{
+ EXPECT_THROW(JointSpline({ 0.0 }, { vec({ 0.0 }) }), std::invalid_argument);
+ EXPECT_THROW(JointSpline({}, {}), std::invalid_argument);
+}
+
+TEST(JointSpline, RejectsParameterCountMismatch)
+{
+ EXPECT_THROW(JointSpline({ 0.0, 0.5, 1.0 }, { vec({ 0.0 }), vec({ 1.0 }) }), std::invalid_argument);
+}
+
+TEST(JointSpline, RejectsNonIncreasingParameters)
+{
+ EXPECT_THROW(JointSpline({ 0.0, 0.0 }, { vec({ 0.0 }), vec({ 1.0 }) }), std::invalid_argument);
+ EXPECT_THROW(JointSpline({ 1.0, 0.0 }, { vec({ 0.0 }), vec({ 1.0 }) }), std::invalid_argument);
+}
+
+TEST(JointSpline, RejectsKnotsOfDifferentWidths)
+{
+ EXPECT_THROW(JointSpline({ 0.0, 1.0 }, { vec({ 0.0 }), vec({ 1.0, 2.0 }) }), std::invalid_argument);
+}
+
+TEST(JointSpline, PassesThroughEveryKnotExactly)
+{
+ const JointSpline spline = threeKnotSpline();
+ EXPECT_NEAR(spline.position(0.0)[0], 0.0, 1e-12);
+ EXPECT_NEAR(spline.position(0.25)[0], 0.5, 1e-12);
+ EXPECT_NEAR(spline.position(0.25)[1], -1.0, 1e-12);
+ EXPECT_NEAR(spline.position(1.0)[0], 2.0, 1e-12);
+}
+
+TEST(JointSpline, StartsAndEndsAtRest)
+{
+ const JointSpline spline = threeKnotSpline();
+ EXPECT_NEAR(spline.velocity(0.0).cwiseAbs().maxCoeff(), 0.0, 1e-9);
+ EXPECT_NEAR(spline.velocity(1.0).cwiseAbs().maxCoeff(), 0.0, 1e-9);
+}
+
+TEST(JointSpline, FlowsThroughInteriorKnotsWithoutStopping)
+{
+ // The reason for one spline over the whole chain rather than a clamped move per segment:
+ // chaining would pin the interior knot to zero velocity and stop the arm at every waypoint.
+ const JointSpline spline = threeKnotSpline();
+ EXPECT_GT(spline.velocity(0.25).cwiseAbs().maxCoeff(), 0.5);
+}
+
+TEST(JointSpline, IsContinuousInVelocityAcrossAnInteriorKnot)
+{
+ const JointSpline spline = threeKnotSpline();
+ const Eigen::VectorXd before = spline.velocity(0.25 - 1e-7);
+ const Eigen::VectorXd after = spline.velocity(0.25 + 1e-7);
+ EXPECT_LT((after - before).cwiseAbs().maxCoeff(), 1e-5);
+}
+
+TEST(JointSpline, PeaksAtOneAndAHalfTimesTheMeanOnASingleSegment)
+{
+ // Pins the profile: a clamped cubic peaks at 1.5x its mean speed. Smootherstep would be
+ // 1.875 and a linear ramp 1.0, so this fails if the interpolation is swapped out.
+ EXPECT_NEAR(twoKnotSpline().peakSpeed()[0], 1.5, 1e-9);
+ EXPECT_NEAR(twoKnotSpline().velocity(0.5)[0], 1.5, 1e-9);
+}
+
+TEST(JointSpline, ReportsPeakSpeedPerJointIndependently)
+{
+ const JointSpline spline = JointSpline({ 0.0, 1.0 }, { vec({ 0.0, 0.0 }), vec({ 1.0, 4.0 }) });
+ const Eigen::VectorXd peak = spline.peakSpeed();
+ EXPECT_NEAR(peak[0], 1.5, 1e-9);
+ EXPECT_NEAR(peak[1], 6.0, 1e-9);
+}
+
+TEST(JointSpline, SaturatesOutsideTheParameterRangeRatherThanExtrapolating)
+{
+ // A cubic run past its last knot diverges fast, so evaluating off the end holds the knot.
+ const JointSpline spline = twoKnotSpline();
+ EXPECT_NEAR(spline.position(-0.5)[0], 0.0, 1e-12);
+ EXPECT_NEAR(spline.position(1.5)[0], 1.0, 1e-12);
+ EXPECT_NEAR(spline.velocity(1.5)[0], 0.0, 1e-9);
+}
+
+TEST(SegmentDuration, TakesTheCartesianBudgetWhenTheJointCapIsSlack)
+{
+ // 0.2 m at 0.065 m/s is 3.08 s; the joint cap allows far quicker, so it does not bind.
+ EXPECT_NEAR(segmentDuration(0.2, vec({ 0.1, 0.1 }), 0.065, vec({ 0.695, 0.695 })), 0.2 / 0.065, 1e-9);
+}
+
+TEST(SegmentDuration, TakesTheJointCapWhenThePathIsShortButTheArmTurnsFar)
+{
+ // A wrist flip in place: no Cartesian distance to pay for, but the joint still has to
+ // stay under its limit.
+ const double duration = segmentDuration(0.0, vec({ 3.0 }), 0.065, vec({ 0.695 }));
+ EXPECT_NEAR(duration, 1.875 * 3.0 / 0.695, 1e-9);
+}
+
+TEST(SegmentDuration, IgnoresJointsWithoutAVelocityLimit)
+{
+ EXPECT_NEAR(segmentDuration(0.1, vec({ 3.0 }), 0.065, vec({ 0.0 })), 0.1 / 0.065, 1e-9);
+}
+
+TEST(SplineKnotParameters, SpansZeroToOneInProportionToDuration)
+{
+ const std::vector parameters = splineKnotParameters({ 1.0, 3.0 });
+ ASSERT_EQ(parameters.size(), 3u);
+ EXPECT_NEAR(parameters[0], 0.0, 1e-12);
+ EXPECT_NEAR(parameters[1], 0.25, 1e-12);
+ EXPECT_NEAR(parameters[2], 1.0, 1e-12);
+}
+
+TEST(SplineKnotParameters, StaysStrictlyIncreasingThroughAZeroLengthSegment)
+{
+ // Two waypoints at the same place would otherwise collapse onto one parameter and make
+ // the spline unsolvable.
+ const std::vector parameters = splineKnotParameters({ 1.0, 0.0, 1.0 });
+ EXPECT_GT(parameters[2], parameters[1]);
+ EXPECT_NO_THROW(JointSpline(parameters, { vec({ 0.0 }), vec({ 1.0 }), vec({ 1.0 }), vec({ 2.0 }) }));
+}
+
+TEST(SplineDuration, SpendsTheCartesianLengthAtTheRequestedSpeed)
+{
+ // The oracle's regime: the tip speed budget binds and the joint caps stay slack, so the
+ // whole segment takes exactly length / speed.
+ const JointSpline spline = twoKnotSpline();
+ EXPECT_NEAR(splineDuration(spline, 0.381, 0.065, vec({ 0.695 })), 0.381 / 0.065, 1e-9);
+}
+
+TEST(SplineDuration, StretchesUntilThePeakJointSpeedFitsUnderTheCap)
+{
+ // Peak speed is 1.5 rad per unit parameter, so a 0.5 rad/s cap needs 3 s.
+ const JointSpline spline = twoKnotSpline();
+ EXPECT_NEAR(splineDuration(spline, 0.0, 0.065, vec({ 0.5 })), 3.0, 1e-9);
+}
+
+TEST(SplineDuration, ClampsToASaneRange)
+{
+ const JointSpline spline = twoKnotSpline();
+ EXPECT_NEAR(splineDuration(spline, 1e-9, 0.065, vec({ 1e6 })), 0.2, 1e-9);
+ EXPECT_NEAR(splineDuration(spline, 1e6, 0.065, vec({ 1e6 })), 60.0, 1e-9);
+}
diff --git a/src/moveit_pro_kinova_configs/kinova_vla_test_sim_behaviors/test/test_send_gripper_command.cpp b/src/moveit_pro_kinova_configs/kinova_vla_test_sim_behaviors/test/test_send_gripper_command.cpp
new file mode 100644
index 000000000..1c451eb42
--- /dev/null
+++ b/src/moveit_pro_kinova_configs/kinova_vla_test_sim_behaviors/test/test_send_gripper_command.cpp
@@ -0,0 +1,163 @@
+// Copyright 2026 PickNik Inc.
+// All rights reserved.
+//
+// Unauthorized copying of this code base via any medium is strictly prohibited.
+// Proprietary and confidential.
+
+#include
+#include