How to use gazebo world file objects for planning with Moveit

[Bahar]

Hello,

I already asked this question in ROS answers, but I wanted to post in here as well, just in case. I was wondering if there is a way to use the gazebo world objects in the .world file for planning with Moveit in code? When I write them as collision objects within C++ code, they appear in rviz but not in gazebo, that's why I want to use the world file instead, so I can use those objects to do pick and place operations with them. Or is there a way to make the collision objects in code appear in gazebo as well?

Also is there a way to disable collision checking for collision objects defined in gazebo world? I tried using the code written here 

http://moveit.ros.org/wiki/Environment_Representation/C%2B%2B_API

but it doesn't return any collision as a collision_result even before applying it, so it isn't working for some reason and the arm 'escapes' from the object (doesn't go to the given position or goes to a new position when in collision with the object) even without using trajectory planning. 

Any suggestions?

Thanks.

Bahar

[Jonathan Bohren]

This is a gazebo plugin that publishes models in the world as a moveit planning scene message:

https://github.com/jhu-lcsr-forks/gazebo_ros_pkgs/blob/hydro-devel/gazebo_plugins/include/gazebo_plugins/gazebo_ros_moveit_planning_scene.h#L50-L82

-j

[Bahar]

Thank you very much for your quick response. I used the plugin, and it works great at recognizing the collision objects, however, I am having trouble in the group.pick(objectName, grasps) command of moveit. The errors I'm getting are:

No end-effector specified for pick action

Fail: ABORTED: Must specify group in motion plan request

I checked the group object that I'm using and getEndEffectorLink() returns the correct link and the "arm" is specified for the group object. 

Also how can I copy the collision object that I read the name from planning_scene_interface.getKnownObjectNames to moveit_msgs::AttachedCollisionObject? Or is there another direct way to read the collision object?

Thanks again.

Bahar

[Bahar]

Hi Jonathan,

The plugin you recommended makes rviz+moveit (motion planning) give a segmentation fault and close. Here is the output I get from gdb:

Program received signal SIGSEGV, Segmentation fault.

0x0000000000000000 in ?? ()

#0  0x0000000000000000 in ?? ()

#1  0x00007fff8d3d0344 in ?? () from /opt/ros/indigo/lib/libccd.so.2

#2  0x00007fff8d3d1477 in ccdMPRPenetration ()

   from /opt/ros/indigo/lib/libccd.so.2

#3  0x00007fff8e03b9b8 in fcl::details::GJKCollide(void*, void (*)(void const*, ---Type <return> to continue, or q <return> to quit---

_ccd_vec3_t const*, _ccd_vec3_t*), void (*)(void const*, _ccd_vec3_t*), void*, void (*)(void const*, _ccd_vec3_t const*, _ccd_vec3_t*), void (*)(void const*, _ccd_vec3_t*), unsigned int, double, fcl::Vec3fX<fcl::details::Vec3Data<double> >*, double*, fcl::Vec3fX<fcl::details::Vec3Data<double> >*) ()

   from /opt/ros/indigo/lib/libfcl.so

#4  0x00007fff8df34dd1 in fcl::ShapeCollisionTraversalNode<fcl::Plane, fcl::Cylinder, fcl::GJKSolver_libccd>::leafTesting(int, int) const ()

   from /opt/ros/indigo/lib/libfcl.so

#5  0x00007fff8ded5891 in unsigned long fcl::ShapeShapeCollide<fcl::Plane, fcl::Cylinder, fcl::GJKSolver_libccd>(fcl::CollisionGeometry const*, fcl::Transform3f const&, fcl::CollisionGeometry const*, fcl::Transform3f const&, fcl::GJKSolver_libccd const*, fcl::CollisionRequest const&, fcl::CollisionResult&) ()

   from /opt/ros/indigo/lib/libfcl.so

#6  0x00007fff8e02ce06 in unsigned long fcl::collide<fcl::GJKSolver_libccd>(fcl::CollisionGeometry const*, fcl::Transform3f const&, fcl::CollisionGeometry const*, fcl::Transform3f const&, fcl::GJKSolver_libccd const*, fcl::CollisionRequest const&, fcl::CollisionResult&) () from /opt/ros/indigo/lib/libfcl.so

#7  0x00007fff8e02cb2a in fcl::collide(fcl::CollisionObject const*, fcl::CollisionObject const*, fcl::CollisionRequest const&, fcl::CollisionResult&) ()

   from /opt/ros/indigo/lib/libfcl.so

#8  0x00007fff8f5ba475 in collision_detection::collisionCallback(fcl::CollisionObject*, fcl::CollisionObject*, void*) ()

   from /opt/ros/indigo/lib/libmoveit_collision_detection_fcl.so

---Type <return> to continue, or q <return> to quit---

