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 f5b549d245..c3aea90ff6 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 @@ -97,6 +97,51 @@ class CartesianInterpolator // TODO(mlautman): Eventually, this planner should be moved out of robot_state public: + struct Percentage + { + // value must be in [0,1] + Percentage(double value) : value(value) + { + if (value < 0.0 || value > 1.0) + throw std::runtime_error("Percentage values must be between 0 and 1, inclusive"); + } + operator double() + { + return value; + } + double operator*() + { + return value; + } + Percentage operator*(const Percentage& p) + { + Percentage res(value * p.value); + return res; + } + double value; + }; + + struct Distance + { + Distance(double meters) : meters(meters) + { + } + operator double() + { + return meters; + } + double operator*() + { + return meters; + } + Distance operator*(const Percentage& p) + { + Distance res(meters * p.value); + return res; + } + double meters; + }; + /** \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 @@ -127,7 +172,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 + static Distance computeCartesianPath(RobotState* start_state, const JointModelGroup* group, std::vector>& traj, const LinkModel* link, const Eigen::Vector3d& direction, bool global_reference_frame, double distance, @@ -141,7 +186,7 @@ class CartesianInterpolator 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. */ - static double + static Percentage 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, @@ -155,7 +200,7 @@ class CartesianInterpolator 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 preceding waypoint. The link needs to move in a straight line between two consecutive waypoints. All other comments apply. */ - static double + static Percentage computeCartesianPath(RobotState* start_state, const JointModelGroup* group, std::vector>& traj, const LinkModel* link, const EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame, @@ -180,8 +225,8 @@ class CartesianInterpolator TODO: move to more appropriate location */ - static double checkJointSpaceJump(const JointModelGroup* group, std::vector>& traj, - const JumpThreshold& jump_threshold); + static Percentage checkJointSpaceJump(const JointModelGroup* group, std::vector>& traj, + const JumpThreshold& jump_threshold); /** \brief Tests for relative joint space jumps of the trajectory \e traj. @@ -195,9 +240,9 @@ class CartesianInterpolator TODO: move to more appropriate location */ - static double checkRelativeJointSpaceJump(const JointModelGroup* group, - std::vector>& traj, - double jump_threshold_factor); + static Percentage checkRelativeJointSpaceJump(const JointModelGroup* group, + std::vector>& traj, + double jump_threshold_factor); /** \brief Tests for absolute joint space jumps of the trajectory \e traj. @@ -213,9 +258,9 @@ class CartesianInterpolator TODO: move to more appropriate location */ - static double checkAbsoluteJointSpaceJump(const JointModelGroup* group, - std::vector>& traj, - double revolute_jump_threshold, double prismatic_jump_threshold); + static Percentage checkAbsoluteJointSpaceJump(const JointModelGroup* group, + std::vector>& traj, + double revolute_jump_threshold, double prismatic_jump_threshold); }; } // end of namespace core diff --git a/moveit_core/robot_state/src/cartesian_interpolator.cpp b/moveit_core/robot_state/src/cartesian_interpolator.cpp index 87ceb56839..d9ad07bdd3 100644 --- a/moveit_core/robot_state/src/cartesian_interpolator.cpp +++ b/moveit_core/robot_state/src/cartesian_interpolator.cpp @@ -52,13 +52,11 @@ static const std::size_t MIN_STEPS_FOR_JUMP_THRESH = 10; static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_robot_state.cartesian_interpolator"); -double CartesianInterpolator::computeCartesianPath(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 JumpThreshold& jump_threshold, - const GroupStateValidityCallbackFn& validCallback, - const kinematics::KinematicsQueryOptions& options) +CartesianInterpolator::Distance CartesianInterpolator::computeCartesianPath( + 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 JumpThreshold& jump_threshold, const GroupStateValidityCallbackFn& validCallback, + const kinematics::KinematicsQueryOptions& options) { // this is the Cartesian pose we start from, and have to move in the direction indicated // getGlobalLinkTransform() returns a valid isometry by contract @@ -72,16 +70,16 @@ double CartesianInterpolator::computeCartesianPath(RobotState* start_state, cons target_pose.translation() += rotated_direction * distance; // call computeCartesianPath for the computed target pose in the global reference frame - return (distance * computeCartesianPath(start_state, group, traj, link, target_pose, true, max_step, jump_threshold, - validCallback, options)); + return CartesianInterpolator::Distance(distance) * computeCartesianPath(start_state, group, traj, link, target_pose, + true, max_step, jump_threshold, validCallback, + options); } -double CartesianInterpolator::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, const JumpThreshold& jump_threshold, - const GroupStateValidityCallbackFn& validCallback, - const kinematics::KinematicsQueryOptions& options) +CartesianInterpolator::Percentage CartesianInterpolator::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, + const JumpThreshold& jump_threshold, const GroupStateValidityCallbackFn& validCallback, + const kinematics::KinematicsQueryOptions& options) { const std::vector& cjnt = group->getContinuousJointModels(); // make sure that continuous joints wrap @@ -170,16 +168,14 @@ double CartesianInterpolator::computeCartesianPath(RobotState* start_state, cons last_valid_percentage *= checkJointSpaceJump(group, traj, jump_threshold); - return last_valid_percentage; + return CartesianInterpolator::Percentage(last_valid_percentage); } -double CartesianInterpolator::computeCartesianPath(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 JumpThreshold& jump_threshold, - const GroupStateValidityCallbackFn& validCallback, - const kinematics::KinematicsQueryOptions& options) +CartesianInterpolator::Percentage CartesianInterpolator::computeCartesianPath( + 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 JumpThreshold& jump_threshold, const GroupStateValidityCallbackFn& validCallback, + const kinematics::KinematicsQueryOptions& options) { double percentage_solved = 0.0; for (std::size_t i = 0; i < waypoints.size(); ++i) @@ -211,11 +207,12 @@ double CartesianInterpolator::computeCartesianPath(RobotState* start_state, cons percentage_solved *= checkJointSpaceJump(group, traj, jump_threshold); - return percentage_solved; + return CartesianInterpolator::Percentage(percentage_solved); } -double CartesianInterpolator::checkJointSpaceJump(const JointModelGroup* group, std::vector& traj, - const JumpThreshold& jump_threshold) +CartesianInterpolator::Percentage CartesianInterpolator::checkJointSpaceJump(const JointModelGroup* group, + std::vector& traj, + const JumpThreshold& jump_threshold) { double percentage_solved = 1.0; if (traj.size() <= 1) @@ -227,12 +224,12 @@ double CartesianInterpolator::checkJointSpaceJump(const JointModelGroup* group, if (jump_threshold.revolute > 0.0 || jump_threshold.prismatic > 0.0) percentage_solved *= checkAbsoluteJointSpaceJump(group, traj, jump_threshold.revolute, jump_threshold.prismatic); - return percentage_solved; + return CartesianInterpolator::Percentage(percentage_solved); } -double CartesianInterpolator::checkRelativeJointSpaceJump(const JointModelGroup* group, - std::vector& traj, - double jump_threshold_factor) +CartesianInterpolator::Percentage CartesianInterpolator::checkRelativeJointSpaceJump(const JointModelGroup* group, + std::vector& traj, + double jump_threshold_factor) { if (traj.size() < MIN_STEPS_FOR_JUMP_THRESH) { @@ -264,12 +261,13 @@ double CartesianInterpolator::checkRelativeJointSpaceJump(const JointModelGroup* break; } - return percentage; + return CartesianInterpolator::Percentage(percentage); } -double CartesianInterpolator::checkAbsoluteJointSpaceJump(const JointModelGroup* group, - std::vector& traj, double revolute_threshold, - double prismatic_threshold) +CartesianInterpolator::Percentage CartesianInterpolator::checkAbsoluteJointSpaceJump(const JointModelGroup* group, + std::vector& traj, + double revolute_threshold, + double prismatic_threshold) { bool check_revolute = revolute_threshold > 0.0; bool check_prismatic = prismatic_threshold > 0.0; @@ -316,10 +314,10 @@ double CartesianInterpolator::checkAbsoluteJointSpaceJump(const JointModelGroup* { double percent_valid = (double)(traj_ix + 1) / (double)(traj.size()); traj.resize(traj_ix + 1); - return percent_valid; + return CartesianInterpolator::Percentage(percent_valid); } } - return 1.0; + return CartesianInterpolator::Percentage(1.0); } } // end of namespace core