How to obtain object bounding box and reposition robot?

[belledon]

I am working with an object database that I imported from Shapenet (as .wrl). At the moment grasp_planner.cpp places both the hand and the object at the origin which tends to lead towards the robot grabbing the object near that location (or at times grabbing objects from the interior, ie the hand is grabbing from an interior surface). I would like to be able to position the robot's hand along positions on these object's bounding boxes. I was thinking that I could just grab the max and min indeces from the object files (maybe by reading the wrl file directly). I noticed around line 247 of grasp_planner.cpp that it is possible to transform these objects using the translate method. I was wondering where to find the documentation of that method (I probably missed it in the ros docs). Does it just receive a 3d vector for position relative to the origin (ie replace the input in line 251)?

[JenniferBuehler]

Good idea - yeah the search algorithm in graspit (simulated annealing) is not random for long enough in the search sometimes. The parameter which decreases the "temperature" (simulated annealing parameter) for the translations/rotations would need to be decreased, so it tries more different poses before settling for the best. But those parameters are not exposed at this point. Your idea to re-plan from different starting points is great, we'll have to see if it settles for better solutions then.

Admittedly the transformation of objects and robots is only documented in GraspItSceneManager::loadRobot() / loadObject() - you just pass the transform matrix for it, there's not more to it. The EigenTransform type is a matrix: It contains a translation and a rotation. EigenTransform uses the Eigen typedef, and is defined in GraspItTypes.h. You may just want to use a translation for what you're trying to do, at least to start with. You'll have to manually edit grasp_planning.cpp for your first test, it's not a command line argument yet, I'll try to add it asap. For example:

#include <grasp_planning_graspit/GraspitTypes.h>
GraspIt::EigenTransform objectTransform;
objectTransform.translate(Eigen::Vector3d(x,y,z));
...
 graspitMgr->loadObject(objectFilename, objectName, true, objectTransform)
...

See also the Eigen Matrix documentation.

I'll look into how to get the bounding box, at the moment there isn't a function exposing this, but I may add it tomorrow. Will keep you posted.

[JenniferBuehler]

I've had a quick look. You can get the bounding box of an IV file as follows:

#include <Inventor/SoDB.h>
#include <Inventor/SoInput.h>
#include <Inventor/SbBox.h>
#include <Inventor/actions/SoGetBoundingBoxAction.h>
#include <iostream>

int main(int argc, char**argv)
{
    if (argc < 2)
    {
         std::cout << "Pass filename as argument" <<std::endl;
         return 1;
    }
    //  ---- Read from IV file (or use a SoNode directly instead)  ----
    std::string filename(argv[1]);
    SoInput in;
    SoNode  *model = NULL;
    if (!in.openFile(filename.c_str())
        || !SoDB::read(&in, model)
        || !model)
    {
        std::cout << "Could not read " << filename << std::endl;
        return 1;
    }
    in.closeFile();

    // --- Construct the bounding box ----

    // viewport required for any viewport-dependent 
    // nodes (eg text), but not required for others
    SbViewportRegion anyVP(0,0);  
    SoGetBoundingBoxAction bbAction( anyVP );
    bbAction.apply( model );
    SbBox3f bbox = bbAction.getBoundingBox();
    const SbVec3f& min = bbox.getMin();
    const SbVec3f& max = bbox.getMax();
    std::cout <<"Bounding box min: " << min[0] << ", " << min[1] << ", " << min[2] << std::endl;
    std::cout <<"Bounding box max: " << min[0] << ", " << min[1] << ", " << min[2] << std::endl;
    return 0;
}

[belledon]

Wow this is terrific and a great time saver, thanks! I'll tinker around with this and let you know how it goes

[JenniferBuehler]

No worries. This is a subject I've been thinking about as well. I'd be curious to see as well how changing the initial position/orientation of the object influences the planning results, but I'm not sure if it would make a huge difference - it may just as well end up with the same result (getting stuck in a local minima). It's probably worth changing the simulated annealing search parameters instead (temperature and that parameter to decrease it, which I forget the name of). I've been wanting to look into this anyway.

I'll add the command line parameter to change the initial object pose so you can play around with it to see if it's worth taking this approach at all, before putting more work into it. Forcing many re-starts of the planner will also end up in much longer overall planning times, but that could be acceptable for some purposes.

[JenniferBuehler]

You can now optionally change the object pose and specify the max number of iterations as well (recommended minimum 40000):

