Add named builder methods to JumpThreshold

moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h

@@ -44,32 +44,36 @@ 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. */
-struct JumpThreshold
+/** \brief Class with options for defining joint-space jump thresholds. */
+class JumpThreshold
 {
-  double factor = 0.0;
-  double revolute = 0.0;   // Radians
-  double prismatic = 0.0;  // Meters
+public:
+  /** \brief Do not define any jump threshold, i.e., disable joint-space jump detection. */
+  static JumpThreshold Disabled();
 
-  explicit JumpThreshold() = default;
+  /** \brief Detect joint-space jumps relative to the average joint-space displacement along the path.
 
-  // For relative jump detection, the average joint-space distance between consecutive points in the path is
-  // computed. If any individual joint-space motion delta is larger than this average distance by a factor of
-  // \e jt_factor, it is considered the path has a jump.
-  explicit JumpThreshold(double jt_factor) : JumpThreshold()
-  {
-    factor = jt_factor;
-  }
+     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);
 
-  explicit JumpThreshold(double jt_revolute, double jt_prismatic) : JumpThreshold()
-  {
-    revolute = jt_revolute;    // Radians
-    prismatic = jt_prismatic;  // Meters
-  }
+  /** \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
+
+private:
+  // Private constructors. Construct using builder methods in the public interface.
+  JumpThreshold() = default;
+  JumpThreshold(double relative_factor);
+  JumpThreshold(double revolute, double prismatic);
 };
 
 /** \brief Struct for containing max_step for computeCartesianPath
@@ -155,20 +159,8 @@ class CartesianInterpolator
 
      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 path 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.
+     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. */
@@ -242,12 +234,11 @@ class CartesianInterpolator
 };
 
 /** \brief Checks if a joint-space path has jumps larger than the given 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.
- */
+
+   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<int> hasJointSpaceJumps(const std::vector<moveit::core::RobotStatePtr>& waypoints,
                                       const moveit::core::JointModelGroup* group,
                                       const moveit::core::JumpThreshold& jump_threshold);

moveit_core/robot_state/src/cartesian_interpolator.cpp

@@ -130,6 +130,31 @@ std::optional<int> hasAbsoluteJointSpaceJump(const std::vector<moveit::core::Rob
 }
 }  // namespace
 
+JumpThreshold JumpThreshold::Disabled()
+{
+  return JumpThreshold();
+}
+
+JumpThreshold JumpThreshold::Relative(double relative_factor)
+{
+  return JumpThreshold(relative_factor);
+}
+
+JumpThreshold JumpThreshold::Absolute(double revolute, double prismatic)
+{
+  return JumpThreshold(revolute, prismatic);
+}
+
+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<RobotStatePtr>& path, const LinkModel* link,
     const Eigen::Vector3d& translation, bool global_reference_frame, const MaxEEFStep& max_step,
@@ -197,7 +222,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
@@ -266,7 +291,7 @@ CartesianInterpolator::Percentage CartesianInterpolator::computeCartesianPath(
   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 path.
-    static const JumpThreshold NO_JOINT_SPACE_JUMP_TEST;
+    static const JumpThreshold NO_JOINT_SPACE_JUMP_TEST = JumpThreshold::Disabled();
     std::vector<RobotStatePtr> waypoint_path;
     double wp_percentage_solved =
         computeCartesianPath(start_state, group, waypoint_path, link, waypoints[i], global_reference_frame, max_step,
@@ -321,9 +346,9 @@ std::optional<int> hasJointSpaceJumps(const std::vector<moveit::core::RobotState
     return std::nullopt;
   }
 
-  if (jump_threshold.factor > 0.0)
+  if (jump_threshold.relative_factor > 0.0)
   {
-    return hasRelativeJointSpaceJump(waypoints, group, jump_threshold.factor);
+    return hasRelativeJointSpaceJump(waypoints, group, jump_threshold.relative_factor);
   }
 
   if (jump_threshold.revolute > 0.0 || jump_threshold.prismatic > 0.0)

moveit_core/robot_state/test/test_cartesian_interpolator.cpp

@@ -143,25 +143,25 @@ TEST_F(SimpleRobot, checkAbsoluteJointSpaceJump)
 
   // 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(1.0, 0.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(0.0, 1.0));
   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));
+  fraction = CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj, JumpThreshold::Absolute(0.0, 0.0));
   EXPECT_EQ(full_traj_len, traj.size());  // traj should not be cut
   EXPECT_NEAR(1.0, fraction, 0.01);
 }
@@ -184,13 +184,13 @@ TEST_F(SimpleRobot, checkRelativeJointSpaceJump)
 
   // 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);
 }
@@ -225,15 +225,17 @@ 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());
 //   }
 //
 //   double computeCartesianPath(std::vector<std::shared_ptr<RobotState>>& result, const Eigen::Isometry3d& target,
 //                               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);
 //   }
@@ -325,7 +327,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;

moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp

@@ -171,7 +171,8 @@ bool MoveGroupCartesianPathService::computeService(
           std::vector<moveit::core::RobotStatePtr> 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);