Add an analytic IK solver (IkFast) #887

jslee02 · 2017-05-18T14:33:04Z

This pull request introduces an API for IkFast, which is an analytic IK solver that is a submodule of OpenRAVE.

Here is an example of Solving IK using IkFast for Barrett WAM Arm:

TODOs

Add benchmark (IKFast vs iterative Jacobian based IK solver) of the Barret WAM Arm demo

codecov · 2017-05-18T19:53:00Z

Codecov Report

Merging #887 into master will increase coverage by 0.58%.
The diff coverage is 55.19%.

@@            Coverage Diff             @@
##           master     #887      +/-   ##
==========================================
+ Coverage   50.59%   51.17%   +0.58%     
==========================================
  Files         299      307       +8     
  Lines       23408    23691     +283     
  Branches     3015     3065      +50     
==========================================
+ Hits        11843    12124     +281     
+ Misses      10236    10203      -33     
- Partials     1329     1364      +35

Impacted Files	Coverage Δ
dart/dynamics/InverseKinematics.hpp	`90.47% <ø> (+18.25%)`	⬆️
dart/dynamics/IkFast.hpp	`100% <100%> (ø)`
dart/dynamics/SharedLibraryIkFast.hpp	`100% <100%> (ø)`
dart/common/detail/SharedLibraryManager.hpp	`100% <100%> (ø)`
dart/dynamics/InverseKinematics.cpp	`41.4% <20%> (+12.75%)`	⬆️
dart/dynamics/SharedLibraryIkFast.cpp	`32.63% <32.63%> (ø)`
dart/common/detail/SharedLibraryManager.cpp	`53.84% <53.84%> (ø)`
dart/common/SharedLibrary.cpp	`62.5% <62.5%> (ø)`
dart/dynamics/IkFast.cpp	`72.22% <72.22%> (ø)`
dart/dynamics/SimpleFrame.hpp	`100% <0%> (ø)`	⬆️
... and 17 more

mkoval

This generally looks good. Very nice work @jslee02 - this clearly took a lot of work to implement and the solution is much cleaner than I imagined it could be.

My biggest request is documentation. Specifically:

If I am a new user to DART, how to do I generate an IKFast solution for my robot? (I can help write this one.)
If I have the output of IKFast, how do I load it into DART using the functionality you added?
Once I have the IK solver loaded, how does using Analytical differ from using other types of IK? How do I get multiple solutions?

Additionally, I would like to have optional support for checking the solution returned by IKFast against forward kinematics. This is generally useful for analytic IK solvers, so I suggest moving it to the Analytical base class.

Finally, I am not sure where this class enumerates the free parameters to generate a set of IK solutions. Can you point me to that code so I can take another look at it?

mkoval · 2017-05-19T01:49:53Z

dart/common/SharedLibrary.cpp

+struct HINSTANCE__;
+typedef struct HINSTANCE__* hInstance;
+
+#endif


Well this is pretty horrifying. I have no doubt that it's necessary on Windows. Can you add a few comments explaining what all of this does?

mkoval · 2017-05-19T01:52:46Z

dart/common/SharedLibrary.cpp

+    nameWithExtension += ".so";
+#elif DART_OS_MACOS
+  // dlopen() does not add .dylib to the filename, like windows does for .dll
+  if (nameWithExtension.substr(fileName.length() - 6, 6) != ".dylib")


This will cause an unsigned overflow if fileName.length() < 6. Then it will likely SEGFAULT because the index is out of bounds.

mkoval · 2017-05-19T01:54:43Z

dart/common/SharedLibrary.cpp

+#endif
+
+  mInstance
+      = static_cast<DYNLIB_HANDLE>(DYNLIB_LOAD(nameWithExtension.c_str()));


DYNLIB_LOAD seems to only be used here. I think it would be cleaner to simply load the library in the chain of #ifdef blocks above instead of duplicating the same preprocessor logic in two places (this function and the implementation of the DYNLIB_LOAD macro).

mkoval · 2017-05-19T01:56:32Z

dart/common/SharedLibrary.cpp

