Revert unnecessary changes

sjahr · Aug 12, 2024 · 3d3d1b7 · 3d3d1b7
1 parent fe953d0
commit 3d3d1b7
Show file tree

Hide file tree

Showing 2 changed files with 18 additions and 19 deletions.
diff --git 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
@@ -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;