fcl::distance return same distance for all links

[YIMING YANG]

Hi all,

I am working on a project using ROS and MoveIt, and I had a problem with MoveIt and FCL. 

Basically, I want to retrieve the closest distances between a robot link and all other objects(either another robot link or an obstacle). Since moveit has all those nice interfaces, I just want to retrieve FCL objects from robot_state and planning_world, then directly call fcl::distance() to get the distance.

I have two classes inherited from collision_robot_fcl and collision_world_fcl, then we can get the fcl collision objects out from them. Then call fcl::distance() for robot's each fcl_object and all others, and we should be able to get all the distances and find the minimum ones.

However, the distances returned were all the same. I had printed out the geometries and translations, which looks correct. I dont know if I missed some updates or something.

Here is some key codes and outputs

//Class inherited from collision world fcl
class MyCollisionWorld: public collision_detection::CollisionWorldFCL
        {
                public:
                        MyCollisionWorld(const collision_detection::WorldPtr & world) :
                                        CollisionWorldFCL(world)
                        {
                        }
                        void getCollisionObject(std::vector<boost::shared_ptr<fcl::CollisionObject> > & obj)
                        {
                                std::map<std::string, collision_detection::FCLObject>::iterator it =
                                                fcl_objs_.begin();
                                uint size = 0, i = 0;
                                if (it != fcl_objs_.end())
                                {
                                        size += it->second.collision_objects_.size();
                                }
                                obj.reserve(size);
                                it = fcl_objs_.begin();
                                if (it != fcl_objs_.end())
                                {
                                        for (int j = 0; j < it->second.collision_objects_.size(); j++)
                                        {
                                                obj.push_back(it->second.collision_objects_[j]);
                                        }
                                }
                        }
        };


//Class inherited from collision robot fcl
class MyCollisionRobot: public collision_detection::CollisionRobotFCL
        {
                public:
                        MyCollisionRobot(const robot_model::RobotModelConstPtr &model) :
                                        CollisionRobotFCL(model)
                        {
                        }
                        void getCollisionObject(const robot_state::RobotState & state,
                                        std::vector<boost::shared_ptr<fcl::CollisionObject> > & obj)
                        {
                                collision_detection::FCLObject fcl_obj;
                                constructFCLObject(state, fcl_obj);
                                obj = fcl_obj.collision_objects_;
                        }
        };



//Related key codes

MyCollisionWorld cworld(scene_->getPlanningScene().get()->getWorldNonConst());
robot_state::RobotState state(scene_->getPlanningScene().get()->getCurrentState());
state.update();
state.updateCollisionBodyTransforms();

MyCollisionRobot crobot(state.getRobotModel());
std::vector<boost::shared_ptr<fcl::CollisionObject> > robot_obj, world_obj;

crobot.getCollisionObject(state, robot_obj);
cworld.getCollisionObject(world_obj);

fcl::DistanceRequest req(true);
fcl::DistanceResult res;
double d, dmin = 1000;

for (int i = 0; i < robot_obj.size(); i++)
    {
        const collision_detection::CollisionGeometryData* cd =
                static_cast<const collision_detection::CollisionGeometryData*>(robot_obj[i]->getCollisionGeometry()->getUserData());

        for (int it = 0; it < seg_names_.size(); it++)
        {
            if (seg_names_[it].compare(cd->getID()) == 0)
            {
                for (int j = 0; j < world_obj.size(); j++)
                {
                    double dist = fcl::distance(robot_obj[i].get(), world_obj[j].get(), req, res);

                    std::cout << "Robot Link" << cd->getID() << " at(" << robot_obj[i]->getTranslation().data.vs[0] << "," << robot_obj[i]->getTranslation().data.vs[1] << "," << robot_obj[i]->getTranslation().data.vs[2] << ")";
                    std::cout << "Object at(" << world_obj[j]->getTranslation().data.vs[0] << "," << world_obj[j]->getTranslation().data.vs[1] << "," << world_obj[j]->getTranslation().data.vs[2] << ")" << std::endl;
                    p1 << res.nearest_points[0].data.vs[0], res.nearest_points[0].data.vs[1], res.nearest_points[0].data.vs[2];
                    p2 << res.nearest_points[1].data.vs[0], res.nearest_points[1].data.vs[1], res.nearest_points[1].data.vs[2];
                    d = (p1 - p2).norm();
                    std::cout << "Robot closest at (" << p1.transpose() << ") object closest at("
                            << p2.transpose() << ") dist from fcl=" << dist << " computed d=" << d
                            << std::endl << std::endl;
                    if (d < dmin)
                        dmin = d;
                }
                break;
            }
        }
    }



//Out put
//Different links are at different position, but the returned closest distances and points are all the same.

