refactor logic to handle ik_frame

v4hn · v4hn · commit 3b835986e34e · 2021-11-12T14:02:46.000+01:00
fallbacks and verification.
diff --git a/core/include/moveit/task_constructor/utils.h b/core/include/moveit/task_constructor/utils.h
@@ -42,14 +42,25 @@
 #include <type_traits>
 #include <initializer_list>
 
+#include <Eigen/Geometry>
+
+#include <moveit/macros/class_forward.h>
+
+namespace planning_scene {
+MOVEIT_CLASS_FORWARD(PlanningScene);
+}
+
 namespace moveit {
 
 namespace core {
-class LinkModel;
-class RobotState;
+MOVEIT_CLASS_FORWARD(LinkModel);
+MOVEIT_CLASS_FORWARD(JointModelGroup);
+MOVEIT_CLASS_FORWARD(RobotState);
 }  // namespace core
 
 namespace task_constructor {
+MOVEIT_CLASS_FORWARD(Property);
+MOVEIT_CLASS_FORWARD(SolutionBase);
 
 namespace utils {
 
@@ -133,6 +144,10 @@ class Flags
 
 const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const moveit::core::RobotState& state,
                                                                   std::string frame);
+
+bool getRobotTipForFrame(const Property& property, const planning_scene::PlanningScene& scene,
+                         const moveit::core::JointModelGroup* jmg, SolutionBase& solution,
+                         const moveit::core::LinkModel*& robot_link, Eigen::Isometry3d& tip_in_global_frame);
 }  // namespace utils
 }  // namespace task_constructor
 }  // namespace moveit
diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp
@@ -193,24 +193,11 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
 		success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints);
 	} else {
 		// Cartesian targets require an IK reference frame
-		geometry_msgs::PoseStamped ik_pose_msg;
 		const moveit::core::LinkModel* link;
-		const boost::any& value = props.get("ik_frame");
-		if (value.empty()) {  // property undefined
-			//  determine IK link from group
-			if (!(link = jmg->getOnlyOneEndEffectorTip())) {
-				solution.markAsFailure("missing ik_frame");
-				return false;
-			}
-			ik_pose_msg.header.frame_id = link->getName();
-			ik_pose_msg.pose.orientation.w = 1.0;
-		} else {
-			ik_pose_msg = boost::any_cast<geometry_msgs::PoseStamped>(value);
-			if (!(link = robot_model->getLinkModel(ik_pose_msg.header.frame_id))) {
-				solution.markAsFailure("unknown link for ik_frame: " + ik_pose_msg.header.frame_id);
-				return false;
-			}
-		}
+		Eigen::Isometry3d ik_pose_world;
+
+		if (!utils::getRobotTipForFrame(props.property("ik_frame"), *scene, jmg, solution, link, ik_pose_world))
+			return false;
 
 		bool use_rotation_distance = false;  // measure achieved distance as rotation?
 		Eigen::Vector3d linear;  // linear translation
@@ -291,9 +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
-		Eigen::Isometry3d ik_pose;
-		tf2::fromMsg(ik_pose_msg.pose, ik_pose);
-		target_eigen = target_eigen * ik_pose.inverse();
+		target_eigen = target_eigen * scene->getCurrentState().getGlobalLinkTransform(link).inverse() * ik_pose_world;
 
 		success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, path_constraints);
 
diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp
@@ -208,44 +208,10 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
 		// What frame+offset in the robot should go there?
 		geometry_msgs::PoseStamped ik_pose_msg;
 
