New implementation for computeCartesianPath (#3618)

* Deprecate JumpThreshold-based computeCartesianPath()
* Remove deprecated methods RobotState::computeCartesianPath()
* Replace deprecated functions

moveit_commander/demos/plan_with_constraints.py

@@ -71,5 +71,5 @@
     wpose.position.y += 0.05
     waypoints.append(wpose)
 
-    plan, fraction = a.compute_cartesian_path(waypoints, 0.01, 0.0, path_constraints=c)
+    plan, fraction = a.compute_cartesian_path(waypoints, 0.01, True, path_constraints=c)
     print("Plan success percent: ", fraction)

moveit_commander/src/moveit_commander/move_group.py

@@ -648,7 +648,6 @@ def compute_cartesian_path(
         self,
         waypoints,
         eef_step,
-        jump_threshold,
         avoid_collisions=True,
         path_constraints=None,
     ):
@@ -670,15 +669,13 @@ def compute_cartesian_path(
             (ser_path, fraction) = self._g.compute_cartesian_path(
                 [conversions.pose_to_list(p) for p in waypoints],
                 eef_step,
-                jump_threshold,
                 avoid_collisions,
                 constraints_str,
             )
         else:
             (ser_path, fraction) = self._g.compute_cartesian_path(
                 [conversions.pose_to_list(p) for p in waypoints],
                 eef_step,
-                jump_threshold,
                 avoid_collisions,
             )
 

moveit_commander/test/python_time_parameterization.py

@@ -60,7 +60,7 @@ def plan(self):
         goal_pose = self.group.get_current_pose().pose
         goal_pose.position.z -= 0.1
         (plan, fraction) = self.group.compute_cartesian_path(
-            [start_pose, goal_pose], 0.005, 0.0
+            [start_pose, goal_pose], 0.005
         )
         self.assertEqual(fraction, 1.0, "Cartesian path plan failed")
         return plan

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

@@ -46,6 +46,14 @@ namespace moveit
 {
 namespace core
 {
+/** Struct defining linear and rotational precision */
+struct CartesianPrecision
+{
+  double translational = 0.001;  //< max deviation in translation (meters)
+  double rotational = 0.01;      //< max deviation in rotation (radians)
+  double max_resolution = 1e-5;  //< max resolution for waypoints (fraction of total path)
+};
+
 /** \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
@@ -97,6 +105,75 @@ class CartesianInterpolator
   // TODO(mlautman): Eventually, this planner should be moved out of robot_state
 
 public:
+  /** \brief Compute the sequence of joint values that correspond to a straight Cartesian path for a particular link.
+
+     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.
+     The resulting joint values are stored in the vector \e traj, one by one. The interpolation 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.
+
+     The struct CartesianPrecision specifies the precision to which the path should follow the Cartesian straight line.
+     If the deviation at the mid point of two consecutive waypoints is larger than the specified precision, another waypoint
+     will be inserted at that mid point. The precision is specified separately for translation and rotation.
+     The maximal resolution to consider (as fraction of the total path length) is specified by max_resolution.
+  */
+  static double
+  computeCartesianPath(const RobotState* start_state, const JointModelGroup* group,
+                       std::vector<std::shared_ptr<RobotState>>& traj, const LinkModel* link,
+                       const Eigen::Vector3d& translation, bool global_reference_frame, const MaxEEFStep& max_step,
+                       const CartesianPrecision& precision,
+                       const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(),
+                       const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions());
+
+  /** \brief Compute the sequence of joint values that correspond to a straight Cartesian path, for a particular link.
+
+     In contrast to the previous function, the translation vector is specified as a (unit) direction vector and
+     a distance. */
+  static double
+  computeCartesianPath(const RobotState* start_state, const JointModelGroup* group,
+                       std::vector<std::shared_ptr<RobotState>>& traj, const LinkModel* link,
+                       const Eigen::Vector3d& direction, bool global_reference_frame, double distance,
+                       const MaxEEFStep& max_step, const CartesianPrecision& precision,
+                       const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(),
+                       const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions())
+  {
+    return computeCartesianPath(start_state, group, traj, link, distance * direction, global_reference_frame, max_step,
+                                precision, validCallback, options);
+  }
+
+  /** \brief Compute the sequence of joint values that correspond to a straight Cartesian path, for a particular frame.
+
+     In contrast to the previous function, the Cartesian path is specified as a target frame to be reached (\e target)
+     for a virtual frame attached to the robot \e link with the given \e link_offset.
+     The target frame is assumed to be specified either w.r.t. to the global reference frame or the virtual link frame.
+     This function returns the fraction (0..1) of path that was achieved. All other comments from the previous function apply. */
+  static double computeCartesianPath(const RobotState* start_state, const JointModelGroup* group,
+                                     std::vector<RobotStatePtr>& traj, const LinkModel* link,
+                                     const Eigen::Isometry3d& target, bool global_reference_frame,
+                                     const MaxEEFStep& max_step, const CartesianPrecision& precision,
+                                     const GroupStateValidityCallbackFn& validCallback,
+                                     const kinematics::KinematicsQueryOptions& options,
+                                     const Eigen::Isometry3d& link_offset = Eigen::Isometry3d::Identity());
+
+  /** \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. */
+  static double
+  computeCartesianPath(const RobotState* start_state, const JointModelGroup* group,
+                       std::vector<std::shared_ptr<RobotState>>& traj, const LinkModel* link,
+                       const EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame,
+                       const MaxEEFStep& max_step, const CartesianPrecision& precision,
+                       const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(),
+                       const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
+                       const Eigen::Isometry3d& link_offset = Eigen::Isometry3d::Identity());
+
   /** \brief Compute the sequence of joint values that correspond to a straight Cartesian path for a particular link.
 
      The Cartesian path to be followed is specified as a \e translation vector to be followed by the robot \e link.
@@ -125,7 +202,7 @@ class CartesianInterpolator
      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.*/
-  static double
+  [[deprecated("Replace JumpThreshold with CartesianPrecision")]] static double
   computeCartesianPath(RobotState* start_state, const JointModelGroup* group,
                        std::vector<std::shared_ptr<RobotState>>& traj, const LinkModel* link,
                        const Eigen::Vector3d& translation, bool global_reference_frame, const MaxEEFStep& max_step,
@@ -137,16 +214,19 @@ class CartesianInterpolator
 
      In contrast to the previous function, the translation vector is specified as a (unit) direction vector and
      a distance. */
-  static double
+  [[deprecated("Replace JumpThreshold with CartesianPrecision")]] static double
   computeCartesianPath(RobotState* start_state, const JointModelGroup* group,
                        std::vector<std::shared_ptr<RobotState>>& traj, 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())
   {
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
     return computeCartesianPath(start_state, group, traj, link, distance * direction, global_reference_frame, max_step,
                                 jump_threshold, validCallback, options);
+#pragma GCC diagnostic pop
   }
 
   /** \brief Compute the sequence of joint values that correspond to a straight Cartesian path, for a particular frame.
@@ -155,7 +235,7 @@ class CartesianInterpolator
      for a virtual frame attached to the robot \e link with the given \e link_offset.
      The target frame is assumed to be specified either w.r.t. to the global reference frame or the virtual link frame.
      This function returns the fraction (0..1) of path that was achieved. All other comments from the previous function apply. */
-  static double
+  [[deprecated("Replace JumpThreshold with CartesianPrecision")]] static double
   computeCartesianPath(RobotState* start_state, const JointModelGroup* group,
                        std::vector<std::shared_ptr<RobotState>>& traj, const LinkModel* link,
                        const Eigen::Isometry3d& target, bool global_reference_frame, const MaxEEFStep& max_step,
@@ -170,7 +250,7 @@ class CartesianInterpolator
      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 double
+  [[deprecated("Replace JumpThreshold with CartesianPrecision")]] static double
   computeCartesianPath(RobotState* start_state, const JointModelGroup* group,
                        std::vector<std::shared_ptr<RobotState>>& traj, const LinkModel* link,
                        const EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame,

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

@@ -1122,78 +1122,6 @@ class RobotState
   bool setFromDiffIK(const JointModelGroup* group, const geometry_msgs::Twist& twist, const std::string& tip, double dt,
                      const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn());
 
-  /** \brief Compute the sequence of joint values that correspond to a straight Cartesian path for a particular group.
-
-     The Cartesian path to be followed is specified as a direction of motion (\e direction, unit vector) for the origin
-     The Cartesian path to be followed is specified as a direction of motion (\e direction, unit vector) for the origin
-     of a robot link (\e link). The direction is assumed to be either in a global reference frame or in the local
-     reference frame of the link. In the latter case (\e global_reference_frame is false) the \e direction is rotated
-     accordingly. The link needs to move in a straight line, following the specified direction, for the desired \e
-     distance. The resulting joint values are stored in the vector \e traj, 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 computed 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
-     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.
-
-     NOTE: As of ROS-Melodic these are deprecated and should not be used
-     */
-  [[deprecated]] double
-  computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
-                       const Eigen::Vector3d& direction, bool global_reference_frame, double distance, double max_step,
-                       double jump_threshold_factor,
-                       const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(),
-                       const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions());
-
-  /** \brief Compute the sequence of joint values that correspond to a straight Cartesian path, for a particular group.
-
-     In contrast to the previous function, the Cartesian path is specified as a target frame to be reached (\e target)
-     for the origin of a robot link (\e link). The target frame is assumed to be either in a global reference frame or
-     in the local reference frame of the link. In the latter case (\e global_reference_frame is false) the \e target is
-     rotated accordingly. All other comments from the previous function apply.
-
-     NOTE: As of ROS-Melodic these are deprecated and should not be used
-     */
-  [[deprecated]] double
-  computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
-                       const Eigen::Isometry3d& target, bool global_reference_frame, double max_step,
-                       double jump_threshold_factor,
-                       const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(),
-                       const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions());
-
-  /** \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 for the origin of a robot link (\e link). The waypoints are transforms given either in a global reference
-     frame or in the local reference frame of the link at the immediately preceeding waypoint. The link needs to move
-     in a straight line between two consecutive waypoints. All other comments apply.
-
-     NOTE: As of ROS-Melodic these are deprecated and should not be used
-     */
-  [[deprecated]] double
-  computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
-                       const EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame, double max_step,
-                       double jump_threshold_factor,
-                       const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(),
-                       const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions());
-
   /** \brief Compute the Jacobian with reference to a particular point on a given link, for a specified group.
    * \param group The group to compute the Jacobian for
    * \param link The link model to compute the Jacobian for