diff --git a/moveit_commander/demos/plan_with_constraints.py b/moveit_commander/demos/plan_with_constraints.py index 01b69ed1a1b..6ab57f5a959 100755 --- a/moveit_commander/demos/plan_with_constraints.py +++ b/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) diff --git a/moveit_commander/src/moveit_commander/move_group.py b/moveit_commander/src/moveit_commander/move_group.py index da3a4dfc463..a5509f92537 100644 --- a/moveit_commander/src/moveit_commander/move_group.py +++ b/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,7 +669,6 @@ 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, ) @@ -678,7 +676,6 @@ 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, ) diff --git a/moveit_commander/test/python_time_parameterization.py b/moveit_commander/test/python_time_parameterization.py index 1cf587d6afb..c373ea9c167 100755 --- a/moveit_commander/test/python_time_parameterization.py +++ b/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 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 40ed1163ce1..0a6de5c207a 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 @@ -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>& 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>& 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& 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>& 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>& traj, const LinkModel* link, const Eigen::Vector3d& translation, bool global_reference_frame, const MaxEEFStep& max_step, @@ -137,7 +214,7 @@ 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>& traj, const LinkModel* link, const Eigen::Vector3d& direction, bool global_reference_frame, double distance, @@ -145,8 +222,11 @@ class CartesianInterpolator 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>& 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>& traj, const LinkModel* link, const EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame, diff --git a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h index d07fea302c6..4462b1ed033 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h +++ b/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& 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& 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& 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 diff --git a/moveit_core/robot_state/src/cartesian_interpolator.cpp b/moveit_core/robot_state/src/cartesian_interpolator.cpp index 4d8d99f68f4..2d87ae98fda 100644 --- a/moveit_core/robot_state/src/cartesian_interpolator.cpp +++ b/moveit_core/robot_state/src/cartesian_interpolator.cpp @@ -34,7 +34,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Ioan Sucan, Sachin Chitta, Acorn Pooley, Mario Prats, Dave Coleman */ +/* Author: Ioan Sucan, Sachin Chitta, Acorn Pooley, Mario Prats, Dave Coleman, Robert Haschke */ #include #include @@ -52,6 +52,173 @@ static const std::size_t MIN_STEPS_FOR_JUMP_THRESH = 10; const std::string LOGNAME = "cartesian_interpolator"; +bool validateAndImproveInterval(const RobotState& start_state, const RobotState& end_state, + const Eigen::Isometry3d& start_pose, const Eigen::Isometry3d& end_pose, + std::vector& traj, double& percentage, const double width, + const JointModelGroup* group, const LinkModel* link, + const CartesianPrecision& precision, const GroupStateValidityCallbackFn& validCallback, + const kinematics::KinematicsQueryOptions& options, const Eigen::Isometry3d& link_offset) +{ + // compute pose at joint-space midpoint between start_state and end_state + RobotState mid_state(start_state.getRobotModel()); + start_state.interpolate(end_state, 0.5, mid_state); + Eigen::Isometry3d fk_pose = mid_state.getGlobalLinkTransform(link) * link_offset; + + // compute pose at Cartesian-space midpoint between start_pose and end_pose + Eigen::Isometry3d mid_pose(Eigen::Quaterniond(start_pose.linear()).slerp(0.5, Eigen::Quaterniond(end_pose.linear()))); + mid_pose.translation() = 0.5 * (start_pose.translation() + end_pose.translation()); + + // if deviation between both poses, fk_pose and mid_pose is within precision, we are satisfied + double linear_distance = (mid_pose.translation() - fk_pose.translation()).norm(); + double angular_distance = Eigen::Quaterniond(mid_pose.linear()).angularDistance(Eigen::Quaterniond(fk_pose.linear())); + if (linear_distance <= precision.translational && angular_distance <= precision.rotational) + { + traj.push_back(std::make_shared(end_state)); + return true; + } + + if (width < precision.max_resolution) + return false; // failed to find linear interpolation within max_resolution + + // otherwise subdivide interval further, computing IK for mid_pose + if (!mid_state.setFromIK(group, mid_pose * link_offset.inverse(), link->getName(), 0.0, validCallback, options)) + return false; + + // and recursively processing the two sub-intervals + const auto half_width = width / 2.0; + const auto old_percentage = percentage; + percentage = percentage - half_width; + if (!validateAndImproveInterval(start_state, mid_state, start_pose, mid_pose, traj, percentage, half_width, group, + link, precision, validCallback, options, link_offset)) + return false; + + percentage = old_percentage; + return validateAndImproveInterval(mid_state, end_state, mid_pose, end_pose, traj, percentage, half_width, group, link, + precision, validCallback, options, link_offset); +} + +double CartesianInterpolator::computeCartesianPath(const RobotState* start_state, const JointModelGroup* group, + std::vector& traj, const LinkModel* link, + const Eigen::Vector3d& translation, bool global_reference_frame, + const MaxEEFStep& max_step, const CartesianPrecision& precision, + const GroupStateValidityCallbackFn& validCallback, + const kinematics::KinematicsQueryOptions& options) +{ + const double distance = translation.norm(); + // The target pose is obtained by adding the translation vector to the link's current pose + Eigen::Isometry3d pose = start_state->getGlobalLinkTransform(link); + + // the translation direction can be specified w.r.t. the local link frame (then rotate into global frame) + pose.translation() += global_reference_frame ? translation : pose.linear() * translation; + + // call computeCartesianPath for the computed target pose in the global reference frame + return distance * + computeCartesianPath(start_state, group, traj, link, pose, true, max_step, precision, validCallback, options); +} + +double CartesianInterpolator::computeCartesianPath(const RobotState* start_state, const JointModelGroup* group, + std::vector& 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) +{ + // check unsanitized inputs for non-isometry + ASSERT_ISOMETRY(target) + ASSERT_ISOMETRY(link_offset) + + RobotState state(*start_state); + + const std::vector& cjnt = group->getContinuousJointModels(); + // make sure that continuous joints wrap + for (const JointModel* joint : cjnt) + state.enforceBounds(joint); + + // Cartesian pose we start from + Eigen::Isometry3d start_pose = state.getGlobalLinkTransform(link) * link_offset; + Eigen::Isometry3d inv_offset = link_offset.inverse(); + + // the target can be in the local reference frame (in which case we rotate it) + Eigen::Isometry3d rotated_target = global_reference_frame ? target : start_pose * target; + + Eigen::Quaterniond start_quaternion(start_pose.linear()); + Eigen::Quaterniond target_quaternion(rotated_target.linear()); + + 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 + std::size_t translation_steps = 0; + if (max_step.translation > 0.0) + translation_steps = floor(translation_distance / max_step.translation); + + std::size_t rotation_steps = 0; + if (max_step.rotation > 0.0) + rotation_steps = floor(rotation_distance / max_step.rotation); + std::size_t steps = std::max(translation_steps, rotation_steps) + 1; + + traj.clear(); + traj.push_back(std::make_shared(*start_state)); + + double last_valid_percentage = 0.0; + Eigen::Isometry3d prev_pose = start_pose; + RobotState prev_state(state); + for (std::size_t i = 1; i <= steps; ++i) + { + double percentage = (double)i / (double)steps; + + Eigen::Isometry3d pose(start_quaternion.slerp(percentage, target_quaternion)); + pose.translation() = percentage * rotated_target.translation() + (1 - percentage) * start_pose.translation(); + + if (!state.setFromIK(group, pose * inv_offset, link->getName(), 0.0, validCallback, options) || + !validateAndImproveInterval(prev_state, state, prev_pose, pose, traj, percentage, 1.0 / (double)steps, group, + link, precision, validCallback, options, link_offset)) + break; + + prev_pose = pose; + prev_state = state; + last_valid_percentage = percentage; + } + + return last_valid_percentage; +} + +double CartesianInterpolator::computeCartesianPath( + const RobotState* start_state, const JointModelGroup* group, std::vector& traj, + const LinkModel* link, const EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame, + const MaxEEFStep& max_step, const CartesianPrecision& precision, const GroupStateValidityCallbackFn& validCallback, + const kinematics::KinematicsQueryOptions& options, const Eigen::Isometry3d& link_offset) +{ + double percentage_solved = 0.0; + for (std::size_t i = 0; i < waypoints.size(); ++i) + { + std::vector waypoint_traj; + double wp_percentage_solved = + computeCartesianPath(start_state, group, waypoint_traj, link, waypoints[i], global_reference_frame, max_step, + precision, validCallback, options, link_offset); + if (fabs(wp_percentage_solved - 1.0) < std::numeric_limits::epsilon()) + { + percentage_solved = (double)(i + 1) / (double)waypoints.size(); + std::vector::iterator start = waypoint_traj.begin(); + if (i > 0 && !waypoint_traj.empty()) + std::advance(start, 1); + traj.insert(traj.end(), start, waypoint_traj.end()); + } + else + { + percentage_solved += wp_percentage_solved / (double)waypoints.size(); + std::vector::iterator start = waypoint_traj.begin(); + if (i > 0 && !waypoint_traj.empty()) + std::advance(start, 1); + traj.insert(traj.end(), start, waypoint_traj.end()); + break; + } + } + + return percentage_solved; +} + double CartesianInterpolator::computeCartesianPath(RobotState* start_state, const JointModelGroup* group, std::vector& traj, const LinkModel* link, const Eigen::Vector3d& translation, bool global_reference_frame, @@ -66,9 +233,12 @@ double CartesianInterpolator::computeCartesianPath(RobotState* start_state, cons // the translation direction can be specified w.r.t. the local link frame (then rotate into global frame) pose.translation() += global_reference_frame ? translation : pose.linear() * translation; +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" // call computeCartesianPath for the computed target pose in the global reference frame return distance * computeCartesianPath(start_state, group, traj, link, pose, true, max_step, jump_threshold, validCallback, options); +#pragma GCC diagnostic pop } double CartesianInterpolator::computeCartesianPath(RobotState* start_state, const JointModelGroup* group, @@ -184,9 +354,12 @@ double CartesianInterpolator::computeCartesianPath( // 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; +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" double wp_percentage_solved = computeCartesianPath(start_state, group, waypoint_traj, link, waypoints[i], global_reference_frame, max_step, NO_JOINT_SPACE_JUMP_TEST, validCallback, options, link_offset); +#pragma GCC diagnostic pop if (fabs(wp_percentage_solved - 1.0) < std::numeric_limits::epsilon()) { percentage_solved = (double)(i + 1) / (double)waypoints.size(); diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 5c455acbb19..ab357ffa019 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -2027,39 +2027,6 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: return false; } -double RobotState::computeCartesianPath(const JointModelGroup* group, std::vector& traj, - const LinkModel* link, const Eigen::Vector3d& direction, - bool global_reference_frame, double distance, double max_step, - double jump_threshold_factor, const GroupStateValidityCallbackFn& validCallback, - const kinematics::KinematicsQueryOptions& options) -{ - return CartesianInterpolator::computeCartesianPath(this, group, traj, link, direction, global_reference_frame, - distance, MaxEEFStep(max_step), - JumpThreshold(jump_threshold_factor), validCallback, options); -} - -double RobotState::computeCartesianPath(const JointModelGroup* group, std::vector& traj, - const LinkModel* link, const Eigen::Isometry3d& target, - bool global_reference_frame, double max_step, double jump_threshold_factor, - const GroupStateValidityCallbackFn& validCallback, - const kinematics::KinematicsQueryOptions& options) -{ - return CartesianInterpolator::computeCartesianPath(this, group, traj, link, target, global_reference_frame, - MaxEEFStep(max_step), JumpThreshold(jump_threshold_factor), - validCallback, options); -} - -double RobotState::computeCartesianPath(const JointModelGroup* group, std::vector& traj, - const LinkModel* link, const EigenSTL::vector_Isometry3d& waypoints, - bool global_reference_frame, double max_step, double jump_threshold_factor, - const GroupStateValidityCallbackFn& validCallback, - const kinematics::KinematicsQueryOptions& options) -{ - return CartesianInterpolator::computeCartesianPath(this, group, traj, link, waypoints, global_reference_frame, - MaxEEFStep(max_step), JumpThreshold(jump_threshold_factor), - validCallback, options); -} - void RobotState::computeAABB(std::vector& aabb) const { BOOST_VERIFY(checkLinkTransforms()); diff --git a/moveit_core/robot_state/test/test_cartesian_interpolator.cpp b/moveit_core/robot_state/test/test_cartesian_interpolator.cpp index 5f6426709af..8f876aff3ca 100644 --- a/moveit_core/robot_state/test/test_cartesian_interpolator.cpp +++ b/moveit_core/robot_state/test/test_cartesian_interpolator.cpp @@ -246,14 +246,16 @@ class PandaRobot : public testing::Test bool global) { return CartesianInterpolator::computeCartesianPath(start_state_.get(), jmg_, result, link_, translation, global, - MaxEEFStep(0.1), JumpThreshold(), GroupStateValidityCallbackFn(), + MaxEEFStep(0.1), CartesianPrecision{}, + GroupStateValidityCallbackFn(), kinematics::KinematicsQueryOptions()); } double computeCartesianPath(std::vector>& 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), CartesianPrecision{ 0.01, 0.01 }, + GroupStateValidityCallbackFn(), kinematics::KinematicsQueryOptions(), offset); } diff --git a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/approach_and_translate_stage.h b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/approach_and_translate_stage.h index 8c7014495f7..c20d210f7c9 100644 --- a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/approach_and_translate_stage.h +++ b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/approach_and_translate_stage.h @@ -58,6 +58,5 @@ class ApproachAndTranslateStage : public ManipulationStage unsigned int max_goal_count_; unsigned int max_fail_; double max_step_; - double jump_factor_; }; } // namespace pick_place diff --git a/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp b/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp index ac261c46481..260897616e8 100644 --- a/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp +++ b/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp @@ -51,7 +51,6 @@ ApproachAndTranslateStage::ApproachAndTranslateStage( max_goal_count_ = GetGlobalPickPlaceParams().max_goal_count_; max_fail_ = GetGlobalPickPlaceParams().max_fail_; max_step_ = GetGlobalPickPlaceParams().max_step_; - jump_factor_ = GetGlobalPickPlaceParams().jump_factor_; } namespace @@ -250,7 +249,7 @@ bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr& plan) const double d_close_up = moveit::core::CartesianInterpolator::computeCartesianPath( close_up_state.get(), plan->shared_data_->planning_group_, close_up_states, plan->shared_data_->ik_link_, approach_direction, approach_direction_is_global_frame, MAX_CLOSE_UP_DIST, - moveit::core::MaxEEFStep(max_step_), moveit::core::JumpThreshold(jump_factor_), approach_valid_callback); + moveit::core::MaxEEFStep(max_step_), moveit::core::CartesianPrecision{}, approach_valid_callback); // if progress towards the object was made, update the desired goal state if (d_close_up > 0.0 && close_up_states.size() > 1) *plan->possible_goal_states_[i] = *close_up_states[close_up_states.size() - 2]; @@ -263,8 +262,8 @@ bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr& plan) const double d_approach = moveit::core::CartesianInterpolator::computeCartesianPath( first_approach_state.get(), plan->shared_data_->planning_group_, approach_states, plan->shared_data_->ik_link_, -approach_direction, approach_direction_is_global_frame, - plan->approach_.desired_distance, moveit::core::MaxEEFStep(max_step_), - moveit::core::JumpThreshold(jump_factor_), approach_valid_callback); + plan->approach_.desired_distance, moveit::core::MaxEEFStep(max_step_), moveit::core::CartesianPrecision{}, + approach_valid_callback); // if we were able to follow the approach direction for sufficient length, try to compute a retreat direction if (d_approach > plan->approach_.min_distance && !signal_stop_) @@ -298,8 +297,8 @@ bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr& plan) const double d_retreat = moveit::core::CartesianInterpolator::computeCartesianPath( last_retreat_state.get(), plan->shared_data_->planning_group_, retreat_states, plan->shared_data_->ik_link_, retreat_direction, retreat_direction_is_global_frame, - plan->retreat_.desired_distance, moveit::core::MaxEEFStep(max_step_), - moveit::core::JumpThreshold(jump_factor_), retreat_valid_callback); + plan->retreat_.desired_distance, moveit::core::MaxEEFStep(max_step_), moveit::core::CartesianPrecision{}, + retreat_valid_callback); // if sufficient progress was made in the desired direction, we have a goal state that we can consider for // future stages 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 c0f033f8b1d..f840b7b53b8 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 @@ -158,14 +158,14 @@ bool MoveGroupCartesianPathService::computeService(moveit_msgs::GetCartesianPath } ROS_INFO_NAMED(getName(), "Attempting to follow %u waypoints for link '%s' using a step of %lf m " - "and jump threshold %lf (in %s reference frame)", - (unsigned int)waypoints.size(), link_name.c_str(), req.max_step, req.jump_threshold, + "(in %s reference frame)", + (unsigned int)waypoints.size(), link_name.c_str(), req.max_step, global_frame ? "global" : "link"); std::vector traj; res.fraction = moveit::core::CartesianInterpolator::computeCartesianPath( &start_state, jmg, traj, link_model, waypoints, global_frame, moveit::core::MaxEEFStep(req.max_step), - moveit::core::JumpThreshold(req.jump_threshold), constraint_fn, kinematics::KinematicsQueryOptions(), + moveit::core::CartesianPrecision{}, constraint_fn, kinematics::KinematicsQueryOptions(), start_state.getGlobalLinkTransform(link_model).inverse() * frame_pose); moveit::core::robotStateToRobotStateMsg(start_state, res.start_state); diff --git a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h index 6079d50dea3..e54c676ba3f 100644 --- a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h +++ b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h @@ -779,7 +779,14 @@ class MoveGroupInterface Return a value that is between 0.0 and 1.0 indicating the fraction of the path achieved as described by the waypoints. Return -1.0 in case of error. */ - double computeCartesianPath(const std::vector& waypoints, double eef_step, double jump_threshold, + [[deprecated("Drop jump_threshold")]] double // + computeCartesianPath(const std::vector& waypoints, double eef_step, double /*jump_threshold*/, + moveit_msgs::RobotTrajectory& trajectory, bool avoid_collisions = true, + moveit_msgs::MoveItErrorCodes* error_code = nullptr) + { + return computeCartesianPath(waypoints, eef_step, trajectory, avoid_collisions, error_code); + } + double computeCartesianPath(const std::vector& waypoints, double eef_step, moveit_msgs::RobotTrajectory& trajectory, bool avoid_collisions = true, moveit_msgs::MoveItErrorCodes* error_code = nullptr); @@ -795,7 +802,14 @@ class MoveGroupInterface Return a value that is between 0.0 and 1.0 indicating the fraction of the path achieved as described by the waypoints. Return -1.0 in case of error. */ - double computeCartesianPath(const std::vector& waypoints, double eef_step, double jump_threshold, + [[deprecated("Drop jump_threshold")]] double // + computeCartesianPath(const std::vector& waypoints, double eef_step, double /*jump_threshold*/, + moveit_msgs::RobotTrajectory& trajectory, const moveit_msgs::Constraints& path_constraints, + bool avoid_collisions = true, moveit_msgs::MoveItErrorCodes* error_code = nullptr) + { + return computeCartesianPath(waypoints, eef_step, trajectory, path_constraints, avoid_collisions, error_code); + } + double computeCartesianPath(const std::vector& waypoints, double eef_step, moveit_msgs::RobotTrajectory& trajectory, const moveit_msgs::Constraints& path_constraints, bool avoid_collisions = true, moveit_msgs::MoveItErrorCodes* error_code = nullptr); diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp index 119883b64f4..e8c6b1dfe52 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp @@ -941,7 +941,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } } - double computeCartesianPath(const std::vector& waypoints, double step, double jump_threshold, + double computeCartesianPath(const std::vector& waypoints, double step, moveit_msgs::RobotTrajectory& msg, const moveit_msgs::Constraints& path_constraints, bool avoid_collisions, moveit_msgs::MoveItErrorCodes& error_code) { @@ -954,7 +954,6 @@ class MoveGroupInterface::MoveGroupInterfaceImpl req.header.stamp = ros::Time::now(); req.waypoints = waypoints; req.max_step = step; - req.jump_threshold = jump_threshold; req.path_constraints = path_constraints; req.avoid_collisions = avoid_collisions; req.link_name = getEndEffectorLink(); @@ -1595,22 +1594,20 @@ moveit::core::MoveItErrorCode MoveGroupInterface::place(const moveit_msgs::Place } double MoveGroupInterface::computeCartesianPath(const std::vector& waypoints, double eef_step, - double jump_threshold, moveit_msgs::RobotTrajectory& trajectory, - bool avoid_collisions, moveit_msgs::MoveItErrorCodes* error_code) + moveit_msgs::RobotTrajectory& trajectory, bool avoid_collisions, + moveit_msgs::MoveItErrorCodes* error_code) { - return computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, moveit_msgs::Constraints(), - avoid_collisions, error_code); + return computeCartesianPath(waypoints, eef_step, trajectory, moveit_msgs::Constraints(), avoid_collisions, error_code); } double MoveGroupInterface::computeCartesianPath(const std::vector& waypoints, double eef_step, - double jump_threshold, moveit_msgs::RobotTrajectory& trajectory, + moveit_msgs::RobotTrajectory& trajectory, const moveit_msgs::Constraints& path_constraints, bool avoid_collisions, moveit_msgs::MoveItErrorCodes* error_code) { moveit_msgs::MoveItErrorCodes err_tmp; moveit_msgs::MoveItErrorCodes& err = error_code ? *error_code : err_tmp; - return impl_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints, - avoid_collisions, err); + return impl_->computeCartesianPath(waypoints, eef_step, trajectory, path_constraints, avoid_collisions, err); } void MoveGroupInterface::stop() diff --git a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp b/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp index d3c55acd399..8918709257f 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp @@ -493,24 +493,22 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer return py_bindings_tools::serializeMsg(request); } - bp::tuple computeCartesianPathPython(const bp::list& waypoints, double eef_step, double jump_threshold, - bool avoid_collisions) + bp::tuple computeCartesianPathPython(const bp::list& waypoints, double eef_step, bool avoid_collisions) { moveit_msgs::Constraints path_constraints_tmp; - return doComputeCartesianPathPython(waypoints, eef_step, jump_threshold, avoid_collisions, path_constraints_tmp); + return doComputeCartesianPathPython(waypoints, eef_step, avoid_collisions, path_constraints_tmp); } - bp::tuple computeCartesianPathConstrainedPython(const bp::list& waypoints, double eef_step, double jump_threshold, - bool avoid_collisions, + bp::tuple computeCartesianPathConstrainedPython(const bp::list& waypoints, double eef_step, bool avoid_collisions, const py_bindings_tools::ByteString& path_constraints_str) { moveit_msgs::Constraints path_constraints; py_bindings_tools::deserializeMsg(path_constraints_str, path_constraints); - return doComputeCartesianPathPython(waypoints, eef_step, jump_threshold, avoid_collisions, path_constraints); + return doComputeCartesianPathPython(waypoints, eef_step, avoid_collisions, path_constraints); } - bp::tuple doComputeCartesianPathPython(const bp::list& waypoints, double eef_step, double jump_threshold, - bool avoid_collisions, const moveit_msgs::Constraints& path_constraints) + bp::tuple doComputeCartesianPathPython(const bp::list& waypoints, double eef_step, bool avoid_collisions, + const moveit_msgs::Constraints& path_constraints) { std::vector poses; convertListToArrayOfPoses(waypoints, poses); @@ -518,7 +516,7 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer double fraction; { GILReleaser gr; - fraction = computeCartesianPath(poses, eef_step, jump_threshold, trajectory, path_constraints, avoid_collisions); + fraction = computeCartesianPath(poses, eef_step, trajectory, path_constraints, avoid_collisions); } return bp::make_tuple(py_bindings_tools::serializeMsg(trajectory), fraction); } diff --git a/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp b/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp index 00ee97f18e5..d5b75eb77de 100644 --- a/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp +++ b/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp @@ -238,6 +238,10 @@ TEST_F(MoveGroupTestFixture, PathConstraintCollisionTest) // clear path constraints move_group_->clearPathConstraints(); + + // move back to ready pose + move_group_->setNamedTarget("ready"); + planAndMove(); } TEST_F(MoveGroupTestFixture, ModifyPlanningSceneAsyncInterfaces) @@ -311,11 +315,10 @@ TEST_F(MoveGroupTestFixture, CartPathTest) waypoints.push_back(target_waypoint); // up and left moveit_msgs::RobotTrajectory trajectory; - const auto jump_threshold = 0.0; const auto eef_step = 0.01; // test below is meaningless if Cartesian planning did not succeed - ASSERT_GE(EPSILON + move_group_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory), 1.0); + ASSERT_GE(EPSILON + move_group_->computeCartesianPath(waypoints, eef_step, trajectory), 1.0); // Execute trajectory EXPECT_EQ(move_group_->execute(trajectory), moveit::core::MoveItErrorCode::SUCCESS); diff --git a/moveit_ros/planning_interface/test/subframes_test.cpp b/moveit_ros/planning_interface/test/subframes_test.cpp index 6f5bf3e1de3..54a2b1f3a3e 100644 --- a/moveit_ros/planning_interface/test/subframes_test.cpp +++ b/moveit_ros/planning_interface/test/subframes_test.cpp @@ -103,7 +103,7 @@ bool moveCartesianPath(const geometry_msgs::PoseStamped& pose, moveit::planning_ std::vector waypoints; waypoints.push_back(pose.pose); moveit_msgs::RobotTrajectory trajectory; - double percent = group.computeCartesianPath(waypoints, 0.01, 0, trajectory, true); + double percent = group.computeCartesianPath(waypoints, 0.01, trajectory, true); if (percent == 1.0) { group.execute(trajectory); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp index 07e42b8838a..4a75865f66f 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp @@ -129,13 +129,11 @@ bool MotionPlanningFrame::computeCartesianPlan() // setup default params double cart_step_size = 0.01; - double cart_jump_thresh = 0.0; bool avoid_collisions = true; // compute trajectory moveit_msgs::RobotTrajectory trajectory; - double fraction = - move_group_->computeCartesianPath(waypoints, cart_step_size, cart_jump_thresh, trajectory, avoid_collisions); + double fraction = move_group_->computeCartesianPath(waypoints, cart_step_size, trajectory, avoid_collisions); if (fraction >= 1.0) {