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 d1af7d41bf..089eba6b8f 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,6 +44,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 with options for defining joint-space jump thresholds. */ struct JumpThreshold { @@ -144,6 +152,75 @@ class CartesianInterpolator double meters; }; + /** \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 Distance 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(), + const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn()); + + /** \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 Distance 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(), + const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn()) + { + return computeCartesianPath(start_state, group, traj, link, distance * direction, global_reference_frame, max_step, + precision, validCallback, options, cost_function); + } + + /** \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 Percentage 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 kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn(), + 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 Percentage 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 kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn(), + 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. @@ -158,12 +235,19 @@ 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 argument is provided. If a jump detection is enabled and a jump is - found, the path is truncated up to just before the jump. + 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. To this end, the average joint-space distance + between consecutive points in the trajectory is computed. If any individual joint-space motion delta is larger than + this average distance multiplied by \e jump_threshold_factor, this step is considered a jump. + + Otherwise (if all params are zero), jump detection is disabled. + If a jump is detected, 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( + [[deprecated("Replace JumpThreshold with CartesianPrecision")]] static Distance computeCartesianPath( 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, @@ -175,7 +259,7 @@ class CartesianInterpolator In contrast to the previous function, the translation vector is specified as a (unit) direction vector and a distance. */ - static Distance computeCartesianPath( + [[deprecated("Replace JumpThreshold with CartesianPrecision")]] static Distance computeCartesianPath( 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, @@ -183,8 +267,11 @@ class CartesianInterpolator const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn()) { +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" return computeCartesianPath(start_state, group, path, link, distance * direction, global_reference_frame, max_step, jump_threshold, validCallback, options, cost_function); +#pragma GCC diagnostic pop } /** \brief Compute the sequence of joint values that correspond to a straight Cartesian path, for a particular frame. @@ -194,7 +281,7 @@ class CartesianInterpolator The target frame is assumed to be specified either w.r.t. to the global reference frame or the virtual link frame (\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( + [[deprecated("Replace JumpThreshold with CartesianPrecision")]] static Percentage computeCartesianPath( 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, @@ -209,7 +296,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 Percentage computeCartesianPath( + [[deprecated("Replace JumpThreshold with CartesianPrecision")]] static Percentage computeCartesianPath( 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, diff --git a/moveit_core/robot_state/src/cartesian_interpolator.cpp b/moveit_core/robot_state/src/cartesian_interpolator.cpp index 02707c3481..995338badd 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 @@ -60,6 +60,54 @@ rclcpp::Logger getLogger() return moveit::getLogger("moveit.core.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 kinematics::KinematicsBase::IKCostFn& cost_function, + 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, + cost_function)) + 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, cost_function, 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, cost_function, link_offset); +} + std::optional hasRelativeJointSpaceJump(const std::vector& waypoints, const moveit::core::JointModelGroup& group, double jump_threshold_factor) { @@ -136,6 +184,129 @@ std::optional hasAbsoluteJointSpaceJump(const 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 kinematics::KinematicsBase::IKCostFn& cost_function) +{ + 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 CartesianInterpolator::Distance(distance) * computeCartesianPath(start_state, group, traj, link, pose, true, + max_step, precision, validCallback, options, + cost_function); +} + +CartesianInterpolator::Percentage 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 kinematics::KinematicsBase::IKCostFn& cost_function, + 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 = static_cast(i) / static_cast(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, cost_function) || + !validateAndImproveInterval(prev_state, state, prev_pose, pose, traj, percentage, + 1.0 / static_cast(steps), group, link, precision, validCallback, options, + cost_function, link_offset)) + break; + + prev_pose = pose; + prev_state = state; + last_valid_percentage = percentage; + } + + return last_valid_percentage; +} + +CartesianInterpolator::Percentage 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 kinematics::KinematicsBase::IKCostFn& cost_function, + 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, 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::advance(start, 1); + traj.insert(traj.end(), start, waypoint_traj.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::advance(start, 1); + traj.insert(traj.end(), start, waypoint_traj.end()); + break; + } + } + + return percentage_solved; +} + JumpThreshold JumpThreshold::disabled() { return JumpThreshold(); @@ -184,10 +355,13 @@ CartesianInterpolator::Distance CartesianInterpolator::computeCartesianPath( // 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 CartesianInterpolator::Distance(distance) * computeCartesianPath(start_state, group, path, link, pose, true, max_step, jump_threshold, validCallback, options, cost_function); +#pragma GCC diagnostic pop } CartesianInterpolator::Percentage CartesianInterpolator::computeCartesianPath( @@ -310,9 +484,12 @@ CartesianInterpolator::Percentage CartesianInterpolator::computeCartesianPath( // 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; +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" double wp_percentage_solved = 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); +#pragma GCC diagnostic pop if (fabs(wp_percentage_solved - 1.0) < std::numeric_limits::epsilon()) { percentage_solved = static_cast((i + 1)) / static_cast(waypoints.size()); 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 c868454fed..34af2a8094 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 @@ -185,7 +185,7 @@ 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), jump_threshold, constraint_fn); + moveit::core::MaxEEFStep(req->max_step), moveit::core::CartesianPrecision{}, constraint_fn); moveit::core::robotStateToRobotStateMsg(start_state, res->start_state); robot_trajectory::RobotTrajectory rt(context_->planning_scene_monitor_->getRobotModel(), req->group_name); 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 4f82643017..be02e8c6d2 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 @@ -765,9 +765,16 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT 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. */ + [[deprecated("Drop jump_threshold")]] double // + computeCartesianPath(const std::vector& waypoints, double eef_step, + double /*jump_threshold*/, moveit_msgs::msg::RobotTrajectory& trajectory, + bool avoid_collisions = true, moveit_msgs::msg::MoveItErrorCodes* error_code = nullptr) + { + return computeCartesianPath(waypoints, eef_step, trajectory, avoid_collisions, error_code); + } double computeCartesianPath(const std::vector& waypoints, double eef_step, - double jump_threshold, moveit_msgs::msg::RobotTrajectory& trajectory, - bool avoid_collisions = true, moveit_msgs::msg::MoveItErrorCodes* error_code = nullptr); + moveit_msgs::msg::RobotTrajectory& trajectory, bool avoid_collisions = true, + moveit_msgs::msg::MoveItErrorCodes* error_code = nullptr); /** \brief Compute a Cartesian path that follows specified waypoints with a step size of at most \e eef_step meters between end effector configurations of consecutive points in the result \e trajectory. The reference frame for the @@ -781,8 +788,16 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT 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. */ + [[deprecated("Drop jump_threshold")]] double // + computeCartesianPath(const std::vector& waypoints, double eef_step, + double /*jump_threshold*/, moveit_msgs::msg::RobotTrajectory& trajectory, + const moveit_msgs::msg::Constraints& path_constraints, bool avoid_collisions = true, + moveit_msgs::msg::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, - double jump_threshold, moveit_msgs::msg::RobotTrajectory& trajectory, + moveit_msgs::msg::RobotTrajectory& trajectory, const moveit_msgs::msg::Constraints& path_constraints, bool avoid_collisions = true, moveit_msgs::msg::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 3fbf9e3c63..84a46005ce 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 @@ -860,7 +860,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } double computeCartesianPath(const std::vector& waypoints, double step, - double jump_threshold, moveit_msgs::msg::RobotTrajectory& msg, + moveit_msgs::msg::RobotTrajectory& msg, const moveit_msgs::msg::Constraints& path_constraints, bool avoid_collisions, moveit_msgs::msg::MoveItErrorCodes& error_code) { @@ -874,7 +874,6 @@ class MoveGroupInterface::MoveGroupInterfaceImpl req->header.stamp = getClock()->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(); @@ -1553,31 +1552,29 @@ moveit::core::MoveItErrorCode MoveGroupInterface::plan(Plan& plan) //} double MoveGroupInterface::computeCartesianPath(const std::vector& waypoints, double eef_step, - double jump_threshold, moveit_msgs::msg::RobotTrajectory& trajectory, - bool avoid_collisions, moveit_msgs::msg::MoveItErrorCodes* error_code) + moveit_msgs::msg::RobotTrajectory& trajectory, bool avoid_collisions, + moveit_msgs::msg::MoveItErrorCodes* error_code) { moveit_msgs::msg::Constraints path_constraints_tmp; - return computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, moveit_msgs::msg::Constraints(), - avoid_collisions, error_code); + return computeCartesianPath(waypoints, eef_step, trajectory, moveit_msgs::msg::Constraints(), avoid_collisions, + error_code); } double MoveGroupInterface::computeCartesianPath(const std::vector& waypoints, double eef_step, - double jump_threshold, moveit_msgs::msg::RobotTrajectory& trajectory, + moveit_msgs::msg::RobotTrajectory& trajectory, const moveit_msgs::msg::Constraints& path_constraints, bool avoid_collisions, moveit_msgs::msg::MoveItErrorCodes* error_code) { if (error_code) { - return impl_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints, - avoid_collisions, *error_code); + return impl_->computeCartesianPath(waypoints, eef_step, trajectory, path_constraints, avoid_collisions, *error_code); } else { moveit_msgs::msg::MoveItErrorCodes err_tmp; err_tmp.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; moveit_msgs::msg::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); } } 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 370d2f72df..2b0595005e 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/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 e79a026806..746685ed43 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 @@ -135,13 +135,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::msg::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) {