diff --git a/moveit_ros/trajectory_cache/CMakeLists.txt b/moveit_ros/trajectory_cache/CMakeLists.txt index 452e535d7ca..daa558a7f78 100644 --- a/moveit_ros/trajectory_cache/CMakeLists.txt +++ b/moveit_ros/trajectory_cache/CMakeLists.txt @@ -47,7 +47,8 @@ ament_target_dependencies(moveit_ros_trajectory_cache_utils_lib # Features library add_library(moveit_ros_trajectory_features_lib - src/features/motion_plan_request_features.cpp) + src/features/motion_plan_request_features.cpp + src/features/get_cartesian_path_request_features.cpp) generate_export_header(moveit_ros_trajectory_features_lib) target_link_libraries(moveit_ros_trajectory_features_lib moveit_ros_trajectory_cache_utils_lib) diff --git a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/features/get_cartesian_path_request_features.hpp b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/features/get_cartesian_path_request_features.hpp new file mode 100644 index 00000000000..39e953517a2 --- /dev/null +++ b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/features/get_cartesian_path_request_features.hpp @@ -0,0 +1,245 @@ +// Copyright 2024 Intrinsic Innovation LLC. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/** @file + * @brief moveit_msgs::srv::GetCartesianPath::Request features to key the trajectory cache on. + * @see FeaturesInterface + * + * @author methylDragon + */ + +#pragma once + +#include +#include +#include + +#include + +namespace moveit_ros +{ +namespace trajectory_cache +{ + +// "Start" features. =============================================================================== + +/** @class CartesianWorkspaceFeatures + * @brief Extracts group name and frame ID from the plan request. + */ +class CartesianWorkspaceFeatures : public FeaturesInterface +{ +public: + CartesianWorkspaceFeatures(); + + std::string getName() const override; + + moveit::core::MoveItErrorCode appendFeaturesAsFuzzyFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double exact_match_precision) const override; + + moveit::core::MoveItErrorCode appendFeaturesAsExactFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double exact_match_precision) const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsInsertMetadata(warehouse_ros::Metadata& metadata, + const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group) const override; + +private: + const std::string name_; +}; + +/** @class CartesianStartStateJointStateFeatures + * @brief Extracts details of the joint state from the `start_state` field in the plan request. + * + * The start state will always be re-interpreted into explicit joint state positions. + * + * WARNING: + * MultiDOF joints and attached collision objects are not supported. + */ +class CartesianStartStateJointStateFeatures : public FeaturesInterface +{ +public: + CartesianStartStateJointStateFeatures(double match_tolerance); + + std::string getName() const override; + + moveit::core::MoveItErrorCode appendFeaturesAsFuzzyFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double exact_match_precision) const override; + + moveit::core::MoveItErrorCode appendFeaturesAsExactFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double exact_match_precision) const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsInsertMetadata(warehouse_ros::Metadata& metadata, + const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group) const override; + +private: + moveit::core::MoveItErrorCode appendFeaturesAsFetchQueryWithTolerance( + warehouse_ros::Query& query, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double match_tolerance) const; + + const std::string name_; + const double match_tolerance_; +}; + +// "Goal" features. ================================================================================ + +/** @class CartesianMaxSpeedAndAccelerationFeatures + * @brief Extracts max velocity and acceleration scaling, and cartesian speed limits from the plan request. + * + * These features will be extracted as less-than-or-equal features. + * + * NOTE: In accordance with the source message's field descriptions: + * If the max scaling factors are outside the range of (0, 1], they will be set to 1. + * If max_cartesian_speed is <= 0, it will be ignored instead. + */ +class CartesianMaxSpeedAndAccelerationFeatures : public FeaturesInterface +{ +public: + CartesianMaxSpeedAndAccelerationFeatures(); + + std::string getName() const override; + + moveit::core::MoveItErrorCode appendFeaturesAsFuzzyFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double exact_match_precision) const override; + + moveit::core::MoveItErrorCode appendFeaturesAsExactFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double exact_match_precision) const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsInsertMetadata(warehouse_ros::Metadata& metadata, + const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group) const override; + +private: + const std::string name_; +}; + +/** @class CartesianMaxStepAndJumpThresholdFeatures + * @brief Extracts max step and jump thresholds from the plan request. + * + * These features will be extracted as less-than-or-equal features. + * + * NOTE: In accordance with the source message's field descriptions: + * If a jump threshold is set to 0, it will be ignored instead. + */ +class CartesianMaxStepAndJumpThresholdFeatures : public FeaturesInterface +{ +public: + CartesianMaxStepAndJumpThresholdFeatures(); + + std::string getName() const override; + + moveit::core::MoveItErrorCode appendFeaturesAsFuzzyFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double exact_match_precision) const override; + + moveit::core::MoveItErrorCode appendFeaturesAsExactFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double exact_match_precision) const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsInsertMetadata(warehouse_ros::Metadata& metadata, + const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group) const override; + +private: + const std::string name_; +}; + +/** @class CartesianWaypointsFeatures + * @brief Extracts features fom the `waypoints` and `link_name` field in the plan request. + * + * `link_name` is extracted here because it is what the waypoints are stated with reference to. + * Additionally, the waypoints will be restated in the robot's model frame. + * + * NOTE: In accordance with the source message's field descriptions: + * If link_name is empty, uses move_group.getEndEffectorLink(). + * + * @see appendConstraintsAsFetchQueryWithTolerance + * @see appendConstraintsAsInsertMetadata + */ +class CartesianWaypointsFeatures : public FeaturesInterface +{ +public: + CartesianWaypointsFeatures(double match_tolerance); + + std::string getName() const override; + + moveit::core::MoveItErrorCode appendFeaturesAsFuzzyFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double exact_match_precision) const override; + + moveit::core::MoveItErrorCode appendFeaturesAsExactFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double exact_match_precision) const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsInsertMetadata(warehouse_ros::Metadata& metadata, + const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group) const override; + +private: + moveit::core::MoveItErrorCode appendFeaturesAsFetchQueryWithTolerance( + warehouse_ros::Query& query, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double match_tolerance) const; + + const std::string name_; + const double match_tolerance_; +}; + +/** @class CartesianPathConstraintsFeatures + * @brief Extracts features fom the `path_constraints` field in the plan request. + * + * @see appendConstraintsAsFetchQueryWithTolerance + * @see appendConstraintsAsInsertMetadata + */ +class CartesianPathConstraintsFeatures : public FeaturesInterface +{ +public: + CartesianPathConstraintsFeatures(double match_tolerance); + + std::string getName() const override; + + moveit::core::MoveItErrorCode appendFeaturesAsFuzzyFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double exact_match_precision) const override; + + moveit::core::MoveItErrorCode appendFeaturesAsExactFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double exact_match_precision) const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsInsertMetadata(warehouse_ros::Metadata& metadata, + const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group) const override; + +private: + moveit::core::MoveItErrorCode appendFeaturesAsFetchQueryWithTolerance( + warehouse_ros::Query& query, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double match_tolerance) const; + + const std::string name_; + const double match_tolerance_; +}; + +} // namespace trajectory_cache +} // namespace moveit_ros diff --git a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp index db28f7414bb..7f41630334a 100644 --- a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp +++ b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp @@ -286,23 +286,6 @@ class TrajectoryCache */ /**@{*/ - /** - * @brief Constructs a GetCartesianPath request. - * - * This mimics the move group computeCartesianPath signature (without path constraints). - * - * @param[in] move_group. The manipulator move group, used to get its state, frames, and link. - * @param[in] waypoints. The cartesian waypoints to request the path for. - * @param[in] max_step. The value to populate into the `GetCartesianPath` request's max_step field. - * @param[in] jump_threshold. The value to populate into the `GetCartesianPath` request's jump_threshold field. - * @param[in] avoid_collisions. The value to populate into the `GetCartesianPath` request's avoid_collisions field. - * @returns - */ - moveit_msgs::srv::GetCartesianPath::Request - constructGetCartesianPathRequest(moveit::planning_interface::MoveGroupInterface& move_group, - const std::vector& waypoints, double max_step, - double jump_threshold, bool avoid_collisions = true); - /** * @brief Fetches all cartesian trajectories that fit within the requested tolerances for start and goal conditions, * returning them as a vector, sorted by some cache column. diff --git a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/utils/utils.hpp b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/utils/utils.hpp index 2c65767cd5f..98790978a32 100644 --- a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/utils/utils.hpp +++ b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/utils/utils.hpp @@ -47,6 +47,32 @@ std::string getWorkspaceFrameId(const moveit::planning_interface::MoveGroupInter std::string getCartesianPathRequestFrameId(const moveit::planning_interface::MoveGroupInterface& move_group, const moveit_msgs::srv::GetCartesianPath::Request& path_request); +// Request Construction. =========================================================================== + +/** + * @brief Constructs a GetCartesianPath request. + * + * This is a convenience function. + * This mimics the move group computeCartesianPath signature (without path constraints). + * + * WARNING: The following fields are not supported, if you want to specify them, add them in yourself. + * - prismatic_jump_threshold + * - revolute_jump_threshold + * - cartesian_speed_limited_link + * - max_cartesian_speed + * + * @param[in] move_group. The manipulator move group, used to get its state, frames, and link. + * @param[in] waypoints. The cartesian waypoints to request the path for. + * @param[in] max_step. The value to populate into the `GetCartesianPath` request's max_step field. + * @param[in] jump_threshold. The value to populate into the `GetCartesianPath` request's jump_threshold field. + * @param[in] avoid_collisions. The value to populate into the `GetCartesianPath` request's avoid_collisions field. + * @returns + */ +moveit_msgs::srv::GetCartesianPath::Request +constructGetCartesianPathRequest(moveit::planning_interface::MoveGroupInterface& move_group, + const std::vector& waypoints, double max_step, + double jump_threshold, bool avoid_collisions = true); + // Queries. ======================================================================================== /** @brief Appends a range inclusive query with some tolerance about some center value. */ diff --git a/moveit_ros/trajectory_cache/src/features/get_cartesian_path_request_features.cpp b/moveit_ros/trajectory_cache/src/features/get_cartesian_path_request_features.cpp new file mode 100644 index 00000000000..b1fdd9b92c4 --- /dev/null +++ b/moveit_ros/trajectory_cache/src/features/get_cartesian_path_request_features.cpp @@ -0,0 +1,486 @@ +// Copyright 2024 Intrinsic Innovation LLC. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/** @file + * @brief Implementation of moveit_msgs::srv::GetCartesianPath::Request features to key the trajectory cache on. + * @see FeaturesInterface + * + * @author methylDragon + */ + +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace moveit_ros +{ +namespace trajectory_cache +{ + +// "Start" features. =============================================================================== + +// CartesianWorkspaceFeatures. + +CartesianWorkspaceFeatures::CartesianWorkspaceFeatures() : name_("CartesianWorkspaceFeatures") +{ +} + +std::string CartesianWorkspaceFeatures::getName() const +{ + return name_; +} + +moveit::core::MoveItErrorCode CartesianWorkspaceFeatures::appendFeaturesAsFuzzyFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double exact_match_precision) const +{ + return appendFeaturesAsExactFetchQuery(query, source, move_group, exact_match_precision); +}; + +moveit::core::MoveItErrorCode CartesianWorkspaceFeatures::appendFeaturesAsExactFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double /*exact_match_precision*/) const +{ + query.append(name_ + ".group_name", source.group_name); + query.append(name_ + ".header.frame_id", getCartesianPathRequestFrameId(move_group, source)); + return moveit::core::MoveItErrorCode::SUCCESS; +}; + +moveit::core::MoveItErrorCode CartesianWorkspaceFeatures::appendFeaturesAsInsertMetadata( + warehouse_ros::Metadata& metadata, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group) const +{ + metadata.append(name_ + ".group_name", source.group_name); + metadata.append(name_ + ".header.frame_id", getCartesianPathRequestFrameId(move_group, source)); + return moveit::core::MoveItErrorCode::SUCCESS; +}; + +// CartesianStartStateJointStateFeatures. + +CartesianStartStateJointStateFeatures::CartesianStartStateJointStateFeatures(double match_tolerance) + : name_("CartesianStartStateJointStateFeatures"), match_tolerance_(match_tolerance) +{ +} + +std::string CartesianStartStateJointStateFeatures::getName() const +{ + return name_; +} + +moveit::core::MoveItErrorCode CartesianStartStateJointStateFeatures::appendFeaturesAsFuzzyFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double exact_match_precision) const +{ + return appendFeaturesAsFetchQueryWithTolerance(query, source, move_group, match_tolerance_ + exact_match_precision); +}; + +moveit::core::MoveItErrorCode CartesianStartStateJointStateFeatures::appendFeaturesAsExactFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double exact_match_precision) const +{ + return appendFeaturesAsFetchQueryWithTolerance(query, source, move_group, exact_match_precision); +}; + +moveit::core::MoveItErrorCode CartesianStartStateJointStateFeatures::appendFeaturesAsInsertMetadata( + warehouse_ros::Metadata& metadata, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group) const +{ + return appendRobotStateJointStateAsInsertMetadata(metadata, source.start_state, move_group, name_ + ".start_state"); +}; + +moveit::core::MoveItErrorCode CartesianStartStateJointStateFeatures::appendFeaturesAsFetchQueryWithTolerance( + warehouse_ros::Query& query, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double match_tolerance) const +{ + return appendRobotStateJointStateAsFetchQueryWithTolerance(query, source.start_state, move_group, match_tolerance, + name_ + ".start_state"); +}; + +// "Goal" features. ================================================================================ + +// CartesianMaxSpeedAndAccelerationFeatures. + +CartesianMaxSpeedAndAccelerationFeatures::CartesianMaxSpeedAndAccelerationFeatures() + : name_("CartesianMaxSpeedAndAccelerationFeatures") +{ +} + +std::string CartesianMaxSpeedAndAccelerationFeatures::getName() const +{ + return name_; +} + +moveit::core::MoveItErrorCode CartesianMaxSpeedAndAccelerationFeatures::appendFeaturesAsFuzzyFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double exact_match_precision) const +{ + return appendFeaturesAsExactFetchQuery(query, source, move_group, exact_match_precision); +}; + +moveit::core::MoveItErrorCode CartesianMaxSpeedAndAccelerationFeatures::appendFeaturesAsExactFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& /*move_group*/, double /*exact_match_precision*/) const +{ + if (source.max_velocity_scaling_factor <= 0 || source.max_velocity_scaling_factor > 1.0) + { + query.appendLTE(name_ + ".max_velocity_scaling_factor", 1.0); + } + else + { + query.appendLTE(name_ + ".max_velocity_scaling_factor", source.max_velocity_scaling_factor); + } + + if (source.max_acceleration_scaling_factor <= 0 || source.max_acceleration_scaling_factor > 1.0) + { + query.appendLTE(name_ + ".max_acceleration_scaling_factor", 1.0); + } + else + { + query.appendLTE(name_ + ".max_acceleration_scaling_factor", source.max_acceleration_scaling_factor); + } + + if (source.max_cartesian_speed > 0) + { + query.append(name_ + ".cartesian_speed_limited_link", source.cartesian_speed_limited_link); + query.appendLTE(name_ + ".max_cartesian_speed", source.max_cartesian_speed); + } + + return moveit::core::MoveItErrorCode::SUCCESS; +}; + +moveit::core::MoveItErrorCode CartesianMaxSpeedAndAccelerationFeatures::appendFeaturesAsInsertMetadata( + warehouse_ros::Metadata& metadata, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& /*move_group*/) const +{ + if (source.max_velocity_scaling_factor <= 0 || source.max_velocity_scaling_factor > 1.0) + { + metadata.append(name_ + ".max_velocity_scaling_factor", 1.0); + } + else + { + metadata.append(name_ + ".max_velocity_scaling_factor", source.max_velocity_scaling_factor); + } + + if (source.max_acceleration_scaling_factor <= 0 || source.max_acceleration_scaling_factor > 1.0) + { + metadata.append(name_ + ".max_acceleration_scaling_factor", 1.0); + } + else + { + metadata.append(name_ + ".max_acceleration_scaling_factor", source.max_acceleration_scaling_factor); + } + + if (source.max_cartesian_speed > 0) + { + metadata.append(name_ + ".cartesian_speed_limited_link", source.cartesian_speed_limited_link); + metadata.append(name_ + ".max_cartesian_speed", source.max_cartesian_speed); + } + + return moveit::core::MoveItErrorCode::SUCCESS; +}; + +// CartesianMaxStepAndJumpThresholdFeatures. + +CartesianMaxStepAndJumpThresholdFeatures::CartesianMaxStepAndJumpThresholdFeatures() + : name_("CartesianMaxStepAndJumpThresholdFeatures") +{ +} + +std::string CartesianMaxStepAndJumpThresholdFeatures::getName() const +{ + return name_; +} + +moveit::core::MoveItErrorCode CartesianMaxStepAndJumpThresholdFeatures::appendFeaturesAsFuzzyFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double exact_match_precision) const +{ + return appendFeaturesAsExactFetchQuery(query, source, move_group, exact_match_precision); +}; + +moveit::core::MoveItErrorCode CartesianMaxStepAndJumpThresholdFeatures::appendFeaturesAsExactFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& /*move_group*/, double /*exact_match_precision*/) const +{ + query.appendLTE(name_ + ".max_step", source.max_step); + + if (source.jump_threshold > 0) + { + query.appendLTE(name_ + ".jump_threshold", source.jump_threshold); + } + if (source.prismatic_jump_threshold > 0) + { + query.appendLTE(name_ + ".prismatic_jump_threshold", source.prismatic_jump_threshold); + } + if (source.revolute_jump_threshold > 0) + { + query.appendLTE(name_ + ".revolute_jump_threshold", source.revolute_jump_threshold); + } + + return moveit::core::MoveItErrorCode::SUCCESS; +}; + +moveit::core::MoveItErrorCode CartesianMaxStepAndJumpThresholdFeatures::appendFeaturesAsInsertMetadata( + warehouse_ros::Metadata& metadata, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& /*move_group*/) const +{ + metadata.append(name_ + ".max_step", source.max_step); + + if (source.jump_threshold > 0) + { + metadata.append(name_ + ".jump_threshold", source.jump_threshold); + } + if (source.prismatic_jump_threshold > 0) + { + metadata.append(name_ + ".prismatic_jump_threshold", source.prismatic_jump_threshold); + } + if (source.revolute_jump_threshold > 0) + { + metadata.append(name_ + ".revolute_jump_threshold", source.revolute_jump_threshold); + } + + return moveit::core::MoveItErrorCode::SUCCESS; +}; + +// CartesianWaypointsFeatures. + +CartesianWaypointsFeatures::CartesianWaypointsFeatures(double match_tolerance) + : name_("CartesianWaypointsFeatures"), match_tolerance_(match_tolerance) +{ +} + +std::string CartesianWaypointsFeatures::getName() const +{ + return name_; +} + +moveit::core::MoveItErrorCode CartesianWaypointsFeatures::appendFeaturesAsFuzzyFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double exact_match_precision) const +{ + return appendFeaturesAsFetchQueryWithTolerance(query, source, move_group, match_tolerance_ + exact_match_precision); +}; + +moveit::core::MoveItErrorCode CartesianWaypointsFeatures::appendFeaturesAsExactFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double exact_match_precision) const +{ + return appendFeaturesAsFetchQueryWithTolerance(query, source, move_group, exact_match_precision); +}; + +moveit::core::MoveItErrorCode CartesianWaypointsFeatures::appendFeaturesAsInsertMetadata( + warehouse_ros::Metadata& metadata, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group) const +{ + std::string path_request_frame_id = getCartesianPathRequestFrameId(move_group, source); + std::string base_frame = move_group.getRobotModel()->getModelFrame(); + + metadata.append(name_ + ".link_name", source.link_name); + metadata.append(name_ + ".robot_model.frame_id", base_frame); + + // Waypoints. + + // Restating them in terms of the robot model frame (usually base_link) + double x_offset = 0; + double y_offset = 0; + double z_offset = 0; + + geometry_msgs::msg::Quaternion quat_offset; + quat_offset.x = 0; + quat_offset.y = 0; + quat_offset.z = 0; + quat_offset.w = 1; + + if (base_frame != path_request_frame_id) + { + try + { + auto transform = move_group.getTF()->lookupTransform(path_request_frame_id, base_frame, tf2::TimePointZero); + x_offset = transform.transform.translation.x; + y_offset = transform.transform.translation.y; + z_offset = transform.transform.translation.z; + quat_offset = transform.transform.rotation; + } + catch (tf2::TransformException& ex) + { + // NOTE: methyldragon - + // Ideally we would restore the original state here and undo our changes, however copy of the query is not + // supported. + std::stringstream ss; + ss << "Skipping " << name_ << " metadata append: " << "Could not get transform for translation " << base_frame + << " to " << path_request_frame_id << ": " << ex.what(); + return moveit::core::MoveItErrorCode(moveit_msgs::msg::MoveItErrorCodes::FRAME_TRANSFORM_FAILURE, ss.str()); + } + } + + tf2::Quaternion tf2_quat_frame_offset(quat_offset.x, quat_offset.y, quat_offset.z, quat_offset.w); + tf2_quat_frame_offset.normalize(); + + size_t waypoint_idx = 0; + for (auto& waypoint : source.waypoints) + { + std::string meta_name = name_ + ".waypoints_" + std::to_string(waypoint_idx++); + + // Apply offsets + // Position + metadata.append(meta_name + ".position.x", x_offset + waypoint.position.x); + metadata.append(meta_name + ".position.y", y_offset + waypoint.position.y); + metadata.append(meta_name + ".position.z", z_offset + waypoint.position.z); + + // Orientation + tf2::Quaternion tf2_quat_goal_offset(waypoint.orientation.x, waypoint.orientation.y, waypoint.orientation.z, + waypoint.orientation.w); + tf2_quat_goal_offset.normalize(); + + auto final_quat = tf2_quat_goal_offset * tf2_quat_frame_offset; + final_quat.normalize(); + + metadata.append(meta_name + ".orientation.x", final_quat.getX()); + metadata.append(meta_name + ".orientation.y", final_quat.getY()); + metadata.append(meta_name + ".orientation.z", final_quat.getZ()); + metadata.append(meta_name + ".orientation.w", final_quat.getW()); + } + + return moveit::core::MoveItErrorCode::SUCCESS; +}; + +moveit::core::MoveItErrorCode CartesianWaypointsFeatures::appendFeaturesAsFetchQueryWithTolerance( + warehouse_ros::Query& query, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double match_tolerance) const +{ + std::string path_request_frame_id = getCartesianPathRequestFrameId(move_group, source); + std::string base_frame = move_group.getRobotModel()->getModelFrame(); + + query.append(name_ + ".link_name", source.link_name); + query.append(name_ + ".robot_model.frame_id", base_frame); + + // Waypoints. + + // Restating them in terms of the robot model frame (usually base_link) + double x_offset = 0; + double y_offset = 0; + double z_offset = 0; + + geometry_msgs::msg::Quaternion quat_offset; + quat_offset.x = 0; + quat_offset.y = 0; + quat_offset.z = 0; + quat_offset.w = 1; + + if (base_frame != path_request_frame_id) + { + try + { + auto transform = move_group.getTF()->lookupTransform(path_request_frame_id, base_frame, tf2::TimePointZero); + x_offset = transform.transform.translation.x; + y_offset = transform.transform.translation.y; + z_offset = transform.transform.translation.z; + quat_offset = transform.transform.rotation; + } + catch (tf2::TransformException& ex) + { + // NOTE: methyldragon - + // Ideally we would restore the original state here and undo our changes, however copy of the query is not + // supported. + std::stringstream ss; + ss << "Skipping " << name_ << " query append: " << "Could not get transform for translation " << base_frame + << " to " << path_request_frame_id << ": " << ex.what(); + return moveit::core::MoveItErrorCode(moveit_msgs::msg::MoveItErrorCodes::FRAME_TRANSFORM_FAILURE, ss.str()); + } + } + + tf2::Quaternion tf2_quat_frame_offset(quat_offset.x, quat_offset.y, quat_offset.z, quat_offset.w); + tf2_quat_frame_offset.normalize(); + + size_t waypoint_idx = 0; + for (auto& waypoint : source.waypoints) + { + std::string meta_name = name_ + ".waypoints_" + std::to_string(waypoint_idx++); + + // Apply offsets + // Position + queryAppendCenterWithTolerance(query, meta_name + ".position.x", x_offset + waypoint.position.x, match_tolerance); + queryAppendCenterWithTolerance(query, meta_name + ".position.y", y_offset + waypoint.position.y, match_tolerance); + queryAppendCenterWithTolerance(query, meta_name + ".position.z", z_offset + waypoint.position.z, match_tolerance); + + // Orientation + tf2::Quaternion tf2_quat_goal_offset(waypoint.orientation.x, waypoint.orientation.y, waypoint.orientation.z, + waypoint.orientation.w); + tf2_quat_goal_offset.normalize(); + + auto final_quat = tf2_quat_goal_offset * tf2_quat_frame_offset; + final_quat.normalize(); + + queryAppendCenterWithTolerance(query, meta_name + ".orientation.x", final_quat.getX(), match_tolerance); + queryAppendCenterWithTolerance(query, meta_name + ".orientation.y", final_quat.getY(), match_tolerance); + queryAppendCenterWithTolerance(query, meta_name + ".orientation.z", final_quat.getZ(), match_tolerance); + queryAppendCenterWithTolerance(query, meta_name + ".orientation.w", final_quat.getW(), match_tolerance); + } + + return moveit::core::MoveItErrorCode::SUCCESS; +}; + +// CartesianPathConstraintsFeatures. + +CartesianPathConstraintsFeatures::CartesianPathConstraintsFeatures(double match_tolerance) + : name_("CartesianPathConstraintsFeatures"), match_tolerance_(match_tolerance) +{ +} + +std::string CartesianPathConstraintsFeatures::getName() const +{ + return name_; +} + +moveit::core::MoveItErrorCode CartesianPathConstraintsFeatures::appendFeaturesAsFuzzyFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double exact_match_precision) const +{ + return appendFeaturesAsFetchQueryWithTolerance(query, source, move_group, match_tolerance_ + exact_match_precision); +}; + +moveit::core::MoveItErrorCode CartesianPathConstraintsFeatures::appendFeaturesAsExactFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double exact_match_precision) const +{ + return appendFeaturesAsFetchQueryWithTolerance(query, source, move_group, exact_match_precision); +}; + +moveit::core::MoveItErrorCode CartesianPathConstraintsFeatures::appendFeaturesAsInsertMetadata( + warehouse_ros::Metadata& metadata, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group) const +{ + return appendConstraintsAsInsertMetadata(metadata, { source.path_constraints }, move_group, + getCartesianPathRequestFrameId(move_group, source), + name_ + ".path_constraints"); +}; + +moveit::core::MoveItErrorCode CartesianPathConstraintsFeatures::appendFeaturesAsFetchQueryWithTolerance( + warehouse_ros::Query& query, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double match_tolerance) const +{ + return appendConstraintsAsFetchQueryWithTolerance(query, { source.path_constraints }, move_group, match_tolerance, + getCartesianPathRequestFrameId(move_group, source), + name_ + ".path_constraints"); +}; + +} // namespace trajectory_cache +} // namespace moveit_ros diff --git a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp index fe7c66cbad1..6d81e32a812 100644 --- a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp +++ b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp @@ -279,31 +279,6 @@ bool TrajectoryCache::insertTrajectory(const moveit::planning_interface::MoveGro // CARTESIAN TRAJECTORY CACHING // ============================================================================= -moveit_msgs::srv::GetCartesianPath::Request -TrajectoryCache::constructGetCartesianPathRequest(moveit::planning_interface::MoveGroupInterface& move_group, - const std::vector& waypoints, - double max_step, double jump_threshold, bool avoid_collisions) -{ - moveit_msgs::srv::GetCartesianPath::Request out; - - move_group.constructRobotState(out.start_state); - - out.group_name = move_group.getName(); - out.max_velocity_scaling_factor = move_group.getMaxVelocityScalingFactor(); - out.max_acceleration_scaling_factor = move_group.getMaxVelocityScalingFactor(); - - out.header.frame_id = move_group.getPoseReferenceFrame(); - out.waypoints = waypoints; - out.max_step = max_step; - out.jump_threshold = jump_threshold; - out.path_constraints = moveit_msgs::msg::Constraints(); - out.avoid_collisions = avoid_collisions; - out.link_name = move_group.getEndEffectorLink(); - out.header.stamp = move_group.getNode()->now(); - - return out; -} - std::vector::ConstPtr> TrajectoryCache::fetchAllMatchingCartesianTrajectories(const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& cache_namespace, diff --git a/moveit_ros/trajectory_cache/src/utils/utils.cpp b/moveit_ros/trajectory_cache/src/utils/utils.cpp index 87fa0ca999a..cce269cde8b 100644 --- a/moveit_ros/trajectory_cache/src/utils/utils.cpp +++ b/moveit_ros/trajectory_cache/src/utils/utils.cpp @@ -74,6 +74,33 @@ std::string getCartesianPathRequestFrameId(const moveit::planning_interface::Mov } } +// Request Construction. =========================================================================== + +moveit_msgs::srv::GetCartesianPath::Request +constructGetCartesianPathRequest(moveit::planning_interface::MoveGroupInterface& move_group, + const std::vector& waypoints, double max_step, + double jump_threshold, bool avoid_collisions) +{ + moveit_msgs::srv::GetCartesianPath::Request out; + + move_group.constructRobotState(out.start_state); + + out.group_name = move_group.getName(); + out.max_velocity_scaling_factor = move_group.getMaxVelocityScalingFactor(); + out.max_acceleration_scaling_factor = move_group.getMaxVelocityScalingFactor(); + + out.header.frame_id = move_group.getPoseReferenceFrame(); + out.waypoints = waypoints; + out.max_step = max_step; + out.jump_threshold = jump_threshold; + out.path_constraints = moveit_msgs::msg::Constraints(); + out.avoid_collisions = avoid_collisions; + out.link_name = move_group.getEndEffectorLink(); + out.header.stamp = move_group.getNode()->now(); + + return out; +} + // Queries. ======================================================================================== void queryAppendCenterWithTolerance(warehouse_ros::Query& query, const std::string& name, double center, diff --git a/moveit_ros/trajectory_cache/test/CMakeLists.txt b/moveit_ros/trajectory_cache/test/CMakeLists.txt index 1c1f919dc84..1b9b8ef1338 100644 --- a/moveit_ros/trajectory_cache/test/CMakeLists.txt +++ b/moveit_ros/trajectory_cache/test/CMakeLists.txt @@ -49,6 +49,17 @@ if(BUILD_TESTING) "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}" "test_executable:=test_constant_features_with_move_group") + # Test GetCartesianPath::Request features library. + ament_add_gtest_executable(test_get_cartesian_path_request_features_with_move_group + features/test_get_cartesian_path_request_features_with_move_group.cpp) + target_link_libraries(test_get_cartesian_path_request_features_with_move_group + moveit_ros_trajectory_features_lib + move_group_fixture) + add_ros_test(gtest_with_move_group.py + TARGET test_get_cartesian_path_request_features_with_move_group TIMEOUT 30 ARGS + "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}" + "test_executable:=test_get_cartesian_path_request_features_with_move_group") + # Test MotionPlanRequest features library. ament_add_gtest_executable(test_motion_plan_request_features_with_move_group features/test_motion_plan_request_features_with_move_group.cpp) diff --git a/moveit_ros/trajectory_cache/test/features/test_get_cartesian_path_request_features_with_move_group.cpp b/moveit_ros/trajectory_cache/test/features/test_get_cartesian_path_request_features_with_move_group.cpp new file mode 100644 index 00000000000..734345ef1f4 --- /dev/null +++ b/moveit_ros/trajectory_cache/test/features/test_get_cartesian_path_request_features_with_move_group.cpp @@ -0,0 +1,114 @@ +// Copyright 2024 Intrinsic Innovation LLC. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/** @file + * @author methylDragon + * + * Sanity tests for the moveit_msgs::srv::GetCartesianPath::Request feature extractors. + * + * @see get_cartesian_path_request_features.hpp + * + * WARNING: + * These tests currently do not cover the implementation details, they are just the first sanity check for + * ensuring the most basic roundtrip functionality works. + * + * For example, some features might not have any resulting changes to the metadata or query due to + * the nature of what is contained in the MotionPlanRequest passed to them. + */ + +#include + +#include +#include +#include + +#include +#include +#include + +#include "../move_group_fixture.hpp" + +namespace +{ + +using ::warehouse_ros::MessageCollection; +using ::warehouse_ros::Metadata; +using ::warehouse_ros::Query; + +using ::moveit_msgs::srv::GetCartesianPath; + +using ::moveit_ros::trajectory_cache::constructGetCartesianPathRequest; +using ::moveit_ros::trajectory_cache::FeaturesInterface; + +using ::moveit_ros::trajectory_cache::CartesianMaxSpeedAndAccelerationFeatures; +using ::moveit_ros::trajectory_cache::CartesianMaxStepAndJumpThresholdFeatures; +using ::moveit_ros::trajectory_cache::CartesianPathConstraintsFeatures; +using ::moveit_ros::trajectory_cache::CartesianStartStateJointStateFeatures; +using ::moveit_ros::trajectory_cache::CartesianWaypointsFeatures; +using ::moveit_ros::trajectory_cache::CartesianWorkspaceFeatures; + +TEST_F(MoveGroupFixture, GetCartesianPathRequestRoundTrip) +{ + // Test cases. + double match_tolerance = 0.001; + + std::vector>> features_under_test; + + features_under_test.push_back(std::make_unique()); + features_under_test.push_back(std::make_unique()); + features_under_test.push_back(std::make_unique(match_tolerance)); + features_under_test.push_back(std::make_unique(match_tolerance)); + features_under_test.push_back(std::make_unique(match_tolerance)); + features_under_test.push_back(std::make_unique()); + + // Setup. + std::vector waypoints = { move_group_->getRandomPose().pose, + move_group_->getRandomPose().pose }; + + moveit_msgs::srv::GetCartesianPath::Request msg = constructGetCartesianPathRequest( + *move_group_, waypoints, /*max_step=*/1.0, /*jump_threshold=*/0.0, /*avoid_collisions=*/false); + + // Core test. + for (auto& feature : features_under_test) + { + MessageCollection coll = + db_->openCollection("test_db", feature->getName()); + + SCOPED_TRACE(feature->getName()); + + Query::Ptr fuzzy_query = coll.createQuery(); + Query::Ptr exact_query = coll.createQuery(); + Metadata::Ptr metadata = coll.createMetadata(); + + EXPECT_TRUE(feature->appendFeaturesAsInsertMetadata(*metadata, msg, *move_group_)); + coll.insert(msg, metadata); + + EXPECT_TRUE(feature->appendFeaturesAsFuzzyFetchQuery(*fuzzy_query, msg, *move_group_, 0.0)); + EXPECT_TRUE(feature->appendFeaturesAsExactFetchQuery(*exact_query, msg, *move_group_, 0.0)); + + EXPECT_EQ(coll.queryList(fuzzy_query).size(), 1); + EXPECT_EQ(coll.queryList(exact_query).size(), 1); + } +} + +} // namespace + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} \ No newline at end of file diff --git a/moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp b/moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp index dfe8ef7e47b..888be26f4cb 100644 --- a/moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp +++ b/moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp @@ -21,11 +21,13 @@ #include #include #include +#include #include #include using moveit::planning_interface::MoveGroupInterface; +using moveit_ros::trajectory_cache::constructGetCartesianPathRequest; using moveit_ros::trajectory_cache::TrajectoryCache; const std::string ROBOT_NAME = "panda"; @@ -585,7 +587,7 @@ void testCartesianTrajectories(const std::shared_ptr& move_g int test_jump = 2; auto test_waypoints = getDummyWaypoints(); auto cartesian_plan_req_under_test = - cache->constructGetCartesianPathRequest(*move_group, test_waypoints, test_step, test_jump, false); + constructGetCartesianPathRequest(*move_group, test_waypoints, test_step, test_jump, false); checkAndEmit(cartesian_plan_req_under_test.waypoints == test_waypoints && static_cast(cartesian_plan_req_under_test.max_step) == test_step && @@ -600,7 +602,7 @@ void testCartesianTrajectories(const std::shared_ptr& move_g // Plain start auto waypoints = getDummyWaypoints(); - auto cartesian_plan_req = cache->constructGetCartesianPathRequest(*move_group, waypoints, 1, 1, false); + auto cartesian_plan_req = constructGetCartesianPathRequest(*move_group, waypoints, 1, 1, false); cartesian_plan_req.start_state.multi_dof_joint_state.joint_names.clear(); cartesian_plan_req.start_state.multi_dof_joint_state.transforms.clear(); cartesian_plan_req.start_state.multi_dof_joint_state.twist.clear();