Add approximate solution joint jump threshold parameter (#42)

* Add approximate solution joint jump threshold parameter

* Fix param specification

* Make FK function thread-safe

include/pick_ik/fk_moveit.hpp

@@ -5,6 +5,7 @@
 #include <memory>
 #include <moveit/robot_model/joint_model_group.h>
 #include <moveit/robot_model/robot_model.h>
+#include <mutex>
 #include <vector>
 
 namespace pick_ik {
@@ -13,6 +14,7 @@ using FkFn = std::function<std::vector<Eigen::Isometry3d>(std::vector<double> co
 
 auto make_fk_fn(std::shared_ptr<moveit::core::RobotModel const> robot_model,
                 moveit::core::JointModelGroup const* jmg,
+                std::mutex& mx,
                 std::vector<size_t> tip_link_indices) -> FkFn;
 
 }  // namespace pick_ik

src/fk_moveit.cpp

@@ -10,14 +10,15 @@ namespace pick_ik {
 
 auto make_fk_fn(std::shared_ptr<moveit::core::RobotModel const> robot_model,
                 moveit::core::JointModelGroup const* jmg,
+                std::mutex& mx,
                 std::vector<size_t> tip_link_indices) -> FkFn {
     auto robot_state = moveit::core::RobotState(robot_model);
     robot_state.setToDefaultValues();
 
     // IK function is mutable so it re-uses the robot_state instead of creating
-    // new copies. This function should not be shared between threads.
-    // It is however safe to make copies of.
-    return [=](std::vector<double> const& active_positions) mutable {
+    // new copies. This function accepts a mutex so that it can be made thread-safe.
+    return [=, &mx](std::vector<double> const& active_positions) mutable {
+        std::scoped_lock lock(mx);
         robot_state.setJointGroupPositions(jmg, active_positions);
         robot_state.updateLinkTransforms();
 

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: {
+      gt_eq<>: [0.0],
+    },
+  }
   cost_threshold: {
     type: double,
     default_value: 0.001,

src/pick_ik_plugin.cpp

@@ -29,6 +29,8 @@ class PickIKPlugin : public kinematics::KinematicsBase {
     std::vector<size_t> tip_link_indices_;
     Robot robot_;
 
+    mutable std::mutex fk_mutex_;
+
    public:
     virtual bool initialize(rclcpp::Node::SharedPtr const& node,
                             moveit::core::RobotModel const& robot_model,
@@ -127,7 +129,7 @@ class PickIKPlugin : public kinematics::KinematicsBase {
             make_pose_cost_functions(goal_frames, params.rotation_scale);
 
         // forward kinematics function
-        auto const fk_fn = make_fk_fn(robot_model_, jmg_, tip_link_indices_);
+        auto const fk_fn = make_fk_fn(robot_model_, jmg_, fk_mutex_, tip_link_indices_);
 
         // Create goals (weighted cost functions)
         auto goals = std::vector<Goal>{};
@@ -209,10 +211,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 +233,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 +257,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();
 

tests/ik_memetic_tests.cpp

@@ -41,7 +41,8 @@ auto solve_memetic_ik_test(moveit::core::RobotModelPtr robot_model,
     // Make forward kinematics function
     auto const jmg = robot_model->getJointModelGroup(group_name);
     auto const tip_link_indices = pick_ik::get_link_indices(robot_model, {goal_frame_name}).value();
-    auto const fk_fn = pick_ik::make_fk_fn(robot_model, jmg, tip_link_indices);
+    std::mutex mx;
+    auto const fk_fn = pick_ik::make_fk_fn(robot_model, jmg, mx, tip_link_indices);
     auto const robot = pick_ik::Robot::from(robot_model, jmg, tip_link_indices);
 
     // Make goal function(s)
@@ -93,7 +94,8 @@ TEST_CASE("Panda model Memetic IK") {
 
     auto const jmg = robot_model->getJointModelGroup("panda_arm");
     auto const tip_link_indices = pick_ik::get_link_indices(robot_model, {"panda_hand"}).value();
-    auto const fk_fn = pick_ik::make_fk_fn(robot_model, jmg, tip_link_indices);
+    std::mutex mx;
+    auto const fk_fn = pick_ik::make_fk_fn(robot_model, jmg, mx, tip_link_indices);
 
     std::vector<double> const home_joint_angles =
         {0.0, -M_PI_4, 0.0, -3.0 * M_PI_4, 0.0, M_PI_2, M_PI_4};

tests/ik_tests.cpp

@@ -53,7 +53,8 @@ TEST_CASE("RR model FK") {
     auto const jmg = robot_model->getJointModelGroup("group");
     auto const tip_link_indices = pick_ik::get_link_indices(robot_model, {"ee"}).value();
 
-    auto const fk_fn = pick_ik::make_fk_fn(robot_model, jmg, tip_link_indices);
+    std::mutex mx;
+    auto const fk_fn = pick_ik::make_fk_fn(robot_model, jmg, mx, tip_link_indices);
 
     SECTION("Zero joint position") {
         std::vector<double> const joint_vals = {0.0, 0.0};
@@ -93,7 +94,8 @@ auto solve_ik_test(moveit::core::RobotModelPtr robot_model,
     // Make forward kinematics function
     auto const jmg = robot_model->getJointModelGroup(group_name);
     auto const tip_link_indices = pick_ik::get_link_indices(robot_model, {goal_frame_name}).value();
-    auto const fk_fn = pick_ik::make_fk_fn(robot_model, jmg, tip_link_indices);
+    std::mutex mx;
+    auto const fk_fn = pick_ik::make_fk_fn(robot_model, jmg, mx, tip_link_indices);
 
     // Make solution function
     auto const test_rotation = (params.rotation_scale > 0.0);
@@ -234,7 +236,8 @@ TEST_CASE("Panda model IK") {
 
     auto const jmg = robot_model->getJointModelGroup("panda_arm");
     auto const tip_link_indices = pick_ik::get_link_indices(robot_model, {"panda_hand"}).value();
-    auto const fk_fn = pick_ik::make_fk_fn(robot_model, jmg, tip_link_indices);
+    std::mutex mx;
+    auto const fk_fn = pick_ik::make_fk_fn(robot_model, jmg, mx, tip_link_indices);
 
     std::vector<double> const home_joint_angles =
         {0.0, -M_PI_4, 0.0, -3.0 * M_PI_4, 0.0, M_PI_2, M_PI_4};