+  if (!isValid())
+    return nullptr;
+
+  auto symbol = DYNLIB_GETSYM(mInstance, symbolName.c_str());


Same comment here: could we embed the logic instead of wrapping it in a macro?

mkoval · 2017-05-19T01:58:33Z

dart/common/SharedLibrary.hpp

+  /// Windows).
+  /// \return Pointer to the created SharedLibrary upon success in loading.
+  /// Otherwise, returns nullptr.
+  explicit SharedLibrary(ProtectedContructionTag, const std::string& fileName);


Add a comment explaining that ProtectedContructionTag is necessary because you want to use std::make_shared to create this type, which does not inherit the friend class designation below. We presumably want to use std::make_shared because it eliminates a memory allocation that would otherwise be necessary for the shared_ptr's control block.

mkoval · 2017-05-19T02:40:38Z

dart/gui/osg/DragAndDrop.cpp

@@ -814,6 +814,8 @@ void BodyNodeDnD::saveState()
  else
  {
    mIK = bn->createIK();
+    // TODO(JS): I guess this should be changed to:
+    // mIK = bn->getIK(true);


Is this necessary?

Will open as a separate PR

mkoval · 2017-05-19T02:41:08Z

data/urdf/wam/wam.urdf

@@ -0,0 +1,231 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from /home/js/dev/catkin_workspaces/dev_libherb/src/herb_description/robots/wam_standalone.urdf.xacro | -->


Nit: Remove the path from this comment.

mkoval · 2017-05-19T02:42:14Z

data/urdf/wam/wam.urdf

+    <child link="/wam1"/>
+    <axis xyz="0 0 1"/>
+    <!-- HERB's J1 joints are modified to have a +180 degree offset. -->
+    <limit effort="1.8" lower="0.54159265359" upper="5.74159265359" velocity="0.75"/>


If you are going to distribute this file with DART, then I suggest reverting to the default joint limits of the Barrett WAM arm. HERB's WAMs were specially modified by Barrett to have different joint limits, to improve the workspace when two WAMs are mounted side-by-side.

mkoval · 2017-05-19T02:44:25Z

unittests/unit/SharedLibraryWamIkFast.hpp

+#include "dart/common/SharedLibrary.hpp"
+#include "dart/dynamics/IkFast.hpp"
+
+class SharedLibraryWamIkFast : public dart::dynamics::IkFast


Is this the code that an end-user would implement to use IKFast on their robot? Why is this necessary versus just passing the path to the .so file into one of the SharedLibrary functions?

An end-user would juse use this class. This class enables the user to use DART's inverse kinematics framework with IkFast .so file without any further modification. All the user needs to do is passing the path to the .so file and the list of free/non-free joint indices like:

std::string libName = "libGeneratedWamIkFast.so"; std::vector<std::size_t> ikFastDofs{0, 1, 3, 4, 5, 6}; std::vector<std::size_t> ikFastFreeDofs{2}; ik->setGradientMethod<dynamics::SharedLibraryIkFast>( libName, ikFastDofs, ikFastFreeDofs);

SharedLibrary only loads arbitrary .so file and extracts symbols from the file, but doens't have any information how to interact with IkFast nor the inverse kinematics framework.

@jslee02 Could you give an example how to generate .so file with wam robot?

Once you have a .cpp file generated for WAM, you just compile the file with any compiler you like. Hope this helps. Note https://github.com/dartsim/dart/blob/master/examples/osgExamples/osgWamIkFast/ikfast/ikfast71.Transform6D.4_6_9_10_11_12_f8.cpp is just an example of IK for WAM. You might want different free joints setting for the same robot.

mkoval · 2017-05-19T02:48:34Z

dart/dynamics/IkFast.cpp

+
+    index++;
+  }
+


This class - or the Analytic base class - should support a bool flag that toggles error checking. It's inexpensive to verify that running the IK solver's solution through DART's forward kinematics returns the expected BodyNode pose.

