diff --git a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h index 3558739da9..956b45054d 100644 --- a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h +++ b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h @@ -66,9 +66,6 @@ namespace kinematic_constraints moveit_msgs::msg::Constraints mergeConstraints(const moveit_msgs::msg::Constraints& first, const moveit_msgs::msg::Constraints& second); -/** \brief Check if any constraints were specified */ -[[deprecated("Use moveit/utils/message_checks.h instead")]] bool isEmpty(const moveit_msgs::msg::Constraints& constr); - std::size_t countIndividualConstraints(const moveit_msgs::msg::Constraints& constr); /** @@ -78,8 +75,8 @@ std::size_t countIndividualConstraints(const moveit_msgs::msg::Constraints& cons * * @param [in] state The state from which to generate goal joint constraints * @param [in] jmg The group for which to generate goal joint constraints - * @param [in] tolerance_below The below tolerance to apply to all constraints - * @param [in] tolerance_above The above tolerance to apply to all constraints + * @param [in] tolerance_below The below tolerance to apply to all constraints [rad or meters for prismatic joints] + * @param [in] tolerance_above The above tolerance to apply to all constraints [rad or meters for prismatic joints] * * @return A full constraint message containing all the joint constraints */ @@ -94,7 +91,8 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const moveit::core::Robot * * @param [in] state The state from which to generate goal joint constraints * @param [in] jmg The group for which to generate joint constraints - * @param [in] tolerance A tolerance to apply both above and below for all constraints + * @param [in] tolerance An angular tolerance to apply both above and below for all constraints [rad or meters for + * prismatic joints] * * @return A full constraint message containing all the joint constraints */ @@ -102,6 +100,18 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const moveit::core::Robot const moveit::core::JointModelGroup* jmg, double tolerance = std::numeric_limits::epsilon()); +/** + * \brief Update joint constraints with a new JointModelGroup state + * + * @param [in, out] constraints Previously-constructed constraints to update + * @param [in] state The new target state + * @param [in] jmg Specify which JointModelGroup to update + * + * @return true if all joint constraints were updated + */ +bool updateJointConstraints(moveit_msgs::msg::Constraints& constraints, const moveit::core::RobotState& state, + const moveit::core::JointModelGroup* jmg); + /** * \brief Generates a constraint message intended to be used as a goal * constraint for a given link. The full constraint will contain a @@ -111,8 +121,7 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const moveit::core::Robot * * @param [in] link_name The link name for both constraints * @param [in] pose The pose stamped to be used for the target region. - * @param [in] tolerance_pos The dimension of the sphere associated with the target region of the \ref - *PositionConstraint + * @param [in] tolerance_pos The radius of a sphere defining a \ref PositionConstraint * @param [in] tolerance_angle The value to assign to the absolute tolerances of the \ref OrientationConstraint * * @return A full constraint message containing both constraints @@ -141,6 +150,18 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_n const std::vector& tolerance_pos, const std::vector& tolerance_angle); +/** + * \brief Update a pose constraint for one link with a new pose + * + * @param [in, out] constraints Previously-constructed constraints to update + * @param [in] link The link to update for + * @param [in] pose The new target pose + * + * @return true if the constraint was updated + */ +bool updatePoseConstraint(moveit_msgs::msg::Constraints& constraints, const std::string& link_name, + const geometry_msgs::msg::PoseStamped& pose); + /** * \brief Generates a constraint message intended to be used as a goal * constraint for a given link. The full constraint message will @@ -156,6 +177,18 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_n const geometry_msgs::msg::QuaternionStamped& quat, double tolerance = 1e-2); +/** + * \brief Update an orientation constraint for one link with a new quaternion + * + * @param [in, out] constraints Previously-constructed constraints to update + * @param [in] link The link to update for + * @param [in] quat The new target quaternion + * + * @return true if the constraint was updated + */ +bool updateOrientationConstraint(moveit_msgs::msg::Constraints& constraints, const std::string& link_name, + const geometry_msgs::msg::QuaternionStamped& quat); + /** * \brief Generates a constraint message intended to be used as a goal * constraint for a given link. The full constraint message will @@ -165,7 +198,7 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_n * @param [in] link_name The link name for the \ref PositionConstraint * @param [in] reference_point A point corresponding to the target_point_offset of the \ref PositionConstraint * @param [in] goal_point The position associated with the constraint region - * @param [in] tolerance The radius associated with the sphere volume associated with the constraint region + * @param [in] tolerance The radius of a sphere defining a \ref PositionConstraint * * @return A full constraint message containing the position constraint */ @@ -182,7 +215,7 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_n * * @param [in] link_name The link name for the \ref PositionConstraint * @param [in] goal_point The position associated with the constraint region - * @param [in] tolerance The radius associated with the sphere volume associated with the constraint region + * @param [in] tolerance The radius of a sphere defining a \ref PositionConstraint * * @return A full constraint message containing the position constraint */ @@ -190,6 +223,18 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_n const geometry_msgs::msg::PointStamped& goal_point, double tolerance = 1e-3); +/** + * \brief Update a position constraint for one link with a new position + * + * @param [in, out] constraints Previously-constructed constraints to update + * @param [in] link The link to update for + * @param [in] goal_point The new target point + * + * @return true if the constraint was updated + */ +bool updatePositionConstraint(moveit_msgs::msg::Constraints& constraints, const std::string& link_name, + const geometry_msgs::msg::PointStamped& goal_point); + /** * \brief extract constraint message from node parameters. * diff --git a/moveit_core/kinematic_constraints/src/utils.cpp b/moveit_core/kinematic_constraints/src/utils.cpp index 788acf89c6..7903702e2e 100644 --- a/moveit_core/kinematic_constraints/src/utils.cpp +++ b/moveit_core/kinematic_constraints/src/utils.cpp @@ -34,6 +34,8 @@ /* Author: Ioan Sucan */ +#include + #include #include #include @@ -128,11 +130,6 @@ moveit_msgs::msg::Constraints mergeConstraints(const moveit_msgs::msg::Constrain return r; } -bool isEmpty(const moveit_msgs::msg::Constraints& constr) -{ - return moveit::core::isEmpty(constr); -} - std::size_t countIndividualConstraints(const moveit_msgs::msg::Constraints& constr) { return constr.position_constraints.size() + constr.orientation_constraints.size() + @@ -165,6 +162,28 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const moveit::core::Robot return goal; } +bool updateJointConstraints(moveit_msgs::msg::Constraints& constraints, const moveit::core::RobotState& state, + const moveit::core::JointModelGroup* jmg) +{ + const std::vector& jmg_active_joints = jmg->getActiveJointModelNames(); + + // For each constraint, update it if the joint is found within jmg + for (auto& constraint : constraints.joint_constraints) + { + const auto itr = find(jmg_active_joints.begin(), jmg_active_joints.end(), constraint.joint_name); + if (itr != jmg_active_joints.end()) + { + constraint.position = state.getVariablePosition(constraint.joint_name); + } + // The joint was not found within jmg + else + { + return false; + } + } + return true; +} + moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name, const geometry_msgs::msg::PoseStamped& pose, double tolerance_pos, double tolerance_angle) @@ -227,6 +246,22 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_n return goal; } +bool updatePoseConstraint(moveit_msgs::msg::Constraints& constraints, const std::string& link_name, + const geometry_msgs::msg::PoseStamped& pose) +{ + // Convert message types so the existing functions can be used + geometry_msgs::msg::PointStamped point; + point.point.x = pose.pose.position.x; + point.point.y = pose.pose.position.y; + point.point.z = pose.pose.position.z; + + geometry_msgs::msg::QuaternionStamped quat_stamped; + quat_stamped.quaternion = pose.pose.orientation; + + return updatePositionConstraint(constraints, link_name, point) && + updateOrientationConstraint(constraints, link_name, quat_stamped); +} + moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name, const geometry_msgs::msg::QuaternionStamped& quat, double tolerance) @@ -244,6 +279,20 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_n return goal; } +bool updateOrientationConstraint(moveit_msgs::msg::Constraints& constraints, const std::string& link_name, + const geometry_msgs::msg::QuaternionStamped& quat) +{ + for (auto& constraint : constraints.orientation_constraints) + { + if (constraint.link_name == link_name) + { + constraint.orientation = quat.quaternion; + return true; + } + } + return false; +} + moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name, const geometry_msgs::msg::PointStamped& goal_point, double tolerance) @@ -255,6 +304,22 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_n return constructGoalConstraints(link_name, p, goal_point, tolerance); } +bool updatePositionConstraint(moveit_msgs::msg::Constraints& constraints, const std::string& link_name, + const geometry_msgs::msg::PointStamped& goal_point) +{ + for (auto& constraint : constraints.position_constraints) + { + if (constraint.link_name == link_name) + { + constraint.constraint_region.primitive_poses.at(0).position.x = goal_point.point.x; + constraint.constraint_region.primitive_poses.at(0).position.y = goal_point.point.y; + constraint.constraint_region.primitive_poses.at(0).position.z = goal_point.point.z; + return true; + } + } + return false; +} + moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name, const geometry_msgs::msg::Point& reference_point, const geometry_msgs::msg::PointStamped& goal_point, @@ -532,10 +597,8 @@ bool constructConstraints(const rclcpp::Node::SharedPtr& node, const std::string return collectConstraints(node, constraint_ids, constraints); } -} // namespace kinematic_constraints -bool kinematic_constraints::resolveConstraintFrames(const moveit::core::RobotState& state, - moveit_msgs::msg::Constraints& constraints) +bool resolveConstraintFrames(const moveit::core::RobotState& state, moveit_msgs::msg::Constraints& constraints) { for (auto& c : constraints.position_constraints) { @@ -581,3 +644,4 @@ bool kinematic_constraints::resolveConstraintFrames(const moveit::core::RobotSta } return true; } +} // namespace kinematic_constraints