How to Obtain the "Distance" Information Between Joints?

[Jazzzzon]

When using MoveIt 2 for collision detection, I want to determine the distance between joints in a specific state (or the current state) or the minimum distance before collision occurs. According to the API, this information should be retrievable from CollisionResult. However, the actual returned value is 1.79769e+308 (i.e., std::numeric_limits::max()). I tried other interfaces but still received the same result.

Potential Issues I Suspect:
Am I using the wrong API?
Is there an initialization error or missing configuration steps?
Are there specific conditions required for distance calculation that I haven’t met?

Hear is my code:
`#include
#include
#include
#include

#include <rclcpp/rclcpp.hpp>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/kinematic_constraints/utils.h>
#include <moveit/moveit_cpp/moveit_cpp.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>
#include <moveit_msgs/msg/planning_scene.hpp>
#include <moveit/collision_detection_fcl/collision_common.h>
#include <moveit/collision_detection_fcl/collision_env_fcl.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/kinematic_constraints/utils.h>
#include <moveit/moveit_cpp/moveit_cpp.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>
#include <moveit/collision_detection/collision_detector_allocator.h>
#include <moveit/collision_detection_fcl/collision_common.h>
#include <moveit/collision_detection_fcl/collision_env_fcl.h>
#include <moveit/collision_detection_fcl/collision_detector_allocator_fcl.h>
#include <moveit/collision_detection_fcl/collision_detector_fcl_plugin_loader.h>
#include <moveit/collision_detection_fcl/fcl_compat.h>
#include <moveit/rdf_loader/rdf_loader.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit_visual_tools/moveit_visual_tools.h>

// #include <shape_msgs/msg/solid_primitive.hpp>

#define PI 3.1415926

using namespace std;

static const rclcpp::Logger LOGGER = rclcpp::get_logger("collision_node");

int main(int argc, char **argv)
{
// ROS2 init
rclcpp::init(argc, argv);
rclcpp::NodeOptions node_options;
node_options.automatically_declare_parameters_from_overrides(true);
auto collision_node = rclcpp::Node::make_shared("collision_node", node_options);

rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(collision_node);
std::thread(&executor
{ executor.spin(); })
.detach();
robot_model_loader::RobotModelLoader robot_model_loader(collision_node, "robot_description");
const moveit::core::RobotModelPtr &robot_model = robot_model_loader.getModel();
planning_scene::PlanningScene planning_scene(robot_model);
moveit::core::RobotState &current_state = planning_scene.getCurrentStateNonConst();

// ----- check SRDF ----- //
srdf::ModelConstSharedPtr model_srdf = robot_model->getSRDF();
RCLCPP_INFO_STREAM(LOGGER, "SRDF model load " << (model_srdf ? "SUCCESSED." : "FAILED!"));

// ----- check ACM ----- //
collision_detection::AllowedCollisionMatrix &acm = planning_scene.getAllowedCollisionMatrixNonConst();
RCLCPP_INFO_STREAM(LOGGER, "acm.Size = " << acm.getSize());
std::vectorstd::string allEntrynames;
acm.getAllEntryNames(allEntrynames);
for (auto name : allEntrynames)
RCLCPP_INFO_STREAM(LOGGER, "Entryname : " << name);
// acm.print(std::cout);

// ----- check disabled and enabled collision pairs ----- //
std::vectorsrdf::Model::CollisionPair disabled_collision_pairs = model_srdf->getDisabledCollisionPairs();
std::vectorsrdf::Model::CollisionPair enabled_collision_pairs = model_srdf->getEnabledCollisionPairs();
RCLCPP_INFO_STREAM(LOGGER, "enabled collision pair size: " << enabled_collision_pairs.size()); // empty
// for (auto it : enabled_collision_pairs) RCLCPP_INFO_STREAM(LOGGER, "enabled collision pair: (" << it.link1_ << " - " << it.link2_ << ")");
RCLCPP_INFO_STREAM(LOGGER, "disabled collision pair size: (" << disabled_collision_pairs.size());
for (auto it : disabled_collision_pairs)
RCLCPP_INFO_STREAM(LOGGER, "disabled collision pair: (" << it.link1_ << " - " << it.link2_ << ") | reason: " << it.reason_);

// ----- use CollisionEnvFCL -> distanceRobot() ----- //
{
RCLCPP_INFO_STREAM(LOGGER, "----- use CollisionEnvFCL -> distanceRobot() -----");
RCLCPP_INFO_STREAM(LOGGER, "std::numeric_limits::max = " << std::numeric_limits::max());

current_state.setToDefaultValues();
collision_detection::CollisionEnvFCL collision_env(robot_model, 0.0, 1.0);

collision_detection::DistanceRequest distance_request;
distance_request.enable_nearest_points = true;
distance_request.enable_signed_distance = true;
distance_request.type = collision_detection::DistanceRequestType::GLOBAL;
distance_request.distance_threshold = 10;

collision_detection::DistanceResult distance_result;
collision_detection::DistanceResultsData distance_res_data = distance_result.minimum_distance;
auto min_dis_link_names = distance_res_data.link_names;
double distance_res_data_min_distance = distance_res_data.distance;
collision_detection::DistanceMap distance_map = distance_result.distances;

distance_result.clear();
current_state.setToDefaultValues();
RCLCPP_INFO(LOGGER, "  Default Positions >>>");
collision_env.distanceRobot(distance_request, distance_result, current_state);
RCLCPP_INFO_STREAM(LOGGER, "\tMinimum distance between: " << min_dis_link_names[0] << " and " << min_dis_link_names[1]);
RCLCPP_INFO_STREAM(LOGGER, "\tMinimum distance is : " << distance_result.minimum_distance.distance);
RCLCPP_INFO_STREAM(LOGGER, "\tdistance_result.minimum_distance.distance : " << distance_res_data_min_distance);
RCLCPP_INFO_STREAM(LOGGER, "\tdistance_map is " << (distance_map.empty() ? "" : "not") << " empty");

distance_result.clear();
current_state.setToRandomPositions();
RCLCPP_INFO_STREAM(LOGGER, "  Random Positions >>>");
collision_env.distanceRobot(distance_request, distance_result, current_state);
RCLCPP_INFO_STREAM(LOGGER, "\tMinimum distance between: " << min_dis_link_names[0] << " and " << min_dis_link_names[1]); //both are empty
RCLCPP_INFO_STREAM(LOGGER, "\tMinimum distance is : " << distance_result.minimum_distance.distance); // 1.79769e+308
RCLCPP_INFO_STREAM(LOGGER, "\tdistance_result.minimum_distance.distance : " << distance_res_data_min_distance); // 1.79769e+308
RCLCPP_INFO_STREAM(LOGGER, "\tdistance_map is " << (distance_map.empty() ? "" : "not") << "empty"); // empty

RCLCPP_INFO_STREAM(LOGGER, "===== END : use CollisionEnvFCL -> distanceRobot() =====" << std::endl);

}

// ----- use PlanningScene -> checkSelfCollision() ----- //
{
RCLCPP_INFO_STREAM(LOGGER, "----- use PlanningScene -> checkSelfCollision() -----");
collision_detection::CollisionRequest collision_request;
collision_request.distance = true;

collision_detection::CollisionResult collision_result;
double distance = collision_result.distance;
collision_detection::DistanceResult distance_result = collision_result.distance_result;
collision_detection::DistanceResultsData distance_res_data = distance_result.minimum_distance;
double distance_res_data_min_distance = distance_res_data.distance;
collision_detection::DistanceMap distance_map = distance_result.distances;

collision_result.clear();
current_state.setToDefaultValues();
RCLCPP_INFO(LOGGER, "  Default Positions >>>");
planning_scene.checkSelfCollision(collision_request, collision_result);
RCLCPP_INFO_STREAM(LOGGER, "\tCurrent state is " << (collision_result.collision ? "in" : "not in") << " self collision"); // in self collision
RCLCPP_INFO_STREAM(LOGGER, "\tdistance : " << distance); // 1.79769e+308
RCLCPP_INFO_STREAM(LOGGER, "\tdistance_result.minimum_distance.distance : " << distance_res_data_min_distance); // 1.79769e+308
RCLCPP_INFO_STREAM(LOGGER, "\tdistance_map is " << (distance_map.empty() ? "" : "not") << " empty"); // empty

bool loopFlag = true;
while (loopFlag)
{
  collision_result.clear();
  current_state.setToRandomPositions();
  RCLCPP_INFO_STREAM(LOGGER, "  Random Positions >>>");
  planning_scene.checkSelfCollision(collision_request, collision_result);
  if (collision_result.collision)
    loopFlag = false;
  RCLCPP_INFO_STREAM(LOGGER, "\tCurrent state is " << (collision_result.collision ? "in" : "not in") << " self collision");
  RCLCPP_INFO_STREAM(LOGGER, "\tdistance : " << distance); // 1.79769e+308
  RCLCPP_INFO_STREAM(LOGGER, "\tdistance_result.minimum_distance.distance : " << distance_res_data_min_distance); // 1.79769e+308
  RCLCPP_INFO_STREAM(LOGGER, "\tdistance_map is " << (distance_map.empty() ? "" : "not") << " empty"); // empty
}

RCLCPP_INFO_STREAM(LOGGER, "===== END : use PlanningScene -> checkSelfCollision() =====" << std::endl);

}

// ----- use PlanningScene -> distanceToCollision() ----- //
{
RCLCPP_INFO_STREAM(LOGGER, "----- use PlanningScene -> checkSelfCollision() -----");

current_state.setToDefaultValues();
RCLCPP_INFO(LOGGER, "  Default Positions >>>");
double distance_default_pos = planning_scene.distanceToCollision(current_state);
RCLCPP_INFO_STREAM(LOGGER, "    distance : " << distance_default_pos); // 1.79769e+308

current_state.setToRandomPositions();
RCLCPP_INFO(LOGGER, "  Random Positions >>>");
double distance_random_pos = planning_scene.distanceToCollision(current_state);
RCLCPP_INFO_STREAM(LOGGER, "    distance : " << distance_random_pos); // 1.79769e+308

RCLCPP_INFO_STREAM(LOGGER, "===== END : use PlanningScene -> checkSelfCollision() =====" << std::endl);

}

// ----- use PlanningScene -> checkCollision() ----- //
{
RCLCPP_INFO_STREAM(LOGGER, "----- use PlanningScene -> checkCollision() -----");

collision_detection::CollisionRequest collision_request;
collision_request.distance = true;

collision_detection::CollisionResult collision_result;
double distance = collision_result.distance;
collision_detection::DistanceResult distance_result = collision_result.distance_result;
collision_detection::DistanceResultsData distance_res_data = distance_result.minimum_distance;
double distance_res_data_min_distance = distance_res_data.distance;
collision_detection::DistanceMap distance_map = distance_result.distances;

collision_result.clear();
current_state.setToDefaultValues();
RCLCPP_INFO(LOGGER, "  Default Positions >>>");
planning_scene.checkCollision(collision_request, collision_result);
RCLCPP_INFO_STREAM(LOGGER, "\tCurrent state is " << (collision_result.collision ? "in" : "not in") << " self collision");
RCLCPP_INFO_STREAM(LOGGER, "\tdistance : " << distance); // 1.79769e+308
RCLCPP_INFO_STREAM(LOGGER, "\tdistance_result.minimum_distance.distance : " << distance_res_data_min_distance); // 1.79769e+308
RCLCPP_INFO_STREAM(LOGGER, "\tdistance_map is " << (distance_map.empty() ? "" : "not") << " empty"); // empty

bool loopFlag = true;
while (loopFlag)
{
  collision_result.clear();
  current_state.setToRandomPositions();
  RCLCPP_INFO_STREAM(LOGGER, "  Random Positions >>>");
  planning_scene.checkCollision(collision_request, collision_result);
  if (collision_result.collision)
    loopFlag = false;
  RCLCPP_INFO_STREAM(LOGGER, "\tCurrent state is " << (collision_result.collision ? "in" : "not in") << " self collision");
  RCLCPP_INFO_STREAM(LOGGER, "\tdistance : " << distance); // 1.79769e+308
  RCLCPP_INFO_STREAM(LOGGER, "\tdistance_result.minimum_distance.distance : " << distance_res_data_min_distance); // 1.79769e+308
  RCLCPP_INFO_STREAM(LOGGER, "\tdistance_map is " << (distance_map.empty() ? "" : "not") << " empty"); // empty
}

RCLCPP_INFO_STREAM(LOGGER, "===== END : use PlanningScene -> checkCollision() =====" << std::endl);

}

rclcpp::shutdown();
return 0;
}`

[Jazzzzon]

All the code should belong to a single .cpp file, but it was unintentionally split separate, and I can’t figure out why.

[Jazzzzon]

Clarification: what I want to know is the distance between Links, not the distance between Joints.

[Jazzzzon]

Update: Added the use of the CollisionEnvFCL::distanceSelf method. The configuration method is consistent with the CollisionEnvFCL::distanceRobot method used in the aforementioned code, and the results are identical.

[Jazzzzon]

Heads-up about a common pitfall! I initialized the "result" variable and assigned it to another variable prematurely. This caused the other variable to persistently hold the initial value (1.79769e+308, default max distance). The fix was simple—just re-fetch the updated result after calling the relevant method.
Don't repeat my mistake!