Cleaned up Moveit plugin comments, added better processing of callback function.

ur_kinematics/include/ur_kinematics/ur_moveit_plugin.h

@@ -14,62 +14,25 @@ public:
 * @brief Plugin-able interface to the ur kinematics
 */
   URKinematicsPlugin();
-// 
-//   //~URKinematicsPlugin();
 
-// 
-//   virtual bool getPositionFK(const std::vector<std::string> &link_names,
-//                              const std::vector<double> &joint_angles,
-//                              std::vector<geometry_msgs::Pose> &poses) const;
-// 
-/**
-* @brief Initialization function for the kinematics
-* @return True if initialization was successful, false otherwise
-*/
-virtual bool initialize(const std::string& robot_description,
-                        const std::string& group_name,
-                        const std::string& base_frame,
-                        const std::string& tip_frame,
-                        double search_discretization);
-// 
-//   /**
-// * @brief Return all the joint names in the order they are used internally
-// */
-//   const std::vector<std::string>& getJointNames() const;
-// 
-//   /**
-// * @brief Return all the link names in the order they are represented internally
-// */
-//   const std::vector<std::string>& getLinkNames() const;
-// 
+  virtual bool initialize(const std::string& robot_description,
+                          const std::string& group_name,
+                          const std::string& base_frame,
+                          const std::string& tip_frame,
+                          double search_discretization);
+
 protected:
 
-  /**
-* @brief Given a desired pose of the end-effector, search for the joint angles required to reach it.
-* This particular method is intended for "searching" for a solutions by stepping through the redundancy
-* (or other numerical routines).
-* @param ik_pose the desired pose of the link
-* @param ik_seed_state an initial guess solution for the inverse kinematics
-* @param timeout The amount of time (in seconds) available to the solver
-* @param solution the solution vector
-* @param solution_callback A callback solution for the IK solution
-* @param error_code an error code that encodes the reason for failure or success
-* @param check_consistency Set to true if consistency check needs to be performed
-* @param redundancy The index of the redundant joint
-* @param consistency_limit The returned solutuion will contain a value for the redundant joint in the range [seed_state(redundancy_limit)-consistency_limit,seed_state(redundancy_limit)+consistency_limit]
-* @return True if a valid solution was found, false otherwise
-*/
-    bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
-                          const std::vector<double> &ik_seed_state,
-                          double timeout,
-                          std::vector<double> &solution,
-                          const IKCallbackFn &solution_callback,
-                          moveit_msgs::MoveItErrorCodes &error_code,
-                          const std::vector<double> &consistency_limits,
-                          const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
+  bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
+                        const std::vector<double> &ik_seed_state,
+                        double timeout,
+                        std::vector<double> &solution,
+                        const IKCallbackFn &solution_callback,
+                        moveit_msgs::MoveItErrorCodes &error_code,
+                        const std::vector<double> &consistency_limits,
+                        const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
 
   std::vector<double> ik_weights_;
-// 
-//   moveit_msgs::KinematicSolverInfo fk_solver_info_, ik_solver_info_;
+
 };
 }

ur_kinematics/src/ur_moveit_plugin.cpp

@@ -5,76 +5,86 @@
 #include <pluginlib/class_list_macros.h>
 #include <ur_kinematics/ur_kin.h>
 #include <tf_conversions/tf_kdl.h>