This is a very important check - to the extent that we may want to enable it by default - because it catches to broad classes of errors: (1) using an analytic IK solution for a slightly different model and (2) bugs in the analytic IK solution. OpenRAVE avoids the former by hashing the model - which is not really possible here. I have also been burned by the latter due to bugs in IKFast.

I believe I've addressed this in my top-level comment, but let me know if I'm misunderstanding your concern.

mxgrey · 2017-05-19T05:38:56Z

Once I have the IK solver loaded, how does using Analytical differ from using other types of IK? How do I get multiple solutions?

Suppose you have an InverseKinematicsPtr object called ik and it's been assigned an Analytical gradient method. You can access the multiple solutions of the Analytical method using:

std::vector<InverseKinematics::Analytical::Solution> solutions =
          ik->getAnalytical()->getSolutions();

Additionally, I would like to have optional support for checking the solution returned by IKFast against forward kinematics.

This can already be done in two ways. If you implicitly use the Analytical solver by calling ik->solve(), then the boolean return value will be false if a valid solution was not actually found. Alternatively, if you get the entire set of options returned by the Analytical solver using ik->getAnalytical()->getSolutions(), then each object InverseKinematics::Analytical::Solution has a bit field mValidity which is 00 when valid, 01 when the target could not be reached, 10 when the solution is violating joint limits, and 11 when the target is not reached and the joint limits are violated.

So I believe the functionality that you're looking for is already immediately available. If you have ideas for ways to improve it or make it more accessible, I'd be very open to making changes.

Finally, I am not sure where this class enumerates the free parameters to generate a set of IK solutions. Can you point me to that code so I can take another look at it?

If you're referring to the Analytical class, you can use InverseKinematics::setDofs(~) to affect which DoFs are available for the solver to use, and then the behavior of any DoFs which are not used directly in the analytical solution can be changed using Analytical::setExtraDofUtilization(~). Currently there is no method built into the base Analytical class that would sample values for those extra DoFs to produce a set of solutions, but that can certainly be done in the class that derives Analytical, or we could add that as one of the options for setExtraDofUtilization(~).

That being said, I'm personally not satisfied at all with the current design of the "extra DoF utilization". I mean, it works for my own personal use cases (which is why I made it in the first place), but it's kind of clumsy and not at all extensible, so I'd love to hear ideas for alternative designs.

mkoval · 2017-05-23T01:26:05Z

This can already be done in two ways. If you implicitly use the Analytical solver by calling ik->solve(), then the boolean return value will be false if a valid solution was not actually found. Alternatively, if you get the entire set of options returned by the Analytical solver using ik->getAnalytical()->getSolutions(), then each object InverseKinematics::Analytical::Solution has a bit field mValidity which is 00 when valid, 01 when the target could not be reached, 10 when the solution is violating joint limits, and 11 when the target is not reached and the joint limits are violated.

So I believe the functionality that you're looking for is already immediately available. If you have ideas for ways to improve it or make it more accessible, I'd be very open to making changes.

I don't think this addresses my concern. Suppose you call q = InverseKinematics(x). I want to check that IsClose(ForwardKinematics(q), x). If that condition is not satisfied, then there is a bug in the analytical solution and we should: (1) ignore the solution and (2) print a warning.

This most often occurs because there is a mismatch between the kinematics used to create the analytical solution and the Skeleton in DART. It can also occasionally happen due to bugs in IKFast. This is an insidious bug because it is often is not obvious upon casual inspection.

I am fine with putting this in the IKFast class. However, I think it could be useful as a first-class citizen in the Analytical base class because this class of bugs can occur for any type of analytical solution.

# Conflicts: # CHANGELOG.md

jslee02 · 2017-10-26T02:28:33Z

I believe I've addressed all the @mkoval's comments except for the platform specific macros. Let me handle that in a separate PR.

jslee02 added 12 commits May 4, 2017 00:38

Implementing IKFast interface

a79005e

Implementing IKFast interface (2)

e3cb034

Remove plugin and version

23663fe

Adding osgWamIkfast

420fdcc

Split osgWamIkfast into multiple files

58def8d

Minor updates

b2c375c

Simplify the WAM example

