Merge pull request #304 from v4hn/pr-move-to-tests

Add MoveTo tests & make them pass

Author: v4hn

core/include/moveit/task_constructor/moveit_compat.h

@@ -55,3 +55,5 @@
 
 // use object shape poses relative to a single object pose
 #define MOVEIT_HAS_OBJECT_POSE MOVEIT_VERSION_GE(1, 1, 6)
+
+#define MOVEIT_HAS_STATE_RIGID_PARENT_LINK MOVEIT_VERSION_GE(1, 1, 6)

core/include/moveit/task_constructor/stage.h

@@ -84,7 +84,7 @@ enum InterfaceFlag
 	GENERATE = WRITES_PREV_END | WRITES_NEXT_START,
 };
 
-using InterfaceFlags = Flags<InterfaceFlag>;
+using InterfaceFlags = utils::Flags<InterfaceFlag>;
 
 /** invert interface such that
  * - new end can connect to old start

core/include/moveit/task_constructor/stages/fixed_state.h

@@ -48,7 +48,7 @@ namespace stages {
 class FixedState : public Generator
 {
 public:
-	FixedState(const std::string& name = "initial state");
+	FixedState(const std::string& name = "initial state", planning_scene::PlanningScenePtr = nullptr);
 	void setState(const planning_scene::PlanningScenePtr& scene);
 
 	void reset() override;

core/include/moveit/task_constructor/stages/move_to.h

@@ -99,7 +99,7 @@ class MoveTo : public PropagatingEitherWay
 	bool getJointStateGoal(const boost::any& goal, const core::JointModelGroup* jmg, moveit::core::RobotState& state);
 	bool getPoseGoal(const boost::any& goal, const planning_scene::PlanningScenePtr& scene,
 	                 Eigen::Isometry3d& target_eigen);
-	bool getPointGoal(const boost::any& goal, const moveit::core::LinkModel* link,
+	bool getPointGoal(const boost::any& goal, const Eigen::Isometry3d& ik_pose,
 	                  const planning_scene::PlanningScenePtr& scene, Eigen::Isometry3d& target_eigen);
 
 protected:

core/include/moveit/task_constructor/utils.h

@@ -32,15 +32,38 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  *********************************************************************/
 
-/* Authors: Robert Haschke
-   Desc:    Project-agnostic utility classes
+/* Authors: Robert Haschke, Michael 'v4hn' Goerner
+   Desc:    Miscellaneous utilities
 */
 
 #pragma once
 
+#include <string>
 #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 {
+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 {
+
 /** template class to compose flags from enums in a type-safe fashion */
 template <typename Enum>
 class Flags
@@ -118,3 +141,13 @@ class Flags
 };
 
 #define DECLARE_FLAGS(Flags, Enum) using Flags = QFlags<Enum>;
+
+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

core/src/CMakeLists.txt