Robot Link [ltorso] at(-0.0125,0,0) Object at(1.51,0,0.69)
Robot closest at ( 0.0289969 -0.0059492  0.0662392) object closest at(1.17662 -0.0103892 0.69) 
dist from fcl=1.3062 computed d=1.3062

Robot Link [mtorso] at(-0.0125,0,0.16197) Object at(1.51,0,0.69)
Robot closest at ( 0.007516 -0.001713  0.223745) object closest at( 1.17664 -0.00711173 0.69) 
dist from fcl=1.25868 computed d=1.25868

Robot Link [utorso] at(-0.0125,0,0.21197) Object at(1.51,0,0.69)
Robot closest at ( 0.2492  0.1355 0.73137) object closest at( 1.17703 0.131218 0.736375) 
dist from fcl=0.927851 computed d=0.927851

Robot Link [utorso] at(-0.0125,0,0.21197) Object at(1.51,0,0.69)
Robot closest at ( 0.2492  0.1355 0.73137) object closest at( 1.17703 0.131218 0.736375) 
dist from fcl=0.927851 computed d=0.927851

Robot Link [l_clav] at(0.05191,0.13866,0.31915) Object at(1.51,0,0.69)
Robot closest at ( 0.2492  0.1355 0.73137) object closest at( 1.17703 0.131218 0.736375) 
dist from fcl=0.927851 computed d=0.927851

Robot Link [l_scap] at(0.05191,0.27901,0.51524) Object at(1.51,0,0.69)
Robot closest at ( 0.2492  0.1355 0.73137) object closest at( 1.17703 0.131218 0.736375) 
dist from fcl=0.927851 computed d=0.927851

Robot Link [l_uarm] at(0.05191,0.46601,0.53124) Object at(1.51,0,0.69)
Robot closest at ( 0.2492  0.1355 0.73137) object closest at( 1.17703 0.131218 0.736375) 
dist from fcl=0.927851 computed d=0.927851

Robot Link [l_larm] at(0.05191,0.58501,0.54045) Object at(1.51,0,0.69)
Robot closest at ( 0.2492  0.1355 0.73137) object closest at( 1.17703 0.131218 0.736375) 
dist from fcl=0.927851 computed d=0.927851

Robot Link [l_farm] at(0.05191,0.77201,0.53124) Object at(1.51,0,0.69)
Robot closest at ( 0.2492  0.1355 0.73137) object closest at( 1.17703 0.131218 0.736375) 
dist from fcl=0.927851 computed d=0.927851

Robot Link [l_hand] at(0.05191,0.89101,0.54045) Object at(1.51,0,0.69)
Robot closest at ( 0.2492  0.1355 0.73137) object closest at( 1.17703 0.131218 0.736375) 
dist from fcl=0.927851 computed d=0.927851

Robot Link [r_clav] at(0.05191,-0.13866,0.31915) Object at(1.51,0,0.69)
Robot closest at ( 0.2492  0.1355 0.73137) object closest at( 1.17703 0.131218 0.736375) 
dist from fcl=0.927851 computed d=0.927851

Robot Link [r_scap] at(0.05191,-0.27901,0.51524) Object at(1.51,0,0.69)
Robot closest at ( 0.2492  0.1355 0.73137) object closest at( 1.17703 0.131218 0.736375) 
dist from fcl=0.927851 computed d=0.927851

Robot Link [r_uarm] at(0.05191,-0.46601,0.53124) Object at(1.51,0,0.69)
Robot closest at ( 0.2492  0.1355 0.73137) object closest at( 1.17703 0.131218 0.736375) 
dist from fcl=0.927851 computed d=0.927851

Robot Link [r_larm] at(0.05191,-0.58501,0.54045) Object at(1.51,0,0.69)
Robot closest at ( 0.2492  0.1355 0.73137) object closest at( 1.17703 0.131218 0.736375) 
dist from fcl=0.927851 computed d=0.927851

Robot Link [r_farm] at(0.05191,-0.77201,0.53124) Object at(1.51,0,0.69)
Robot closest at ( 0.2492  0.1355 0.73137) object closest at( 1.17703 0.131218 0.736375) 
dist from fcl=0.927851 computed d=0.927851

Robot Link [r_hand] at(0.05191,-0.89101,0.54045) Object at(1.51,0,0.69)
Robot closest at ( 0.2492  0.1355 0.73137) object closest at( 1.17703 0.131218 0.736375) 
dist from fcl=0.927851 computed d=0.927851

Thanks,

Yim

[aka...@greyorange.sg]

Hello , I am working around same scenerio. Was some solution found for this ? 

[Jorge Nicho]

A related PR is in the works at the moment

[aka...@greyorange.sg]

Thanks for the quick update.PR seems to be under review.