Reversed nearest points result when computing distance with octree

[Schackma]

When computing the nearest points between an object and an octree, the first point is always a point on the octree even if the octree is the second collision object. This is not consitant with the behavior when colliding other shapes.

Below is a test case comparing the results of colliding two boxes and colliding a box and an octree:

#include <fcl/fcl.h>
#include <iostream>
#include <octomap/OcTree.h>


using namespace fcl;
using namespace std;

void printPoints(CollisionObject<double> *c1,
                 CollisionObject<double> *c2) {
  DistanceRequest<double> req(true);
  DistanceResult<double> res;
  distance(c1,c2,req,res);

  cout << "first point:" << endl;
  cout << res.nearest_points[0][0] << ", " << res.nearest_points[0][1] << ", " <<
      res.nearest_points[0][2] << endl;
  cout << "second point:" << endl;
  cout << res.nearest_points[1][0] << ", " << res.nearest_points[1][1] << ", " <<
      res.nearest_points[1][2] << endl;

}

int main(int argc, char *argv[]) {
  double dim = 0.1;

  // create OcTree
  shared_ptr<octomap::OcTree> tree(new octomap::OcTree(0.1));
  tree->updateNode(1.0, 0.0, 0.0, (float) 1.0);  // occupied 1m from center
  tree->updateInnerOccupancy();

  // make 2 boxes and an octree geometries
  shared_ptr<Box<double> > b1(new Box<double>(dim, dim, dim));
  shared_ptr<Box<double> > b2(new Box<double>(dim, dim, dim));
  shared_ptr<OcTree<double> > o(new OcTree<double>(tree));

  // make collision objects
  CollisionObject<double> c_b1(b1);
  CollisionObject<double> c_b2(b2);
  CollisionObject<double> c_o(o);

  // transform box2
  Vector3<double> v_b2(1,0,0);
  Transform3<double> t_b2 = Transform3<double>::Identity();
  t_b2.translation() = v_b2;
  c_b2.setTransform(t_b2);

  // closest points between box 1 and box 2
  cout << "points 1 to 2" << endl;
  printPoints(&c_b1, &c_b2);
  cout << "points 2 to 1" << endl;
  printPoints(&c_b2, &c_b1);

  // closest points between box 1 and octree
  cout << "points 1 to octree" << endl;
  printPoints(&c_b1, &c_o);
  cout << "points octree to 1" << endl;
  printPoints(&c_o, &c_b1);

  return 0;
}

Which results in an output of:

first point:
0.05, 0.05, 0.05
second point:
0.95, 0.05, 0.05
points 2 to 1
first point:
0.95, 0.05, 0.05
second point:
0.05, 0.05, 0.05
points 1 to octree
first point:
1, 0.025, 0.025
second point:
0.05, 0.025, 0.025
points octree to 1
first point:
1, 0.025, 0.025
second point:
0.05, 0.025, 0.025

When we collide two boxes, the closest points always correspond their respective collision objects (e.g. The 0th index is the first object and the first index is the second object). However, when colliding a box and an octree, the octree is always the 0th index even if it is the second collision object.

I am using this in a robotics application where I am trying to keep a robot from hitting a wall. However, when I compute the distance (and nearest points) I don't know which object the octree is and thus I can't be sure which direction to move the robot to avoid the wall.

[mjpatell]

Hello,

I had similar problem. In my case, a nearest point won't be reversed but values remain same. Anyway, nearest points calculate wrong values. It computes inside of octree instead of external collision like robot arm.

[alaurenzi]

Hi all, at our group we are also facing this issue. Do you have any plans to work on it?

Either ways, thanks for the amazing library!

[unishuai]

Hello, i alse had similar problem.if they were colleded,the distance wrong starngely