Modify ikfast_template for getPositionIK single solution results #537

nsnitish · 2017-06-22T19:47:08Z

Description

IKfast moveit plugin is expected to return the solution (when called with getPositionIK(ik_pose, ik_seed_state, solution, error_code,options)) which is closest solution to the ik_seed_state. Originally, the code was returning the first solution it could find. Modified the template to return the closest solution satisfying the joint limits after harmonizing it(returning the solution between -pi to pi).

@shaun-edwards! Can you please have a look?

davetcoleman · 2017-06-23T16:29:21Z

Please fix the formatting issues so that it passes the test. Note we have an auto-formatter available: http://moveit.ros.org/documentation/contributing/code/

davetcoleman

This seems like a good contribution, thanks so much! I'd like someone else to review it also - perhaps @gavanderhoorn can weight in?

davetcoleman · 2017-06-24T22:32:55Z

moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp

@@ -180,7 +180,7 @@ class IKFastKinematicsPlugin : public kinematics::KinematicsBase
   * @return True if a valid solution was found, false otherwise
   */

-  // Returns the first IK solution that is within joint limits, this is called by get_ik() service
+  // Returns the IK solution that is within joint limits closest to ik_seed_state, this is called by get_ik() service


I know this is legacy code that you aren't involved in, but could you remove the comment ", this is called by get_ik() service" - that is referring to some really old code that no longer exists

davetcoleman · 2017-06-24T22:35:34Z

moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp

  }
-  return dist_sqr;
+  return dist_abs;


could you elaborate in your PR description what these changes are for? was this harmonize() function not being used previously? was it this broken?

I will add the comment explaining this. Actually, this function harmonize wasn't written correctly. I think the the solution output from solve(frame, vfree, solutions) as well as seed input can be something out of range (not between -pi to pi). So, this function transforms the solution in the range(-pi to pi) and finds its L1 norm from the transformed seed value.
Originally, this function was not being used. But I found it useful in my implementation. So, I finally modified this because it was running on wrong definition.

@nsnitish: could you take a look at moveit/moveit_ikfast#53?

This is exactly what I am talking about. It had wrong definition. Now it makes more sense and I have also added a new PR which tests for the getPositionIK call for closest solution to seed. So, Indirectly this test is testing the harmonize function as well. But adding an explicit test for this can be helpful.

davetcoleman · 2017-06-24T22:37:28Z

moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp

+  // Among the solutions under limits, find the one that is closest to ik_seed_state
+  if (solutions_obey_limits.size() != 0)
+  {
+    double mindist = DBL_MAX;


in moveit we prefer std::numeric_limits<double>::max()

davetcoleman · 2017-06-24T22:37:44Z

moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp

+    int minindex = -1;
+    std::vector<double> sol;
+
+    for (size_t i = 0; i < solutions_obey_limits.size(); ++i)


std::size_t please

davetcoleman · 2017-06-24T22:38:14Z

moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp

+    {
+      sol = solutions_obey_limits[i];
+      double dist = harmonize(ik_seed_state, sol);
+      ROS_INFO_STREAM_NAMED("ikfast", "Dist " << i << " dist " << dist);


this should probably be ROS_DEBUG right?

davetcoleman · 2017-06-24T22:38:48Z

moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp

+  if (solutions_obey_limits.size() != 0)
+  {
+    double mindist = DBL_MAX;
+    int minindex = -1;


can you make these variables use underscore e.g. min_index?

jrgnicho · 2017-06-25T17:18:36Z

moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp

-        getSolution(solutions, s, solution);
-        error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
-        return true;
+        // All elements of thi solution obey limits


typo "// All elements of thi solution obey limits`"

jrgnicho · 2017-06-25T17:40:24Z

moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp

+    error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
+    return true;
+  }
+


I'd approach this by sorting the solutions in ascending order using sort and returning the first element from the sorted solutions vector. This could be useful in the getPositionIK that returns multiple solutions. This is only a suggestion, I'm OK with your method too.

Hi @jrgnicho ! What are the expectations from the getPositionIK call for multiple solutions? Am I being over-concerned about scanning the list of solutions in O(n) and not sorting them in n*logn?
I mean does this complexity make any difference? Because this was the only reason I didn't try sorting the elements. If it doesn't matter, I think your approach will be really beneficial and I will do that.

Since we're only going to be sorting a handful of solution I don't think the performance hit is of much concern.

Added the sort for solutions for getPositionIK with single solution but haven't modified the getPositionIK that returns multiple solutions. Lets leave this for some other PR

nsnitish · 2017-06-30T14:56:17Z

@davetcoleman @gavanderhoorn @shaun-edwards Can you please have a look and review?

jrgnicho · 2017-06-30T18:03:51Z

moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp

+          dists[j] = dists[j + 1];
+          dists[j + 1] = temp_dist;
+          swap_flag = true;
+        }
      }


