Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Addressing discrepancies in parameter definition between IKFast solve…
…rs 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.
- Loading branch information
1 parent
8f39ef7
commit 9003ea9