Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Clean up kinematic_constraints/utils #1875

Merged
merged 4 commits into from
Jan 24, 2023
Merged

Clean up kinematic_constraints/utils #1875

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
b7f68bf
Better comments, delete long-deprecated function
AndyZe Jan 19, 2023
c7bf63b
Add updateJointConstraints() function
AndyZe Jan 21, 2023
9c32f1c
Add update() functions for other constraint types
AndyZe Jan 22, 2023
5a19c4a
Clean up updatePoseConstraint()
AndyZe Jan 23, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
65 changes: 55 additions & 10 deletions moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h
View file
Open in desktop
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);
AndyZe marked this conversation as resolved.
Show resolved Hide resolved

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
Open in desktop
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,
henningkayser marked this conversation as resolved.
Show resolved Hide resolved
const geometry_msgs::msg::PoseStamped& pose)
{
// Convert message types so the existing functions can be used
geometry_msgs::msg::PointStamped point;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@AndyZe I don't know why I missed this, but I think we have to pass the stamp information with this as well, since that one is defining the reference frame

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

#1890

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

#1890

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