Function moveit_ros::trajectory_cache::appendConstraintsAsFetchQueryWithTolerance

Function Documentation

moveit::core::MoveItErrorCode moveit_ros::trajectory_cache::appendConstraintsAsFetchQueryWithTolerance(warehouse_ros::Query &query, std::vector<moveit_msgs::msg::Constraints> constraints, const moveit::planning_interface::MoveGroupInterface &move_group, double match_tolerance, const std::string &reference_frame_id, const std::string &prefix)

Extracts relevant features from a vector of moveit_msgs::msg::Constraints messages to a fetch query, with tolerance.

This will extract relevant features from the joint, position, and orientation constraints per element. This exists because many keyable messages contain constraints which should be handled similarly.

WARNING: Visibility constraints are not supported.

Additionally, the component constraints within each vector element are sorted to reduce cache cardinality. For the same reason, constraints with frames are restated in terms of the workspace frame.

We copy the input constraints to support this.

Parameters:

  • query. – [inout] The query to add features to.

  • constraints. – [in] The constraints to extract features from.

  • move_group. – [in] The manipulator move group, used to get its state.

  • reference_frame_id. – [in] The frame to restate constraints in.

  • prefix. – [in] A prefix to add to feature keys.

Returns:

moveit::core::MoveItErrorCode::SUCCESS if successfully appended. Otherwise, will return a different error code, in which case the query should not be reused.