Validate connect stage deviation: end trajectory with goal

If not within the threshold, mark solution as a failure and set comment.

Author: captain-yoshi

core/src/stages/connect.cpp

@@ -43,6 +43,8 @@
 #include <moveit/planning_scene/planning_scene.h>
 #include <moveit/trajectory_processing/time_optimal_trajectory_generation.h>
 
+#include <angles/angles.h>
+
 using namespace trajectory_processing;
 
 namespace moveit {
@@ -146,6 +148,8 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
 	intermediate_scenes.push_back(start);
 
 	bool success = false;
+	bool deviation = false;
+	std::stringstream deviation_comment;
 	std::vector<double> positions;
 	for (const GroupPlannerVector::value_type& pair : planner_) {
 		// set intermediate goal state
@@ -164,6 +168,45 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
 		if (!success)
 			break;
 
+		// validate position deviation from trajectory last waypoint to goal
+		const std::size_t waypoint_count = trajectory->getWayPointCount();
+
+		if (waypoint_count) {
+			for (const moveit::core::JointModel* jm : jmg->getJointModels()) {
+				const unsigned int num = jm->getVariableCount();
+
+				if (!num)
+					continue;
+
+				Eigen::Map<const Eigen::VectorXd> positions_goal(goal_state.getJointPositions(jm), num);
+				Eigen::Map<const Eigen::VectorXd> positions_last_waypoint(
+				    trajectory->getWayPoint(waypoint_count - 1).getJointPositions(jm), num);
+
+				for (std::size_t i = 0; i < num; ++i) {
+					double min_distance = angles::shortest_angular_distance(positions_last_waypoint[i], positions_goal[i]);
+					ROS_DEBUG_STREAM_NAMED(
+					    "Connect", "angular deviation: " << min_distance << " between trajectory last waypoint and goal.");
+
+					if (std::abs(min_distance) > 1e-4) {
+						deviation_comment << "Deviation in joint " << jm->getName() << ": ["
+						                  << positions_last_waypoint.transpose() << "] != [" << positions_goal.transpose()
+						                  << "]";
+						ROS_ERROR_STREAM_NAMED("Connect", deviation_comment.str());
+
+						deviation = true;
+						success = false;
+						break;
+					}
+				}
+
+				if (deviation)
+					break;
+			}
+		}
+
+		if (deviation)
+			break;
+
 		// continue from reached state
 		start = end;
 	}
@@ -175,6 +218,8 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
 		solution = makeSequential(sub_trajectories, intermediate_scenes, from, to);
 	if (!success)  // error during sequential planning
 		solution->markAsFailure();
+	if (deviation)  // deviation error
+		solution->setComment(deviation_comment.str());
 	connect(from, to, solution);
 }