Skip to content

Fixes merge conflicts (humble backport 3077)#3087

Merged
sjahr merged 2 commits intomoveit:mergify/bp/humble/pr-3077from
TSNoble:mergify/bp/humble/pr-3077
Nov 15, 2024
Merged

Fixes merge conflicts (humble backport 3077)#3087
sjahr merged 2 commits intomoveit:mergify/bp/humble/pr-3077from
TSNoble:mergify/bp/humble/pr-3077

Conversation

@TSNoble
Copy link
Copy Markdown
Contributor

@TSNoble TSNoble commented Nov 13, 2024 •
edited
Loading

Description

Fixes merge conflicts in the backport PR

@mergify
Copy link
Copy Markdown

mergify bot commented Nov 13, 2024

Please target the main branch for development, we will backport the changes to mergify/bp/humble/pr-3077 for you if approved and if they don't break API.

@riv-tnoble
Copy link
Copy Markdown
Contributor

riv-tnoble commented Nov 14, 2024 •
edited
Loading

I'm a little confused by the test failures:

    [unittest_trajectory_functions-1] The given transform is not an isometry! Its linear part involves non-unit scaling. The scaling matrix diagonal differs from [1 1 1] by [-1 -1 -1] but the required precision is 1e-12.
    [unittest_trajectory_functions-1] unittest_trajectory_functions: /home/runner/work/moveit2/moveit2/.work/target_ws/src/moveit2/moveit_core/robot_state/src/attached_body.cpp:67: moveit::core::AttachedBody::AttachedBody(const moveit::core::LinkModel*, const string&, const Isometry3d&, const std::vector<std::shared_ptr<const shapes::Shape> >&, const vector_Isometry3d&, const std::set<std::__cxx11::basic_string<char> >&, const JointTrajectory&, const FixedTransformsMap&): Assertion `!"Invalid isometry transform"' failed.

and

"/home/runner/work/moveit2/moveit2/.work/target_ws/src/moveit2/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp", line 230, in make_unique<moveit::core::AttachedBody, const moveit::core::LinkModel*&, const std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&, const Eigen::Transform<double, 3, 1, 0>&, std::vector<std::shared_ptr<const shapes::Shape>, std::allocator<std::shared_ptr<const shapes::Shape> > >, std::vector<Eigen::Transform<double, 3, 1, 0>, Eigen::aligned_allocator<Eigen::Transform<double, 3, 1, 0> > >, std::set<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::less<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > >, trajectory_msgs::msg::JointTrajectory_<std::allocator<void> >, const std::map<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, Eigen::Transform<double, 3, 1, 0>, std::less<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >, Eigen::aligned_allocator<std::pair<const std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, Eigen::Transform<double, 3, 1, 0> > > >&>
    [unittest_trajectory_functions-1]     |   228:                                                const moveit::core::FixedTransformsMap& subframes)

seem to suggest it's an issue building collision objects... will investigate.

@riv-tnoble
Copy link
Copy Markdown
Contributor

riv-tnoble commented Nov 14, 2024 

The given transform is not an isometry!

This line in particular is a bit strange, as the only transforms we're passing to objects & subframes are identities, or isometries with an applied translation and rotation.

@riv-tnoble
Copy link
Copy Markdown
Contributor

riv-tnoble commented Nov 14, 2024 

  state.attachBody(std::make_unique<moveit::core::AttachedBody>(
      link, object_name, object_pose, std::vector<shapes::ShapeConstPtr>{}, EigenSTL::vector_Isometry3d{},
      std::set<std::string>{}, trajectory_msgs::msg::JointTrajectory{}, subframes));

maybe it's the empty shape isometry vector we're passing in?

@riv-tnoble
Copy link
Copy Markdown
Contributor

riv-tnoble commented Nov 14, 2024 •
edited
Loading

Weirdly the same test seems to be failing on the master branch.

    [unittest_trajectory_functions-1] [ RUN      ] TrajectoryFunctionsTestFlangeAndGripper.testIKRobotStateWithIdentityCollisionObject
    [unittest_trajectory_functions-1] [INFO] [1731505133.662894558] [unittest_trajectory_functions.moveit.ros.rdf_loader]: Loaded robot model in 0.00184161 seconds
    [unittest_trajectory_functions-1] [INFO] [1731505133.662935985] [unittest_trajectory_functions.moveit.core.robot_model]: Loading robot model 'prbt'...
    [unittest_trajectory_functions-1] [INFO] [1731505133.665731588] [ikfast]: Using link_prefix: 'prbt_'
    [unittest_trajectory_functions-1] The given transform is not an isometry! Its linear part involves non-unit scaling. The scaling matrix diagonal differs from [1 1 1] by [-1 -1 -1] but the required precision is 1e-12.
    [unittest_trajectory_functions-1] unittest_trajectory_functions: /home/runner/work/moveit2/moveit2/.work/target_ws/src/moveit2/moveit_core/robot_state/src/attached_body.cpp:67: moveit::core::AttachedBody::AttachedBody(const moveit::core::LinkModel*, const std::string&, const Eigen::Isometry3d&, const std::vector<std::shared_ptr<const shapes::Shape> >&, const EigenSTL::vector_Isometry3d&, const std::set<std::__cxx11::basic_string<char> >&, const trajectory_msgs::msg::JointTrajectory&, const moveit::core::FixedTransformsMap&): Assertion `!"Invalid isometry transform"' failed.
