Possible issue with GraspitSceneManagerHeadless GraspitCore Handling

[belledon]

Let me preface this with the fact that I am not very experienced with C++ and thread related structures so I apologize in advance if this seems like a silly issue. I was implementing a class that inherits GraspItAccessor by essentially using EigenGraspPlanner as template. Its a very simple class that retrieves the current hand, calls autograsp, then returns the dofs. I copied the necessary methods from EigenGraspPlanner (removing ivIdleCallback because this "planner" doesn't update states). The main problem I am having is during runtime. When an executable analogous to grasp_planning (except that it uses the autograsp planner rather than EigenGraspPlanner) that I'll call autograsp_planning is called repeatedly (in a for loop), something from the previous iteration remains initialized and I receive this stdout followed by hanging.

Singleton already set, overwriting!
Initializing GraspIt - autograsp_planning.cpp, 81
Enter INVENTOR thread loop - GraspItSceneManagerHeadless.cpp, 108
Starting with args GraspIt, --headless - GraspItSceneManagerHeadless.cpp, 114
Coin warning in SoQt::init(): This method should be called only once.
Coin error in SoType::createType(): a type with name ``SoComplexShape'' already created
Coin error in SoType::createType(): a type with name ``SoArrow'' already created
Coin error in SoType::createType(): a type with name ``SoTorquePointer'' already created
Created. - GraspItSceneManagerHeadless.cpp, 116
QObject::startTimer: QTimer can only be used with threads started with QThread
QObject::startTimer: QTimer can only be used with threads started with QThread
QObject::startTimer: QTimer can only be used with threads started with QThread
QObject::startTimer: QTimer can only be used with threads started with QThread
QApplication::exec: Must be called from the main thread
QObject::startTimer: QTimer can only be used with threads started with QThread
Exit INVENTOR thread loop - GraspItSceneManagerHeadless.cpp, 133

If I call autograsp_planning once and exit, there is no issue; however, for the sake of efficiency I would prefer not to. Below is the stdout that would normally precede the error. The class ContactGetter inherits GraspitAccessor. Also just for the sake of clarifcation, I am wrapping autograsp_planning for python and calling its method in a for loop (or list comprehension, no difference here) in python.

Getting hand - ContactGetter.cpp, 140
Performing autograsp - ContactGetter.cpp, 142
Getting hand dofs - ContactGetter.cpp, 160
Obtained hand dofs - ContactGetter.cpp, 170
Cleaning up - ContactGetter.cpp, 171
ContactGetter destructor - ContactGetter.cpp, 39
Removing idle listeners - ContactGetter.cpp, 40
Unregistering ContactGetter - GraspItSceneManager.cpp, 187
Exit ContactGetter destructor - ContactGetter.cpp, 58
GraspItSceneManagerHeadless::destroyCore() - GraspItSceneManagerHeadless.cpp, 60
Now exit Inventor thread. - GraspItSceneManagerHeadless.cpp, 77
Exit INVENTOR thread loop - GraspItSceneManagerHeadless.cpp, 132
QThreadStorage: Thread 0x7fbaa0001c40 exited after QThreadStorage 5 destroyed
GraspItSceneManager destructor - GraspItSceneManager.cpp, 93

I searching the errors and found that you had a similar issue in June when trying to communicate between the headless scene manager and graspitCore. From what I can tell above, it seems that my class ContactGetter destroys properly.

The two most relevant functions are included below:

std::vector<double> ContactGetter::autoGrasp(){
    PRINTMSG("Getting hand");
    Hand *h = getGraspItSceneManager()->getCurrentHand();
    PRINTMSG("Performing autograsp");
    GraspPlanningState gst = GraspPlanningState(h);
    const GraspPlanningState *s;
    s = &gst;
    if (!s->getHand()->autoGrasp(false))
    {
        PRINTWARN("Could not correctly open hand with auto-grasp, the pre-grasp state may not be ideal.");
    }
    
    const PostureState* handPosture = s->readPosture();
    if (!handPosture)
    {
        PRINTERROR("Posture is NULL!");
    }

    // std::string robotName="Robot1";
    // Robot *r = getGraspItSceneManager()->getRobot(robotName);
    // h = getGraspItSceneManager()->getCurrentHand();
    PRINTMSG("Getting hand dofs");
    const int numDOF = s->getHand()->getNumDOF();
    double * _dofs = new double[numDOF];
    handPosture->getHandDOF(_dofs);
    std::vector<double> dofs;
    for (int k = 0; k < numDOF; ++k)
    {
        dofs.push_back(_dofs[k]);
    }
    // r->getDOFVals(dofs);
    PRINTMSG("Obtained hand dofs");
    PRINTMSG("Cleaning up");
    // delete s;
    return dofs;
}

-------------------------------------
std::vector<double> quickGrasp(
    std::string& objectFilename, 
    std::string& robotFilename,
    Eigen::Vector3d& robPos, 
    const std::vector<double>& robRot)
{
    signal(SIGSEGV, handler);
    signal(SIGABRT, handler);
    PRINT_INIT_STD();
    PRINTMSG("Initializing GraspIt")
    SHARED_PTR<GraspIt::GraspItSceneManager> graspitMgr(new GraspIt::GraspItSceneManagerHeadless());  
    SHARED_PTR<GraspIt::ContactGetter> cg(new GraspIt::ContactGetter("ContactGetter", graspitMgr));
    GraspIt::EigenTransform robotTransform;
    GraspIt::EigenTransform objectTransform;
    // We want to keep the object at the absolute origin
    robotTransform.setIdentity();
    objectTransform.setIdentity();
    robotTransform.translate(robPos);
    // Have to do this because Eigen::Quaternion isn't covered by pybind/eigen
      // Eigen::Quaterniond q(2, 0, 1, -3); 
    Eigen::Quaterniond robRotQ(robRot[0], robRot[1], robRot[2], robRot[3]);
    robotTransform.rotate(robRotQ);
    // robotTransform.rotate(robRot);
    
    std::string robotName="Robot1";
    std::string objectName="Object1";

    if (graspitMgr->loadRobot(robotFilename, robotName, robotTransform) != 0)
    {
        PRINTERROR("Could not load robot");
        
    }

    if (graspitMgr->loadObject(objectFilename, objectName, true, objectTransform))
    {
        PRINTERROR("Could not load object");
    }
    
    std::vector<double> dofs = cg->autoGrasp();
    // cg.reset();
    // graspitMgr.reset();
    signal(SIGSEGV, handler);
    signal(SIGABRT, handler);
    // PRINT_INIT_STD();
    
    return dofs;
}

[belledon]

I removed the mentions to ContactGetter from quickGrasp just to be doubly sure that these errors have nothing to do with that class. In addition, I gutted the function with all mentions to graspit and it ran fine. If I only include the line that assigns the shared ptr to GraspitSceneManager, the error occurs again. I am very confident that it is an issue with GraspitCore. I can post an issue on their repo instead, if you'd prefer.

[belledon]

After addind SoQt::done() to the GraspitCore deconstructor, the errors

Coin error in SoType::createType(): a type with name ``SoComplexShape'' already created
Coin error in SoType::createType(): a type with name ``SoArrow'' already created
Coin error in SoType::createType(): a type with name ``SoTorquePointer'' already created

where removed. It looks like that this was forgotten in the main repo.

[JenniferBuehler]

Hi,
thanks for reporting this. Qt is very iffy with being run from a thread other than QThread, which is always annoying, so the QTimer errors are related to that, though so far any multi-threading complaints were only warnings. I'll have to look into this specific one again. Which exact version of Qt4 are you using?

Also, Qt can't really be run twice, hence the error with the SoQt::init() being called twice. So you can only run one GUI Element.
It's been a few months since I've worked on this so it would be easier to me if I was able to debug the thing myself, taking your initial findings as guideline. Any chance you have that stuff in a public repo somewhere?

I will try to get around to it this weekend, but can't promise. It may get to next week.

Cheers
Jenny

[belledon]

Hey Jenny,

Super glad to hear that you'll take a look. Just so you know, there's an easy work around for me (calling it as a main and parsing the stdout, which isn't super pretty but it'll work for now). And yes there is a public repo. There are two ways you could go about building the environment. You could clone and build both the graspit and graspit-pkgs forks from here. Or if you want the exact environment you could either build or download a Singularity container. Singularity is super cool... All you have to do is clone and build from their repo. Because I have to run most of my applications on a cluster, the admins really prefer us to use this container since it allows us to build whatever crazy environment we want... And if we want to share the environment (say for reproducability or just being nice) all you have to do is cp the container file and bam! You have the exact environment the guy who is making you look at 3 month code is using. If you prefer to build the repo's yourself, then I would still recommend using Singularity, because I could give you my bootstrap file (kind of like a Vagrant init file but much more helpful). The bootstrap file contains all of the command line arguments you would need to pass in order to build the container to the same state as the one I am running (except you just have to run one command via singularity).

Just to show how cool it is, this is how you would go about it:

Step 1) Singularity