./grasp_planning --wld <worldfile.xml> --dir <result-dir> --save-separate --obj-pos <x> <y> <z> --iter 70000

[belledon]

I'd be happy to share what the results! I still need to get a few more things done on my end but you've greatly expedited the process .

[JenniferBuehler]

Ok, great! Let me know how it goes.

[belledon]

Quick question. I am having trouble compiling the bound box script because of undefined references to the classes in the Inventor library. Is the Inventor library part of Coin?

[belledon]

Nevermind, found it

[JenniferBuehler]

Oh, just saw it now. Guess you found the library :)

[belledon]

I keep running into a weird error during compilation (tried with gcc and using make). The SoGetBoundingBoxAction.h cannot be linked. Shouldn't if be discoverable with the call: -lInventor ?

[belledon]

Just to be thorough, this is the entire call gcc -Wall -g -v getIvBB.cpp -lInventor -lstdc++ -o getIvBB Also if its not immediately clear what I'm doing wrong, I can always just implement the method I mentioned earlier =)

[JenniferBuehler]

Did you try g++ getIvBB.cpp -lSoQt4 -lCoin -o getIvBB?

[belledon]

It could not find them. I will try to include their directories (they are installed at /usr/lib/x86_64-linux-gnu/)

[belledon]

After inlcuding the absolute path it compiled and assembled. But It generated this warning Coin warning in SoAction::apply(): The root node that the SoGetBoundingBoxAction was applied to has a reference count equal to zero. This is a bug in your application code which you should rectify: you need to ref() (and later unref()) the top-level root node to make sure you avoid memory leaks (bad) and / or premature memory destruction (*really* bad) under certain conditions. Coin has an internal workaround to avoid just responding with mysterious crashes, but as it is not possible to cover _all_ cases of what can go wrong with this workaround you are *strongly* advised to fix the bug in your application code.

Should I try to fix it or is it ignorable?

[belledon]

I managed to butcher my way to this and it runs but ends with segmentation fault

#include <Inventor/SoDB.h>
#include <Inventor/SoInput.h>
#include <Inventor/SbBox.h>
#include <Inventor/actions/SoGetBoundingBoxAction.h>
#include <Inventor/nodes/SoSeparator.h>
#include <iostream>

int main(int argc, char**argv)
{
    if (argc < 2)
    {
         std::cout << "Pass filename as argument" <<std::endl;
         return 1;
    }
    //  ---- Read from IV file (or use a SoNode directly instead)  ----
    std::string filename(argv[1]);
    SoDB::init();
    SoInput in;
    in.openFile(filename.c_str());

    SoSeparator *rootsep = SoDB::readAll(&in);
    if (rootsep == NULL)
    {
        std::cout << "Error on read" << filename << std::endl;
        return 1;
    }

    // SoNode  *model = NULL;
    // if (!in.openFile(filename.c_str())
    //     || !SoDB::read(&in, model)
    //     || !model)
    // {
    //     std::cout << "Could not read " << filename << std::endl;
    //     return 1;
    // }
    in.closeFile();
    rootsep->ref();

    // --- Construct the bounding box ----

    // viewport required for any viewport-dependent 
    // nodes (eg text), but not required for others

    SbViewportRegion anyVP(0,0);  
    SoGetBoundingBoxAction bbAction(anyVP);
    bbAction.apply(rootsep);
    SbBox3f bbox = bbAction.getBoundingBox();
    const SbVec3f& min = bbox.getMin();
    const SbVec3f& max = bbox.getMax();
    std::cout <<"Bounding box min: " << min[0] << ", " << min[1] << ", " << min[2] << std::endl;
    std::cout <<"Bounding box max: " << max[0] << ", " << max[1] << ", " << max[2] << std::endl;
    rootsep->unref();
    SoDB::finish();
    return 0;
}

result: Bounding box min: -0.106837, -0.431453, -0.228986 Bounding box max: 0.106837, 0.431453, 0.228986 Segmentation fault

[belledon]

Got it. The call to SoDB::Finish was redundant after unreffing the SoSeparator. Just to be thorough here is the working version:

#include <Inventor/SoDB.h>
#include <Inventor/SoInput.h>
#include <Inventor/SbBox.h>
#include <Inventor/actions/SoGetBoundingBoxAction.h>
#include <Inventor/nodes/SoSeparator.h>
#include <iostream>

