How to receive position of closest octomap point to robot (even without being in contact)

[kmerckae]

Hello everyone,

I would like to find the closest points of a static octomap (added to the planning scene) wrt the robot links.

I could already solve this problem for simple geometric shapes (boxes, cylinders, spheres) in the planning scene, see https://github.com/ros-planning/moveit/issues/2998#issuecomment-1022261999.

However, when I add an octomap to the planning scene, the nearest_points vector I receive via CollisionResult.contacts gives me always the same point of the octomap with the accompanying nearest point per robot link, see figure below. This point is clearly not the closest point of the Octomap to the robot links. Below you can see the same figure, but with the robot collision mesh (in red) enabled. When changing the position of the same octomap, to clearly show that other octomap points are closer to the robot, again the same octomap point comes out of the CollisionResult.contacts contact map as before. Below you can see the same figure, but with the robot collision mesh (in red) enabled.

Can anyone help me with this issue?

Thanks, Kelly

[niemsoen]

Hey Kelly, have you had any more luck with this? When I call the distanceToCollision function on my Planning Scene which only has an Octomap as collision geometry, I always get the same double value of 179769313486231570814527423731704356798070567525844996598917476803157260780028538760589558632766878171540458953514382464234321326889464182768467546703537516986049910576551282076245490090389328944075868508455133942304583236903222948165808559332123348274797826204144723168738177180919299881250404026184124858368.000000 which I guess is the maximum value of a double variable (not sure about this).

Anyway, I thought I'd ask if you have any news, your reply is very much appreciated! Sönke

[Hunger-beat]

I investigated bullet, and I found that its collision detection for point clouds does not seem to be very complete. When I use bullet for octomap collision detection or closest point display, it is very inaccurate, and there is also the problem of large distance that @niemsoen said. So I think using FCL may be a more suitable solution, but in this case, if a collision does not occur, the closest point information cannot be returned. I also want to know if there is a better solution.

[Hunger-beat]

I think I've found a suitable solution. FCL can calculate the nearest distance between two non-colliding objects, even with octomap. Some code snippets are shown below:

