Collision distance check not working properly

[hs2041]

I have created my own .urdf model which contains meshes (.stl format) of a collaborative arm. I am unable to calculate the distance to collision using the functions available. I am using FClcollisiondetector with primitiveshapetype set as MESH.

I have also tried using bullet, ode and dart collision detectors but none of them seem to be responding well.
The distance function gives output but the return value keeps varying for a given configuration.

selfcollision.h file:

class Selfcollision
{
  private:
    dart::simulation::WorldPtr world = std::make_shared<dart::simulation::World>();
    dart::collision::FCLCollisionDetectorPtr fcl_mesh_dart = dart::collision::FCLCollisionDetector::create();
    // dart::collision::BulletCollisionDetector* bullet_mesh_dart = new dart::collision::BulletCollisionDetector::create();
    dart::constraint::ConstraintSolver *constraintSolver = world->getConstraintSolver();
    dart::collision::CollisionGroupPtr group = constraintSolver->getCollisionGroup();
    dart::collision::CollisionOption &option = constraintSolver->getCollisionOption();
    dart::utils::DartLoader loader;
    
    dart::dynamics::SkeletonPtr createManipulator();
    dart::dynamics::SkeletonPtr manipulator = loader.parseSkeleton("dart://sample/urdf/KR5/simple_robot.urdf");
    // dart::dynamics::SkeletonPtr manipulator = loader.parseSkeleton("dart://sample/urdf/KR5/a.urdf");

  public:
    Selfcollision();
    void set_model();
    bool check_selfcollision(std::vector<double> joints);
    double find_distance(std::vector<double> joints);
    void reload_file(std::string input_path);
};
#endif

selfcollision.cpp:

void Selfcollision::set_model()
{
    dart::dynamics::SkeletonPtr manipulator = createManipulator(); 
    world->addSkeleton(manipulator);

    fcl_mesh_dart->setPrimitiveShapeType(dart::collision::FCLCollisionDetector::MESH);
    fcl_mesh_dart->setContactPointComputationMethod(dart::collision::FCLCollisionDetector::DART);

    // fcl_mesh_dart->setPrimitiveShapeType(dart::collision::FCLCollisionDetector::);
    // fcl_mesh_dart->setContactPointComputationMethod(dart::collision::FCLCollisionDetector::DART);

    // bullet_mesh_dart->setPrimitiveShapeType(dart::collision::BulletCollisionDetector::MESH);
    // bullet_mesh_dart->setContactPointComputationMethod(dart::collision::BulletCollisionDetector::DART);

    constraintSolver->setCollisionDetector(fcl_mesh_dart);

    // constraintSolver->setCollisionDetector(bullet_mesh_dart);

    std::cout<<"[Self-Collision]: Collision module set with DOFs: "<<world->getSkeleton(0)->getNumDofs()<<std::endl;
}

Selfcollision::Selfcollision()
{
    set_model();
}

dart::dynamics::SkeletonPtr Selfcollision::createManipulator()
{
    // Load the Skeleton from a file
 
    manipulator->setName("manipulator");

    // Position its base in a reasonable way
    Eigen::Isometry3d tf = Eigen::Isometry3d::Identity();
    tf.translation() = Eigen::Vector3d(-0.65, 0.0, 0.0);
    manipulator->getJoint(0)->setTransformFromParentBodyNode(tf);

    // Get it into a useful configuration
    // manipulator->getDof(1)->setPosition(0 * M_PI / 180.0);
    
    // manipulator->getDof(2)->setPosition(0.0 * M_PI / 180.0);
    manipulator->enableSelfCollisionCheck();  
    manipulator->disableAdjacentBodyCheck();

    return manipulator;
}

bool Selfcollision::check_selfcollision(std::vector<double> joints)
{
    //put a catch error argument
    try{    
        if(joints.size() == 6)
        {
            for(int i=0;i<6;i++)
            {
                manipulator->getDof(i)->setPosition(joints[i]);
            }

            return group->collide(option);
        }
        else{
            throw(joints.size());
         
        }
    }
    catch(int DOFs){
        std::cout<<"ERROR: Invalid no. of joints: "<<DOFs<<std::endl;
    }
}
double Selfcollision::find_distance(std::vector<double> joints)
{
    //put a catch error argument
    try{    
        if(joints.size() == 6)
        {
            for(int i=0;i<6;i++)
            {
                manipulator->getDof(i)->setPosition(joints[i]);
            }
            dart::collision::CollisionResult abc;
            group->collide(dart::collision::CollisionOption(true, 50, nullptr), &abc);
            // return group->distance();
            // std::cout<<abc->minDistance<<"  "<<abc->found()<<"\n";
            std::cout<<"IN "<<group->distance()<<"\n";
            // std::cout<<"contacts: "<<abc.getNumContacts()<<"\n";
            auto frames = abc.getCollidingBodyNodes();
            // std::cout<<frames.size()<<" frame count \n";
            // frames(0).getName();
            // if((abc.getContacts()).size()>0)
            // {
            //     for(int i = 0; i <(abc.getContacts()).size(); i++)
            //   std::cout<<abc.getContact(i).penetrationDepth<<" \n";
            //   //std::cout<<abc->shapeFrame1->getName()<<" ";
            //   //std::cout<<abc->shapeFrame2->getName()<<"\n";
            // }
            // else std::cout<<"NO COLLISION\n";
            return 1.0;
        }
        else{
            throw(joints.size());
         
        }
    }
    catch(int DOFs){
        std::cout<<"ERROR: Invalid no. of joints: "<<DOFs<<std::endl;
    }
}