Skip to content

Addressing discrepancies in parameter definition between IKFast solvers and the plugin template#3489

Merged
rhaschke merged 3 commits intomoveit:noetic-develfrom
nothingstopsme:ikfast_template_fixes
Feb 11, 2024
Merged

Addressing discrepancies in parameter definition between IKFast solvers and the plugin template#3489
rhaschke merged 3 commits intomoveit:noetic-develfrom
nothingstopsme:ikfast_template_fixes

Conversation

@nothingstopsme
Copy link
Copy Markdown
Contributor

Description

Two cases have been identified where the discrepancies occur:

  1. When an IKFast solver is of an IK type Translation*AxisAngle4D or Translation*AxisAngle*Norm4D, the current implementation takes the roll, pitch, or yaw value from the pose frame's orientation directly as the angle parameter passed to the solver; however, that does not match the angle definition adopted in IKFast, according to the solver-generating script from line 3403 to 3406.

    A quick example can immediately highlight the difference:
    If the orientation of a pose frame is described by (roll = PI, pitch = 0, yaw = 0), then a direction vector (0, 0, 1) in the
    manipulator's frame will be mapped to (0, 0, -1); in that case, a TranslationXAxisAngle4D solver will view it as PI/2
    from the x-axis, while the current implementation of the plugin template calls the solver with an angle of 0.

  2. There are several IK types (e.g. TranslationDirection5D) of which IKFast solvers require callers to pass in parameters calculated from the manipulator's direction vector defined in its own frame; nonetheless, that vector is currently fixed to (0, 0, 1) in the plugin template, and this assumption might become invalid if users have a different robot setup and manually generate their solver cpp with a customised wrapper XML specifying a different direction.

So this PR contains two commits aimed at addressing these two cases, respectively:

  1. Modifying the plugin template so that it calculates those angles the same way as indicated in the script
  2. Adding an input argument "--eef_direction" to support customising the direction vector when running "create_ikfast_moveit_plugin.py"

Checklist

  • Required by CI: Code is auto formatted using clang-format
  • Extend the tutorials / documentation reference
  • Document API changes relevant to the user in the MIGRATION.md notes
  • Create tests, which fail without this PR reference
  • Include a screenshot if changing a GUI
  • While waiting for someone to review your request, please help review another open pull request to support the maintainers

nothingstopsme added 2 commits August 27, 2023 00:43
…the plugin template

Problem Description:

When determining the angle parameter to a IKFast solver of a type
IKP_Translation*AxisAngle4D/IKP_Translation*AxisAngle*Norm4D,
the current implementation takes the roll, pitch, and yaw value
from the pose frame's orientation directly in each respective
case, which does not match how angles are calculated in IKFast.