I think your sorting code would look cleaner if you were to use the sort algorithm with a predicate function that compares the distances of each argument to the seed. This will do though.

nsnitish · 2017-06-30T22:04:15Z

@davetcoleman! According to this PR, the getPositionIK gives the solution between (-pi to pi). A robust implementation will have solutions between the joint bounds. I am thinking to further work on it. Should I file a different PR for that when I am done or is it worth adding it to this PR Itself?

jrgnicho · 2017-07-03T21:02:37Z

moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp

    {
      ss[i] -= 2 * M_PI;
    }
-    while (ss[i] < -1 * M_PI)
+    while (ss[i] < 2 * M_PI)


Should this be -2?

jrgnicho · 2017-07-03T21:03:53Z

moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp

    {
      ss[i] += 2 * M_PI;
    }
-    while (solution[i] > M_PI)
+    while (solution[i] > 2 * M_PI)


Is this while loop a repeat of the while loop at the top?

Initially when I filed this PR, I had modified the harmonize function to be used in getPositionIK but I modified getPositionIK to remove its dependency on harmonize function. In current commit, It is the original harmonize function which is not in good condition as referred in pull#53.
So, harmonize isn't part of this PR anymore. It will be good if you can review the current commit.

v4hn · 2017-07-04T11:57:29Z

On Mon, Jul 03, 2017 at 02:24:19PM -0700, nsnitish wrote: Initially when I filed this PR, I had modified the harmonize function to be used in getPositionIK but I modified getPositionIK to remove its dependency on harmonize function.

why did you do that? If the harmonize function does not work correctly (and I guess that's the case...) this should be fixed too, right?

nsnitish · 2017-07-04T17:08:05Z

@v4hn! This PR is specifically meant for getPositionIK. Harmonize function has been already discussed in detail in pull#53. We can further discuss this in some other PR.

jrgnicho · 2017-07-04T17:43:19Z

@nsnitish I'm OK with merging these changes, lets see what the other reviewers say, thanks for the contribution.

shaun-edwards · 2017-07-09T18:38:03Z

@davetcoleman, @v4hn, can this be merged?

davetcoleman

I'm really glad this is getting fixed... its been a documented bug since at least 2013!
https://sourceforge.net/p/usarsimros/wiki/IKFast/

davetcoleman · 2017-07-17T06:13:42Z

moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp

+    return false;
+  }
+
+  // Check if seed is in bound


great error checking!

davetcoleman · 2017-07-17T06:14:55Z

moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp

