moveit2
The MoveIt Motion Planning Framework for ROS 2.
Namespaces | Functions
utils.h File Reference
#include <moveit_msgs/msg/constraints.hpp>
#include <geometry_msgs/msg/point_stamped.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/quaternion_stamped.hpp>
#include <moveit/robot_state/robot_state.h>
#include <limits>
Include dependency graph for utils.h:
This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Namespaces

 XmlRpc
 
 kinematic_constraints
 Representation and evaluation of kinematic constraints.
 

Functions

moveit_msgs::msg::Constraints kinematic_constraints::mergeConstraints (const moveit_msgs::msg::Constraints &first, const moveit_msgs::msg::Constraints &second)
 Merge two sets of constraints into one. More...
 
std::size_t kinematic_constraints::countIndividualConstraints (const moveit_msgs::msg::Constraints &constr)
 
moveit_msgs::msg::Constraints kinematic_constraints::constructGoalConstraints (const moveit::core::RobotState &state, const moveit::core::JointModelGroup *jmg, double tolerance_below, double tolerance_above)
 Generates a constraint message intended to be used as a goal constraint for a joint group. The full constraint will contain a vector of type JointConstraint, one for each DOF in the group. More...
 
moveit_msgs::msg::Constraints kinematic_constraints::constructGoalConstraints (const moveit::core::RobotState &state, const moveit::core::JointModelGroup *jmg, double tolerance=std::numeric_limits< double >::epsilon())
 Generates a constraint message intended to be used as a goal constraint for a joint group. The full constraint will contain a vector of type JointConstraint, one for each DOF in the group. More...
 
bool kinematic_constraints::updateJointConstraints (moveit_msgs::msg::Constraints &constraints, const moveit::core::RobotState &state, const moveit::core::JointModelGroup *jmg)
 Update joint constraints with a new JointModelGroup state. More...
 
moveit_msgs::msg::Constraints kinematic_constraints::constructGoalConstraints (const std::string &link_name, const geometry_msgs::msg::PoseStamped &pose, double tolerance_pos=1e-3, double tolerance_angle=1e-2)
 Generates a constraint message intended to be used as a goal constraint for a given link. The full constraint will contain a PositionConstraint and a OrientationConstraint, constructed from the pose. A sphere will be used to represent the constraint region for the PositionConstraint. More...
 
moveit_msgs::msg::Constraints kinematic_constraints::constructGoalConstraints (const std::string &link_name, const geometry_msgs::msg::PoseStamped &pose, const std::vector< double > &tolerance_pos, const std::vector< double > &tolerance_angle)
 Generates a constraint message intended to be used as a goal constraint for a given link. The full constraint will contain a PositionConstraint and a OrientationConstraint, constructed from the pose. A box will be used to represent the constraint region for the PositionConstraint. More...
 
bool kinematic_constraints::updatePoseConstraint (moveit_msgs::msg::Constraints &constraints, const std::string &link_name, const geometry_msgs::msg::PoseStamped &pose)
 Update a pose constraint for one link with a new pose. More...
 
moveit_msgs::msg::Constraints kinematic_constraints::constructGoalConstraints (const std::string &link_name, const geometry_msgs::msg::QuaternionStamped &quat, double tolerance=1e-2)
 Generates a constraint message intended to be used as a goal constraint for a given link. The full constraint message will contain only an OrientationConstraint. More...
 
bool kinematic_constraints::updateOrientationConstraint (moveit_msgs::msg::Constraints &constraints, const std::string &link_name, const geometry_msgs::msg::QuaternionStamped &quat)
 Update an orientation constraint for one link with a new quaternion. More...
 
moveit_msgs::msg::Constraints kinematic_constraints::constructGoalConstraints (const std::string &link_name, const geometry_msgs::msg::Point &reference_point, const geometry_msgs::msg::PointStamped &goal_point, double tolerance=1e-3)
 Generates a constraint message intended to be used as a goal constraint for a given link. The full constraint message will contain only a PositionConstraint. A sphere will be used to represent the constraint region. More...
 
moveit_msgs::msg::Constraints kinematic_constraints::constructGoalConstraints (const std::string &link_name, const geometry_msgs::msg::PointStamped &goal_point, double tolerance=1e-3)
 Generates a constraint message intended to be used as a goal constraint for a given link. The full constraint message will contain only a PositionConstraint. A sphere will be used to represent the constraint region. More...
 
bool kinematic_constraints::updatePositionConstraint (moveit_msgs::msg::Constraints &constraints, const std::string &link_name, const geometry_msgs::msg::PointStamped &goal_point)
 Update a position constraint for one link with a new position. More...
 
bool kinematic_constraints::constructConstraints (const rclcpp::Node::SharedPtr &node, const std::string &constraints_param, moveit_msgs::msg::Constraints &constraints)
 extract constraint message from node parameters. More...
 
bool kinematic_constraints::resolveConstraintFrames (const moveit::core::RobotState &state, moveit_msgs::msg::Constraints &constraints)
 Resolves frames used in constraints to links in the robot model. More...