From 91ff3d24872b483c887c1483f1fc67f0acc44589 Mon Sep 17 00:00:00 2001 From: Tom Noble Date: Fri, 15 Nov 2024 12:15:42 +0000 Subject: [PATCH] Fixes pilz tests (#3095) --- .../unit_tests/src/unittest_trajectory_functions.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp index a0383aaa24..66cee20108 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp @@ -415,9 +415,10 @@ TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIKRobotStateWithIdentityColl const moveit::core::LinkModel* tip_link = robot_model_->getLinkModel(tcp_link_); Eigen::Isometry3d tip_pose_in_base = state.getFrameTransform(tcp_link_); - // Attach an object with no subframes, and no transform + // Attach an object with ignored subframes, and no transform Eigen::Isometry3d object_pose_in_tip = Eigen::Isometry3d::Identity(); - attachToLink(state, tip_link, "object", object_pose_in_tip, { {} }); + moveit::core::FixedTransformsMap subframes({ { "ignored", Eigen::Isometry3d::Identity() } }); + attachToLink(state, tip_link, "object", object_pose_in_tip, subframes); // The RobotState should be able to use an object pose to set the joints Eigen::Isometry3d object_pose_in_base = tip_pose_in_base * object_pose_in_tip; @@ -440,12 +441,12 @@ TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIKRobotStateWithTransformedC const moveit::core::LinkModel* tip_link = robot_model_->getLinkModel(tcp_link_); Eigen::Isometry3d tip_pose_in_base = state.getFrameTransform(tcp_link_); - // Attach an object with no subframes, and a non-trivial transform + // Attach an object with ignored subframes, and a non-trivial transform Eigen::Isometry3d object_pose_in_tip; object_pose_in_tip = Eigen::Translation3d(1, 2, 3); object_pose_in_tip *= Eigen::AngleAxis(M_PI_2, Eigen::Vector3d::UnitX()); - - attachToLink(state, tip_link, "object", object_pose_in_tip, { {} }); + moveit::core::FixedTransformsMap subframes({ { "ignored", Eigen::Isometry3d::Identity() } }); + attachToLink(state, tip_link, "object", object_pose_in_tip, subframes); // The RobotState should be able to use an object pose to set the joints Eigen::Isometry3d object_pose_in_base = tip_pose_in_base * object_pose_in_tip;