#9  0x00007fff8e05a843 in fcl::details::dynamic_AABB_tree::collisionRecurse(fcl::NodeBase<fcl::AABB>*, fcl::CollisionObject*, void*, bool (*)(fcl::CollisionObject*, fcl::CollisionObject*, void*)) () from /opt/ros/indigo/lib/libfcl.so

#10 0x00007fff8e05a843 in fcl::details::dynamic_AABB_tree::collisionRecurse(fcl::NodeBase<fcl::AABB>*, fcl::CollisionObject*, void*, bool (*)(fcl::CollisionObject*, fcl::CollisionObject*, void*)) () from /opt/ros/indigo/lib/libfcl.so

#11 0x00007fff8e05a843 in fcl::details::dynamic_AABB_tree::collisionRecurse(fcl::NodeBase<fcl::AABB>*, fcl::CollisionObject*, void*, bool (*)(fcl::CollisionObject*, fcl::CollisionObject*, void*)) () from /opt/ros/indigo/lib/libfcl.so

#12 0x00007fff8e05cc12 in fcl::DynamicAABBTreeCollisionManager::collide(fcl::CollisionObject*, void*, bool (*)(fcl::CollisionObject*, fcl::CollisionObject*, void*)) const () from /opt/ros/indigo/lib/libfcl.so

#13 0x00007fff8f5c9c78 in collision_detection::CollisionWorldFCL::checkRobotCollisionHelper(collision_detection::CollisionRequest const&, collision_detection::CollisionResult&, collision_detection::CollisionRobot const&, moveit::core::RobotState const&, collision_detection::AllowedCollisionMatrix const*) const ()

   from /opt/ros/indigo/lib/libmoveit_collision_detection_fcl.so

#14 0x00007fff99492181 in planning_scene::PlanningScene::checkCollision(collision_detection::CollisionRequest const&, collision_detection::CollisionResult&, moveit::core::RobotState const&, collision_detection::AllowedCollisionMatrix const&) const () from /opt/ros/indigo/lib/libmoveit_planning_scene.so

#15 0x00007fff994932b5 in planning_scene::PlanningScene::getCollidingPairs(std::map<std::pair<std::string, std::string>, std::vector<collision_detection::Contac---Type <return> to continue, or q <return> to quit---

t, std::allocator<collision_detection::Contact> >, std::less<std::pair<std::string, std::string> >, std::allocator<std::pair<std::pair<std::string, std::string> const, std::vector<collision_detection::Contact, std::allocator<collision_detection::Contact> > > > >&, moveit::core::RobotState const&, collision_detection::AllowedCollisionMatrix const&) const ()

   from /opt/ros/indigo/lib/libmoveit_planning_scene.so

#16 0x00007fff9949669c in planning_scene::PlanningScene::getCollidingLinks(std::vector<std::string, std::allocator<std::string> >&, moveit::core::RobotState const&, collision_detection::AllowedCollisionMatrix const&) const ()

   from /opt/ros/indigo/lib/libmoveit_planning_scene.so

#17 0x00007fff8c32f026 in moveit_rviz_plugin::MotionPlanningDisplay::drawQueryStartState() ()

   from /opt/ros/indigo/lib/libmoveit_motion_planning_rviz_plugin_core.so

#18 0x00007fff8c32f760 in moveit_rviz_plugin::MotionPlanningDisplay::changedQueryStartState() ()

   from /opt/ros/indigo/lib/libmoveit_motion_planning_rviz_plugin_core.so

#19 0x00007fff9a2a1bc6 in moveit_rviz_plugin::PlanningSceneDisplay::executeMainLoopJobs() ()

   from /opt/ros/indigo/lib/libmoveit_planning_scene_rviz_plugin_core.so

#20 0x00007fff9a2a1fc9 in moveit_rviz_plugin::PlanningSceneDisplay::update(float, float) ()

   from /opt/ros/indigo/lib/libmoveit_planning_scene_rviz_plugin_core.so

#21 0x00007ffff7aa7692 in rviz::DisplayGroup::update(float, float) ()

---Type <return> to continue, or q <return> to quit---

   from /opt/ros/indigo/lib/librviz.so

#22 0x00007ffff7b48bbe in rviz::VisualizationManager::onUpdate() ()

   from /opt/ros/indigo/lib/librviz.so

#23 0x00007ffff2eeb87a in QMetaObject::activate(QObject*, QMetaObject const*, int, void**) () from /usr/lib/x86_64-linux-gnu/libQtCore.so.4

#24 0x00007ffff2eefa31 in QObject::event(QEvent*) ()

   from /usr/lib/x86_64-linux-gnu/libQtCore.so.4

#25 0x00007ffff6edce2c in QApplicationPrivate::notify_helper(QObject*, QEvent*)

    () from /usr/lib/x86_64-linux-gnu/libQtGui.so.4

#26 0x00007ffff6ee34a0 in QApplication::notify(QObject*, QEvent*) ()

   from /usr/lib/x86_64-linux-gnu/libQtGui.so.4

