InterpolationPlanner: implement simple IK-based solver for pose targets

Author: v4hn

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/test/test_move_to.cpp

@@ -70,7 +70,7 @@ TEST_F(PandaMoveTo, stateTarget) {
 	EXPECT_ONE_SOLUTION;
 }
 
-TEST_F(PandaMoveTo, DISABLED_pointTarget) {
+TEST_F(PandaMoveTo, pointTarget) {
 	move_to->setGoal([this]() {
 		RobotState state{ scene->getCurrentState() };
 		state.setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"), "extended");
@@ -83,7 +83,7 @@ TEST_F(PandaMoveTo, DISABLED_pointTarget) {
 	EXPECT_ONE_SOLUTION;
 }
 
-TEST_F(PandaMoveTo, DISABLED_poseTarget) {
+TEST_F(PandaMoveTo, poseTarget) {
 	move_to->setGoal([this]() {
 		RobotState state{ scene->getCurrentState() };
 		state.setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"), "extended");