-		const boost::any& value = props.get("ik_frame");
-
-		auto get_tip{ [&jmg](std::string& out) {
-			// determine IK frame from group
-			std::vector<std::string> tips;
-			jmg->getEndEffectorTips(tips);
-			if (tips.size() != 1) {
-				return false;
-			}
-			out = tips[0];
-			return true;
-		} };
-
-		if (value.empty()) {  // property undefined
-			if (!get_tip(ik_pose_msg.header.frame_id)) {
-				solution.markAsFailure("missing ik_frame");
-				return false;
-			}
-			ik_pose_msg.pose.orientation.w = 1.0;
-		} else {
-			ik_pose_msg = boost::any_cast<geometry_msgs::PoseStamped>(value);
-			if (ik_pose_msg.header.frame_id.empty() && !get_tip(ik_pose_msg.header.frame_id)) {
-				solution.markAsFailure("frame_id of ik_frame is empty and no unique group tip was found");
-				return false;
-			} else if (!scene->knowsFrameTransform(ik_pose_msg.header.frame_id)) {
-				std::stringstream ss;
-				ss << "ik_frame specified in unknown frame '" << ik_pose_msg << "'";
-				solution.markAsFailure(ss.str());
-				return false;
-			}
-		}
-
-		Eigen::Isometry3d ik_pose_world{ [&]() {
-			Eigen::Isometry3d t;
-			tf2::fromMsg(ik_pose_msg.pose, t);
-			t = scene->getFrameTransform(ik_pose_msg.header.frame_id) * t;
-			return t;
-		}() };
+		const moveit::core::LinkModel* link;
+		Eigen::Isometry3d ik_pose_world;
+		if (!utils::getRobotTipForFrame(props.property("ik_frame"), *scene, jmg, solution, link, ik_pose_world))
+			return false;
 
 		if (!getPoseGoal(goal, scene, target) && !getPointGoal(goal, ik_pose_world, scene, target)) {
 			solution.markAsFailure(std::string("invalid goal type: ") + goal.type().name());
@@ -263,16 +229,11 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
 		add_frame(target, "target frame");
 		add_frame(ik_pose_world, "ik frame");
 
-		const moveit::core::LinkModel* parent{ utils::getRigidlyConnectedParentLinkModel(scene->getCurrentState(),
-			                                                                              ik_pose_msg.header.frame_id) };
-
 		// transform target pose such that ik frame will reach there if link does
-		Eigen::Isometry3d ik_pose;
-		tf2::fromMsg(ik_pose_msg.pose, ik_pose);
-		target = target * ik_pose_world.inverse() * scene->getFrameTransform(parent->getName());
+		target = target * ik_pose_world.inverse() * scene->getCurrentState().getGlobalLinkTransform(link);
 
 		// plan to Cartesian target
-		success = planner_->plan(state.scene(), *parent, target, jmg, timeout, robot_trajectory, path_constraints);
+		success = planner_->plan(state.scene(), *link, target, jmg, timeout, robot_trajectory, path_constraints);
 	}
 
 	// store result
diff --git a/core/src/utils.cpp b/core/src/utils.cpp
@@ -35,9 +35,14 @@
 
 /* Authors: Michael Goerner, Robert Haschke */
 
+#include <tf2_eigen/tf2_eigen.h>
+
 #include <moveit/robot_state/robot_state.h>
+#include <moveit/planning_scene/planning_scene.h>
 
 #include <moveit/task_constructor/moveit_compat.h>
+#include <moveit/task_constructor/properties.h>
+#include <moveit/task_constructor/storage.h>
 
 namespace moveit {
 namespace task_constructor {
@@ -59,6 +64,50 @@ const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const moveit::
 #endif
 }
 
+bool getRobotTipForFrame(const Property& property, const planning_scene::PlanningScene& scene,
+                         const moveit::core::JointModelGroup* jmg, SolutionBase& solution,
+                         const moveit::core::LinkModel*& robot_link, Eigen::Isometry3d& tip_in_global_frame) {
+	auto get_tip = [&jmg]() -> const moveit::core::LinkModel* {
+		// determine IK frame from group
+		std::vector<const moveit::core::LinkModel*> tips;
+		jmg->getEndEffectorTips(tips);
+		if (tips.size() != 1) {
+			return nullptr;
+		}
+		return tips[0];
+	};
+
+	if (property.value().empty()) {  // property undefined
+		robot_link = get_tip();
+		if (!robot_link) {
+			solution.markAsFailure("missing ik_frame");
+			return false;
+		}
+		tip_in_global_frame = scene.getCurrentState().getGlobalLinkTransform(robot_link);
+	} else {
+		auto ik_pose_msg = boost::any_cast<geometry_msgs::PoseStamped>(property.value());
+		if (ik_pose_msg.header.frame_id.empty()) {
+			if (!(robot_link = get_tip())) {
+				solution.markAsFailure("frame_id of ik_frame is empty and no unique group tip was found");
+				return false;
+			}
+			tf2::fromMsg(ik_pose_msg.pose, tip_in_global_frame);
+			tip_in_global_frame = scene.getCurrentState().getGlobalLinkTransform(robot_link) * tip_in_global_frame;
+		} else if (scene.knowsFrameTransform(ik_pose_msg.header.frame_id)) {
+			robot_link = getRigidlyConnectedParentLinkModel(scene.getCurrentState(), ik_pose_msg.header.frame_id);
+			tf2::fromMsg(ik_pose_msg.pose, tip_in_global_frame);
+			tip_in_global_frame = scene.getFrameTransform(ik_pose_msg.header.frame_id) * tip_in_global_frame;
+		} else {
+			std::stringstream ss;
+			ss << "ik_frame specified in unknown frame '" << ik_pose_msg << "'";
+			solution.markAsFailure(ss.str());
+			return false;
+		}
+	}
+
+	return true;
+}
+
 }  // namespace utils
 }  // namespace task_constructor
 }  // namespace moveit