`#include <ros/ros.h>

include <moveit_msgs/PlanningScene.h>

include <moveit_msgs/CollisionObject.h>

include <geometry_msgs/Pose.h>

include <shape_msgs/SolidPrimitive.h>

include <moveit/planning_scene_interface/planning_scene_interface.h>

include <moveit/planning_scene/planning_scene.h>

include <moveit/collision_detection/collision_tools.h>

include <moveit/planning_scene_interface/planning_scene_interface.h>

include <moveit/move_group_interface/move_group_interface.h>

include <moveit/planning_scene_monitor/planning_scene_monitor.h>

include <moveit/collision_detection_bullet/collision_env_bullet.h>

include <moveit/collision_detection_bullet/collision_detector_allocator_bullet.h>

include <moveit/collision_detection_fcl/collision_env_fcl.h>

include <visualization_msgs/Marker.h>

int main(int argc, char** argv) { ros::init(argc, argv, "update_planning_scene"); ros::NodeHandle node_handle; ros::AsyncSpinner spinner(1); spinner.start();

ros::Publisher marker_pub = node_handle.advertise<visualization_msgs::Marker>("visualization_marker", 10);
ros::Rate r(30);

const std::string robot_description = "robot_description";
auto planning_scene_monitor = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>(robot_description);

ros::Publisher planning_scene_diff_publisher = node_handle.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
ros::WallDuration sleep_t(0.5);
while (planning_scene_diff_publisher.getNumSubscribers() < 1) {
    ROS_INFO("Waiting for subscribers on /planning_scene...");
    sleep_t.sleep();
}

planning_scene_monitor->startSceneMonitor();
planning_scene_monitor->startStateMonitor();
planning_scene_monitor->startWorldGeometryMonitor();

static const std::string PLANNING_GROUP = "manipulator_e5";
moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);

collision_detection::AllowedCollisionMatrix acm = planning_scene_monitor->getPlanningScene()->getAllowedCollisionMatrix();
std::string joint_name1 = "pedestal_Link";  
std::string joint_name2 = "base_link";
std::string joint_name3 = "shoulder_Link";
std::string joint_name4 = "upperArm_Link";
std::string joint_name5 = "foreArm_Link";
std::string joint_name6 = "wrist1_Link";
std::string joint_name7 = "wrist2_Link";
std::string joint_name8 = "wrist3_Link";
std::string obstacle_name = "box"; 

acm.setEntry(joint_name1, obstacle_name, true);
acm.setEntry(joint_name2, obstacle_name, true);
acm.setEntry(joint_name3, obstacle_name, true);
acm.setEntry(joint_name4, obstacle_name, true);
acm.setEntry(joint_name2, joint_name5, true);
acm.setEntry(joint_name2, joint_name6, true);
acm.setEntry(joint_name2, joint_name7, true);
acm.setEntry(joint_name2, joint_name8, true);
acm.setEntry(joint_name3, joint_name5, true);
acm.setEntry(joint_name5, joint_name7, true);
acm.setEntry(joint_name5, joint_name8, true);
acm.setEntry(joint_name3, joint_name7, true);
acm.setEntry(joint_name3, joint_name8, true);
acm.setEntry(joint_name4, joint_name6, true);
acm.setEntry(joint_name4, joint_name7, true);
acm.setEntry(joint_name4, joint_name8, true);

planning_scene_monitor::LockedPlanningSceneRO locked_scene(planning_scene_monitor);
planning_scene::PlanningScenePtr current_scene = planning_scene::PlanningScene::clone(locked_scene);

std::vector<std::vector<double>> test_joint_positions_deg = {
    // {4,27,-99,-34,-92,-1},
    // {8,24,-102,-34,-92,-1},
    // {8,23,-102,-35,-92,7},
    // {10,47,-60,15,-103,-18},
    // {9,46,-63,-11,-90,12},
    {11,23,-98,-30,-92,31}
};

for (const auto& joint_group_positions_deg : test_joint_positions_deg) {
    robot_state::RobotStatePtr start_state(new robot_state::RobotState(move_group.getRobotModel()));
    start_state->setToDefaultValues();

    std::vector<double> start_joint_group_positions_rad;
    for (double deg : joint_group_positions_deg) {
        start_joint_group_positions_rad.push_back(deg * M_PI / 180.0);
    }
    start_state->setJointGroupPositions(move_group.getName(), start_joint_group_positions_rad);
    start_state->update();  

    auto distance_request = collision_detection::DistanceRequest();
    auto distance_result = collision_detection::DistanceResult();
    distance_request.enable_nearest_points =true;
    distance_request.acm = &acm;

    // Check whether a specified state (robot_state) is in collision
    current_scene->getCollisionEnv()->distanceRobot(distance_request, distance_result, *start_state);
    // current_scene->getCollisionEnv()->distanceRobot(*start_state, acm);

    // Show result
    ROS_INFO_STREAM("Current state is " << (distance_result.collision ? "in" : "not in") << " collision");
    ROS_INFO_STREAM("Min distance between two bodies=" << distance_result.minimum_distance.distance << "\n");

    std::cout << "min dist data...\n" <<
            "distance = " << distance_result.minimum_distance.distance << "\n"
            "link name = " << distance_result.minimum_distance.link_names[0] << "\n"
            "nearest pt = " << distance_result.minimum_distance.nearest_points[0] << "\n"
            "link name = " << distance_result.minimum_distance.link_names[1] << "\n"
            "nearest pt = " << distance_result.minimum_distance.nearest_points[1] << "\n"
            << std::endl;

    // visualization_msgs::MarkerArray markers;
    // for (size_t i = 0; i < 2; ++i) {
    //     visualization_msgs::Marker marker;
    //     marker.header.frame_id = "world"; 
    //     marker.header.stamp = ros::Time::now();
    //     marker.ns = "points_and_lines";
    //     marker.id = i;
    //     marker.type = visualization_msgs::Marker::SPHERE;
    //     marker.action = visualization_msgs::Marker::ADD;
    //     marker.pose.position.x = distance_result.minimum_distance.nearest_points[i].x();
    //     marker.pose.position.y = distance_result.minimum_distance.nearest_points[i].y();
    //     marker.pose.position.z = distance_result.minimum_distance.nearest_points[i].z();
    //     marker.scale.x = 0.01; 
    //     marker.scale.y = 0.01;
    //     marker.scale.z = 0.01;
    //     marker.color.r = 1.0f;
    //     marker.color.g = 0.0f;
    //     marker.color.b = 0.0f;
    //     marker.color.a = 1.0;
    //     marker.lifetime = ros::Duration();

    //     marker_pub.publish(marker);
    // }

}

ros::shutdown();
return 0;

}

`