Skip to content

Commit

Permalink
Clean up kinematic_constraints/utils, add update functions (moveit#1875)
Browse files Browse the repository at this point in the history
  • Loading branch information
@AndyZe
AndyZe authored Jan 24, 2023
1 parent f6d9aa4 commit 5eb861c
Show file tree
Hide file tree
Showing 2 changed files with 127 additions and 18 deletions.
65 changes: 55 additions & 10 deletions moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);

/**
Expand All @@ -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
*/
Expand All @@ -94,14 +91,27 @@ 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
*/
moveit_msgs::msg::Constraints constructGoalConstraints(const moveit::core::RobotState& state,
const moveit::core::JointModelGroup* jmg,
double tolerance = std::numeric_limits<double>::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
Expand All @@ -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
Expand Down Expand Up @@ -141,6 +150,18 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_n
const std::vector<double>& tolerance_pos,
const std::vector<double>& 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
Expand All @@ -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
Expand All @@ -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
*/
Expand All @@ -182,14 +215,26 @@ 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
*/
moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name,
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.
*
Expand Down
80 changes: 72 additions & 8 deletions moveit_core/kinematic_constraints/src/utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,8 @@

/* Author: Ioan Sucan */

#include <algorithm>

#include <geometric_shapes/solid_primitive_dims.h>
#include <moveit/kinematic_constraints/utils.h>
#include <moveit/utils/message_checks.h>
Expand Down Expand Up @@ -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() +
Expand Down Expand Up @@ -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<std::string>& 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)
Expand Down Expand Up @@ -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)
Expand All @@ -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)
Expand All @@ -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,
Expand Down Expand Up @@ -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)
{
Expand Down Expand Up @@ -581,3 +644,4 @@ bool kinematic_constraints::resolveConstraintFrames(const moveit::core::RobotSta
}
return true;
}
} // namespace kinematic_constraints

0 comments on commit 5eb861c

Please sign in to comment.