-//#include <moveit/kdl_kinematics_plugin.cpp>
+#include <limits>
 
 //register URKinematicsPlugin as a KinematicsBase implementation
 PLUGINLIB_EXPORT_CLASS(ur_kinematics::URKinematicsPlugin,kinematics::KinematicsBase);
 
 namespace ur_kinematics
 {
-  URKinematicsPlugin::URKinematicsPlugin() 
-    : kdl_kinematics_plugin::KDLKinematicsPlugin(), ik_weights_(6) {}
-//   //URKinematicsPlugin::~URKinematicsPlugin() {}
+URKinematicsPlugin::URKinematicsPlugin() 
+  : kdl_kinematics_plugin::KDLKinematicsPlugin(), ik_weights_(6) {}
 
 bool URKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
-                                           const std::vector<double> &ik_seed_state,
-                                           double timeout,
-                                           std::vector<double> &solution,
-                                           const IKCallbackFn &solution_callback,
-                                           moveit_msgs::MoveItErrorCodes &error_code,
-                                           const std::vector<double> &consistency_limits,
-                                           const kinematics::KinematicsQueryOptions &options) const
+                                          const std::vector<double> &ik_seed_state,
+                                          double timeout,
+                                          std::vector<double> &solution,
+                                          const IKCallbackFn &solution_callback,
+                                          moveit_msgs::MoveItErrorCodes &error_code,
+                                          const std::vector<double> &consistency_limits,
+                                          const kinematics::KinematicsQueryOptions &options) const
 {
-  //double homo_ik_pose[4][4];
-
   KDL::Frame kdl_ik_pose;
   tf::poseMsgToKDL(ik_pose, kdl_ik_pose);
   double homo_ik_pose[4][4];
   kdl_ik_pose.Make4x4((double*) homo_ik_pose);
   for(int i=0; i<3; i++) homo_ik_pose[i][3] *= 1000;
-  double q_ik_sols[8][6];
+  double q_ik_sols[8][6]; // maximum of 8 IK solutions
   uint16_t num_sols = inverse((double*) homo_ik_pose, (double*) q_ik_sols, ik_seed_state[5]);
   if(num_sols <= 0) {
     // NO SOLUTION
     error_code.val = error_code.NO_IK_SOLUTION;
     return false;
   }
-  double min_weighted_diff = 1e9;
-  int min_diff_index = -1;
-  for(uint16_t i=0; i<num_sols; i++) {
-    double cur_weighted_diff = 0;
-    for(uint16_t j=0; j<6; j++) {
-      cur_weighted_diff += ik_weights_[j] * std::fabs(q_ik_sols[i][j] - ik_seed_state[j]);
+
+  std::vector<int> tried_solutions;
+  while(true) {
+    // use weighted absolute deviations to determine the solution closest the seed state
+    double min_weighted_diff = std::numeric_limits<double>::infinity();
+    int min_diff_index = -1;
+    for(uint16_t i=0; i<num_sols; i++) {
+      if(std::find(tried_solutions.begin(), tried_solutions.end(), i) != tried_solutions.end())
+        continue;
+
+      double cur_weighted_diff = 0;
+      for(uint16_t j=0; j<6; j++) {
+        // solution violates the consistency_limits, throw it out
+        if(std::fabs(ik_seed_state[j] - q_ik_sols[i][j]) > consistency_limits[j]) {
+          cur_weighted_diff = std::numeric_limits<double>::infinity();
+          break;
+        }
+
+        cur_weighted_diff += ik_weights_[j] * std::fabs(q_ik_sols[i][j] - ik_seed_state[j]);
+      }
+      if(cur_weighted_diff != std::numeric_limits<double>::infinity() &&
+         cur_weighted_diff < min_weighted_diff) {
+        min_weighted_diff = cur_weighted_diff;
+        min_diff_index = i;
+      }
     }
-    if(cur_weighted_diff < min_weighted_diff) {
-      min_weighted_diff = cur_weighted_diff;
-      min_diff_index = i;
+
+    if(min_diff_index < 0) {
+      // NO SOLUTION, failed consistency_limits/solution_callback
+      error_code.val = error_code.NO_IK_SOLUTION;
+      return false;
     }
-  }
-  solution.resize(6);
-  std::copy(q_ik_sols[min_diff_index], q_ik_sols[min_diff_index+1], solution.begin());
 
-  if(!solution_callback.empty())
-    solution_callback(ik_pose,solution,error_code);
-  else
-    error_code.val = error_code.SUCCESS;
-  return true;
+    // copy the best solution to the output
+    solution.resize(6);
+    std::copy(q_ik_sols[min_diff_index], q_ik_sols[min_diff_index+1], solution.begin());
+
+    if(!solution_callback.empty())
+      solution_callback(ik_pose, solution, error_code);
+    else
+      error_code.val = error_code.SUCCESS;
+
+    if(error_code.val == error_code.SUCCESS) 
+      return true;
+
+    tried_solutions.push_back(min_diff_index);
+  }
 }
-// 
-//   bool URKinematicsPlugin::getPositionFK(const std::vector<std::string> &link_names,
-//                                          const std::vector<double> &joint_angles,
-//                                          std::vector<geometry_msgs::Pose> &poses) const
-//   {
-//     // double q[6];
-//     // for(int i=0; i<6; i++)
-//     //   q[i] = joint_angles[i];
-//     // double T[6][16];
-//     // forward_all(q, T[0], T[1], T[2], T[3], T[4], T[5]);
-//     // for(int i=0; i<6; i++) {
-//     //   aa
-//     // }
-//   }
-// 
+
   /**
 * @brief Initialization function for the kinematics
 * @return True if initialization was successful, false otherwise
@@ -100,20 +110,5 @@ bool URKinematicsPlugin::initialize(const std::string& robot_description,
   return kdl_kinematics_plugin::KDLKinematicsPlugin::initialize(
            robot_description, group_name, base_frame, tip_frame, search_discretization);
 }
-// 
-//   /**
-// * @brief Return all the joint names in the order they are used internally
-// */
-//   const std::vector<std::string>& URKinematicsPlugin::getJointNames() const
-//   {
-//     return fk_solver_info_.joint_names;
-//   }
-// 
-//   /**
-// * @brief Return all the link names in the order they are represented internally
-// */
-//   const std::vector<std::string>& URKinematicsPlugin::getLinkNames() const
-//   {
-//     return fk_solver_info_.link_names;
-//   }
+
 }