In the solver-generating script of IKFast (ikfast.py), these
angles are defined as (in the base link's frame):

1. the angle between the given axis (x/y/z) and the manipulator's
direction vector, or
2. when the corresponding normal vector (z/x/y) is set, the angle
between the given axis (x/y/z) and the projection of the
manipulator's direction vector onto the plane of which the normal
is the aforementioned one.

Therefore, for example, if the orientation of a pose frame is
described by (roll = PI, pitch = 0, yaw = 0), a direction vector
(0, 0, 1) in the manipulator's frame will be mapped to (0, 0, -1);
in that case, a TranslationXAxisAngle4D solver will view it as
PI/2 from the x-axis, while the current implementation only comes
up with 0 angle.

Solution:

This patch has implemented exactly the same way of angle
calculation as done in IKFast, based on the assumption of where
the manipulator's direction vector points to in its own frame.
Note that this assumption is identical to the ones made in other
cases, like IKP_Ray4D or IKP_TranslationDirection5D.
…late through "create_ikfast_moveit_plugin.py" for customisation

Problem Description:

There are several IK types (e.g. TranslationDirection5D) of which
IKFast solvers require callers to pass in parameters calculated
based on the manipulator's direction vector defined in its own
frame; however, that vector is currently fixed to (0, 0, 1) in the
plugin template, and this assumption might become incorrect if users
have a different robot setup and manually generate their solver cpp
with a customised wrapper XML specifying a different direction.

Solution:

Instead of letting users edit the plugin cpp every time after
"create_ikfast_moveit_plugin.py" is run, this patch enhances the
functionality of "create_ikfast_moveit_plugin.py" by allowing users
to provide the direction vector of their choice via an input
argument (--eef_drection); thus not only does the resulting plugin
source file contain the desired setting, but the generated helping
script "update_ikfast_plugin.sh" also keeps track of that argument,
so that later updates will automatically stay consistent.

Note that when not given in the input arguments, "--eef_drection"
is set to (0, 0, 1) by default, and the generation falls back to
using the original assumption.
@welcome
Copy link
Copy Markdown

welcome bot commented Aug 26, 2023

Thanks for helping in improving MoveIt and open source robotics!

@codecov
Copy link
Copy Markdown

codecov bot commented Aug 26, 2023

Codecov Report

Patch coverage has no change and project coverage change: -0.02% ⚠️

Comparison is base (1730d7b) 61.83% compared to head (7d0556d) 61.81%.
Report is 1 commits behind head on noetic-devel.

Additional details and impacted files
@@               Coverage Diff                @@
##           noetic-devel    #3489      +/-   ##
================================================
- Coverage         61.83%   61.81%   -0.02%     
================================================
  Files               385      385              
  Lines             34078    34078              
================================================
- Hits              21069    21061       -8     
- Misses            13009    13017       +8     

see 2 files with indirect coverage changes

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

Copy link
Copy Markdown
Contributor

@rhaschke rhaschke left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thanks for these fixes, which look very reasonable to me. Could you provide some evidence that the behavior is now correct? Ideally, add some unit tests. Minimally, provide some images illustrating the behavior before and after your change.

…4D or Translation*AxisAngle*Norm4D

This modification originates from the need to test the changes
made in commit 8bf43f9 and d9625f1

To run this unit test, given the MoveIt source tree root being
the current working directory, execute the following commands

./moveit_kinematics/test/test_4dof/test_4dof.sh
catkin run_tests --no-deps -i moveit_kinematics

=================================================================

Based on the current testing implementation for kinematics
plugin (test_kinematics_plugin.cpp), the setup of this unit test
involves 3 components:

1. Dedicated model description/config packages that are created
for this unit test, which are packed into test_4dof/packages.tgz

2. A script (test_4dof/test_4dof-ikfast.test) to generate
necessary packages, including IKFast plugins, that allow catkin
to run a predefined test set

3. A test launch XML template (test_4dof/test_4dof-ikfast.test)
describing test parameters and what tests to perform

Note that for being able to reuse those existing test functions
in test_kinematics_plugin.cpp, slight changes have been made to
that cpp for dealing with the following two phenomena emerging
when testing IKFast plugins targeting robots whose dof < 6:

1. Since an IKFast solver in this case can only produce part of
pose information in the FK pass, the current implementation of
getPositionFK() in the plugin template will always return false
to indicate an error, causing the failure of every test run.

To circumvent that behaviour and allow the IK functionality of
plugins to be tested without being disrupted by getPositionFK(),
a launch parameter "plugin_fk_support" with a default value "true"
is introduced for indicating whether plugin's getPositionFK() is
well supported; if it is set to false, poses from the FK pass
will be obtained by updating the robot state with new angles and
then calling moveit::core::RobotState::getGlobalLinkTransform().

2. Since an IKFast solver in this case does not consume all pose
information given for IK calculation, an IK solution can only
guarantee a pose that matches the given one up to a certain
extent; that also implies there might be no solution
(approximately) equal to the joint states leading to the given
pose. So evaluating solutions based on full pose/joint-states
difference will inevitably leads to test failure.

In order to have meaningful comparison, it is desired to only
verify the information that is available and expected to stay
unchanged between the FK/IK passes; and for cases where position
checks remain valid (such as IKFast plugins of an iktype
Translation*AxisAngle4D or Translation*AxisAngle*Norm4), that can
be done via setting the newly added launch parameter
"position_only_check" (false by default) to true to bypass full
pose/joint-states checks and only measure the closeness between
the derived position from solutions and the one from the original
poses.
@nothingstopsme
Copy link
Copy Markdown
Contributor Author

nothingstopsme commented Sep 10, 2023

Hi, @rhaschke, thank you for the feedback.

I have added a unit test based on the current testing implementation for kinematics plugin.

Packages that are needed for this test can be generated by:

# Given the current working directory is at the root of MoveIt source tree
./moveit_kinematics/test/test_4dof/test_4dof.sh

Then this test can be launched together with other plugin tests via:

catkin run_tests --no-deps -i moveit_kinematics

Please note that the package-generating script "test_4dof.sh" needs to be run with a version of "create_ikfast_moveit_plugin.py" supporting the feature "--eef_direction" added in d9625f1, but the IKFast plugin template itself can be either one (before or after this PR being applied). So another way to verify the changes in this PR is to run "test_4dof.sh" with the current IKFast plugin template (without modifications) and then perform the same unit test. The following test report will show that IKFast plugins created this way fail some of the test cases set up by the script.

@v4hn v4hn requested a review from rhaschke November 30, 2023 20:10
@rhaschke rhaschke merged commit 9003ea9 into moveit:noetic-devel Feb 11, 2024
@welcome
Copy link
Copy Markdown

welcome bot commented Feb 11, 2024

Congrats on getting your first MoveIt pull request merged and improving open source robotics!

@rhaschke
Copy link
Copy Markdown
Contributor

Thanks for this contribution, particularly adding a test as well.

riv-tnoble added a commit to rivelinrobotics/rivelin_moveit that referenced this pull request Jun 11, 2024
* MSA: ControllerWidget allow '0' character in controller names (moveit#3528)

* Simplify code waiting for ActionBasedController

- avoid code redundancy
- fix warning interval to 5s
  Having an interval of timeout/3 is strange for very short or very long timeouts

* CI: Fix building of ikfast plugins

Ignore missing authentication for Indigo packages using --force-yes.

* CI: Add new ROS repo key

osrf/docker_images#697 (comment)

* Warn if trajectory becomes invalid during execution (moveit#3536)

Increase debug level from INFO to WARN.

* Enable higher number of self-collision checking samples (moveit#3529)

Replace density value label with spinbox to enable setting higher sampling densities

* Add interrupt button for collision generation

* Reuse existing finishGeneratingCollisionTable() in interrupt()

When interrupted, we don't want to update the collision matrix.

---------

Co-authored-by: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>

* Pass more distance information out from FCL collision check (moveit#3531)

* RobotState: Initialize joint transforms of fixed joints (moveit#3541)

RobotState doesn't update/initialize fixed joint transforms via update() - for optimization.
They are only set to identity via getJointTransform() when called with a non-const RobotState.
To initialize them once, we do that in initTransforms() now.

Co-authored-by: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>

* rviz: correctly edit radian and degree joint values

Before, the radio button radian/degree choice
was not respected when editing unbound joints.

I refactored the degree/radian logic into a custom data role
`JointRangeFractionRole` for interaction with the event filter.

* rviz: support enter to manually type joint values

Without this, there was no keyboard-only way to edit unbound joints.

I consider it a feature that this also works with *all* joints now,
as I see no reason to prevent the user from spinbox-editing arbitrary
joints if they wish to.

* guard <Return> when joint editor is already active

modelled after https://stackoverflow.com/a/18510433/21260084 .
It seems there is no more convenient way to access whether an editor is
already open.

This prevents stderr output saying "edit: editing failed".

* Use new location of boost::progress_display if available (moveit#3546)

* Revert "Use new location of boost::progress_display if available (moveit#3546)"

This reverts commit d03fade.

* Update Dockerfile (moveit#3543)

* use boost::timer::progress_display if available (moveit#3547)

check for header to stay compatible with ubuntu 20.04.

Support boost >= 1.83

Slightly ugly due to the double alias, but boost::timer was a class
before 1.72, so using `boost::timer::progress_display` in the code
breaks with older versions.

* support new ompl cmake targets (moveit#3549)

ompl's main branch changed omplConfig.cmake changed from OMPL_*
variables to imported targets via ompl::ompl in October 2023.

I generalized handling of fcl to support the new dependency.

* fix link names with slashes when looking up rigid parents

since subframes were added,
RobotState::getRigidlyConnectedParentLinkModel
did not see links anymore if they contained slashes.

* cleanup inline-URDF with raw string notation

* test links/object names with '/'

* Sharpen existing unit test

Not only test for the size of vectors, but also for the content.

* Extend subframe test to handle object and subframe with slashes

* Fix for subframes with slashes

* Need to find pybind11_catkin early error (moveit#3552)

* Fix docker configs

- ci-testing requires specific apt key
- use modern LABEL instead of deprecated MAINTAINER tag

* use min instead of multiplication to combine relative and absolute check (moveit#3556)

* 🛠️ Bump codecov/codecov-action from 3 to 4 (moveit#3559)

* Cleanup const-ref arguments to double+int (moveit#3560)

* Add missing word in error message
* Replace const [double|int] reference arguments with plain const args
   There is no value in passing a const reference over passing the value directly.
   The latter saves an extra indirection.

* [ROS-O] update Boost API (melodic-compatible) (moveit#3564)

Debian testing finally warns about API that was already deprecated in Ubuntu 18.04.

* Augment Python PlanningSceneInterface API (moveit#3534)

* Make object creation functions public
* Factor out make_plane
* Provide attach_* methods
* Use existing apply_planning_scene method instead of creating a new service instance
* Replace dangerous default list argument with None
   https://pylint.readthedocs.io/en/latest/user_guide/messages/warning/dangerous-default-value.html
* Expose get_planning_scene to python
* Augment AllowedCollisionMatrix with setter methods
* Add unit test for Python PlanningSceneInterface

* [ROS-O] Fix Pilz unit tests (moveit#3561)

* Avoid double-free of KDL::RotationalInterpolation in KDL >= 1.4.1
  The underlying memory leak was fixed in KDL with orocos/orocos_kinematics_dynamics#180
* Decrease accuracy of quaternion check

* PSM: add rudimentary tests

including a check for whether the monitored scene does not change upon
receiving full scenes (a current bug).

* PSM: keep references to scene_ valid upon receiving full scenes

plan_execution-related modules rely on `plan.planning_scene_` in many places
to point to the currently monitored scene (or a diff on top of it).

Before this patch, if the PSM would receive full scenes during execution,
`plan.planning_scene_` would not include later incremental updates anymore
because the monitor created a new diff scene.

* Addressing discrepancies in parameter definition between IKFast solvers and the plugin template (moveit#3489)

* Correct the inconsistent angle definition between IKFast solvers and the plugin template
   When determining the angle parameter to a IKFast solver of a type IKP_Translation*AxisAngle4D/IKP_Translation*AxisAngle*Norm4D, the current implementation takes the roll, pitch, and yaw value from the pose frame's orientation directly in each respective case, which does not match how angles are calculated in IKFast.
   In the solver-generating script of IKFast (ikfast.py), these angles are defined as (in the base link's frame):
    1. the angle between the given axis (x/y/z) and the manipulator's direction vector, or
    2. when the corresponding normal vector (z/x/y) is set, the angle between the given axis (x/y/z) and the projection of the manipulator's direction vector onto the plane of which the normal is the aforementioned one.
   Therefore, for example, if the orientation of a pose frame is described by (roll = PI, pitch = 0, yaw = 0), a direction vector (0, 0, 1) in the manipulator's frame will be mapped to (0, 0, -1); in that case, a TranslationXAxisAngle4D solver will view it as PI/2 from the x-axis, while the current implementation only comes up with 0 angle.

Solution:
This patch has implemented exactly the same way of angle calculation as done in IKFast, based on the assumption of where the manipulator's direction vector points to in its own frame. Note that this assumption is identical to the ones made in other cases, like IKP_Ray4D or IKP_TranslationDirection5D.

* Allow users to change the direction vector defined in the plugin template through "create_ikfast_moveit_plugin.py" for customization
   There are several IK types (e.g. TranslationDirection5D) of which IKFast solvers require callers to pass in parameters calculated based on the manipulator's direction vector defined in its own frame; however, that vector is currently fixed to (0, 0, 1) in the plugin template, and this assumption might become incorrect if users
have a different robot setup and manually generate their solver cpp with a customised wrapper XML specifying a different direction.

Solution:
Instead of letting users edit the plugin cpp every time after "create_ikfast_moveit_plugin.py" is run, this patch enhances the functionality of "create_ikfast_moveit_plugin.py" by allowing users to provide the direction vector of their choice via an input argument (--eef_drection); thus not only does the resulting plugin source file contain the desired setting, but the generated helping script "update_ikfast_plugin.sh" also keeps track of that argument,
so that later updates will automatically stay consistent.

Note that when not given in the input arguments, "--eef_drection" is set to (0, 0, 1) by default, and the generation falls back to using the original assumption.

* Add a unit test for IKFast plugins of an iktype Translation*AxisAngle4D or Translation*AxisAngle*Norm4D
   To run this unit test, given the MoveIt source tree root being the current working directory, execute the following commands
   ./moveit_kinematics/test/test_4dof/test_4dof.sh
   catkin run_tests --no-deps -i moveit_kinematics

   Based on the current testing implementation for kinematics plugin (test_kinematics_plugin.cpp), the setup of this unit test involves 3 components:
   1. Dedicated model description/config packages that are created for this unit test, which are packed into test_4dof/packages.tgz
   2. A script (test_4dof/test_4dof-ikfast.test) to generate necessary packages, including IKFast plugins, that allow catkin to run a predefined test set
   3. A test launch XML template (test_4dof/test_4dof-ikfast.test) describing test parameters and what tests to perform

   Note that for being able to reuse those existing test functions in test_kinematics_plugin.cpp, slight changes have been made to that cpp for dealing with the following two phenomena emerging when testing IKFast plugins targeting robots whose dof < 6:

   1. Since an IKFast solver in this case can only produce part of pose information in the FK pass, the current implementation of getPositionFK() in the plugin template will always return false to indicate an error, causing the failure of every test run.  
      To circumvent that behaviour and allow the IK functionality of plugins to be tested without being disrupted by getPositionFK(), a launch parameter "plugin_fk_support" with a default value "true" is introduced for indicating whether plugin's getPositionFK() is well supported; if it is set to false, poses from the FK pass will be obtained by updating the robot state with new angles and then calling moveit::core::RobotState::getGlobalLinkTransform().
   2. Since an IKFast solver in this case does not consume all pose information given for IK calculation, an IK solution can only guarantee a pose that matches the given one up to a certain extent; that also implies there might be no solution (approximately) equal to the joint states leading to the given pose. So evaluating solutions based on full pose/joint-states difference will inevitably leads to test failure.
     In order to have meaningful comparison, it is desired to only verify the information that is available and expected to stay unchanged between the FK/IK passes; and for cases where position checks remain valid (such as IKFast plugins of an iktype Translation*AxisAngle4D or Translation*AxisAngle*Norm4), that can be done via setting the newly added launch parameter "position_only_check" (false by default) to true to bypass full pose/joint-states checks and only measure the closeness between the derived position from solutions and the one from the original poses.

* 🛠️ Bump pre-commit/action from 3.0.0 to 3.0.1 (moveit#3568)

* CI: 🛠️ Bump action versions

* MSA: Add missing filename attribute to gazebo plugin tag (moveit#3572)

* Add joints_allowed_start_tolerance parameter (moveit#3287)

... for joint-specific start tolerances for TrajectoryExecutionManager

Co-authored-by: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>

* Fixed order of return values in doc string of compute_cartesian_path() (moveit#3574)

* Fix warehouse database destruction (moveit#3581)

Standalone applications used the convenience function moveit_warehouse::loadDatabase()
to load a database plugin. Internally, this uses a static plugin loader.
As this static variable is released too late during app shutdown, we get an exception:

```
terminate called after throwing an instance of 'class_loader::LibraryUnloadException'
what():  Attempt to unload library that class_loader is unaware of.
```

Explicitly instantiating the loader ensures correct destruction order and avoids the error.

* Benchmarking with Google benchmark (moveit#3565)

* Add benchmark dependency in moveit_core's package.xml

* MoveGroupInterface: allow RobotState diffs as start state (moveit#3592)

The old implementation used a RobotState instance for considered_start_state_, which was transformed into a RobotState message, when filling the actual MotionPlanRequest.
This turned a provided diff message into a full message, which has performance drawbacks when there are many large attached object meshes.
The new implementation uses the message format directly, avoiding unnecessary conversions.

Co-authored-by: Robert Haschke <rhaschke@users.noreply.github.com>

* Allow waiting for joint model group states and retrieval of group-specific timestamps (moveit#3580)

This commit introduces:

1. per-group current state time retrieval
2. per-group complete robot state waiting

This fixes the following issue: 
I want to call waitForCompleteState on group_A, which the API already supports. There is also another joint out_of_group_A in the robot model that is not available until some time after bringup. 
I'm calling waitForCompleteState(group_A, <timeout>) way earlier than out_of_group_A is published by its source.
The problem with the current implementation is that it waits until <timeout> elapses, but returns true since all joints of group_A exist. I don't see a point in waiting for an out-of-group joint in this case.

* Don't copy attached collision objects in CommandListManager::setStartState (moveit#3590)

CommandListManager::setStartState shall update the start state in the next MotionPlanRequest to match the reached robot pose after previous planning steps.
Copying attached collision objects is not necessary there, because the RobotState is initialized once in the beginning. In each iteration of CommandListManager::solveSequenceItems(), only joint states actually update.

Copying the attached bodies is not only a performance penalty, but also introduces a (numerical) drift of the attached object poses as pointed out in moveit#3590 (comment) / moveit#3569.

Co-authored-by: Robert Haschke <rhaschke@users.noreply.github.com>

* Add moveit pro tutorials reference (moveit#3593)

* Allow moving of all shapes of an object in one go (moveit#3599)

* Rename ros-planning org (moveit#3603)

* github.com/ros-planning -> github.com/moveit
* codecov.io/gh/ros-planning -> codecov.io/gh/moveit
* ros-planning.github.io -> moveit.github.io

* 1.1.14

---------

Co-authored-by: Abe Maclean <abemaclean@gmail.com>
Co-authored-by: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Co-authored-by: Michael Görner <me@v4hn.de>
Co-authored-by: Salih Marangoz <12991962+salihmarangoz@users.noreply.github.com>
Co-authored-by: Stephanie Eng <stephanie-eng@users.noreply.github.com>
Co-authored-by: Hugal31 <hla@lescompanions.com>
Co-authored-by: Lucas Walter <wsacul@gmail.com>
Co-authored-by: Simon Schmeisser <simon.schmeisser@optonic.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>
Co-authored-by: Robert Haschke <rhaschke@users.noreply.github.com>
Co-authored-by: nothingstopsme <50317951+nothingstopsme@users.noreply.github.com>
Co-authored-by: Hugal31 <hugo.laloge@gmail.com>
Co-authored-by: IrvingF7 <13810062+IrvingF7@users.noreply.github.com>
Co-authored-by: Tom Noble <53005340+TSNoble@users.noreply.github.com>
Co-authored-by: Sebastian Jahr <sebastian.jahr@picknik.ai>
Co-authored-by: Captain Yoshi <captain.yoshisaur@gmail.com>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

2 participants