@@ -1019,11 +1040,15 @@ bool IKFastKinematicsPlugin::getPositionIK(const geometry_msgs::Pose& ik_pose, c

  IkSolutionList<IkReal> solutions;
  int numsol = solve(frame, vfree, solutions);
+  std::vector<std::vector<double>> solutions_obey_limits;
+  std::vector<double> dists;


could you use a more descriptive var name here like dists_from_seed?

davetcoleman · 2017-07-17T06:16:21Z

moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp

@@ -1019,11 +1040,15 @@ bool IKFastKinematicsPlugin::getPositionIK(const geometry_msgs::Pose& ik_pose, c

  IkSolutionList<IkReal> solutions;
  int numsol = solve(frame, vfree, solutions);
+  std::vector<std::vector<double>> solutions_obey_limits;
+  std::vector<double> dists;
+  bool solution_found = false;


rather than this extra bool, why not just check !solutions_obey_limits.empty()?

davetcoleman · 2017-07-17T06:17:16Z

moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp

+  }
+
+  // Check if seed is in bound
+  for (unsigned int i = 0; i < ik_seed_state.size(); i++)


please use std::size_t per moveit style guidelines

davetcoleman · 2017-07-17T06:24:25Z

moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp

@@ -1060,13 +1093,41 @@ bool IKFastKinematicsPlugin::getPositionIK(const geometry_msgs::Pose& ik_pose, c
    ROS_DEBUG_STREAM_NAMED("ikfast", "No IK solution");
  }

+  // Sort the solutions under limits and find the one that is closest to ik_seed_state


i believe this is bubble sort O(n^2)... the worse case performance? how about wrap the dists and solutions_obey_limits into a common struct, then use std::sort for a clean and optimal O(nlogn)?

I believe this isn't even correct bubble sort: The swap_flag is never read. You could add a while (swap_flag) around the whole thing, making it O(n^3). Alternatively, Wikipedia has an O(n^2) implementation of bubble sort. But I think I like the std::sort suggestion best. g

On second thought, the implementation might be correct, and swap_flag was only intended as an optimization... anyway, why waste brain cycles, let's just use std::sort. :)

davetcoleman · 2017-07-17T06:26:07Z

Is the PR for fixing the harmonize function already available, or just in the old moveit_ikfast repo?

mintar · 2017-07-19T15:15:29Z

moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp

@@ -1060,13 +1093,41 @@ bool IKFastKinematicsPlugin::getPositionIK(const geometry_msgs::Pose& ik_pose, c
    ROS_DEBUG_STREAM_NAMED("ikfast", "No IK solution");
  }

+  // Sort the solutions under limits and find the one that is closest to ik_seed_state


I believe this isn't even correct bubble sort: The swap_flag is never read. You could add a while (swap_flag) around the whole thing, making it O(n^3). Alternatively, Wikipedia has an O(n^2) implementation of bubble sort. But I think I like the std::sort suggestion best. g

nsnitish · 2017-07-22T08:08:56Z

Is the PR for fixing the harmonize function already available, or just in the old moveit_ikfast repo?

@davetcoleman! There isn't any that I know. But PR#53 was closed without resolving the issue.

nsnitish · 2017-07-22T10:02:05Z

@davetcoleman ! Please have a look.

jrgnicho · 2017-07-26T16:02:38Z

moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp

  ROS_DEBUG_STREAM_NAMED("ikfast", "Found " << numsol << " solutions from IKFast");

+  // struct for storing and sorting solutions
+  struct limit_obeying_sol


This struct name may not be following the naming convention

+1 to LimitObeyingSol as name

davetcoleman

Looks great except the one struct rename, thanks!!

davetcoleman · 2017-07-28T02:26:20Z

moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp

  ROS_DEBUG_STREAM_NAMED("ikfast", "Found " << numsol << " solutions from IKFast");

+  // struct for storing and sorting solutions
+  struct limit_obeying_sol


+1 to LimitObeyingSol as name

nsnitish · 2017-07-28T02:58:11Z

@davetcoleman !

Looks great except the one struct rename, thanks!!

Already did it in latest commit

davetcoleman · 2017-07-28T15:00:09Z

Hurray!

Jmeyer1292 · 2017-08-04T16:34:52Z

@nsnitish Thanks for taking up the torch on this issue. I realize it's too late but I'd thought I would throw in some thoughts:

We should REALLY try to avoid dynamic mem allocations in these low level IK calls. It's true the library is fast, but this can be a real burden when you do a lot of them.
This only appears to apply for 6 DOF robots (e.g. one w/o free parameters). I realize this may be out of your scope, but still.

…eit#537) * Modifeid ikfast_template for getPositionIK single solution case * Corrected the harmonize definition * Formated according to style guide and few typos * Sort the solutions before returning closest * Removed the dependency of getPositionIK from harmonize * Removed the dependency of getPositionIK from harmonize * Used std::sort for solution sorting * Changed struct name for storing and sorting the solutions

v4hn · 2017-08-09T11:55:52Z

We should REALLY try to avoid dynamic mem allocations in these low level IK calls. It's true the library is fast, but this can be a real burden when you do a lot of them.

I agree. Do you have concrete proposals to change current upstream?

This only appears to apply for 6 DOF robots (e.g. one w/o free parameters). I realize this may be out of your scope, but still.

I have not worked with >6DOF arms myself. Could you elaborate on this statement please?

davetcoleman · 2017-08-15T14:29:18Z

This only appears to apply for 6 DOF robots (e.g. one w/o free parameters). I realize this may be out of your scope, but still.

IKFast is limited to 6dof or less, but the MoveIt! plugin for IKFast will iteratively search the 7th joint for a solution if you have 7dof robot.

Jmeyer1292 · 2017-08-15T15:11:48Z

IKFast is limited to 6dof or less, but the MoveIt! plugin for IKFast will iteratively search the 7th joint for a solution if you have 7dof robot.

I understand that. I was attempting to make a point about how the function changed is only called in the event we don't have the 7th axis. Though you are correct that the default behaviour, OPTIMIZE_MAX_JOINT, will produce equivalent results because of the search. I overlooked that when I made my comment earlier. You'll still get flips if you use the other behaviour, however, not that its likely.

davetcoleman · 2017-10-11T18:29:30Z

I've created an issue for memory allocation in IK solvers: #629

…eit#537) * Modifeid ikfast_template for getPositionIK single solution case * Corrected the harmonize definition * Formated according to style guide and few typos * Sort the solutions before returning closest * Removed the dependency of getPositionIK from harmonize * Removed the dependency of getPositionIK from harmonize * Used std::sort for solution sorting * Changed struct name for storing and sorting the solutions

nsnitish added 2 commits June 22, 2017 14:38

Modifeid ikfast_template for getPositionIK single solution case

98e52fe

Corrected the harmonize definition

d08803e

Formated according to style guide and few typos

0fc8697

nsnitish mentioned this pull request Jun 23, 2017

Added new test to check if getPositionIK solution is closest to the seed moveit/moveit_kinematics_tests#10

Merged

davetcoleman requested changes Jun 24, 2017

View reviewed changes

jrgnicho reviewed Jun 25, 2017

View reviewed changes

Sort the solutions before returning closest

a6cc992

jrgnicho reviewed Jun 30, 2017

View reviewed changes

nsnitish added 2 commits July 1, 2017 15:39

Removed the dependency of getPositionIK from harmonize

0230511

Removed the dependency of getPositionIK from harmonize

370b01f

jrgnicho reviewed Jul 3, 2017

View reviewed changes

v4hn changed the title ~~Modifeid ikfast_template for getPositionIK single solution case~~ Modify ikfast_template for getPositionIK single solution results Jul 11, 2017

davetcoleman requested changes Jul 17, 2017

View reviewed changes

mintar suggested changes Jul 19, 2017

View reviewed changes

Used std::sort for solution sorting

a5f1d19

jrgnicho reviewed Jul 26, 2017

View reviewed changes

Changed struct name for storing and sorting the solutions

85a3afb

davetcoleman requested changes Jul 28, 2017

View reviewed changes

davetcoleman approved these changes Jul 28, 2017

View reviewed changes

davetcoleman merged commit 45c587d into moveit:kinetic-devel Jul 28, 2017

hartmanndennis mentioned this pull request Aug 3, 2017

Interest in Pull request for generating valid solutions outside -PI, PI range in ikfast plugin? #571

Open

130s mentioned this pull request Aug 7, 2017

[indigo-devel] Modify ikfast_template for getPositionIK single solution results (#537) from kinetic-devel #575

Closed

hartmanndennis mentioned this pull request Aug 8, 2017

[ikfast_template] searchPositionIK doesn't consider other solutions if the best solution returned by getPositionIK is in collision #578

Closed

hartmanndennis mentioned this pull request Aug 10, 2017

[indigo] ikfast plugin fails to compile with c++11 enabled #584

Closed

v4hn mentioned this pull request Aug 14, 2017

IKFastTemplate: searchPositionIK now returns collision-free solution which is nearest to seed state. #585

Merged

nsnitish mentioned this pull request Oct 11, 2017

Updated ikfast plugins for kuka_kr210 and motoman_sia20d moveit/moveit_kinematics_tests#15

Merged

davetcoleman mentioned this pull request Oct 11, 2017

Reduce memory allocations in IK solvers #629

Open

onionsflying mentioned this pull request Jan 15, 2018

Is #585 really good for motion planning? #740

Open

JafarAbdi pushed a commit to JafarAbdi/moveit that referenced this pull request Mar 24, 2022

Add persistent planner support back (moveit#537)

714975b

Modify ikfast_template for getPositionIK single solution results #537

Modify ikfast_template for getPositionIK single solution results #537

Conversation

nsnitish commented Jun 22, 2017 • edited Loading

Description

davetcoleman commented Jun 23, 2017

davetcoleman left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

nsnitish Jun 26, 2017 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

nsnitish commented Jun 30, 2017 • edited Loading

jrgnicho Jun 30, 2017 • edited Loading

Choose a reason for hiding this comment

nsnitish commented Jun 30, 2017

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

v4hn commented Jul 4, 2017 via email

nsnitish commented Jul 4, 2017

jrgnicho commented Jul 4, 2017 • edited Loading

shaun-edwards commented Jul 9, 2017

davetcoleman left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

davetcoleman commented Jul 17, 2017

Choose a reason for hiding this comment

nsnitish commented Jul 22, 2017

nsnitish commented Jul 22, 2017

Choose a reason for hiding this comment

Choose a reason for hiding this comment

davetcoleman left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

nsnitish commented Jul 28, 2017

davetcoleman commented Jul 28, 2017

Jmeyer1292 commented Aug 4, 2017

v4hn commented Aug 9, 2017 • edited Loading

davetcoleman commented Aug 15, 2017

Jmeyer1292 commented Aug 15, 2017

davetcoleman commented Oct 11, 2017

nsnitish commented Jun 22, 2017 •

edited

Loading

nsnitish Jun 26, 2017 •

edited

Loading

nsnitish commented Jun 30, 2017 •

edited

Loading

jrgnicho Jun 30, 2017 •

edited

Loading

jrgnicho commented Jul 4, 2017 •

edited

Loading

v4hn commented Aug 9, 2017 •

edited

Loading