diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 946125bac..dcedfd460 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -316,6 +316,7 @@ void Stage::reset() { impl->next_starts_.reset(); // reset inherited properties impl->properties_.reset(); + impl->total_compute_time_ = std::chrono::duration::zero(); } void Stage::init(const moveit::core::RobotModelConstPtr& /* robot_model */) { diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index 23b7a9dc3..8dd66edd5 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -249,9 +249,9 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning // compute absolute transform for link linear = frame_pose.linear() * linear; angular = frame_pose.linear() * angular; - target_eigen = link_pose; + target_eigen = ik_pose_world; target_eigen.linear() = - target_eigen.linear() * Eigen::AngleAxisd(angular_norm, link_pose.linear().transpose() * angular); + target_eigen.linear() * Eigen::AngleAxisd(angular_norm, ik_pose_world.linear().transpose() * angular); target_eigen.translation() += linear; goto COMPUTE; } catch (const boost::bad_any_cast&) { /* continue with Vector */ @@ -276,7 +276,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning // compute absolute transform for link linear = frame_pose.linear() * linear; - target_eigen = link_pose; + target_eigen = ik_pose_world; target_eigen.translation() += linear; } catch (const boost::bad_any_cast&) { solution.markAsFailure(std::string("invalid direction type: ") + direction.type().name()); @@ -285,7 +285,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning COMPUTE: // transform target pose such that ik frame will reach there if link does - target_eigen = target_eigen * scene->getCurrentState().getGlobalLinkTransform(link).inverse() * ik_pose_world; + target_eigen = target_eigen * ik_pose_world.inverse() * scene->getCurrentState().getGlobalLinkTransform(link); success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, path_constraints); diff --git a/core/test/CMakeLists.txt b/core/test/CMakeLists.txt index 99eb6097b..297d50ce4 100644 --- a/core/test/CMakeLists.txt +++ b/core/test/CMakeLists.txt @@ -26,7 +26,7 @@ if (BUILD_TESTING) if(TEST_FILE) # Add launch test if .launch.py file was specified ament_add_gtest_executable(${PROJECT_NAME}-${TEST_NAME} ${SOURCES}) add_launch_test(${TEST_FILE} - ARGS "test_binary:=$") + TARGET ${TEST_NAME} ARGS "test_binary:=$") else() if("${TYPE}" STREQUAL "gtest") ament_add_gtest(${PROJECT_NAME}-${TEST_NAME} ${SOURCES}) @@ -55,6 +55,7 @@ if (BUILD_TESTING) mtc_add_gmock(test_interface_state.cpp) mtc_add_gtest(test_move_to.cpp move_to.launch.py) + mtc_add_gtest(test_move_relative.cpp move_to.launch.py) # building these integration tests works without moveit config packages ament_add_gtest_executable(pick_ur5 pick_ur5.cpp) diff --git a/core/test/test_move_relative.cpp b/core/test/test_move_relative.cpp new file mode 100644 index 000000000..4f1eeae57 --- /dev/null +++ b/core/test/test_move_relative.cpp @@ -0,0 +1,128 @@ +#include "models.h" + +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include + +using namespace moveit::task_constructor; +using namespace planning_scene; +using namespace moveit::core; + +constexpr double TAU{ 2 * M_PI }; +constexpr double EPS{ 1e-6 }; + +// provide a basic test fixture that prepares a Task +struct PandaMoveRelative : public testing::Test +{ + Task t; + stages::MoveRelative* move; + PlanningScenePtr scene; + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("panda_move_relative"); + + const JointModelGroup* group; + + PandaMoveRelative() { + t.loadRobotModel(node); + + group = t.getRobotModel()->getJointModelGroup("panda_arm"); + + scene = std::make_shared(t.getRobotModel()); + scene->getCurrentStateNonConst().setToDefaultValues(); + scene->getCurrentStateNonConst().setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"), "ready"); + t.add(std::make_unique("start", scene)); + + auto move_relative = std::make_unique("move", std::make_shared()); + move_relative->setGroup(group->getName()); + move = move_relative.get(); + t.add(std::move(move_relative)); + } +}; + +moveit_msgs::msg::AttachedCollisionObject createAttachedObject(const std::string& id) { + moveit_msgs::msg::AttachedCollisionObject aco; + aco.link_name = "panda_hand"; + aco.object.header.frame_id = aco.link_name; + aco.object.operation = aco.object.ADD; + aco.object.id = id; + aco.object.primitives.resize(1, [] { + shape_msgs::msg::SolidPrimitive p; + p.type = p.SPHERE; + p.dimensions.resize(1); + p.dimensions[p.SPHERE_RADIUS] = 0.01; + return p; + }()); + + geometry_msgs::msg::Pose p; + p.position.x = 0.1; + p.orientation.w = 1.0; +#if MOVEIT_HAS_OBJECT_POSE + aco.object.pose = p; +#else + aco.object.primitive_poses.resize(1, p); + aco.object.primitive_poses[0] = p; +#endif + return aco; +} + +inline auto position(const PlanningSceneConstPtr& scene, const std::string& frame) { + return scene->getFrameTransform(frame).translation(); +} + +TEST_F(PandaMoveRelative, cartesianRotateEEF) { + move->setDirection([] { + geometry_msgs::msg::TwistStamped twist; + twist.header.frame_id = "world"; + twist.twist.angular.z = TAU / 8.0; + return twist; + }()); + + ASSERT_TRUE(t.plan()) << "Failed to plan"; + + const auto& tip_name{ group->getOnlyOneEndEffectorTip()->getName() }; + const auto start_eef_position{ position(scene, tip_name) }; + const auto end_eef_position{ position(move->solutions().front()->end()->scene(), tip_name) }; + + EXPECT_TRUE(start_eef_position.isApprox(end_eef_position, EPS)) + << "Cartesian rotation unexpectedly changed position of '" << tip_name << "' (must only change orientation)\n" + << start_eef_position << "\nvs\n" + << end_eef_position; +} + +TEST_F(PandaMoveRelative, cartesianRotateAttachedIKFrame) { + const std::string ATTACHED_OBJECT{ "attached_object" }; + scene->processAttachedCollisionObjectMsg(createAttachedObject(ATTACHED_OBJECT)); + move->setIKFrame(ATTACHED_OBJECT); + + move->setDirection([] { + geometry_msgs::msg::TwistStamped twist; + twist.header.frame_id = "world"; + twist.twist.angular.z = TAU / 8.0; + return twist; + }()); + + ASSERT_TRUE(t.plan()); + + const auto start_eef_position{ position(scene, ATTACHED_OBJECT) }; + const auto end_eef_position{ position(move->solutions().front()->end()->scene(), ATTACHED_OBJECT) }; + + EXPECT_TRUE(start_eef_position.isApprox(end_eef_position, EPS)) + << "Cartesian rotation unexpectedly changed position of ik frame (must only change orientation)\n" + << start_eef_position << "\nvs\n" + << end_eef_position; +} + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + + return RUN_ALL_TESTS(); +}