From 9cdde7dcf47bb66d3efcb3e00ec0be8fa84a8832 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 11 May 2023 11:05:36 +0200 Subject: [PATCH] TMP: Debugging prints --- moveit_core/cost_functions/src/cost_functions.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/moveit_core/cost_functions/src/cost_functions.cpp b/moveit_core/cost_functions/src/cost_functions.cpp index 557adb906c1..eecdb0d431c 100644 --- a/moveit_core/cost_functions/src/cost_functions.cpp +++ b/moveit_core/cost_functions/src/cost_functions.cpp @@ -45,14 +45,22 @@ namespace cost_functions // Create cost function return [](const moveit::core::RobotState& robot_state, const planning_interface::MotionPlanRequest& request, const planning_scene::PlanningSceneConstPtr& planning_scene) { + + std::vector joint_pos; + robot_state.copyJointGroupPositions(request.group_name, joint_pos); + for(auto const val : joint_pos){ + std::cout << "Joint pos: " << val << std::endl; + } auto const shortest_distance_to_collision = planning_scene->distanceToCollisionUnpadded(robot_state); // Return cost based on shortest_distance if the robot is not in contact or penetrated a collision object if (shortest_distance_to_collision > 0.0) { + std::cout << "Cost: " << 1.0/ shortest_distance_to_collision << std::endl; // The closer the collision object the higher the cost return 1.0 / shortest_distance_to_collision; } + std::cout << "Cost: " << std::numeric_limits::infinity() << std::endl; return std::numeric_limits::infinity(); // Return a max cost cost by default }; }