From 3d3d1b7eadf00b98fc6fee962ba0bf371d631107 Mon Sep 17 00:00:00 2001
From: Sebastian Jahr <sebastian.jahr@picknik.ai>
Date: Mon, 12 Aug 2024 14:27:42 +0200
Subject: [PATCH] Revert unnecessary changes

---
 .../src/execute_task_solution_capability.cpp  |  1 +
 core/src/stages/compute_ik.cpp                | 36 +++++++++----------
 2 files changed, 18 insertions(+), 19 deletions(-)

diff --git a/capabilities/src/execute_task_solution_capability.cpp b/capabilities/src/execute_task_solution_capability.cpp
index f604f3c95..ea0583636 100644
--- a/capabilities/src/execute_task_solution_capability.cpp
+++ b/capabilities/src/execute_task_solution_capability.cpp
@@ -73,6 +73,7 @@ const moveit::core::JointModelGroup* findJointModelGroup(const moveit::core::Rob
 				return jmg;
 		}
 	}
+
 	return nullptr;
 }
 }  // namespace
diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp
index 12437c7e2..1d192b3f5 100644
--- a/core/src/stages/compute_ik.cpp
+++ b/core/src/stages/compute_ik.cpp
@@ -341,25 +341,23 @@ void ComputeIK::compute() {
 		marker.color.a *= 0.5;
 		eef_markers.push_back(marker);
 	};
-	auto const& connected_parent_link_model = moveit::core::RobotModel::getRigidlyConnectedParentLinkModel(link);
-	if (connected_parent_link_model && connected_parent_link_model->getParentJointModel()) {
-		const auto& links_to_visualize = connected_parent_link_model->getParentJointModel()->getDescendantLinkModels();
-		if (colliding) {
-			SubTrajectory solution;
-			std::copy(frame_markers.begin(), frame_markers.end(), std::back_inserter(solution.markers()));
-			generateCollisionMarkers(sandbox_state, appender, links_to_visualize);
-			std::copy(eef_markers.begin(), eef_markers.end(), std::back_inserter(solution.markers()));
-			solution.markAsFailure();
-			// TODO: visualize collisions
-			solution.setComment(s.comment() + " eef in collision: " + listCollisionPairs(collisions.contacts, ", "));
-			auto colliding_scene{ scene->diff() };
-			colliding_scene->setCurrentState(sandbox_state);
-			spawn(InterfaceState(colliding_scene), std::move(solution));
-			return;
-		} else {
-			generateVisualMarkers(sandbox_state, appender, links_to_visualize);
-		}
-	}
+	const auto& links_to_visualize = moveit::core::RobotModel::getRigidlyConnectedParentLinkModel(link)
+	                                     ->getParentJointModel()
+	                                     ->getDescendantLinkModels();
+	if (colliding) {
+		SubTrajectory solution;
+		std::copy(frame_markers.begin(), frame_markers.end(), std::back_inserter(solution.markers()));
+		generateCollisionMarkers(sandbox_state, appender, links_to_visualize);
+		std::copy(eef_markers.begin(), eef_markers.end(), std::back_inserter(solution.markers()));
+		solution.markAsFailure();
+		// TODO: visualize collisions
+		solution.setComment(s.comment() + " eef in collision: " + listCollisionPairs(collisions.contacts, ", "));
+		auto colliding_scene{ scene->diff() };
+		colliding_scene->setCurrentState(sandbox_state);
+		spawn(InterfaceState(colliding_scene), std::move(solution));
+		return;
+	} else
+		generateVisualMarkers(sandbox_state, appender, links_to_visualize);
 
 	// determine joint values of robot pose to compare IK solution with for costs
 	std::vector<double> compare_pose;