#27 0x00007ffff2ed74dd in QCoreApplication::notifyInternal(QObject*, QEvent*)

    () from /usr/lib/x86_64-linux-gnu/libQtCore.so.4

#28 0x00007ffff2f07323 in ?? () from /usr/lib/x86_64-linux-gnu/libQtCore.so.4

#29 0x00007ffff2f045f1 in ?? () from /usr/lib/x86_64-linux-gnu/libQtCore.so.4

#30 0x00007ffff184ae04 in g_main_context_dispatch ()

   from /lib/x86_64-linux-gnu/libglib-2.0.so.0

#31 0x00007ffff184b048 in ?? () from /lib/x86_64-linux-gnu/libglib-2.0.so.0

#32 0x00007ffff184b0ec in g_main_context_iteration ()

   from /lib/x86_64-linux-gnu/libglib-2.0.so.0

#33 0x00007ffff2f047be in QEventDispatcherGlib::processEvents(QFlags<QEventLoop::ProcessEventsFlag>) () from /usr/lib/x86_64-linux-gnu/libQtCore.so.4

#34 0x00007ffff6f7ebe6 in ?? () from /usr/lib/x86_64-linux-gnu/libQtGui.so.4

---Type <return> to continue, or q <return> to quit---

#35 0x00007ffff2ed60af in QEventLoop::processEvents(QFlags<QEventLoop::ProcessEventsFlag>) () from /usr/lib/x86_64-linux-gnu/libQtCore.so.4

#36 0x00007ffff2ed63a5 in QEventLoop::exec(QFlags<QEventLoop::ProcessEventsFlag>) () from /usr/lib/x86_64-linux-gnu/libQtCore.so.4

#37 0x00007ffff2edbb79 in QCoreApplication::exec() ()

   from /usr/lib/x86_64-linux-gnu/libQtCore.so.4

#38 0x0000000000400bc7 in main ()

1 events.c: No such file or directory.

Also when I launch the move group with the plugin, there is a warning with 

Returning dirty link transforms

for the links, but this warning doesn't come up without the plugin. Would this create any problems?

I checked and I am able to use the motion planning without problems when I don't the add the plugin, so I think there might be a bug with the plugin. Could you please help me on that as well?

Thanks.

Bahar

[Jonathan Bohren]

On Sun, Jun 14, 2015 at 7:32 AM Bahar <b.ir...@gmail.com> wrote:

The plugin you recommended makes rviz+moveit (motion planning) give a segmentation fault and close. Here is the output I get from gdb:

Bahar,

I'm sorry you're experiencing these problems, but I've never encountered them in my use of the plugin. Obviously there's some disagreement between Gazebo and FCL's mesh representation, or the interpretation of such by the plugin in some cases. Unfortunately, a segfault in FCL doesn't help determine much more than that.

If you need help resolving this issue, I would suggest opening a ticket and providing a complete, self-contained minimal example which causes the crash. I don't have the time right now to help you with this, but if someone else does, that's what they'll need.

If you put together a ROS package which provides a minimal example of the crash, you can host it for free on GitHub, then you can create a ticket with a link to the example here: https://github.com/ros-simulation/gazebo_ros_pkgs/issues/new

best,

-jon

[Bahar]

Thank you Jonathan, I'll put together a package as soon as possible. Could you also help me on my previous question? How can I copy the collision object that I read the name from planning_scene_interface.getKnownObjectNames to moveit_msgs::AttachedCollisionObject? Or is there another direct way to read the collision object?

Thank you so much.

Regards,

Bahar

[William Woodall]

Bahar,

I know this is an old thread, but I think the issue you were running into is this issue:

https://github.com/flexible-collision-library/fcl/issues/12

Which I think was fixed in this pr:

https://github.com/flexible-collision-library/fcl/pull/53

That should be in versions of fcl >= 0.4.0. Unfortunately, Indigo still has 0.3.2.

I ran into this issue, like you, when using Jonathan's neat little plugin. Most likely this plugin exposed the issue, since the error only happens when checking the collision between a plane and a cylinder, I guess the ground plane from gazebo, which is a plane, is what triggers the problem. Since most people use boxes rather than planes in the moveit planning scene, this might be one of the few times this issue arises.

Hopefully this will help others that run into this issue.

[MUNEEB UL HAQ]

i am getting the same problem with my pick ..have you solved this problem ?

On Saturday, June 13, 2015 at 5:08:12 AM UTC+3, Bahar wrote:

[stan25]

Hi Bahar,

I am facing the same main problem of importing world file to RViz MoveIt so thanks for the thoughtful question. Since you have successfully used the plugin, would you mind if I ask some questions:

1. Jonathan provided the plugin as a header file (.h). It seems that we can only compile .cc and .cpp files , not .h. How do you deal with this?
2. How does a successful use of the plugin look like? I assumed that the world object would appear with the robot when we run MoveIt on RViz but having tried to use this plugin, I do not see any world object.

Thank you!

Stan