int main(int argc, char**argv)
{
    if (argc < 2)
    {
         std::cout << "Pass filename as argument" <<std::endl;
         return 1;
    }
    //  ---- Read from IV file (or use a SoNode directly instead)  ----
    std::string filename(argv[1]);
    SoDB::init();
    SoInput in;
    in.openFile(filename.c_str());

    SoSeparator *rootsep = SoDB::readAll(&in);
    in.closeFile();
    if (rootsep == NULL)
    {
        std::cout << "Error on read" << filename << std::endl;
        in.closeFile();
        return 1;
    }
    else
    {
    // SoNode  *model = NULL;
    // if (!in.openFile(filename.c_str())
    //     || !SoDB::read(&in, model)
    //     || !model)
    // {
    //     std::cout << "Could not read " << filename << std::endl;
    //     return 1;
    // }

        // --- Construct the bounding box ----

        // viewport required for any viewport-dependent 
        // nodes (eg text), but not required for others

        rootsep->ref();
        SbViewportRegion anyVP(0,0);  
        SoGetBoundingBoxAction bbAction(anyVP);
        bbAction.apply(rootsep);

        SbBox3f bbox = bbAction.getBoundingBox();
        const SbVec3f& min = bbox.getMin();
        const SbVec3f& max = bbox.getMax();
        std::cout <<"Bounding box min: " << min[0] << ", " << min[1] << ", " << min[2] << std::endl;
        std::cout <<"Bounding box max: " << max[0] << ", " << max[1] << ", " << max[2] << std::endl;

        rootsep->unref();
        //SoDB::finish();
        return 0;

    }
}

[JenniferBuehler]

Oh, I saw your comment only now. But I see you already fixed it. Yes I hadn't tested my code snippet before, it was just a rough test, thanks for correcting it. Jenny

[belledon]

Almost done. I tried to run the lastest grasp_plannning but I got a undefined symbol: _ZN7GraspIt19GraspItSceneManager19saveRobotAsInventorERKSsS2_bb I'm assuming I botched something up. I also found the old grasp_planning in my /usr/local/bin. I messed the with cmake in grasp_planning_graspit to include a modified grasp_planner but I have since removed it and did a clean git clone, still resulting in the above error.

[JenniferBuehler]

I'd recommend to avoid installing packages in system directories, and instead put them in custom ones in your home and set LD_LIBRARY_PATH for it. For cmake, you can pass the flag -DCMAKE_PREFIX_PATH= to make it work (and to install, -DCMAKE_INSTALL_PREFIX).

Suggestion: Get rid of all the installed packages (also remove libgrasp_planning_graspit.so and the include files from system dirs) and re-build completely (if you want to make absolutely sure, delete the build directory and re-make everything from scratch).

[belledon]

Thanks! Its all working now. In case I need to add small things to plan_grasps, would all I have to do is add the name of the other grasp_planner sources and targets are in the cmake file? For example I want to add things like changing the number of grasps saved or passing output names but I really don't want to bug you every time something comes up.

[JenniferBuehler]

I hope I understand correctly what you're asking: You'd like to know how to change the program you posted above to integrate a call to the grasp planning, like grasp_planning.cpp is doing.

In this case, you would need to do pretty much what's done in grasp_planning.cpp, and pack the call to plan() along with changing object pose into the loop which iterates through different start object poses. You would need something like this:

cmake_minimum_required(VERSION 2.8.3)
project(plan_grasps)

find_package (grasp_planning_graspit REQUIRED)
include_directories(${grasp_planning_graspit_INCLUDE_DIRS})
add_executable(plan_grasps src/plan_grasps.cpp)
target_link_libraries(simple_planning_tutorial  ${grasp_planning_graspit_LIBRARIES})

However before putting lots of effort in this I would suggest to try manually a few times how much of an effect changing the object pose has at all to the end result. It could be that changing the object pose doesn't actually do that much, and changing the simulated annealing parameters will do the trick instead. If this is the case, I can help you with exposing the parameters, that would require a few changes.

So I'd first try 4 or 5 different x/y/z values (maybe repeat a few times each value combination) as follows:

./grasp_planning --wld <worldfile.xml> --dir <result-dir-xyz-config-i> --save-separate --obj-pos <x> <y> <z> --iter 70000

to get a feel for how much the result changes when changing x/y/z values. You could choose the x/y/z values based on the output of the bounding box program above. Then, if the result really does change significantly, it could be worth writing a new wrapper around the planning algorithm.

[belledon]

