diff --git a/moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h b/moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h index 68ed3d3dbd..d1af7d41bf 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h +++ b/moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h @@ -44,31 +44,35 @@ namespace moveit { namespace core { -/** \brief Struct for containing jump_threshold. - - For the purposes of maintaining API, we support both \e jump_threshold_factor which provides a scaling factor for - detecting joint space jumps and \e revolute_jump_threshold and \e prismatic_jump_threshold which provide absolute - thresholds for detecting joint space jumps. */ +/** \brief Struct with options for defining joint-space jump thresholds. */ struct JumpThreshold { - double factor; - double revolute; // Radians - double prismatic; // Meters + JumpThreshold() = default; // default is equivalent to disabled(). - explicit JumpThreshold() : factor(0.0), revolute(0.0), prismatic(0.0) - { - } + /** \brief Do not define any jump threshold, i.e., disable joint-space jump detection. */ + static JumpThreshold disabled(); - explicit JumpThreshold(double jt_factor) : JumpThreshold() - { - factor = jt_factor; - } + /** \brief Detect joint-space jumps relative to the average joint-space displacement along the path. - explicit JumpThreshold(double jt_revolute, double jt_prismatic) : JumpThreshold() - { - revolute = jt_revolute; // Radians - prismatic = jt_prismatic; // Meters - } + The average joint-space distance between consecutive points in the path is computed. If any individual joint-space + motion delta is larger than the average distance by a factor of `relative_factor`, it is considered the path has a + jump. For instance, a `relative_factor` of 10.0 will detect joint increments larger than 10x the average increment + */ + static JumpThreshold relative(double relative_factor); + + /** \brief Detect joint-space jumps greater than the given absolute thresholds. + + `revolute` and `prismatic` are absolute joint displacement thresholds, in radians and meters respectively. + If any two consecutive waypoints have a joint-space distance larger than these values, the path has a jump. */ + static JumpThreshold absolute(double revolute, double prismatic); + + double relative_factor = 0.0; + double revolute = 0.0; // Radians + double prismatic = 0.0; // Meters + + // Deprecated constructors. Construct using the builder methods above. + [[deprecated("Use JumpThreshold::relative() instead.")]] JumpThreshold(double relative_factor); + [[deprecated("Use JumpThreshold::absolute() instead.")]] JumpThreshold(double revolute, double prismatic); }; /** \brief Struct for containing max_step for computeCartesianPath @@ -145,33 +149,22 @@ class CartesianInterpolator The Cartesian path to be followed is specified as a \e translation vector to be followed by the robot \e link. This vector is assumed to be specified either in the global reference frame or in the local reference frame of the link (\e global_reference_frame is false). - The resulting joint values are stored in the vector \e traj, one by one. The maximum distance in + The resulting joint values are stored in the vector \e path, one by one. The maximum distance in Cartesian space between consecutive points on the resulting path is specified in the \e MaxEEFStep struct which provides two fields: translation and rotation. If a \e validCallback is specified, this is passed to the internal call to setFromIK(). In case of IK failure, the computation of the path stops and the value returned corresponds to - the distance that was achieved and for which corresponding states were added to the path. At the end of the + the distance that was achieved and for which corresponding states were added to the path. At the end of the function call, the state of the group corresponds to the last attempted Cartesian pose. - During the computation of the trajectory, it is usually preferred if consecutive joint values do not 'jump' by a + During the computation of the path, it is usually preferred if consecutive joint values do not 'jump' by a large amount in joint space, even if the Cartesian distance between the corresponding points is small as expected. - To account for this, the \e jump_threshold struct is provided, which comprises three fields: - \e jump_threshold_factor, \e revolute_jump_threshold and \e prismatic_jump_threshold. - If either \e revolute_jump_threshold or \e prismatic_jump_threshold are non-zero, we test for absolute jumps. - If \e jump_threshold_factor is non-zero, we test for relative jumps. Otherwise (all params are zero), jump - detection is disabled. - - For relative jump detection, the average joint-space distance between consecutive points in the trajectory is - computed. If any individual joint-space motion delta is larger then this average distance by a factor of - \e jump_threshold_factor, this step is considered a failure and the returned path is truncated up to just - before the jump. - - For absolute jump thresholds, if any individual joint-space motion delta is larger then \e revolute_jump_threshold - for revolute joints or \e prismatic_jump_threshold for prismatic joints then this step is considered a failure and - the returned path is truncated up to just before the jump. - - Kinematics solvers may use cost functions to prioritize certain solutions, which may be specified with \e cost_function. */ + To account for this, the \e jump_threshold argument is provided. If a jump detection is enabled and a jump is + found, the path is truncated up to just before the jump. + + Kinematics solvers may use cost functions to prioritize certain solutions, which may be specified with \e + cost_function. */ static Distance computeCartesianPath( - RobotState* start_state, const JointModelGroup* group, std::vector>& traj, + RobotState* start_state, const JointModelGroup* group, std::vector>& path, const LinkModel* link, const Eigen::Vector3d& translation, bool global_reference_frame, const MaxEEFStep& max_step, const JumpThreshold& jump_threshold, const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(), @@ -183,14 +176,14 @@ class CartesianInterpolator In contrast to the previous function, the translation vector is specified as a (unit) direction vector and a distance. */ static Distance computeCartesianPath( - RobotState* start_state, const JointModelGroup* group, std::vector>& traj, + RobotState* start_state, const JointModelGroup* group, std::vector>& path, const LinkModel* link, const Eigen::Vector3d& direction, bool global_reference_frame, double distance, const MaxEEFStep& max_step, const JumpThreshold& jump_threshold, const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn()) { - return computeCartesianPath(start_state, group, traj, link, distance * direction, global_reference_frame, max_step, + return computeCartesianPath(start_state, group, path, link, distance * direction, global_reference_frame, max_step, jump_threshold, validCallback, options, cost_function); } @@ -202,7 +195,7 @@ class CartesianInterpolator (\e global_reference_frame is false). This function returns the percentage (0..1) of the path that was achieved. All other comments from the previous function apply. */ static Percentage computeCartesianPath( - RobotState* start_state, const JointModelGroup* group, std::vector>& traj, + RobotState* start_state, const JointModelGroup* group, std::vector>& path, const LinkModel* link, const Eigen::Isometry3d& target, bool global_reference_frame, const MaxEEFStep& max_step, const JumpThreshold& jump_threshold, const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(), @@ -213,11 +206,11 @@ class CartesianInterpolator /** \brief Compute the sequence of joint values that perform a general Cartesian path. In contrast to the previous functions, the Cartesian path is specified as a set of \e waypoints to be sequentially - reached by the virtual frame attached to the robot \e link. The waypoints are transforms given either w.r.t. the global - reference frame or the virtual frame at the immediately preceding waypoint. The virtual frame needs - to move in a straight line between two consecutive waypoints. All other comments apply. */ + reached by the virtual frame attached to the robot \e link. The waypoints are transforms given either w.r.t. the + global reference frame or the virtual frame at the immediately preceding waypoint. The virtual frame needs to move + in a straight line between two consecutive waypoints. All other comments apply. */ static Percentage computeCartesianPath( - RobotState* start_state, const JointModelGroup* group, std::vector>& traj, + RobotState* start_state, const JointModelGroup* group, std::vector>& path, const LinkModel* link, const EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame, const MaxEEFStep& max_step, const JumpThreshold& jump_threshold, const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(), @@ -225,60 +218,29 @@ class CartesianInterpolator const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn(), const Eigen::Isometry3d& link_offset = Eigen::Isometry3d::Identity()); - /** \brief Tests joint space jumps of a trajectory. + /** \brief Checks if a path has a joint-space jump, and truncates the path at the jump. - If \e jump_threshold_factor is non-zero, we test for relative jumps. - If \e revolute_jump_threshold or \e prismatic_jump_threshold are non-zero, we test for absolute jumps. - Both tests can be combined. If all params are zero, jump detection is disabled. - For relative jump detection, the average joint-space distance between consecutive points in the trajectory is - computed. If any individual joint-space motion delta is larger then this average distance by a factor of - \e jump_threshold_factor, this step is considered a failure and the returned path is truncated up to just - before the jump. + Checks if a path has a jump larger than `jump_threshold` and truncates the path to the waypoint right before the + jump. Returns the percentage of the path that doesn't have jumps. @param group The joint model group of the robot state. - @param traj The trajectory that should be tested. + @param path The path that should be tested. @param jump_threshold The struct holding jump thresholds to determine if a joint space jump has occurred. - @return The fraction of the trajectory that passed. - - TODO: move to more appropriate location + @return The fraction of the path that passed. */ - static Percentage checkJointSpaceJump(const JointModelGroup* group, std::vector>& traj, + static Percentage checkJointSpaceJump(const JointModelGroup* group, std::vector>& path, const JumpThreshold& jump_threshold); +}; - /** \brief Tests for relative joint space jumps of the trajectory \e traj. - - First, the average distance between adjacent trajectory points is computed. If two adjacent trajectory points - have distance > \e jump_threshold_factor * average, the trajectory is truncated at this point. - - @param group The joint model group of the robot state. - @param traj The trajectory that should be tested. - @param jump_threshold_factor The threshold to determine if a joint space jump has occurred . - @return The fraction of the trajectory that passed. - - TODO: move to more appropriate location - */ - static Percentage checkRelativeJointSpaceJump(const JointModelGroup* group, - std::vector>& traj, - double jump_threshold_factor); - - /** \brief Tests for absolute joint space jumps of the trajectory \e traj. - - The joint-space difference between consecutive waypoints is computed for each active joint and compared to the - absolute thresholds \e revolute_jump_threshold for revolute joints and \e prismatic_jump_threshold for prismatic - joints. If these thresholds are exceeded, the trajectory is truncated. - - @param group The joint model group of the robot state. - @param traj The trajectory that should be tested. - @param revolute_jump_threshold Absolute joint-space threshold for revolute joints. - @param prismatic_jump_threshold Absolute joint-space threshold for prismatic joints. - @return The fraction of the trajectory that passed. +/** \brief Checks if a joint-space path has a jump larger than the given threshold. - TODO: move to more appropriate location - */ - static Percentage checkAbsoluteJointSpaceJump(const JointModelGroup* group, - std::vector>& traj, - double revolute_jump_threshold, double prismatic_jump_threshold); -}; + This function computes the distance between every pair of adjacent waypoints (for the given group) and checks if that + distance is larger than the threshold defined by `jump_threshold`. If so, it is considered that the path has a + jump, and the path index where the jump happens is returned as output. + Otherwise the function returns a nullopt. */ +std::optional hasJointSpaceJump(const std::vector& waypoints, + const moveit::core::JointModelGroup& group, + const moveit::core::JumpThreshold& jump_threshold); } // end of namespace core } // end of namespace moveit diff --git a/moveit_core/robot_state/src/cartesian_interpolator.cpp b/moveit_core/robot_state/src/cartesian_interpolator.cpp index b2a2b75d69..b0550b96da 100644 --- a/moveit_core/robot_state/src/cartesian_interpolator.cpp +++ b/moveit_core/robot_state/src/cartesian_interpolator.cpp @@ -41,21 +41,132 @@ #include #include #include - -namespace moveit -{ -namespace core +#include +namespace moveit::core { -/** \brief It is recommended that there are at least 10 steps per trajectory - * for testing jump thresholds with computeCartesianPath. With less than 10 steps - * it is difficult to choose a jump_threshold parameter that effectively separates - * valid paths from paths with large joint space jumps. */ + +// Minimum amount of path waypoints recommended to reliably compute a joint-space increment average. +// If relative jump detection is selected and the path is shorter than `MIN_STEPS_FOR_JUMP_THRESH`, a warning message +// will be printed out. static const std::size_t MIN_STEPS_FOR_JUMP_THRESH = 10; static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_robot_state.cartesian_interpolator"); +namespace +{ +std::optional hasRelativeJointSpaceJump(const std::vector& waypoints, + const moveit::core::JointModelGroup& group, double jump_threshold_factor) +{ + if (waypoints.size() < MIN_STEPS_FOR_JUMP_THRESH) + { + RCLCPP_WARN(LOGGER, + "The computed path is too short to detect jumps in joint-space. " + "Need at least %zu steps, only got %zu. Try a lower max_step.", + MIN_STEPS_FOR_JUMP_THRESH, waypoints.size()); + } + + std::vector dist_vector; + dist_vector.reserve(waypoints.size() - 1); + double total_dist = 0.0; + for (std::size_t i = 1; i < waypoints.size(); ++i) + { + const double dist_prev_point = waypoints[i]->distance(*waypoints[i - 1], &group); + dist_vector.push_back(dist_prev_point); + total_dist += dist_prev_point; + } + + // compute the average distance between the states we looked at. + double thres = jump_threshold_factor * (total_dist / static_cast(dist_vector.size())); + for (std::size_t i = 0; i < dist_vector.size(); ++i) + { + if (dist_vector[i] > thres) + { + return i + 1; + } + } + + return std::nullopt; +} + +std::optional hasAbsoluteJointSpaceJump(const std::vector& waypoints, + const moveit::core::JointModelGroup& group, double revolute_threshold, + double prismatic_threshold) +{ + const bool check_revolute = revolute_threshold > 0.0; + const bool check_prismatic = prismatic_threshold > 0.0; + + const std::vector& joints = group.getActiveJointModels(); + for (std::size_t i = 1; i < waypoints.size(); ++i) + { + for (const auto& joint : joints) + { + const double distance = waypoints[i]->distance(*waypoints[i - 1], joint); + switch (joint->getType()) + { + case moveit::core::JointModel::REVOLUTE: + if (check_revolute && distance > revolute_threshold) + { + return i; + } + break; + case moveit::core::JointModel::PRISMATIC: + if (check_prismatic && distance > prismatic_threshold) + { + return i; + } + break; + default: + RCLCPP_WARN(LOGGER, + "Joint %s has not supported type %s. \n" + "hasAbsoluteJointSpaceJump only supports prismatic and revolute joints. Skipping joint jump " + "check for this joint.", + joint->getName().c_str(), joint->getTypeName().c_str()); + continue; + } + } + } + + return std::nullopt; +} +} // namespace + +JumpThreshold JumpThreshold::disabled() +{ + return JumpThreshold(); +} + +JumpThreshold JumpThreshold::relative(double relative_factor) +{ + rcpputils::require_true(relative_factor > 1.0); + + JumpThreshold threshold; + threshold.relative_factor = relative_factor; + return threshold; +} + +JumpThreshold JumpThreshold::absolute(double revolute, double prismatic) +{ + rcpputils::require_true(revolute > 0.0); + rcpputils::require_true(prismatic > 0.0); + + JumpThreshold threshold; + threshold.revolute = revolute; + threshold.prismatic = prismatic; + return threshold; +} + +JumpThreshold::JumpThreshold(double relative_factor) +{ + this->relative_factor = relative_factor; +} +JumpThreshold::JumpThreshold(double revolute, double prismatic) +{ + this->revolute = revolute; + this->prismatic = prismatic; +} + CartesianInterpolator::Distance CartesianInterpolator::computeCartesianPath( - RobotState* start_state, const JointModelGroup* group, std::vector& traj, const LinkModel* link, + RobotState* start_state, const JointModelGroup* group, std::vector& path, const LinkModel* link, const Eigen::Vector3d& translation, bool global_reference_frame, const MaxEEFStep& max_step, const JumpThreshold& jump_threshold, const GroupStateValidityCallbackFn& validCallback, const kinematics::KinematicsQueryOptions& options, const kinematics::KinematicsBase::IKCostFn& cost_function) @@ -68,13 +179,13 @@ CartesianInterpolator::Distance CartesianInterpolator::computeCartesianPath( pose.translation() += global_reference_frame ? translation : pose.linear() * translation; // call computeCartesianPath for the computed target pose in the global reference frame - return CartesianInterpolator::Distance(distance) * computeCartesianPath(start_state, group, traj, link, pose, true, + return CartesianInterpolator::Distance(distance) * computeCartesianPath(start_state, group, path, link, pose, true, max_step, jump_threshold, validCallback, options, cost_function); } CartesianInterpolator::Percentage CartesianInterpolator::computeCartesianPath( - RobotState* start_state, const JointModelGroup* group, std::vector& traj, const LinkModel* link, + RobotState* start_state, const JointModelGroup* group, std::vector& path, const LinkModel* link, const Eigen::Isometry3d& target, bool global_reference_frame, const MaxEEFStep& max_step, const JumpThreshold& jump_threshold, const GroupStateValidityCallbackFn& validCallback, const kinematics::KinematicsQueryOptions& options, const kinematics::KinematicsBase::IKCostFn& cost_function, @@ -110,7 +221,7 @@ CartesianInterpolator::Percentage CartesianInterpolator::computeCartesianPath( double rotation_distance = start_quaternion.angularDistance(target_quaternion); double translation_distance = (rotated_target.translation() - start_pose.translation()).norm(); - // decide how many steps we will need for this trajectory + // decide how many steps we will need for this path std::size_t translation_steps = 0; if (max_step.translation > 0.0) translation_steps = floor(translation_distance / max_step.translation); @@ -121,7 +232,7 @@ CartesianInterpolator::Percentage CartesianInterpolator::computeCartesianPath( // If we are testing for relative jumps, we always want at least MIN_STEPS_FOR_JUMP_THRESH steps std::size_t steps = std::max(translation_steps, rotation_steps) + 1; - if (jump_threshold.factor > 0 && steps < MIN_STEPS_FOR_JUMP_THRESH) + if (jump_threshold.relative_factor > 0 && steps < MIN_STEPS_FOR_JUMP_THRESH) steps = MIN_STEPS_FOR_JUMP_THRESH; // To limit absolute joint-space jumps, we pass consistency limits to the IK solver @@ -148,8 +259,8 @@ CartesianInterpolator::Percentage CartesianInterpolator::computeCartesianPath( } } - traj.clear(); - traj.push_back(std::make_shared(*start_state)); + path.clear(); + path.push_back(std::make_shared(*start_state)); double last_valid_percentage = 0.0; for (std::size_t i = 1; i <= steps; ++i) @@ -159,12 +270,12 @@ CartesianInterpolator::Percentage CartesianInterpolator::computeCartesianPath( Eigen::Isometry3d pose(start_quaternion.slerp(percentage, target_quaternion)); pose.translation() = percentage * rotated_target.translation() + (1 - percentage) * start_pose.translation(); - // Explicitly use a single IK attempt only: We want a smooth trajectory. - // Random seeding (of additional attempts) would probably create IK jumps. + // Explicitly use a single IK attempt only (by setting a timeout of 0.0), using the current state as the seed. + // Random seeding (of additional attempts) would create large joint-space jumps. if (start_state->setFromIK(group, pose * offset, link->getName(), consistency_limits, 0.0, validCallback, options, cost_function)) { - traj.push_back(std::make_shared(*start_state)); + path.push_back(std::make_shared(*start_state)); } else { @@ -174,13 +285,13 @@ CartesianInterpolator::Percentage CartesianInterpolator::computeCartesianPath( last_valid_percentage = percentage; } - last_valid_percentage *= checkJointSpaceJump(group, traj, jump_threshold); + last_valid_percentage *= checkJointSpaceJump(group, path, jump_threshold); return CartesianInterpolator::Percentage(last_valid_percentage); } CartesianInterpolator::Percentage CartesianInterpolator::computeCartesianPath( - RobotState* start_state, const JointModelGroup* group, std::vector& traj, const LinkModel* link, + RobotState* start_state, const JointModelGroup* group, std::vector& path, const LinkModel* link, const EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame, const MaxEEFStep& max_step, const JumpThreshold& jump_threshold, const GroupStateValidityCallbackFn& validCallback, const kinematics::KinematicsQueryOptions& options, const kinematics::KinematicsBase::IKCostFn& cost_function, @@ -189,147 +300,73 @@ CartesianInterpolator::Percentage CartesianInterpolator::computeCartesianPath( double percentage_solved = 0.0; for (std::size_t i = 0; i < waypoints.size(); ++i) { - // Don't test joint space jumps for every waypoint, test them later on the whole trajectory. - static const JumpThreshold NO_JOINT_SPACE_JUMP_TEST; - std::vector waypoint_traj; + // Don't test joint space jumps for every waypoint, test them later on the whole path. + static const JumpThreshold NO_JOINT_SPACE_JUMP_TEST = JumpThreshold::disabled(); + std::vector waypoint_path; double wp_percentage_solved = - computeCartesianPath(start_state, group, waypoint_traj, link, waypoints[i], global_reference_frame, max_step, + computeCartesianPath(start_state, group, waypoint_path, link, waypoints[i], global_reference_frame, max_step, NO_JOINT_SPACE_JUMP_TEST, validCallback, options, cost_function, link_offset); if (fabs(wp_percentage_solved - 1.0) < std::numeric_limits::epsilon()) { percentage_solved = static_cast((i + 1)) / static_cast(waypoints.size()); - std::vector::iterator start = waypoint_traj.begin(); - if (i > 0 && !waypoint_traj.empty()) + std::vector::iterator start = waypoint_path.begin(); + if (i > 0 && !waypoint_path.empty()) std::advance(start, 1); - traj.insert(traj.end(), start, waypoint_traj.end()); + path.insert(path.end(), start, waypoint_path.end()); } else { percentage_solved += wp_percentage_solved / static_cast(waypoints.size()); - std::vector::iterator start = waypoint_traj.begin(); - if (i > 0 && !waypoint_traj.empty()) + std::vector::iterator start = waypoint_path.begin(); + if (i > 0 && !waypoint_path.empty()) std::advance(start, 1); - traj.insert(traj.end(), start, waypoint_traj.end()); + path.insert(path.end(), start, waypoint_path.end()); break; } } - percentage_solved *= checkJointSpaceJump(group, traj, jump_threshold); + percentage_solved *= checkJointSpaceJump(group, path, jump_threshold); return CartesianInterpolator::Percentage(percentage_solved); } CartesianInterpolator::Percentage CartesianInterpolator::checkJointSpaceJump(const JointModelGroup* group, - std::vector& traj, + std::vector& path, const JumpThreshold& jump_threshold) { - double percentage_solved = 1.0; - if (traj.size() <= 1) - return percentage_solved; - - if (jump_threshold.factor > 0.0) - percentage_solved *= checkRelativeJointSpaceJump(group, traj, jump_threshold.factor); + std::optional jump_index = hasJointSpaceJump(path, *group, jump_threshold); - if (jump_threshold.revolute > 0.0 || jump_threshold.prismatic > 0.0) - percentage_solved *= checkAbsoluteJointSpaceJump(group, traj, jump_threshold.revolute, jump_threshold.prismatic); + double percentage_solved = 1.0; + if (jump_index.has_value()) + { + percentage_solved = static_cast(*jump_index) / static_cast(path.size()); + // Truncate the path at the index right before the jump. + path.resize(jump_index.value()); + } return CartesianInterpolator::Percentage(percentage_solved); } -CartesianInterpolator::Percentage CartesianInterpolator::checkRelativeJointSpaceJump(const JointModelGroup* group, - std::vector& traj, - double jump_threshold_factor) +std::optional hasJointSpaceJump(const std::vector& waypoints, + const moveit::core::JointModelGroup& group, + const moveit::core::JumpThreshold& jump_threshold) { - if (traj.size() < MIN_STEPS_FOR_JUMP_THRESH) + if (waypoints.size() <= 1) { - RCLCPP_WARN(LOGGER, - "The computed trajectory is too short to detect jumps in joint-space " - "Need at least %zu steps, only got %zu. Try a lower max_step.", - MIN_STEPS_FOR_JUMP_THRESH, traj.size()); + return std::nullopt; } - std::vector dist_vector; - dist_vector.reserve(traj.size() - 1); - double total_dist = 0.0; - for (std::size_t i = 1; i < traj.size(); ++i) + if (jump_threshold.relative_factor > 0.0) { - double dist_prev_point = traj[i]->distance(*traj[i - 1], group); - dist_vector.push_back(dist_prev_point); - total_dist += dist_prev_point; + return hasRelativeJointSpaceJump(waypoints, group, jump_threshold.relative_factor); } - double percentage = 1.0; - // compute the average distance between the states we looked at - double thres = jump_threshold_factor * (total_dist / static_cast(dist_vector.size())); - for (std::size_t i = 0; i < dist_vector.size(); ++i) + if (jump_threshold.revolute > 0.0 || jump_threshold.prismatic > 0.0) { - if (dist_vector[i] > thres) - { - RCLCPP_DEBUG(LOGGER, "Truncating Cartesian path due to detected jump in joint-space distance"); - percentage = static_cast(i + 1) / static_cast(traj.size()); - traj.resize(i + 1); - break; - } + return hasAbsoluteJointSpaceJump(waypoints, group, jump_threshold.revolute, jump_threshold.prismatic); } - return CartesianInterpolator::Percentage(percentage); -} - -CartesianInterpolator::Percentage CartesianInterpolator::checkAbsoluteJointSpaceJump(const JointModelGroup* group, - std::vector& traj, - double revolute_threshold, - double prismatic_threshold) -{ - bool check_revolute = revolute_threshold > 0.0; - bool check_prismatic = prismatic_threshold > 0.0; - - double joint_threshold; - bool check_joint; - bool still_valid = true; - const std::vector& joints = group->getActiveJointModels(); - for (std::size_t traj_ix = 0, ix_end = traj.size() - 1; traj_ix != ix_end; ++traj_ix) - { - for (auto& joint : joints) - { - switch (joint->getType()) - { - case JointModel::REVOLUTE: - check_joint = check_revolute; - joint_threshold = revolute_threshold; - break; - case JointModel::PRISMATIC: - check_joint = check_prismatic; - joint_threshold = prismatic_threshold; - break; - default: - RCLCPP_WARN(LOGGER, - "Joint %s has not supported type %s. \n" - "checkAbsoluteJointSpaceJump only supports prismatic and revolute joints.", - joint->getName().c_str(), joint->getTypeName().c_str()); - continue; - } - if (check_joint) - { - double distance = traj[traj_ix]->distance(*traj[traj_ix + 1], joint); - if (distance > joint_threshold) - { - RCLCPP_DEBUG(LOGGER, "Truncating Cartesian path due to detected jump of %.4f > %.4f in joint %s", distance, - joint_threshold, joint->getName().c_str()); - still_valid = false; - break; - } - } - } - - if (!still_valid) - { - double percent_valid = static_cast(traj_ix + 1) / static_cast(traj.size()); - traj.resize(traj_ix + 1); - return CartesianInterpolator::Percentage(percent_valid); - } - } - return CartesianInterpolator::Percentage(1.0); + return std::nullopt; } -} // end of namespace core -} // end of namespace moveit +} // end of namespace moveit::core diff --git a/moveit_core/robot_state/test/test_cartesian_interpolator.cpp b/moveit_core/robot_state/test/test_cartesian_interpolator.cpp index 03c30beb76..c15f172b76 100644 --- a/moveit_core/robot_state/test/test_cartesian_interpolator.cpp +++ b/moveit_core/robot_state/test/test_cartesian_interpolator.cpp @@ -141,34 +141,29 @@ TEST_F(SimpleRobot, checkAbsoluteJointSpaceJump) // Container for results double fraction; - // Direct call of absolute version - fraction = CartesianInterpolator::checkAbsoluteJointSpaceJump(joint_model_group, traj, 1.0, 1.0); - EXPECT_EQ(expected_revolute_jump_traj_len, traj.size()); // traj should be cut - EXPECT_NEAR(expected_revolute_jump_fraction, fraction, 0.01); - - // Indirect call using checkJointSpaceJumps + // Revolute and prismatic joints. generateTestTraj(traj, robot_model_); - fraction = CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj, JumpThreshold(1.0, 1.0)); + fraction = CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj, JumpThreshold::absolute(1.0, 1.0)); EXPECT_EQ(expected_revolute_jump_traj_len, traj.size()); // traj should be cut before the revolute jump EXPECT_NEAR(expected_revolute_jump_fraction, fraction, 0.01); // Test revolute joints generateTestTraj(traj, robot_model_); - fraction = CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj, JumpThreshold(1.0, 0.0)); + fraction = CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj, JumpThreshold::absolute(0.01, 10.0)); EXPECT_EQ(expected_revolute_jump_traj_len, traj.size()); // traj should be cut before the revolute jump EXPECT_NEAR(expected_revolute_jump_fraction, fraction, 0.01); // Test prismatic joints generateTestTraj(traj, robot_model_); - fraction = CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj, JumpThreshold(0.0, 1.0)); + fraction = CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj, JumpThreshold::absolute(10.0, 0.01)); EXPECT_EQ(expected_prismatic_jump_traj_len, traj.size()); // traj should be cut before the prismatic jump EXPECT_NEAR(expected_prismatic_jump_fraction, fraction, 0.01); - // Ignore all absolute jumps - generateTestTraj(traj, robot_model_); - fraction = CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj, JumpThreshold(0.0, 0.0)); - EXPECT_EQ(full_traj_len, traj.size()); // traj should not be cut - EXPECT_NEAR(1.0, fraction, 0.01); + // Pre-condition checks. + EXPECT_ANY_THROW( + CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj, JumpThreshold::absolute(0.0, 0.01))); + EXPECT_ANY_THROW( + CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj, JumpThreshold::absolute(0.01, 0.0))); } TEST_F(SimpleRobot, checkRelativeJointSpaceJump) @@ -185,24 +180,22 @@ TEST_F(SimpleRobot, checkRelativeJointSpaceJump) static_cast(expected_relative_jump_traj_len) / static_cast(full_traj_len); // Container for results - double fraction; + double fraction = 0.0; - // Direct call of relative version: 1.01 > 2.97 * (0.01 * 2 + 1.01 * 2)/6. - fraction = CartesianInterpolator::checkRelativeJointSpaceJump(joint_model_group, traj, 2.97); - EXPECT_EQ(expected_relative_jump_traj_len, traj.size()); // traj should be cut before the first jump of 1.01 - EXPECT_NEAR(expected_relative_jump_fraction, fraction, 0.01); - - // Indirect call of relative version using checkJointSpaceJumps + // Call of relative version: 1.01 > 2.97 * (0.01 * 2 + 1.01 * 2)/6. generateTestTraj(traj, robot_model_); - fraction = CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj, JumpThreshold(2.97)); + fraction = CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj, JumpThreshold::relative(2.97)); EXPECT_EQ(expected_relative_jump_traj_len, traj.size()); // traj should be cut before the first jump of 1.01 EXPECT_NEAR(expected_relative_jump_fraction, fraction, 0.01); // Trajectory should not be cut: 1.01 < 2.98 * (0.01 * 2 + 1.01 * 2)/6. generateTestTraj(traj, robot_model_); - fraction = CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj, JumpThreshold(2.98)); + fraction = CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj, JumpThreshold::relative(2.98)); EXPECT_EQ(full_traj_len, traj.size()); // traj should not be cut EXPECT_NEAR(1.0, fraction, 0.01); + + // Pre-condition checks. + EXPECT_ANY_THROW(CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj, JumpThreshold::relative(0.0))); } // TODO - The tests below fail since no kinematic plugins are found. Move the tests to IK plugin package. @@ -235,7 +228,8 @@ TEST_F(SimpleRobot, checkRelativeJointSpaceJump) // bool global) // { // return CartesianInterpolator::computeCartesianPath(start_state.get(), jmg, result, link, translation, global, -// MaxEEFStep(0.1), JumpThreshold(), GroupStateValidityCallbackFn(), +// MaxEEFStep(0.1), JumpThreshold::Disabled(), +// GroupStateValidityCallbackFn(), // kinematics::KinematicsQueryOptions()); // } // @@ -243,7 +237,8 @@ TEST_F(SimpleRobot, checkRelativeJointSpaceJump) // bool global, const Eigen::Isometry3d& offset = Eigen::Isometry3d::Identity()) // { // return CartesianInterpolator::computeCartesianPath(start_state.get(), jmg, result, link, target, global, -// MaxEEFStep(0.1), JumpThreshold(), GroupStateValidityCallbackFn(), +// MaxEEFStep(0.1), JumpThreshold::Disabled(), +// GroupStateValidityCallbackFn(), // kinematics::KinematicsQueryOptions(), // kinematics::KinematicsBase::IKCostFn(), offset); // } @@ -335,7 +330,8 @@ TEST_F(SimpleRobot, checkRelativeJointSpaceJump) // TEST_F(PandaRobot, testRotationOffset) // { // // define offset to virtual center frame -// Eigen::Isometry3d offset = Eigen::Translation3d(0, 0, 0.2) * Eigen::AngleAxisd(-M_PI / 4, Eigen::Vector3d::UnitZ()); +// Eigen::Isometry3d offset = Eigen::Translation3d(0, 0, 0.2) * Eigen::AngleAxisd(-M_PI / 4, +// Eigen::Vector3d::UnitZ()); // // 45° rotation about center's x-axis // Eigen::Isometry3d rot(Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitX())); // Eigen::Isometry3d goal = start_pose * offset * rot; diff --git a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp index 26a5ff0290..0367ae0360 100644 --- a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp @@ -171,7 +171,8 @@ bool MoveGroupCartesianPathService::computeService( std::vector traj; res->fraction = moveit::core::CartesianInterpolator::computeCartesianPath( &start_state, jmg, traj, start_state.getLinkModel(link_name), waypoints, global_frame, - moveit::core::MaxEEFStep(req->max_step), moveit::core::JumpThreshold(req->jump_threshold), constraint_fn); + moveit::core::MaxEEFStep(req->max_step), moveit::core::JumpThreshold::relative(req->jump_threshold), + constraint_fn); moveit::core::robotStateToRobotStateMsg(start_state, res->start_state); robot_trajectory::RobotTrajectory rt(context_->planning_scene_monitor_->getRobotModel(), req->group_name);