Add approximate solution joint jump threshold parameter

src/goal.cpp

@@ -21,8 +21,8 @@ auto make_frame_test_fn(Eigen::Isometry3d goal_frame,
         auto const q_goal = Eigen::Quaterniond(goal_frame.rotation());
         auto const q_frame = Eigen::Quaterniond(tip_frame.rotation());
         auto const angular_distance = q_frame.angularDistance(q_goal);
-        return ((goal_frame.translation() - tip_frame.translation()).norm() <=
-                position_threshold) &&
+        auto const linear_distance = (goal_frame.translation() - tip_frame.translation()).norm();
+        return (linear_distance <= position_threshold) &&
                (!orientation_threshold.has_value() ||
                 std::abs(angular_distance) <= orientation_threshold.value());
     };

src/pick_ik_parameters.yaml

@@ -65,6 +65,14 @@ pick_ik:
       gt_eq<>: [0.0],
     },
   }
+  approximate_solution_joint_threshold: {
+    type: double,
+    default_value: 0.0,
+    description: "Joint threshold for approximate IK solutions, in radians. If displacement is larger than this, the approximate solution will fall back to the initial guess",
+    validation: {
+      lower_bounds<>: [0.0],
+    },
+  }
   cost_threshold: {
     type: double,
     default_value: 0.001,

src/pick_ik_plugin.cpp

@@ -209,10 +209,11 @@ class PickIKPlugin : public kinematics::KinematicsBase {
             solution = ik_seed_state;
         }
 
-        // If using an approximate solution, check against the maximum allowable pose threshold.
-        // If the approximate solution is too far from the goal frame, fall back to the initial
-        // state.
+        // If using an approximate solution, check against the maximum allowable pose and joint
+        // thresholds. If the approximate solution is too far from the goal frame,
+        // fall back to the initial state.
         if (options.return_approximate_solution) {
+            // Check pose thresholds
             std::optional<double> approximate_solution_orientation_threshold = std::nullopt;
             if (test_rotation) {
                 approximate_solution_orientation_threshold =
@@ -230,6 +231,18 @@ class PickIKPlugin : public kinematics::KinematicsBase {
                     break;
                 }
             }
+
+            // Check joint thresholds
+            if (approx_solution_valid && params.approximate_solution_joint_threshold > 0.0) {
+                for (size_t i = 0; i < solution.size(); ++i) {
+                    if (std::abs(solution[i] - ik_seed_state[i]) >
+                        params.approximate_solution_joint_threshold) {
+                        approx_solution_valid = false;
+                        break;
+                    }
+                }
+            }
+
             if (!approx_solution_valid) {
                 error_code.val = error_code.NO_IK_SOLUTION;
                 solution = ik_seed_state;
@@ -242,7 +255,7 @@ class PickIKPlugin : public kinematics::KinematicsBase {
             solution_callback(ik_poses.front(), solution, error_code);
         }
 
-        return error_code.val == error_code.SUCCESS;
+        return found_solution;
     }
 
     virtual std::vector<std::string> const& getJointNames() const { return joint_names_; }

src/robot.cpp

@@ -19,7 +19,6 @@ auto Robot::from(std::shared_ptr<moveit::core::RobotModel const> const& model,
                  std::vector<size_t> tip_link_indices) -> Robot {
     auto robot = Robot{};
 
-    // jmg_->getKinematicsSolverJointBijection();
     auto const active_variable_indices = get_active_variable_indices(model, jmg, tip_link_indices);
     auto const variable_count = active_variable_indices.size();