Skip to content

[ROS-O] Fix Pilz unit tests#3561

Merged
rhaschke merged 3 commits intomoveit:masterfrom
ubi-agni:fix-pilz-tests
Feb 6, 2024
Merged

[ROS-O] Fix Pilz unit tests#3561
rhaschke merged 3 commits intomoveit:masterfrom
ubi-agni:fix-pilz-tests

Conversation

@rhaschke
Copy link
Copy Markdown
Contributor

@rhaschke rhaschke commented Feb 6, 2024

There are two unit tests consistently failing in Jammy:

  • circle path generation failing with an exception attempts to double-free the passed RotationalInterpolation object.
    Here, KDL 1.4.1 has fixed the memory leak itself. No need to handle that in Pilz code anymore.
  • The accuracy for comparing quaternions is too high for another test.

@rhaschke rhaschke requested a review from jschleicher as a code owner February 6, 2024 08:14
@codecov
Copy link
Copy Markdown

codecov bot commented Feb 6, 2024

Codecov Report

Attention: 4 lines in your changes are missing coverage. Please review.

Comparison is base (9440572) 62.19% compared to head (cd95fbd) 47.14%.
Report is 4 commits behind head on master.

Files Patch % Lines
..._setup_assistant/src/widgets/navigation_widget.cpp 0.00% 2 Missing ⚠️
...anner/src/trajectory_blender_transition_window.cpp 50.00% 1 Missing ⚠️
..._planner/test/integrationtest_command_planning.cpp 0.00% 1 Missing ⚠️
Additional details and impacted files
@@             Coverage Diff             @@
##           master    #3561       +/-   ##
===========================================
- Coverage   62.19%   47.14%   -15.05%     
===========================================
  Files         385      602      +217     
  Lines       34137    58913    +24776     
  Branches        0     6968     +6968     
===========================================
+ Hits        21228    27767     +6539     
- Misses      12909    30734    +17825     
- Partials        0      412      +412     

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

tf2::fromMsg(expected_pose, exp_iso3d_pose);

EXPECT_TRUE(Eigen::Quaterniond(tf.rotation()).isApprox(Eigen::Quaterniond(exp_iso3d_pose.rotation()), EPSILON));
EXPECT_TRUE(Eigen::Quaterniond(tf.rotation()).isApprox(Eigen::Quaterniond(exp_iso3d_pose.rotation()), 2 * EPSILON));
Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

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

If a factor 2 solves the problem for you right now, I approve.

The tight requirement is still quite off the task though because the required plan above specifies a joint angle accuracy of 0.01rad for each absolute axis. So the maximum error we can get without violating the planning request is something way bigger (around 0.05 per array element assuming a unit rotation was requested)

@rhaschke rhaschke merged commit 95dd7fb into moveit:master Feb 6, 2024
@rhaschke rhaschke deleted the fix-pilz-tests branch February 6, 2024 20:29
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