CartesianPath: allow ik_frame definition

... if start and end are given as joint-space poses

Author: rhaschke

core/include/moveit/task_constructor/solvers/cartesian_path.h

@@ -52,6 +52,10 @@ class CartesianPath : public PlannerInterface
 public:
 	CartesianPath();
 
+	void setIKFrame(const geometry_msgs::PoseStamped& pose) { setProperty("ik_frame", pose); }
+	void setIKFrame(const Eigen::Isometry3d& pose, const std::string& link);
+	void setIKFrame(const std::string& link) { setIKFrame(Eigen::Isometry3d::Identity(), link); }
+
 	void setStepSize(double step_size) { setProperty("step_size", step_size); }
 	void setJumpThreshold(double jump_threshold) { setProperty("jump_threshold", jump_threshold); }
 	void setMinFraction(double min_fraction) { setProperty("min_fraction", min_fraction); }

core/src/solvers/cartesian_path.cpp

@@ -37,10 +37,12 @@
 */
 
 #include <moveit/task_constructor/solvers/cartesian_path.h>
+#include <moveit/task_constructor/utils.h>
 #include <moveit/planning_scene/planning_scene.h>
 #include <moveit/trajectory_processing/time_parameterization.h>
 #include <moveit/kinematics_base/kinematics_base.h>
 #include <moveit/robot_state/cartesian_interpolator.h>
+#include <tf2_eigen/tf2_eigen.h>
 
 using namespace trajectory_processing;
 
@@ -50,6 +52,7 @@ namespace solvers {
 
 CartesianPath::CartesianPath() {
 	auto& p = properties();
+	p.declare<geometry_msgs::PoseStamped>("ik_frame", "frame to move linearly (use for joint-space target)");
 	p.declare<double>("step_size", 0.01, "step size between consecutive waypoints");
 	p.declare<double>("jump_threshold", 1.5, "acceptable fraction of mean joint motion per step");
 	p.declare<double>("min_fraction", 1.0, "fraction of motion required for success");
@@ -59,18 +62,31 @@ CartesianPath::CartesianPath() {
 
 void CartesianPath::init(const core::RobotModelConstPtr& /*robot_model*/) {}
 
+void CartesianPath::setIKFrame(const Eigen::Isometry3d& pose, const std::string& link) {
+	geometry_msgs::PoseStamped pose_msg;
+	pose_msg.header.frame_id = link;
+	pose_msg.pose = tf2::toMsg(pose);
+	setIKFrame(pose_msg);
+}
+
 PlannerInterface::Result CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from,
                                              const planning_scene::PlanningSceneConstPtr& to,
                                              const moveit::core::JointModelGroup* jmg, double timeout,
                                              robot_trajectory::RobotTrajectoryPtr& result,
                                              const moveit_msgs::Constraints& path_constraints) {
-	const moveit::core::LinkModel* link = jmg->getOnlyOneEndEffectorTip();
-	if (!link)
-		return { false, "no unique tip for joint model group: " + jmg->getName() };
+	const auto& props = properties();
+	const moveit::core::LinkModel* link;
+	std::string error_msg;
+	Eigen::Isometry3d ik_pose_world;
+
+	if (!utils::getRobotTipForFrame(props.property("ik_frame"), *from, jmg, error_msg, link, ik_pose_world))
+		return { false, error_msg };
+
+	Eigen::Isometry3d offset = from->getCurrentState().getGlobalLinkTransform(link).inverse() * ik_pose_world;
 
 	// reach pose of forward kinematics
-	return plan(from, *link, Eigen::Isometry3d::Identity(), to->getCurrentState().getGlobalLinkTransform(link), jmg,
-	            std::min(timeout, properties().get<double>("timeout")), result, path_constraints);
+	return plan(from, *link, offset, to->getCurrentState().getGlobalLinkTransform(link), jmg,
+	            std::min(timeout, props.get<double>("timeout")), result, path_constraints);
 }
 
 PlannerInterface::Result CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from,