Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

KDL IK solver cleanup #1294

Merged
merged 19 commits into from Jan 23, 2019

Conversation

Projects
None yet
3 participants
@rhaschke
Copy link
Collaborator

commented Jan 4, 2019

Tracking down #1255 and in preparation of #1208, I first cleaned up the KDL IK solver code

  • removing dead code
  • removing redundant joint handling
  • merging ChainIkSolverPos into main plugin source
  • use standard Eigen::SVD that yields higher accuracy

These changes should not affect functionality as verified by kinematics unit tests (#1272). The cleanup yields a strong performance boost (running 10.000 deterministic IK attempts):

  • original code: 410ms
  • after cleanup: 230ms
  • standard Eigen::SVD: 240ms

Please merge in conjuction with #1272 and #1288.

@mlautman

This comment has been minimized.

Copy link
Member

commented Jan 7, 2019

This contribution was clearly a ton of work and makes a ton of improvements. That said, it is really hard to review this for correctness. Can you please add tests to demonstrate the correctness of these changes?

@rhaschke

This comment has been minimized.

Copy link
Collaborator Author

commented Jan 8, 2019

Can you please add tests to demonstrate the correctness of these changes?

As already written in the main comment: These changes are verified by kinematics unit tests (#1272).

rhaschke added some commits Dec 20, 2018

kdl kinematics: improve debug output
- using IO functions of kdl to (efficiently) print joint arrays as row vectors
- print time spent
- rename "iterations" -> "attempts"
cleanup KDL IK
- fixed type of max_solver_iterations_
- directly read parameters into class members
remove ChainIkSolverPos_NR_JL_Mimic::CartToJntAdvanced()
This function is doing exactly the same as CartToJnt().
The extra argument lock_redundant_joints is not used at all.

@rhaschke rhaschke force-pushed the ubi-agni:kdl-cleanup branch from be23258 to 6483bce Jan 10, 2019

rhaschke added a commit to ubi-agni/moveit that referenced this pull request Jan 10, 2019

cleanup LMA kinematics solver
- perform similar cleanup as in ros-planning#1294
- actually use KDL's LMA solver (was instantiated but not used)
- remove notion of mimic joints, as the LMA solver doesn't support this
@mlautman
Copy link
Member

left a comment

+1 from me. @v4hn's or @davetcoleman's, can you also take a look? I would like for another reviewer to look at this before it gets merged.

Show resolved Hide resolved ...it_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h Outdated
* @param ik_pose the desired pose of the link
* @param ik_seed_state an initial guess solution for the inverse kinematics
* @param solution the solution vector
* @param error_code an error code that encodes the reason for failure or success
* @param lock_redundant_joints if setRedundantJoints() was previously called, keep the values of the joints marked as
* redundant the same as in the seed

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Jan 12, 2019

Member
Suggested change
* redundant the same as in the seed
* @param options An option struct which contains the type of redundancy discretization used. This default
* implementation only supports the KinematicSearches::NO_DISCRETIZATION method; requesting any
* other will result in failure.

This comment has been minimized.

Copy link
@rhaschke

rhaschke Jan 14, 2019

Author Collaborator

I don't like to advertize the redundancy parameters anymore. They are not used by MoveIt.
Rather, they are parameters of IKfast only.
However, I added a generic comment as follows:
@param options container for other IK options. See definition of KinematicsQueryOptions for details.

Show resolved Hide resolved ...it_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h
Show resolved Hide resolved ...it_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h
Show resolved Hide resolved ...it_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h
Show resolved Hide resolved ...tics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h Outdated
Show resolved Hide resolved ...tics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h Outdated
Show resolved Hide resolved moveit_kinematics/kdl_kinematics_plugin/src/chainiksolver_vel_mimic_svd.cpp
Show resolved Hide resolved moveit_kinematics/kdl_kinematics_plugin/src/chainiksolver_vel_mimic_svd.cpp
Show resolved Hide resolved moveit_kinematics/kdl_kinematics_plugin/src/chainiksolver_vel_mimic_svd.cpp

rhaschke added some commits Feb 14, 2018

Jacobian solver: reduce code bloat
- merge CartToJntRedundant() and CartToJnt() handling all variants
  - locked/unlocked redundant joints
  - mimic joints
  - full / position-only IK
- use svd_eigen_HH in all cases
- reuse matrices for all variants
limit getPositionIK() to a single attempt
... setting the timeout to zero and ensuring at least one solution attempt
clean IKCallbackFn
remove weird initialization IKCallbackFn solution_callback = 0
but pass default-constructed IKCallbackFn()
remove redundant joints stuff
Special handling of "redundant joints" doesn't make sense for KDL solver.
merge ChainIkSolverPos into main plugin file
... to reduce data copying and memory allocations
ChainIkSolverVel: further cleanup
- passing mimic_joints_ to constructor, storing reference, and avoiding copy
- fix naming of members

rhaschke added some commits Jan 8, 2019

further improve debug output
- providing iteration number
- using some indentation to facilitate grouping of output

@rhaschke rhaschke force-pushed the ubi-agni:kdl-cleanup branch from 9677bcd to 50e71ef Jan 14, 2019

rhaschke added a commit to ubi-agni/moveit that referenced this pull request Jan 14, 2019

cleanup LMA kinematics solver
- perform similar cleanup as in ros-planning#1294
- actually use KDL's LMA solver (was instantiated but not used)
- remove notion of mimic joints, as the LMA solver doesn't support this

rhaschke added a commit to ubi-agni/moveit that referenced this pull request Jan 14, 2019

cleanup LMA kinematics solver
- perform similar cleanup as in ros-planning#1294
- actually use KDL's LMA solver (was instantiated but not used)
- remove notion of mimic joints, as the LMA solver doesn't support this

@rhaschke rhaschke merged commit 50e71ef into ros-planning:melodic-devel Jan 23, 2019

1 check passed

continuous-integration/travis-ci/pr The Travis CI build passed
Details

rhaschke added a commit that referenced this pull request Jan 23, 2019

@rhaschke rhaschke deleted the ubi-agni:kdl-cleanup branch Jan 23, 2019

rhaschke added a commit to ubi-agni/moveit that referenced this pull request Jan 23, 2019

cleanup LMA kinematics solver
- perform similar cleanup as in ros-planning#1294
- actually use KDL's LMA solver (was instantiated but not used)
- remove notion of mimic joints, as the LMA solver doesn't support this

rhaschke added a commit to ubi-agni/moveit that referenced this pull request Jan 23, 2019

cleanup LMA kinematics solver
- perform similar cleanup as in ros-planning#1294
- actually use KDL's LMA solver (was instantiated but not used)
- remove notion of mimic joints, as the LMA solver doesn't support this
@rhaschke

This comment has been minimized.

Copy link
Collaborator Author

commented Jan 23, 2019

Merged after having Mike's approval and having Dave's comments addressed.

rhaschke added a commit to ubi-agni/moveit that referenced this pull request Jan 30, 2019

cleanup LMA kinematics solver
- perform similar cleanup as in ros-planning#1294
- actually use KDL's LMA solver (was instantiated but not used)
- remove notion of mimic joints, as the LMA solver doesn't support this

rhaschke added a commit to ubi-agni/moveit that referenced this pull request Feb 4, 2019

cleanup LMA kinematics solver
- perform similar cleanup as in ros-planning#1294
- actually use KDL's LMA solver (was instantiated but not used)
- remove notion of mimic joints, as the LMA solver doesn't support this

rhaschke added a commit to ubi-agni/moveit that referenced this pull request Feb 4, 2019

cleanup LMA kinematics solver
- perform similar cleanup as in ros-planning#1294
- actually use KDL's LMA solver (was instantiated but not used)
- remove notion of mimic joints, as the LMA solver doesn't support this

rhaschke added a commit to ubi-agni/moveit that referenced this pull request Feb 4, 2019

cleanup LMA kinematics solver
- perform similar cleanup as in ros-planning#1294
- actually use KDL's LMA solver (was instantiated but not used)
- remove notion of mimic joints, as the LMA solver doesn't support this

rhaschke added a commit to ubi-agni/moveit that referenced this pull request Feb 4, 2019

cleanup LMA kinematics solver
- perform similar cleanup as in ros-planning#1294
- actually use KDL's LMA solver (was instantiated but not used)
- remove notion of mimic joints, as the LMA solver doesn't support this
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.