From e1e90c238d46d1c129713b0f50afad246c2e0cbb Mon Sep 17 00:00:00 2001 From: Will Yingling Date: Tue, 5 May 2026 13:22:29 -0600 Subject: [PATCH 1/3] feat(visualization): add TrajectoryToPath behavior Converts a trajectory_msgs::msg::JointTrajectory into a vector of PoseStamped via forward kinematics on a chosen tip link, so the existing VisualizePath behavior can render a joint-space trajectory directly. Co-Authored-By: Claude Opus 4.7 (1M context) --- CMakeLists.txt | 3 +- .../trajectory_to_path.hpp | 53 ++++++ package.xml | 2 + src/register_behaviors.cpp | 2 + src/trajectory_to_path.cpp | 159 ++++++++++++++++++ test/test_behavior_plugins.cpp | 2 + 6 files changed, 220 insertions(+), 1 deletion(-) create mode 100644 include/experimental_behaviors/trajectory_to_path.hpp create mode 100644 src/trajectory_to_path.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 409e719..12b83af 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,7 +4,7 @@ project(experimental_behaviors CXX) find_package(moveit_studio_common REQUIRED) moveit_studio_package() -set(THIS_PACKAGE_INCLUDE_DEPENDS control_msgs geometry_msgs moveit_pro_behavior_interface pluginlib moveit_studio_common behaviortree_cpp tl_expected) +set(THIS_PACKAGE_INCLUDE_DEPENDS control_msgs geometry_msgs moveit_pro_behavior_interface pluginlib moveit_studio_common behaviortree_cpp tl_expected trajectory_msgs moveit_pro_base) foreach(package IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${package} REQUIRED) endforeach() @@ -21,6 +21,7 @@ add_library( src/publish_dynamic_interface_group_values.cpp src/get_blackboard_by_key.cpp src/set_blackboard_by_key.cpp + src/trajectory_to_path.cpp src/register_behaviors.cpp) target_include_directories( experimental_behaviors diff --git a/include/experimental_behaviors/trajectory_to_path.hpp b/include/experimental_behaviors/trajectory_to_path.hpp new file mode 100644 index 0000000..bd9cb14 --- /dev/null +++ b/include/experimental_behaviors/trajectory_to_path.hpp @@ -0,0 +1,53 @@ +// Copyright 2026 PickNik Inc. +// All rights reserved. +// +// Unauthorized copying of this code base via any medium is strictly prohibited. +// Proprietary and confidential. + +#pragma once + +#include +#include +#include + +namespace experimental_behaviors +{ +/** + * @brief Converts a JointTrajectory into a list of stamped Cartesian poses + * using forward kinematics on a user-chosen tip link. + * + * @details + * Designed to sit upstream of `VisualizePath`: the output `path` port plugs + * directly into `VisualizePath`'s `path` input, allowing visualization of a + * joint-space trajectory (e.g. one about to be sent to `ExecuteTrajectory`). + * + * The base FK state is the planning scene's current state; joints named in + * `joint_trajectory.joint_names` overwrite that base for each point so joints + * absent from the trajectory (e.g. a rail or the other arm) keep their + * current values. + * + * | Data Port Name | Port Type | Object Type | + * | ------------------ | --------- | ---------------------------------------------- | + * | joint_trajectory | input | trajectory_msgs::msg::JointTrajectory | + * | tip_link | input | std::string | + * | path | output | std::vector | + * + * Output poses are stamped with the robot model's root frame + * (`robot_model->getModelFrame()`) and the current node clock. + */ +class TrajectoryToPath final : public moveit_pro::behaviors::SharedResourcesNode +{ +public: + static constexpr auto kPortIDJointTrajectory = "joint_trajectory"; + static constexpr auto kPortIDTipLink = "tip_link"; + static constexpr auto kPortIDPath = "path"; + + TrajectoryToPath(const std::string& name, const BT::NodeConfiguration& config, + const std::shared_ptr& shared_resources); + + static BT::PortsList providedPorts(); + static BT::KeyValueVector metadata(); + + BT::NodeStatus tick() override; +}; +} // namespace experimental_behaviors diff --git a/package.xml b/package.xml index 5e66406..2348ed5 100644 --- a/package.xml +++ b/package.xml @@ -15,10 +15,12 @@ control_msgs geometry_msgs + moveit_pro_base moveit_pro_behavior_interface pluginlib behaviortree_cpp tl_expected + trajectory_msgs ament_lint_auto ament_cmake_gtest diff --git a/src/register_behaviors.cpp b/src/register_behaviors.cpp index cbe0b2b..3bae0da 100644 --- a/src/register_behaviors.cpp +++ b/src/register_behaviors.cpp @@ -17,6 +17,7 @@ #include "experimental_behaviors/get_pose_stamped_from_topic.hpp" #include "experimental_behaviors/publish_dynamic_interface_group_values.hpp" #include "experimental_behaviors/set_blackboard_by_key.hpp" +#include "experimental_behaviors/trajectory_to_path.hpp" #include @@ -44,6 +45,7 @@ class ExperimentalBehaviorsLoader : public moveit_pro::behaviors::SharedResource shared_resources); moveit_pro::behaviors::registerBehavior(factory, "GetBlackboardByKey", shared_resources); moveit_pro::behaviors::registerBehavior(factory, "SetBlackboardByKey", shared_resources); + moveit_pro::behaviors::registerBehavior(factory, "TrajectoryToPath", shared_resources); } }; } // namespace experimental_behaviors diff --git a/src/trajectory_to_path.cpp b/src/trajectory_to_path.cpp new file mode 100644 index 0000000..9d241f0 --- /dev/null +++ b/src/trajectory_to_path.cpp @@ -0,0 +1,159 @@ +// 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 +{ +inline constexpr auto kDescriptionTrajectoryToPath = R"( +

+ Converts a trajectory_msgs::msg::JointTrajectory into a list of stamped Cartesian + poses by running forward kinematics on a chosen tip link, one pose per trajectory point. +

+

+ The output port path is shaped to plug directly into VisualizePath's + input, allowing a joint-space trajectory to be visualized before being passed to + ExecuteTrajectory. Joints absent from the trajectory keep their current planning + scene values during FK. +

+ )"; +} // namespace + +namespace experimental_behaviors +{ +TrajectoryToPath::TrajectoryToPath(const std::string& name, const BT::NodeConfiguration& config, + const std::shared_ptr& shared_resources) + : moveit_pro::behaviors::SharedResourcesNode(name, config, shared_resources) +{ +} + +BT::PortsList TrajectoryToPath::providedPorts() +{ + return { + BT::InputPort( + kPortIDJointTrajectory, "{joint_trajectory}", + "Joint-space trajectory to convert. Same type ExecuteTrajectory consumes."), + BT::InputPort( + kPortIDTipLink, "{tip_link}", + "Robot link whose Cartesian pose is reported once per trajectory point."), + BT::OutputPort>( + kPortIDPath, "{path}", + "One stamped pose per trajectory point, in the robot model's root frame. " + "Plugs directly into VisualizePath's `path` input port."), + }; +} + +BT::KeyValueVector TrajectoryToPath::metadata() +{ + return { { moveit_pro::behaviors::kSubcategoryMetadataKey, "Visualization" }, + { moveit_pro::behaviors::kDescriptionMetadataKey, kDescriptionTrajectoryToPath } }; +} + +BT::NodeStatus TrajectoryToPath::tick() +{ + trajectory_msgs::msg::JointTrajectory trajectory; + if (!getInput(kPortIDJointTrajectory, trajectory)) + { + shared_resources_->logger->publishFailureMessage(name(), "Missing required input port [joint_trajectory]."); + return BT::NodeStatus::FAILURE; + } + + std::string tip_link; + if (!getInput(kPortIDTipLink, tip_link) || tip_link.empty()) + { + shared_resources_->logger->publishFailureMessage(name(), "Missing or empty required input port [tip_link]."); + return BT::NodeStatus::FAILURE; + } + + const auto& robot_model = shared_resources_->robot_model; + if (!robot_model) + { + shared_resources_->logger->publishFailureMessage( + name(), "Robot model is not loaded on the BehaviorContext. TrajectoryToPath requires " + "load_robot_model=true."); + return BT::NodeStatus::FAILURE; + } + + // Validate the tip link. + if (!robot_model->hasLinkModel(tip_link)) + { + shared_resources_->logger->publishFailureMessage(name(), + "Unknown tip_link '" + tip_link + "' (not in robot model)."); + return BT::NodeStatus::FAILURE; + } + + // Validate every joint name in the trajectory. + for (const auto& joint_name : trajectory.joint_names) + { + if (!robot_model->hasJointModel(joint_name)) + { + shared_resources_->logger->publishFailureMessage( + name(), "Trajectory contains joint '" + joint_name + "' which is not in the robot model."); + return BT::NodeStatus::FAILURE; + } + } + + // Base state: planning scene's current state. Joints absent from the trajectory keep these values. + moveit_pro::base::planning_scene::PlanningScene planning_scene(robot_model); + const moveit_pro::base::RobotState base_state = planning_scene.getCurrentState(); + + std::vector path; + path.reserve(trajectory.points.size()); + + const auto stamp = shared_resources_->node->get_clock()->now(); + const auto& frame_id = robot_model->getModelFrame(); + + for (const auto& point : trajectory.points) + { + if (point.positions.size() != trajectory.joint_names.size()) + { + shared_resources_->logger->publishFailureMessage( + name(), "Trajectory point has " + std::to_string(point.positions.size()) + + " positions but " + std::to_string(trajectory.joint_names.size()) + " joint names."); + return BT::NodeStatus::FAILURE; + } + + moveit_pro::base::RobotState state = base_state; + try + { + state.setVariablePositions(trajectory.joint_names, point.positions); + } + catch (const std::exception& ex) + { + shared_resources_->logger->publishFailureMessage( + name(), std::string("Failed to set joint positions on robot state: ") + ex.what()); + return BT::NodeStatus::FAILURE; + } + state.updateLinkTransforms(); + + const Eigen::Isometry3d link_pose = state.getGlobalLinkTransform(tip_link); + + geometry_msgs::msg::PoseStamped pose_msg; + pose_msg.header.frame_id = frame_id; + pose_msg.header.stamp = stamp; + pose_msg.pose = tf2::toMsg(link_pose); + path.push_back(pose_msg); + } + + if (path.size() < 2) + { + shared_resources_->logger->publishWarnMessage( + name(), "Trajectory has fewer than 2 points (" + std::to_string(path.size()) + + "); the resulting path will not render as a polyline."); + } + + setOutput(kPortIDPath, path); + return BT::NodeStatus::SUCCESS; +} +} // namespace experimental_behaviors diff --git a/test/test_behavior_plugins.cpp b/test/test_behavior_plugins.cpp index 21eef65..5965b85 100644 --- a/test/test_behavior_plugins.cpp +++ b/test/test_behavior_plugins.cpp @@ -28,6 +28,8 @@ TEST(BehaviorTests, test_load_behavior_plugins) BT::NodeConfiguration())); EXPECT_NO_THROW( (void)factory.instantiateTreeNode("test_get_blackboard_by_key", "GetBlackboardByKey", BT::NodeConfiguration())); + EXPECT_NO_THROW( + (void)factory.instantiateTreeNode("test_trajectory_to_path", "TrajectoryToPath", BT::NodeConfiguration())); } int main(int argc, char** argv) From e4243f31d51b2b4f3e21c4c588077fe5c1c432d0 Mon Sep 17 00:00:00 2001 From: Will Yingling Date: Tue, 5 May 2026 13:40:27 -0600 Subject: [PATCH 2/3] refactor(visualization): apply review fixes to TrajectoryToPath - Hoist RobotState out of the FK loop to avoid per-iteration deep copies. - Document why PlanningScene::getCurrentState() is used rather than URDF defaults. - Polish the behavior description and clarify the single-stamp policy in doxygen. Co-Authored-By: Claude Opus 4.7 (1M context) --- .../experimental_behaviors/trajectory_to_path.hpp | 1 + src/trajectory_to_path.cpp | 14 ++++++++++---- 2 files changed, 11 insertions(+), 4 deletions(-) diff --git a/include/experimental_behaviors/trajectory_to_path.hpp b/include/experimental_behaviors/trajectory_to_path.hpp index bd9cb14..9680756 100644 --- a/include/experimental_behaviors/trajectory_to_path.hpp +++ b/include/experimental_behaviors/trajectory_to_path.hpp @@ -34,6 +34,7 @@ namespace experimental_behaviors * * Output poses are stamped with the robot model's root frame * (`robot_model->getModelFrame()`) and the current node clock. + * All output poses share a single timestamp captured once per tick. */ class TrajectoryToPath final : public moveit_pro::behaviors::SharedResourcesNode { diff --git a/src/trajectory_to_path.cpp b/src/trajectory_to_path.cpp index 9d241f0..3dbe796 100644 --- a/src/trajectory_to_path.cpp +++ b/src/trajectory_to_path.cpp @@ -23,8 +23,8 @@ inline constexpr auto kDescriptionTrajectoryToPath = R"(

The output port path is shaped to plug directly into VisualizePath's - input, allowing a joint-space trajectory to be visualized before being passed to - ExecuteTrajectory. Joints absent from the trajectory keep their current planning + path input, allowing a joint-space trajectory to be visualized before being passed + to ExecuteTrajectory. Joints absent from the trajectory keep their current planning scene values during FK.

)"; @@ -104,7 +104,9 @@ BT::NodeStatus TrajectoryToPath::tick() } } - // Base state: planning scene's current state. Joints absent from the trajectory keep these values. + // Seed FK from the planning scene's current state (not URDF defaults) so joints + // absent from the trajectory — e.g. the rail or the other arm in dual-arm setups — + // keep their actual current values rather than snapping to neutral. moveit_pro::base::planning_scene::PlanningScene planning_scene(robot_model); const moveit_pro::base::RobotState base_state = planning_scene.getCurrentState(); @@ -114,6 +116,11 @@ BT::NodeStatus TrajectoryToPath::tick() const auto stamp = shared_resources_->node->get_clock()->now(); const auto& frame_id = robot_model->getModelFrame(); + // Reuse a single RobotState across iterations: setVariablePositions only touches + // the variables in joint_names, so non-trajectory joints retain their seeded values + // and we avoid a per-point deep copy of the full state. + moveit_pro::base::RobotState state = base_state; + for (const auto& point : trajectory.points) { if (point.positions.size() != trajectory.joint_names.size()) @@ -124,7 +131,6 @@ BT::NodeStatus TrajectoryToPath::tick() return BT::NodeStatus::FAILURE; } - moveit_pro::base::RobotState state = base_state; try { state.setVariablePositions(trajectory.joint_names, point.positions); From 3c7b77900f57766f2090c5402eb4ddc32529925c Mon Sep 17 00:00:00 2001 From: Will Yingling Date: Tue, 5 May 2026 15:50:47 -0600 Subject: [PATCH 3/3] style(visualization): apply clang-format to TrajectoryToPath Mechanical line-wrap cleanup so the file matches the project's clang-format config (CI was rejecting the prior formatting). Co-Authored-By: Claude Opus 4.7 (1M context) --- src/trajectory_to_path.cpp | 25 ++++++++++++------------- 1 file changed, 12 insertions(+), 13 deletions(-) diff --git a/src/trajectory_to_path.cpp b/src/trajectory_to_path.cpp index 3dbe796..58999b5 100644 --- a/src/trajectory_to_path.cpp +++ b/src/trajectory_to_path.cpp @@ -41,12 +41,11 @@ TrajectoryToPath::TrajectoryToPath(const std::string& name, const BT::NodeConfig BT::PortsList TrajectoryToPath::providedPorts() { return { - BT::InputPort( - kPortIDJointTrajectory, "{joint_trajectory}", - "Joint-space trajectory to convert. Same type ExecuteTrajectory consumes."), - BT::InputPort( - kPortIDTipLink, "{tip_link}", - "Robot link whose Cartesian pose is reported once per trajectory point."), + BT::InputPort(kPortIDJointTrajectory, "{joint_trajectory}", + "Joint-space trajectory to convert. Same type " + "ExecuteTrajectory consumes."), + BT::InputPort(kPortIDTipLink, "{tip_link}", + "Robot link whose Cartesian pose is reported once per trajectory point."), BT::OutputPort>( kPortIDPath, "{path}", "One stamped pose per trajectory point, in the robot model's root frame. " @@ -98,8 +97,8 @@ BT::NodeStatus TrajectoryToPath::tick() { if (!robot_model->hasJointModel(joint_name)) { - shared_resources_->logger->publishFailureMessage( - name(), "Trajectory contains joint '" + joint_name + "' which is not in the robot model."); + shared_resources_->logger->publishFailureMessage(name(), "Trajectory contains joint '" + joint_name + + "' which is not in the robot model."); return BT::NodeStatus::FAILURE; } } @@ -126,8 +125,8 @@ BT::NodeStatus TrajectoryToPath::tick() if (point.positions.size() != trajectory.joint_names.size()) { shared_resources_->logger->publishFailureMessage( - name(), "Trajectory point has " + std::to_string(point.positions.size()) + - " positions but " + std::to_string(trajectory.joint_names.size()) + " joint names."); + name(), "Trajectory point has " + std::to_string(point.positions.size()) + " positions but " + + std::to_string(trajectory.joint_names.size()) + " joint names."); return BT::NodeStatus::FAILURE; } @@ -154,9 +153,9 @@ BT::NodeStatus TrajectoryToPath::tick() if (path.size() < 2) { - shared_resources_->logger->publishWarnMessage( - name(), "Trajectory has fewer than 2 points (" + std::to_string(path.size()) + - "); the resulting path will not render as a polyline."); + shared_resources_->logger->publishWarnMessage(name(), "Trajectory has fewer than 2 points (" + + std::to_string(path.size()) + + "); the resulting path will not render as a polyline."); } setOutput(kPortIDPath, path);