[ros2]: Sync with master branch

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<double>::zero();
 }
 
 void Stage::init(const moveit::core::RobotModelConstPtr& /* robot_model */) {

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);
 

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_FILE:${PROJECT_NAME}-${TEST_NAME}>")
+        TARGET ${TEST_NAME} ARGS "test_binary:=$<TARGET_FILE:${PROJECT_NAME}-${TEST_NAME}>")
 		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)

core/test/test_move_relative.cpp

@@ -0,0 +1,128 @@
+#include "models.h"
+
+#include <moveit/task_constructor/task.h>
+#include <moveit/task_constructor/stages/move_relative.h>
+#include <moveit/task_constructor/stages/fixed_state.h>
+#include <moveit/task_constructor/solvers/cartesian_path.h>
+#include <moveit/task_constructor/moveit_compat.h>
+
+#include <moveit/planning_scene/planning_scene.h>
+
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
+
+#include <gtest/gtest.h>
+
+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<PlanningScene>(t.getRobotModel());
+		scene->getCurrentStateNonConst().setToDefaultValues();
+		scene->getCurrentStateNonConst().setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"), "ready");
+		t.add(std::make_unique<stages::FixedState>("start", scene));
+
+		auto move_relative = std::make_unique<stages::MoveRelative>("move", std::make_shared<solvers::CartesianPath>());
+		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();
+}