Regenerated ikfast plugin

ros-industrial · May 22, 2015 · 3d4e907 · 3d4e907
1 parent 11974f6
commit 3d4e907
Show file tree

Hide file tree

Showing 2 changed files with 486 additions and 444 deletions.
diff --git a/...00_moveit_plugins/irb2400_kinematics/src/abb_irb2400_manipulator_ikfast_moveit_plugin.cpp b/...00_moveit_plugins/irb2400_kinematics/src/abb_irb2400_manipulator_ikfast_moveit_plugin.cpp
@@ -52,6 +52,8 @@
 
 // Need a floating point tolerance when checking joint limits, in case the joint starts at limit
 const double LIMIT_TOLERANCE = .0000001;
+/// \brief Search modes for searchPositionIK(), see there
+enum SEARCH_MODE { OPTIMIZE_FREE_JOINT=1, OPTIMIZE_MAX_JOINT=2 };
 
 namespace ikfast_kinematics_plugin
 {
@@ -387,7 +389,8 @@ int IKFastKinematicsPlugin::solve(KDL::Frame &pose_frame, const std::vector<doub
   switch (GetIkType())
   {
     case IKP_Transform6D:
-      // For **Transform6D**, eerot is 9 values for the 3x3 rotation matrix.
+    case IKP_Translation3D:
+      // For **Transform6D**, eerot is 9 values for the 3x3 rotation matrix. For **Translation3D**, these are ignored.
 
       mult = pose_frame.M;
 
@@ -428,7 +431,6 @@ int IKFastKinematicsPlugin::solve(KDL::Frame &pose_frame, const std::vector<doub
       return 0;
 
     case IKP_Rotation3D:
-    case IKP_Translation3D:
     case IKP_Lookat3D:
     case IKP_TranslationXY2D:
     case IKP_TranslationXYOrientation3D:
@@ -578,10 +580,10 @@ bool IKFastKinematicsPlugin::getPositionFK(const std::vector<std::string> &link_
                                            const std::vector<double> &joint_angles,
                                            std::vector<geometry_msgs::Pose> &poses) const
 {
-#ifndef IKTYPE_TRANSFORM_6D
-  ROS_ERROR_NAMED("ikfast", "Can only compute FK for IKTYPE_TRANSFORM_6D!");
-  return false;
-#endif
+  if (GetIkType() != IKP_Transform6D) {
+    ROS_ERROR_NAMED("ikfast", "Can only compute FK for IKTYPE_TRANSFORM_6D!");
+    return false;
+  }
 
   KDL::Frame p_out;
   if(link_names.size() == 0) {
@@ -685,6 +687,9 @@ bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose
 {
   ROS_DEBUG_STREAM_NAMED("ikfast","searchPositionIK");
 
+  /// search_mode is currently fixed during code generation
+  SEARCH_MODE search_mode = OPTIMIZE_MAX_JOINT;
+
   // Check if there are no redundant joints
   if(free_params_.size()==0)
   {
@@ -782,6 +787,12 @@ bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose
   // Begin searching
 
   ROS_DEBUG_STREAM_NAMED("ikfast","Free param is " << free_params_[0] << " initial guess is " << initial_guess << ", # positive increments: " << num_positive_increments << ", # negative increments: " << num_negative_increments);
+  if ((search_mode & OPTIMIZE_MAX_JOINT) && (num_positive_increments + num_negative_increments) > 1000)
+      ROS_WARN_STREAM_ONCE_NAMED("ikfast", "Large search space, consider increasing the search discretization");
+
+  double best_costs = -1.0;
+  std::vector<double> best_solution;
+  int nattempts = 0, nvalid = 0;
 
   while(true)
   {
@@ -796,6 +807,7 @@ bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose
     {
       for(int s = 0; s < numsol; ++s)
       {
+        nattempts++;
         std::vector<double> sol;
         getSolution(solutions,s,sol);
 
@@ -825,23 +837,52 @@ bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose
 
           if(error_code.val == error_code.SUCCESS)
           {
-            return true;
+            nvalid++;
+            if (search_mode & OPTIMIZE_MAX_JOINT)
+            {
+              // Costs for solution: Largest joint motion
+              double costs = 0.0;
+              for(unsigned int i = 0; i < solution.size(); i++)
+              {
+                double d = fabs(ik_seed_state[i] - solution[i]);
+                if (d > costs)
+                  costs = d;
+              }
+              if (costs < best_costs || best_costs == -1.0)
+              {
+                best_costs = costs;
+                best_solution = solution;
+              }
+            }
+            else
+              // Return first feasible solution
+              return true;
           }
         }
       }
     }
 
-    if(!getCount(counter, num_positive_increments, num_negative_increments))
+    if(!getCount(counter, num_positive_increments, -num_negative_increments))
     {
+      // Everything searched
       error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
-      return false;
+      break;
     }
 
     vfree[0] = initial_guess+search_discretization_*counter;
-    ROS_DEBUG_STREAM_NAMED("ikfast","Attempt " << counter << " with 0th free joint having value " << vfree[0]);
+    //ROS_DEBUG_STREAM_NAMED("ikfast","Attempt " << counter << " with 0th free joint having value " << vfree[0]);
+  }
+
+  ROS_DEBUG_STREAM_NAMED("ikfast", "Valid solutions: " << nvalid << "/" << nattempts);
+
+  if ((search_mode & OPTIMIZE_MAX_JOINT) && best_costs != -1.0)
+  {
+    solution = best_solution;
+    error_code.val = error_code.SUCCESS;
+    return true;
   }
 
-  // not really needed b/c shouldn't ever get here
+  // No solution found
   error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
   return false;
 }