@@ -5,6 +5,7 @@ add_library(${PROJECT_NAME}
 	${PROJECT_INCLUDE}/introspection.h
 	${PROJECT_INCLUDE}/marker_tools.h
 	${PROJECT_INCLUDE}/merge.h
+	${PROJECT_INCLUDE}/moveit_compat.h
 	${PROJECT_INCLUDE}/properties.h
 	${PROJECT_INCLUDE}/stage.h
 	${PROJECT_INCLUDE}/stage_p.h
@@ -27,6 +28,7 @@ add_library(${PROJECT_NAME}
 	stage.cpp
 	storage.cpp
 	task.cpp
+	utils.cpp
 
 	solvers/planner_interface.cpp
 	solvers/cartesian_path.cpp

core/src/solvers/joint_interpolation.cpp

@@ -40,6 +40,8 @@
 #include <moveit/planning_scene/planning_scene.h>
 #include <moveit/trajectory_processing/iterative_time_parameterization.h>
 
+#include <chrono>
+
 namespace moveit {
 namespace task_constructor {
 namespace solvers {
@@ -53,9 +55,9 @@ void JointInterpolationPlanner::init(const core::RobotModelConstPtr& /*robot_mod
 
 bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
                                      const planning_scene::PlanningSceneConstPtr& to,
-                                     const moveit::core::JointModelGroup* jmg, double timeout,
+                                     const moveit::core::JointModelGroup* jmg, double /*timeout*/,
                                      robot_trajectory::RobotTrajectoryPtr& result,
-                                     const moveit_msgs::Constraints& path_constraints) {
+                                     const moveit_msgs::Constraints& /*path_constraints*/) {
 	const auto& props = properties();
 
 	// Get maximum joint distance
@@ -95,12 +97,38 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
 	return true;
 }
 
-bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& /*from*/,
-                                     const moveit::core::LinkModel& /*link*/, const Eigen::Isometry3d& /*target_eigen*/,
-                                     const moveit::core::JointModelGroup* /*jmg*/, double /*timeout*/,
-                                     robot_trajectory::RobotTrajectoryPtr& /*result*/,
-                                     const moveit_msgs::Constraints& /*path_constraints*/) {
-	throw std::runtime_error("Not yet implemented");
+bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
+                                     const moveit::core::LinkModel& link, const Eigen::Isometry3d& target_eigen,
+                                     const moveit::core::JointModelGroup* jmg, double timeout,
+                                     robot_trajectory::RobotTrajectoryPtr& result,
+                                     const moveit_msgs::Constraints& path_constraints) {
+	const auto start_time = std::chrono::steady_clock::now();
+
+	auto to{ from->diff() };
+
+	kinematic_constraints::KinematicConstraintSet constraints{ to->getRobotModel() };
+	constraints.add(path_constraints, from->getTransforms());
+
+	auto is_valid{ [&constraints, &to](moveit::core::RobotState* robot_state, const moveit::core::JointModelGroup* jmg,
+		                                const double* joint_values) -> bool {
+		robot_state->setJointGroupPositions(jmg, joint_values);
+		robot_state->update();
+		return to->isStateValid(*robot_state, constraints, jmg->getName());
+	} };
+
+	if (!to->getCurrentStateNonConst().setFromIK(jmg, target_eigen, link.getName(), timeout, is_valid)) {
+		// TODO(v4hn): planners need a way to add feedback to failing plans
+		// in case of an invalid solution feedback should include unwanted collisions or violated constraints
+		ROS_WARN_NAMED("JointInterpolationPlanner", "IK failed for pose target");
+		return false;
+	}
+	to->getCurrentStateNonConst().update();
+
+	timeout = std::chrono::duration<double>(std::chrono::steady_clock::now() - start_time).count();
+	if (timeout <= 0.0)
+		return false;
+
+	return plan(from, to, jmg, timeout, result, path_constraints);
 }
 }  // namespace solvers
 }  // namespace task_constructor

core/src/stages/compute_ik.cpp

@@ -287,31 +287,17 @@ void ComputeIK::compute() {
 		ik_pose_msg = boost::any_cast<geometry_msgs::PoseStamped>(value);
 		Eigen::Isometry3d ik_pose;
 		tf2::fromMsg(ik_pose_msg.pose, ik_pose);
-		if (robot_model->hasLinkModel(ik_pose_msg.header.frame_id)) {
-			link = robot_model->getLinkModel(ik_pose_msg.header.frame_id);
-		} else {
-			const robot_state::AttachedBody* attached =
-			    scene->getCurrentState().getAttachedBody(ik_pose_msg.header.frame_id);
-			if (!attached) {
-				ROS_WARN_STREAM_NAMED("ComputeIK", "Unknown frame: " << ik_pose_msg.header.frame_id);
-				return;
-			}
-			const EigenSTL::vector_Isometry3d& tf =
-#if MOVEIT_HAS_OBJECT_POSE
-			    attached->getShapePosesInLinkFrame();
-#else
-			    attached->getFixedTransforms();
-#endif
-			if (tf.empty()) {
-				ROS_WARN_STREAM_NAMED("ComputeIK", "Attached body doesn't have shapes.");
-				return;
-			}
-			// prepend link
-			link = attached->getAttachedLink();
-			ik_pose = tf[0] * ik_pose;
+
+		if (!scene->getCurrentState().knowsFrameTransform(ik_pose_msg.header.frame_id)) {
+			ROS_WARN_STREAM_NAMED("ComputeIK", "ik frame unknown in robot: '" << ik_pose_msg.header.frame_id << "'");
+			return;
 		}
+		ik_pose = scene->getCurrentState().getFrameTransform(ik_pose_msg.header.frame_id) * ik_pose;
+
+		link = utils::getRigidlyConnectedParentLinkModel(scene->getCurrentState(), ik_pose_msg.header.frame_id);
+
 		// transform target pose such that ik frame will reach there if link does
-		target_pose = target_pose * ik_pose.inverse();
+		target_pose = target_pose * ik_pose.inverse() * scene->getCurrentState().getFrameTransform(link->getName());
 	}
 
 	// validate placed link for collisions
@@ -323,7 +309,7 @@ void ComputeIK::compute() {
 	// markers used for failures
 	std::deque<visualization_msgs::Marker> failure_markers;
 	// frames at target pose and ik frame
-	rviz_marker_tools::appendFrame(failure_markers, target_pose_msg, 0.1, "ik frame");
+	rviz_marker_tools::appendFrame(failure_markers, target_pose_msg, 0.1, "target frame");
 	rviz_marker_tools::appendFrame(failure_markers, ik_pose_msg, 0.1, "ik frame");
 	// visualize placed end-effector
 	auto appender = [&failure_markers](visualization_msgs::Marker& marker, const std::string& /*name*/) {

core/src/stages/fixed_state.cpp

@@ -43,7 +43,8 @@ namespace moveit {
 namespace task_constructor {
 namespace stages {
 
-FixedState::FixedState(const std::string& name) : Generator(name) {
+FixedState::FixedState(const std::string& name, planning_scene::PlanningScenePtr scene)
+  : Generator(name), scene_(scene) {
 	setCostTerm(std::make_unique<cost::Constant>(0.0));
 }
 

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