mkdir <some_git_dir>
cd <some_git_dir>
git clone https://github.com/singularityware/singularity.git
cd singularity
./autogen.sh
./configure --prefix=/usr/local
make
sudo make install

Step 2) Create container (does not initialize environment, really just creates a mount).
(Also I don't think the container needs to be 10G, but its better than having to expand it later (which is also very easy)

mkdir containers && cd containers
sudo singularity create  -s 10000 <your_container_name>.img 

Step3) Bootstrap the container and tada! You have the exact same environment I'm using.
This will install all necessary libraries via apt-get or pip

sudo singularity bootstrap <your_container_name>.img v2_ubuntu.def

I can provide the bootstrap file (v2_ubuntu.def) if you're down for trying this out. If not, I'll post the cmake commands to build the repos. Another possibility is downloading the container (still have to clone/build Singularity).

[JenniferBuehler]

Hi,
sorry I didn't get to it on the weekend, will try one of the next evenings. Crazy busy at the moment..

Thanks for the detailed instructions about singularity. I may just check the source code first just looking at it, I may have a suggestion when I see what the issue may be and then will let you know so you can try it. Sorry my time is so limited at the moment.
If I don't see anything obvious, I may do the Singularity option as you suggested. Will let you know as soon as I get to it.

Quick question: Did you modify the sources either in graspit or graspit-pkgs in your forks, or are you just adding your own extensions?

Cheers
Jenny