diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index dbf5ab7f..f9b2825f 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -243,9 +243,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 */ @@ -269,7 +269,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()); @@ -278,7 +278,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 808e7b0d..3918b584 100644 --- a/core/test/CMakeLists.txt +++ b/core/test/CMakeLists.txt @@ -46,6 +46,7 @@ if (CATKIN_ENABLE_TESTING) mtc_add_gmock(test_interface_state.cpp) mtc_add_gtest(test_move_to.cpp move_to.test) + mtc_add_gtest(test_move_relative.cpp move_relative.test) # building these integration tests works without moveit config packages add_executable(pick_ur5 pick_ur5.cpp) diff --git a/core/test/move_relative.test b/core/test/move_relative.test new file mode 100644 index 00000000..6dbb0446 --- /dev/null +++ b/core/test/move_relative.test @@ -0,0 +1,7 @@ + + + + + + + diff --git a/core/test/test_move_relative.cpp b/core/test/test_move_relative.cpp new file mode 100644 index 00000000..8b59074a --- /dev/null +++ b/core/test/test_move_relative.cpp @@ -0,0 +1,129 @@ +#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; + + const JointModelGroup* group; + + PandaMoveRelative() { + t.setRobotModel(loadModel()); + + 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::AttachedCollisionObject createAttachedObject(const std::string& id) { + moveit_msgs::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::SolidPrimitive p; + p.type = p.SPHERE; + p.dimensions.resize(1); + p.dimensions[p.SPHERE_RADIUS] = 0.01; + return p; + }()); + + geometry_msgs::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::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::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); + ros::init(argc, argv, "move_relative_test"); + ros::AsyncSpinner spinner(1); + spinner.start(); + + return RUN_ALL_TESTS(); +}