I think we're on the same page. What I did before was essentially copy/paste/edit grasp_planning and everywhere I was grasp_planning in the cmake I aslo included the edited version. As of now, I take the 6 axis extremes according to the bounding box (I plan on making this more sophisticated later) and pass them through as positions. There is one issue though, I can't pass negative values without the console thinking I am passing another variable. Do you know of a way around this? It shouldn't matter for most objects and I can always debug it on my end.

I just tested some runs. From a quick glance, the relationship between the hand and the object depends mostly on the objects symmetry. The more axis of symmetry and object has, the more likely the grasps will be mirror images or identical. However it seems that for objects with one a few axis of symmetry, the hand prioritizes approaching the object more so than the lowest energy grasp. I will go in a more in depth tomorrow. Do you know of a way of obtaining the grasp's energy?

[JenniferBuehler]

The negative numbers could maybe have been quoted, but I've changed the call to the command line parsers in any case, and tried it, so now negative numbers are supported (just do a git pull).

You should not need to use the whole CMakeLists.txt as in grasp_planning_graspit. Try the cmake file I posted above. Also, did you see the grasp planning C++ tutorial? This may help you as well. You can use a copy/paste of grasp_planning.cpp for your program of course, and essentially create an advanced version of it. You only need to include grasp_planning_graspit as in the CMakeLists.txt quoted above (see also the CMakeLists.txt in the "tutorial" folder).

[JenniferBuehler]

PS: In the tutorial (link in last post) section "Option 2: Processing the grasp results." you can see how to iterate through the results. You can also get the energy from the EigenGraspResult::getEnergy().

[belledon]

Thanks! I stuck with your quote idea (it accepted the first negative number, but any subsequent negative doubles crashed). Sorry, but I'm still having trouble compiling the modified version. I created the cmake as you described. I also tried to compile the example from the tutorial. Both result in this error:

CMake Error at /home/user/graspit-pkgs/grasp_planning_graspit/cmake/grasp_planning_graspitConfig.cmake:43 (include):
  include could not find load file:

    /home/user/graspit-pkgs/grasp_planning_graspit/cmake/grasp_planning_graspit-targets.cmake
Call Stack (most recent call first):
  CMakeLists.txt:4 (find_package)

SENDL_ERRORLooking for grasp_planning_graspit library -- not foundPlease install grasp_planning_graspit or adjust CMAKE_PREFIX_PATHe.g. cmake -DCMAKE_PREFIX_PATH=/path-to-grasp_planning_graspit/lib ... 

When calling cmake with cmake -DGRASP_PLANNING_GRASPIT_PATH=/home/user/graspit-pkgs/grasp_planning_graspit .. from the build directory of the new script. I also did some variations (after clearing the build folder) including the full path to that library (which was unchanged in .../grasp_planning_graspit/lib)

[JenniferBuehler]

Oh, the -DGRASP_PLANNING_GRASPIT_PATH is obsolete, must be stuck in the documentation somewhere still... thanks for reporting it, will edit it. You should install grasp_planning_graspit (with -DCMAKE_INSTALL_PREFIX=<path> and then make install), and then for compiling your code, use -DCMAKE_PREFIX_PATH=<path>. Let me know if there's still trouble with it and I'll look into it.

You don't need to quote the negative values. I tried it without after my chages. Don't forget to do a git pull and another make && make install.

[JenniferBuehler]

I updated the tutorial. Sorry, I wasn't aware the old cmake variables were still in there. Thanks for letting me know.

[belledon]

Thanks! Also, I forgot to mentioned that I noticed this earlier. I am not using catking but in the CMakeFiles.txt in grasp_planning_graspit/ Enforce catkin is set to true (I always have to set it to false to get it to run without it mentioning something about not finding catkin).

[belledon]

Another quick question, I haven't been able to find a IV converter that can handle the variances in the robot iv files. I was wondering how I could apply the SoToVRML2Action class to save it as a vrml2.0 style file (which I think I can import better). I'm assuming I can just use the base script from the bounding box that you had included earlier as well as the write section of GraspitSceneManager::saveRobotAsInventor ? Essentially I just don't what action to pass to write the file

[JenniferBuehler]

Yes, ENFORCE_CATKIN is currently switched to true while I keep updating the ROS builds (on jenkins it needs to use catkin). I know it's an awkward solution but there's no better way to do this at the moment unfortunately. In a few weeks when the builds are stable this will default to false again. You can also pass -DENFORCE_CATKIN=false to avoid editing the CMakeLists.

