Needed to make source code modifications for STOMP pipeline planner support

[timling96]

Description of Issue

When integrating the stomp motion plan capabilities into the MTC framework using the pipeline_planner, the planning would result in the following warings and errors.
[ WARN] [1628763414.607187821]: JointLimits/panda_arm Requested Start State is out of bounds
[ERROR] [1628763420.218975661]: STOMP failed to find a valid solution after 41 iterations

I managed to work around this problem, by making some changes to the "pipeline_planner.cpp" file. (changes made are described below)
Is there a more elegant solution to this issue, without changing the source code of the library?

System Configuration

Ubuntu 20.04.2 LTS
ROS distro: noetic
ROS version: 1

Component Version

MoveIt version: master branch, built from source (https://github.com/ros-planning/moveit.git)
MTC: master branch, built from source (https://github.com/ros-planning/moveit_task_constructor.git)
stomp_ros: melodic-devel branch, built from source (https://github.com/ros-industrial/stomp_ros.git)
panda_moveit_config: melodic-devel branch (https://github.com/ros-planning/panda_moveit_config.git)

Steps to Reproduce the Issue

1. Install dependencies
2. Create simple cpp motion planning task using the stomp pipeline planner

Solution

(Necessary changes made to moveit_task_constructor/core/src/solvers/pipeline_planner.cpp)

1. Include moveit/robot_state/conversions.h
2. Copy the current state information from the "from" planning scene (from->CurrentState()) into the motion planning request start_state

#include <moveit/task_constructor/solvers/pipeline_planner.h>
#include <moveit/task_constructor/task.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/planning_pipeline/planning_pipeline.h>
#include <moveit_msgs/MotionPlanRequest.h>
#include <moveit/kinematic_constraints/utils.h>
#include <eigen_conversions/eigen_msg.h>

--> #include <moveit/robot_state/conversions.h>

...
bool PipelinePlanner::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 auto& props = properties();
	moveit_msgs::MotionPlanRequest req;
	initMotionPlanRequest(req, props, jmg, timeout);

	req.goal_constraints.resize(1);
	req.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(to->getCurrentState(), jmg,
	                                                                          props.get<double>("goal_joint_tolerance"));
	req.path_constraints = path_constraints;
	--> if (planner_->getPlannerPluginName() == "stomp_moveit/StompPlannerManager")
	-->    moveit::core::robotStateToRobotStateMsg(from->getCurrentState(), req.start_state);
	::planning_interface::MotionPlanResponse res;
	bool success = planner_->generatePlan(from, req, res);
	result = res.trajectory_;
	return success;
}
...

[v4hn]

This is a known bug in the stomp_ros package with a trivial PR to fix this open for over a year now.

What's the situation there @gavanderhoorn ?