Error: ROR] [unittest_trajectory_functions-1]: process has died [pid 37461, exit code -6, cmd '/home/runner/work/moveit2/moveit2/.work/target_ws/install/pilz_industrial_motion_planner/lib/pilz_industrial_motion_planner/unittest_trajectory_functions --ros-args -r __node:=unittest_trajectory_functions --params-file /tmp/launch_params_5hcz6wul --params-file /tmp/launch_params_j8xlj9p_'].

I'll take a look into this tonight

@riv-tnoble
Copy link
Copy Markdown
Contributor

riv-tnoble commented Nov 15, 2024 •
edited
Loading

@sea-bass I tried reproducing the issues locally last night, but was unable to. A few observations:

If you get time, would you be able to run the pilz tests locally and see if you can reproduce the issue? No issues if not. Just want to check that my sanity is still intact 😛

@riv-tnoble
Copy link
Copy Markdown
Contributor

riv-tnoble commented Nov 15, 2024

@sea-bass Tests should fixed now 🤞

Shoutout to @rr-mark for finding the issue in this line of code:

attachToLink(state, tip_link, "object", object_pose_in_tip, { {} });

Specifically, I was attempting to pass in an empty map of subframes, but was instead passing in a map containing one empty item. I've changed the non-subframe tests to pass in a subframe called "ignored" for parity with the other tests.

This will also need applying to rolling, so I'll raise a corresponding PR if these tests pass.

@riv-tnoble
Copy link
Copy Markdown
Contributor

riv-tnoble commented Nov 15, 2024

@sjahr @sea-bass Success 🥳

Could I get a review when either of you are free?

sjahr
sjahr approved these changes Nov 15, 2024
View reviewed changes
Copy link
Copy Markdown
Contributor

@sjahr sjahr left a comment

Choose a reason for hiding this comment

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

Puh, nice work and thanks for your perseverance

@sjahr sjahr merged commit f07f427 into moveit:mergify/bp/humble/pr-3077 Nov 15, 2024
@TSNoble TSNoble deleted the mergify/bp/humble/pr-3077 branch November 15, 2024 19:07
sea-bass added a commit that referenced this pull request Nov 16, 2024
@mergify @TSNoble
@riv-tnoble @sea-bass
Allow RobotState::setFromIK to work with subframes (backport #3077) (#…
8c5cc4a 
…3085)

* Allow RobotState::setFromIK to work with subframes (#3077)

* Adds regression tests for setFromIK with objects. Adds failing tests demonstrating failure with subframes

* Modifies RobotState::setFromIK to account for subframes

* Fixes formatting

* Fixes formatting

* Fixes formatting

* Applies PR suggestions

* Applies PR comments

---------

Co-authored-by: Tom Noble <tom@rivelinrobotics.com>
Co-authored-by: Sebastian Jahr <sebastian.jahr@picknik.ai>
(cherry picked from commit ab34495)

# Conflicts:
#	moveit_core/robot_state/src/robot_state.cpp

* Fixes merge conflicts (humble backport 3077) (#3087)

Co-authored-by: Tom Noble <tom@rivelinrobotics.com>

---------

Co-authored-by: Tom Noble <53005340+TSNoble@users.noreply.github.com>
Co-authored-by: Tom Noble <tom@rivelinrobotics.com>
Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Reviewers

@sjahr sjahr sjahr approved these changes

Assignees

No one assigned

Labels

None yet

Projects

None yet

Milestone

No milestone

Development

Successfully merging this pull request may close these issues.

3 participants

@TSNoble @riv-tnoble @sjahr