For writing VRML, you could try this (copy paste from somewhere else, haven't tested it, but SoToVRML2Action should do it).

SoToVRML2Action *toVRML2Action = new SoToVRML2Action;
toVRML2Action->apply(<the-iv-root>);

SoNode *convertedRoot = (SoNode *) toVRML2Action->getVRML2SceneGraph();
convertedRoot->ref();

SoOutput out;
out.openFile(outputfile);
out.setBinary(FALSE);
out.setHeaderString("#VRML V2.0 utf8");

SoWriteAction writeAction(&out);
writeAction.apply(convertedRoot);

out.closeFile();

[belledon]

Yep this worked fantastically. What kind of data would you like from the grasp transforms (at the moment I have the hand at each of the 8 corners of)? If you're still interested in seeing its effects on grasp planning, I'd be happy to share anything you'd find useful. I'm going to run large scale test soon (Hopefully by the end of this week). (Sorry this was supposed to send yesterday)

[belledon]

I tried compiling my own script with cmake -DCMAKE_PREFIX_PATH=/home/mario/graspit-pkgs/grasp_planning_graspit .. and got the error:

CMake Error at /usr/local/lib/grasp_planning_graspit/grasp_planning_graspitConfig.cmake:73 (find_package):
  By not providing "Findgraspit.cmake" in CMAKE_MODULE_PATH this project has
  asked CMake to find a package configuration file provided by "graspit", but
  CMake did not find one.

  Could not find a package configuration file provided by "graspit" with any
  of the following names:

    graspitConfig.cmake
    graspit-config.cmake

  Add the installation prefix of "graspit" to CMAKE_PREFIX_PATH or set
  "graspit_DIR" to a directory containing one of the above files.  If
  "graspit" provides a separate development package or SDK, be sure it has
  been installed.
Call Stack (most recent call first):
  CMakeLists.txt:4 (find_package)

CMake Error at CMakeLists.txt:4 (find_package):
  Found package configuration file:

    /usr/local/lib/grasp_planning_graspit/grasp_planning_graspitConfig.cmake

  but it set grasp_planning_graspit_FOUND to FALSE so package
  "grasp_planning_graspit" is considered to be NOT FOUND.  Reason given by
  package:

  The following imported targets are referenced, but are missing: graspit

-- Configuring incomplete, errors occurred!
See also "/home/mario/graspit-pkgs/grasp_planning_graspit/om/build/CMakeFiles/CMakeOutput.log".

I also tried the call cmake -DCMAKE_PREFIX_PATH=/home/mario/graspit-pkgs/grasp_planning_graspit/:/home/mario/graspit .. as described in the tutorial (small typo where the colon was a semicolon). When I installed grasp_planning_graspit, I didn't give cmake my graspit path so it built its own. After searching, I found the missing graspitConfig.cmake file under /home/mario/graspit-pkgs/grasp_planning_graspit/build/graspit_external/repo/graspit/cmake. When checking, I also found that there was nothing but a cmake file in .../grasp_planning_graspit/graspit_external. Should the graspit fork be installed there?

[JenniferBuehler]

Hi, I'll reply to your first question in more detail tomorrow as I'm not at home until then. Yes you would need graspit fork installed as it looks you built grasp_planning_graspit with the flag BUILD_GRASPIT=true, I think I had it on true accidently for a few hours, you must have pulled then. Easiest to fix this for now is probably to install graspit fork to same install dir (best if it's not a system dir)

[JenniferBuehler]

I mean BUILD_GRASPIT was false, not true

[belledon]

I just ended up using the master CMakeLists.txt... Also, being able to change the simulated annealing search parameters like you mentioded earlier should be interesting for both of us. I would be happy to implement some modifications assuming its principally through EigenGraspPlanner

[JenniferBuehler]

Yes, I'll look into changing the simulated annealing parameters. If you find something before, you can post a new issue with this topic.

Sorry about the trouble with the cmake files, I tried it again and it works for me. Will try it on my other ubuntu machine soon and see if something needs fixing. Meanwhile, using the master cmakelists.txt which you modified should be fine as well.

Assuming the bounding box issue is resolved, can I close this thread?

[belledon]

Sorry for my late response. Yes you can close the thread! Thanks again. I found some of it in the graspit docs. I'll try to hack something up today.

[JenniferBuehler]

Great, let me know how it goes, I'll get around to look at it tomorrow or Wednesday as well. Feel free to open a new thread with this subject when you find something.