2fb0598

Clean up

a6400fe

Don't treat free joint as variable in solving ik

7208821

Cleanup

f02d6e4

First working version of IkFast

54d8b8f

Discard invalid analytical solution

5772209

jslee02 added this to the DART 6.3.0 milestone May 18, 2017

jslee02 and others added 4 commits May 18, 2017 10:33

Merge branch 'master' into feature/ikfast

ddad0be

Bump version to 6.3.0

1f1c109

Update CHANGELOG.md

4267167

Resolve issues on macOS

19b59a9

Merge branch 'master' into feature/ikfast

a83f2b3

mkoval requested changes May 19, 2017

View reviewed changes

jslee02 and others added 2 commits May 20, 2017 22:33

Rename fileName to path

139a481

Merge branch 'master' into feature/ikfast

4ef6064

jslee02 changed the base branch from master to release-6.3 August 7, 2017 19:18

jslee02 added 5 commits September 14, 2017 12:30

Merge remote-tracking branch 'origin/release-6.3' into feature/ikfast

b562f27

# Conflicts: # CHANGELOG.md

Set unittest output to the default

d506cb5

Addressing Mike's comments -- incomplete

3eb787c

Improve free joint checking routine

4be6222

Use boost::filesystem::path

a5dc8b1

Merge branch 'release-6.3' into feature/ikfast

0c91abc

jslee02 modified the milestones: DART 6.3.0, DART 6.4.0 Oct 4, 2017

jslee02 changed the title ~~Add IkFast support~~ Add an analytic IK solver (IkFast) Oct 6, 2017

jslee02 changed the base branch from release-6.3 to release-6.4 October 6, 2017 23:15

jslee02 added 10 commits October 6, 2017 16:16

Merge branch 'release-6.4' into feature/ikfast

6f5f518

Merge branch 'release-6.4' into feature/ikfast

92a7916

Revert to the default joint limits of the Barrett WAM arm

5a91bf3

Add docstrings

8720989

Update copyright header

6bbf6e4

Remove unrelated comment to this PR

f4efb77

Merge branch 'release-6.4' into feature/ikfast

9cc2fd0

Remove unnecessary check of success on symbol loading

e70d4bd

Fix typo

db317d5

Add comment

554f6bb

jslee02 added the PR: Will merge soon unless anyone has an objection label Oct 26, 2017

jslee02 merged commit cb4e855 into release-6.4 Oct 26, 2017

jslee02 deleted the feature/ikfast branch October 26, 2017 06:35

Tokrat mentioned this pull request Nov 9, 2020

No documentation for IkFast #1526

Open

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add an analytic IK solver (IkFast) #887

Add an analytic IK solver (IkFast) #887

jslee02 commented May 18, 2017 •

edited

Loading

codecov bot commented May 18, 2017 •

edited

Loading

mkoval left a comment

mkoval May 19, 2017

mkoval May 19, 2017

mkoval May 19, 2017

mkoval May 19, 2017

mkoval May 19, 2017

mkoval May 19, 2017

jslee02 Oct 24, 2017

mkoval May 19, 2017

mkoval May 19, 2017

mkoval May 19, 2017

jslee02 Oct 24, 2017

junhyeokahn Jul 17, 2018

jslee02 Jul 17, 2018

mkoval May 19, 2017

mxgrey May 19, 2017

mxgrey commented May 19, 2017 •

edited

Loading

mkoval commented May 23, 2017

jslee02 commented Oct 26, 2017

Add an analytic IK solver (IkFast) #887

Add an analytic IK solver (IkFast) #887

Conversation

jslee02 commented May 18, 2017 • edited Loading

codecov bot commented May 18, 2017 • edited Loading

Codecov Report

mkoval 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

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

mxgrey commented May 19, 2017 • edited Loading

mkoval commented May 23, 2017

jslee02 commented Oct 26, 2017

jslee02 commented May 18, 2017 •

edited

Loading

codecov bot commented May 18, 2017 •

edited

Loading

mxgrey commented May 19, 2017 •

edited

Loading