From e86c571a2944f1125b9c7170ac6e2b7b0965a891 Mon Sep 17 00:00:00 2001 From: Mario Prats Date: Tue, 31 Oct 2023 10:33:15 +0100 Subject: [PATCH] Add named builder methods to JumpThreshold --- .../robot_state/cartesian_interpolator.h | 75 ++++++++----------- .../src/cartesian_interpolator.cpp | 33 +++++++- .../test/test_cartesian_interpolator.cpp | 21 +++--- .../cartesian_path_service_capability.cpp | 3 +- 4 files changed, 76 insertions(+), 56 deletions(-) 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 24e702634c5..9a39bf2d66b 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,32 +44,36 @@ namespace moveit { namespace core { -/** \brief Struct for containing jump_threshold. - - For the purposes of maintaining API, we support both \e jump_threshold_factor which provides a scaling factor for - detecting joint space jumps and \e revolute_jump_threshold and \e prismatic_jump_threshold which provide absolute - thresholds for detecting joint space jumps. */ -struct JumpThreshold +/** \brief Class with options for defining joint-space jump thresholds. */ +class JumpThreshold { - double factor = 0.0; - double revolute = 0.0; // Radians - double prismatic = 0.0; // Meters +public: + /** \brief Do not define any jump threshold, i.e., disable joint-space jump detection. */ + static JumpThreshold Disabled(); - explicit JumpThreshold() = default; + /** \brief Detect joint-space jumps relative to the average joint-space displacement along the path. - // For relative jump detection, the average joint-space distance between consecutive points in the path is - // computed. If any individual joint-space motion delta is larger than this average distance by a factor of - // \e jt_factor, it is considered the path has a jump. - explicit JumpThreshold(double jt_factor) : JumpThreshold() - { - factor = jt_factor; - } + The average joint-space distance between consecutive points in the path is computed. If any individual joint-space + motion delta is larger than the average distance by a factor of `relative_factor`, it is considered the path has a + jump. For instance, a `relative_factor` of 10.0 will detect joint increments larger than 10x the average increment + */ + static JumpThreshold Relative(double relative_factor); - explicit JumpThreshold(double jt_revolute, double jt_prismatic) : JumpThreshold() - { - revolute = jt_revolute; // Radians - prismatic = jt_prismatic; // Meters - } + /** \brief Detect joint-space jumps greater than the given absolute thresholds. + + `revolute` and `prismatic` are absolute joint displacement thresholds, in radians and meters respectively. + If any two consecutive waypoints have a joint-space distance larger than these values, the path has a jump. */ + static JumpThreshold Absolute(double revolute, double prismatic); + + double relative_factor = 0.0; + double revolute = 0.0; // Radians + double prismatic = 0.0; // Meters + +private: + // Private constructors. Construct using builder methods in the public interface. + JumpThreshold() = default; + JumpThreshold(double relative_factor); + JumpThreshold(double revolute, double prismatic); }; /** \brief Struct for containing max_step for computeCartesianPath @@ -155,20 +159,8 @@ class CartesianInterpolator During the computation of the path, it is usually preferred if consecutive joint values do not 'jump' by a large amount in joint space, even if the Cartesian distance between the corresponding points is small as expected. - To account for this, the \e jump_threshold struct is provided, which comprises three fields: - \e jump_threshold_factor, \e revolute_jump_threshold and \e prismatic_jump_threshold. - If either \e revolute_jump_threshold or \e prismatic_jump_threshold are non-zero, we test for absolute jumps. - If \e jump_threshold_factor is non-zero, we test for relative jumps. Otherwise (all params are zero), jump - detection is disabled. - - For relative jump detection, the average joint-space distance between consecutive points in the path is - computed. If any individual joint-space motion delta is larger then this average distance by a factor of - \e jump_threshold_factor, this step is considered a failure and the returned path is truncated up to just - before the jump. - - For absolute jump thresholds, if any individual joint-space motion delta is larger then \e revolute_jump_threshold - for revolute joints or \e prismatic_jump_threshold for prismatic joints then this step is considered a failure and - the returned path is truncated up to just before the jump. + To account for this, the \e jump_threshold argument is provided. If a jump detection is enabled and a jump is + found, the path is truncated up to just before the jump. Kinematics solvers may use cost functions to prioritize certain solutions, which may be specified with \e cost_function. */ @@ -242,12 +234,11 @@ class CartesianInterpolator }; /** \brief Checks if a joint-space path has jumps larger than the given threshold. - * - * This function computes the distance between every pair of adjacent waypoints (for the given group) and checks if that - * distance is larger than the threshold defined by `jump_threshold`. If so, it is considered that the path has a - * jump, and the path index where the jump happens is returned as output. - * Otherwise the function returns a nullopt. - */ + + This function computes the distance between every pair of adjacent waypoints (for the given group) and checks if that + distance is larger than the threshold defined by `jump_threshold`. If so, it is considered that the path has a + jump, and the path index where the jump happens is returned as output. + Otherwise the function returns a nullopt. */ std::optional hasJointSpaceJumps(const std::vector& waypoints, const moveit::core::JointModelGroup* group, const moveit::core::JumpThreshold& jump_threshold); diff --git a/moveit_core/robot_state/src/cartesian_interpolator.cpp b/moveit_core/robot_state/src/cartesian_interpolator.cpp index 9404ec2ef93..5a235229129 100644 --- a/moveit_core/robot_state/src/cartesian_interpolator.cpp +++ b/moveit_core/robot_state/src/cartesian_interpolator.cpp @@ -130,6 +130,31 @@ std::optional hasAbsoluteJointSpaceJump(const std::vectorrelative_factor = relative_factor; +} +JumpThreshold::JumpThreshold(double revolute, double prismatic) +{ + this->revolute = revolute; + this->prismatic = prismatic; +} + CartesianInterpolator::Distance CartesianInterpolator::computeCartesianPath( RobotState* start_state, const JointModelGroup* group, std::vector& path, const LinkModel* link, const Eigen::Vector3d& translation, bool global_reference_frame, const MaxEEFStep& max_step, @@ -197,7 +222,7 @@ CartesianInterpolator::Percentage CartesianInterpolator::computeCartesianPath( // If we are testing for relative jumps, we always want at least MIN_STEPS_FOR_JUMP_THRESH steps std::size_t steps = std::max(translation_steps, rotation_steps) + 1; - if (jump_threshold.factor > 0 && steps < MIN_STEPS_FOR_JUMP_THRESH) + if (jump_threshold.relative_factor > 0 && steps < MIN_STEPS_FOR_JUMP_THRESH) steps = MIN_STEPS_FOR_JUMP_THRESH; // To limit absolute joint-space jumps, we pass consistency limits to the IK solver @@ -266,7 +291,7 @@ CartesianInterpolator::Percentage CartesianInterpolator::computeCartesianPath( for (std::size_t i = 0; i < waypoints.size(); ++i) { // Don't test joint space jumps for every waypoint, test them later on the whole path. - static const JumpThreshold NO_JOINT_SPACE_JUMP_TEST; + static const JumpThreshold NO_JOINT_SPACE_JUMP_TEST = JumpThreshold::Disabled(); std::vector waypoint_path; double wp_percentage_solved = computeCartesianPath(start_state, group, waypoint_path, link, waypoints[i], global_reference_frame, max_step, @@ -321,9 +346,9 @@ std::optional hasJointSpaceJumps(const std::vector 0.0) + if (jump_threshold.relative_factor > 0.0) { - return hasRelativeJointSpaceJump(waypoints, group, jump_threshold.factor); + return hasRelativeJointSpaceJump(waypoints, group, jump_threshold.relative_factor); } if (jump_threshold.revolute > 0.0 || jump_threshold.prismatic > 0.0) diff --git a/moveit_core/robot_state/test/test_cartesian_interpolator.cpp b/moveit_core/robot_state/test/test_cartesian_interpolator.cpp index e2a36d7d18b..9c4d8c5f36f 100644 --- a/moveit_core/robot_state/test/test_cartesian_interpolator.cpp +++ b/moveit_core/robot_state/test/test_cartesian_interpolator.cpp @@ -143,25 +143,25 @@ TEST_F(SimpleRobot, checkAbsoluteJointSpaceJump) // Revolute and prismatic joints. generateTestTraj(traj, robot_model_); - fraction = CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj, JumpThreshold(1.0, 1.0)); + fraction = CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj, JumpThreshold::Absolute(1.0, 1.0)); EXPECT_EQ(expected_revolute_jump_traj_len, traj.size()); // traj should be cut before the revolute jump EXPECT_NEAR(expected_revolute_jump_fraction, fraction, 0.01); // Test revolute joints generateTestTraj(traj, robot_model_); - fraction = CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj, JumpThreshold(1.0, 0.0)); + fraction = CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj, JumpThreshold::Absolute(1.0, 0.0)); EXPECT_EQ(expected_revolute_jump_traj_len, traj.size()); // traj should be cut before the revolute jump EXPECT_NEAR(expected_revolute_jump_fraction, fraction, 0.01); // Test prismatic joints generateTestTraj(traj, robot_model_); - fraction = CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj, JumpThreshold(0.0, 1.0)); + fraction = CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj, JumpThreshold::Absolute(0.0, 1.0)); EXPECT_EQ(expected_prismatic_jump_traj_len, traj.size()); // traj should be cut before the prismatic jump EXPECT_NEAR(expected_prismatic_jump_fraction, fraction, 0.01); // Ignore all absolute jumps generateTestTraj(traj, robot_model_); - fraction = CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj, JumpThreshold(0.0, 0.0)); + fraction = CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj, JumpThreshold::Absolute(0.0, 0.0)); EXPECT_EQ(full_traj_len, traj.size()); // traj should not be cut EXPECT_NEAR(1.0, fraction, 0.01); } @@ -184,13 +184,13 @@ TEST_F(SimpleRobot, checkRelativeJointSpaceJump) // Call of relative version: 1.01 > 2.97 * (0.01 * 2 + 1.01 * 2)/6. generateTestTraj(traj, robot_model_); - fraction = CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj, JumpThreshold(2.97)); + fraction = CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj, JumpThreshold::Relative(2.97)); EXPECT_EQ(expected_relative_jump_traj_len, traj.size()); // traj should be cut before the first jump of 1.01 EXPECT_NEAR(expected_relative_jump_fraction, fraction, 0.01); // Trajectory should not be cut: 1.01 < 2.98 * (0.01 * 2 + 1.01 * 2)/6. generateTestTraj(traj, robot_model_); - fraction = CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj, JumpThreshold(2.98)); + fraction = CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj, JumpThreshold::Relative(2.98)); EXPECT_EQ(full_traj_len, traj.size()); // traj should not be cut EXPECT_NEAR(1.0, fraction, 0.01); } @@ -225,7 +225,8 @@ TEST_F(SimpleRobot, checkRelativeJointSpaceJump) // bool global) // { // return CartesianInterpolator::computeCartesianPath(start_state.get(), jmg, result, link, translation, global, -// MaxEEFStep(0.1), JumpThreshold(), GroupStateValidityCallbackFn(), +// MaxEEFStep(0.1), JumpThreshold::Disabled(), +// GroupStateValidityCallbackFn(), // kinematics::KinematicsQueryOptions()); // } // @@ -233,7 +234,8 @@ TEST_F(SimpleRobot, checkRelativeJointSpaceJump) // bool global, const Eigen::Isometry3d& offset = Eigen::Isometry3d::Identity()) // { // return CartesianInterpolator::computeCartesianPath(start_state.get(), jmg, result, link, target, global, -// MaxEEFStep(0.1), JumpThreshold(), GroupStateValidityCallbackFn(), +// MaxEEFStep(0.1), JumpThreshold::Disabled(), +// GroupStateValidityCallbackFn(), // kinematics::KinematicsQueryOptions(), // kinematics::KinematicsBase::IKCostFn(), offset); // } @@ -325,7 +327,8 @@ TEST_F(SimpleRobot, checkRelativeJointSpaceJump) // TEST_F(PandaRobot, testRotationOffset) // { // // define offset to virtual center frame -// Eigen::Isometry3d offset = Eigen::Translation3d(0, 0, 0.2) * Eigen::AngleAxisd(-M_PI / 4, Eigen::Vector3d::UnitZ()); +// Eigen::Isometry3d offset = Eigen::Translation3d(0, 0, 0.2) * Eigen::AngleAxisd(-M_PI / 4, +// Eigen::Vector3d::UnitZ()); // // 45° rotation about center's x-axis // Eigen::Isometry3d rot(Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitX())); // Eigen::Isometry3d goal = start_pose * offset * rot; 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 26a5ff0290a..b07020b5631 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 @@ -171,7 +171,8 @@ 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), moveit::core::JumpThreshold(req->jump_threshold), constraint_fn); + moveit::core::MaxEEFStep(req->max_step), moveit::core::JumpThreshold::Relative(req->jump_threshold), + constraint_fn); moveit::core::robotStateToRobotStateMsg(start_state, res->start_state); robot_trajectory::RobotTrajectory rt(context_->planning_scene_monitor_->getRobotModel(), req->group_name);