OMPL planning pipeline crashes when using path constraints

[Stefan Kohlbrecher]

Hi everyone,

I´m using the MoveIt! API with a custom move_group plugin to have more control over planning scene settings. I now tried adding 3 joint constraints that should be used during planning. Whenever those are set, OMPL crashes during planning. See attached screenshots of debugging using QtCreator. The path_constraints that have been set are visible in the top right, while the (not very informative) backtrace is in the lower left. They look sane to me, so I´m at a loss as to why planning crashes (and even if they didn´t look sane, it shouldn´t crash). Producing a proper backtrace would require me to build OMPL from source I guess, so I wanted to hold off on that until absolutely necessary. Any ideas what could be wrong with my path_constraints? If do not set those, everything works fine.

regards,

Stefan

[Stefan Kohlbrecher]

Setting all weights differently also does not improve things. Setup is current hydro_devel MoveIt! from source/Ubuntu 12.04/64Bit.

[Stefan Kohlbrecher]

Given that what I´m trying to achieve essentially is the same as temporarily giving the underlying RobotModel different joint limits temporarily, I´m inclined to try that. Any hints on how to best achieve this are appreciated. I´d especially like to know if I have to copy the whole planning scene or what other options are there (given that most things are const, which makes sense, unless one wants to work around issues like in my case ;) ).

[Stefan Kohlbrecher]

-After searching around some more, I came across Sachin´s recommendation to set the workspace bounds here: http://answers.ros.org/question/62396/plan-self-motion-in-moveit/

This does not prevent the crashed either, unfortunately.

-I also tried the superhacky way of 

  const robot_model::RobotModelConstPtr& robot_model_const = planning_scene->getRobotModel();

  boost::shared_ptr<robot_model::RobotModel> robot_model (boost::const_pointer_cast<robot_model::RobotModel>(robot_model_const));

  //Change robot_model VariableBounds here

and it sort of works, but this clearly is very unclean.

Am Freitag, 22. November 2013 07:54:23 UTC-5 schrieb Stefan Kohlbrecher:

[Ioan Sucan]

Stefan,

Thank you for your detailed e-mails! This is very useful information. From your description I have a feeling your problem is related to this bug:
https://github.com/ros-planning/moveit_ros/issues/366

I suspect that if you constrain (using JointConstraints) the limits of some joints, but not all the joints in the group, there is an indexing error and memory gets overwritten and all sorts of bad things happen. I unfortunately did not have a chance to look at this yet. I suspect the bug is in the constraints_sampling library, in moveit_core.

Just in case my hunch is correct, please use constraints for all joints in your group (except fixed joints, if any, of course). For the joints you do not want to constrain, just use the full range of the joint. If you could report whether this works or not, that would be very useful.

Sorry for all the trouble this is causing you! We really appreciate the feedback though!

Ioan

[Stefan Kohlbrecher]

Hi Ioan,

thanks for the pointer! Will look into it and report back. Not immediately, but likely in the next 2 days.

regards,

Stefan

[Stefan Kohlbrecher]

Ok, I can confirm that setting JointConstraints for all joints in the used group fixes the crashes ..99% of the time. There appears to be a corner case where the FixStartStatePathConstraints planning_request_adapter will crash if the start state violates the path constraints and it has to perform work (see semi-informative backtrace below). That only happens occassionally though, oftentimes it runs without crashing, even when fixing start state. The start states it generates are often very far away from both the (constraints violating) original start state and the goal state, however.

I also noticed that I (often, or even always) get "Attempted to merge incompatible constraints for joint XY. Discarding constraint." when the FixStartStatePathConstraints has done work and the path from new start state to original goal is performed.

Thread 7 (Thread 0x7ffc0e7fc700 (LWP 7924)):

#0  0x00007ffc24b72425 in __GI_raise (sig=<optimized out>) at ../nptl/sysdeps/unix/sysv/linux/raise.c:64

        resultvar = 0

        pid = <optimized out>

        selftid = 7924

#1  0x00007ffc24b75b8b in __GI_abort () at abort.c:91

        save_stage = 2

        act = {__sigaction_handler = {sa_handler = 0x4, sa_sigaction = 0x4}, sa_mask = {__val = {5, 140720966396336, 31, 140720925795063, 1, 140720924319953, 5, 140720925799307, 3, 140720551713758, 2, 140720925795010, 1, 140720925804007, 3, 140720551713732}}, sa_flags = 12, sa_restorer = 0x7ffc24cb85eb}

        sigs = {__val = {32, 0 <repeats 15 times>}}

#2  0x00007ffc24bb039e in __libc_message (do_abort=2, fmt=0x7ffc24cba748 "*** glibc detected *** %s: %s: 0x%s ***\n") at ../sysdeps/unix/sysv/linux/libc_fatal.c:201

        ap = {{gp_offset = 40, fp_offset = 48, overflow_arg_area = 0x7ffc0e7f6540, reg_save_area = 0x7ffc0e7f6450}}

        ap_copy = {{gp_offset = 16, fp_offset = 48, overflow_arg_area = 0x7ffc0e7f6540, reg_save_area = 0x7ffc0e7f6450}}

        fd = 2

        on_2 = <optimized out>

        list = <optimized out>

        nlist = <optimized out>

        cp = <optimized out>

        written = <optimized out>

#3  0x00007ffc24bbab96 in malloc_printerr (action=3, str=0x7ffc24cba910 "free(): invalid next size (fast)", ptr=<optimized out>) at malloc.c:5039

        buf = "00007ffbf8067df0"

        cp = <optimized out>

#4  0x00007ffc0c0b4c26 in constraint_samplers::JointConstraintSampler::~JointConstraintSampler() () from /home/kohlbrecher/flor_repo/catkin_ws/devel/lib/libmoveit_constraint_samplers.so

No symbol table info available.

#5  0x00007ffc0c0b4cd9 in constraint_samplers::JointConstraintSampler::~JointConstraintSampler() () from /home/kohlbrecher/flor_repo/catkin_ws/devel/lib/libmoveit_constraint_samplers.so

No symbol table info available.

#6  0x0000000000410c29 in boost::detail::shared_count::~shared_count() ()

No symbol table info available.

#7  0x00007ffc0c90c23a in ompl_interface::ConstrainedGoalSampler::~ConstrainedGoalSampler() () from /home/kohlbrecher/flor_repo/catkin_ws/devel/lib/libmoveit_ompl_interface.so

No symbol table info available.

#8  0x0000000000410c29 in boost::detail::shared_count::~shared_count() ()

No symbol table info available.

#9  0x00007ffc0c8ee4d9 in ompl_interface::ModelBasedPlanningContext::clear() () from /home/kohlbrecher/flor_repo/catkin_ws/devel/lib/libmoveit_ompl_interface.so

No symbol table info available.

#10 0x00007ffc0c8c83b9 in ompl_interface::PlanningContextManager::getPlanningContext(boost::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void> > const&, moveit_msgs::MoveItErrorCodes_<std::allocator<void> >&) const () from /home/kohlbrecher/flor_repo/catkin_ws/devel/lib/libmoveit_ompl_interface.so

No symbol table info available.

#11 0x00007ffc0c8bda40 in ompl_interface::OMPLInterface::getPlanningContext(boost::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void> > const&, moveit_msgs::MoveItErrorCodes_<std::allocator<void> >&) const () from /home/kohlbrecher/flor_repo/catkin_ws/devel/lib/libmoveit_ompl_interface.so

No symbol table info available.

#12 0x00007ffc0cb505c7 in ompl_interface::OMPLPlannerManager::getPlanningContext(boost::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void> > const&, moveit_msgs::MoveItErrorCodes_<std::allocator<void> >&) const () from /home/kohlbrecher/flor_repo/catkin_ws/devel/lib/libmoveit_ompl_planner_plugin.so

No symbol table info available.

#13 0x00007ffc1fe89f81 in planning_request_adapter::(anonymous namespace)::callPlannerInterfaceSolve(planning_interface::PlannerManager const*, boost::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&) () from /home/kohlbrecher/flor_repo/catkin_ws/devel/lib/libmoveit_planning_request_adapter.so

No symbol table info available.

#14 0x00007ffbf795dc2c in default_planner_request_adapters::FixStartStatePathConstraints::adaptAndPlan(boost::function<bool (boost::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&)> const&, boost::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&, std::vector<unsigned long, std::allocator<unsigned long> >&) const () from /home/kohlbrecher/flor_repo/catkin_ws/devel/lib/libmoveit_default_planning_request_adapter_plugins.so

No symbol table info available.

#15 0x00007ffc1fe8a0ad in planning_request_adapter::PlanningRequestAdapter::adaptAndPlan(boost::shared_ptr<planning_interface::PlannerManager> const&, boost::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&, std::vector<unsigned long, std::allocator<unsigned long> >&) const () from /home/kohlbrecher/flor_repo/catkin_ws/devel/lib/libmoveit_planning_request_adapter.so

No symbol table info available.

#16 0x00007ffc1fe8a159 in planning_request_adapter::(anonymous namespace)::callAdapter1(planning_request_adapter::PlanningRequestAdapter const*, boost::shared_ptr<planning_interface::PlannerManager> const&, boost::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&, std::vector<unsigned long, std::allocator<unsigned long> >&) () from /home/kohlbrecher/flor_repo/catkin_ws/devel/lib/libmoveit_planning_request_adapter.so

No symbol table info available.

#17 0x00007ffbf79565c8 in default_planner_request_adapters::FixStartStateCollision::adaptAndPlan(boost::function<bool (boost::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&)> const&, boost::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&, std::vector<unsigned long, std::allocator<unsigned long> >&) const () from /home/kohlbrecher/flor_repo/catkin_ws/devel/lib/libmoveit_default_planning_request_adapter_plugins.so

No symbol table info available.

#18 0x00007ffc1fe8a34a in planning_request_adapter::(anonymous namespace)::callAdapter2(planning_request_adapter::PlanningRequestAdapter const*, boost::function<bool (boost::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&)> const&, boost::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&, std::vector<unsigned long, std::allocator<unsigned long> >&) () from /home/kohlbrecher/flor_repo/catkin_ws/devel/lib/libmoveit_planning_request_adapter.so

No symbol table info available.

#19 0x00007ffbf7952cad in default_planner_request_adapters::FixStartStateBounds::adaptAndPlan(boost::function<bool (boost::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&)> const&, boost::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&, std::vector<unsigned long, std::allocator<unsigned long> >&) const () from /home/kohlbrecher/flor_repo/catkin_ws/devel/lib/libmoveit_default_planning_request_adapter_plugins.so

No symbol table info available.

#20 0x00007ffc1fe8a34a in planning_request_adapter::(anonymous namespace)::callAdapter2(planning_request_adapter::PlanningRequestAdapter const*, boost::function<bool (boost::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&)> const&, boost::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&, std::vector<unsigned long, std::allocator<unsigned long> >&) () from /home/kohlbrecher/flor_repo/catkin_ws/devel/lib/libmoveit_planning_request_adapter.so

No symbol table info available.

#21 0x00007ffc1fe8d457 in boost::function3<bool, boost::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&>::operator()(boost::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&) const () from /home/kohlbrecher/flor_repo/catkin_ws/devel/lib/libmoveit_planning_request_adapter.so

No symbol table info available.

#22 0x00007ffbf795fcf7 in default_planner_request_adapters::FixWorkspaceBounds::adaptAndPlan(boost::function<bool (boost::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&)> const&, boost::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&, std::vector<unsigned long, std::allocator<unsigned long> >&) const () from /home/kohlbrecher/flor_repo/catkin_ws/devel/lib/libmoveit_default_planning_request_adapter_plugins.so

No symbol table info available.

#23 0x00007ffc1fe8a34a in planning_request_adapter::(anonymous namespace)::callAdapter2(planning_request_adapter::PlanningRequestAdapter const*, boost::function<bool (boost::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&)> const&, boost::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&, std::vector<unsigned long, std::allocator<unsigned long> >&) () from /home/kohlbrecher/flor_repo/catkin_ws/devel/lib/libmoveit_planning_request_adapter.so

No symbol table info available.

#24 0x00007ffbf7960da9 in default_planner_request_adapters::AddTimeParameterization::adaptAndPlan(boost::function<bool (boost::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&)> const&, boost::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&, std::vector<unsigned long, std::allocator<unsigned long> >&) const () from /home/kohlbrecher/flor_repo/catkin_ws/devel/lib/libmoveit_default_planning_request_adapter_plugins.so

No symbol table info available.

#25 0x00007ffc1fe8a34a in planning_request_adapter::(anonymous namespace)::callAdapter2(planning_request_adapter::PlanningRequestAdapter const*, boost::function<bool (boost::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&)> const&, boost::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&, std::vector<unsigned long, std::allocator<unsigned long> >&) () from /home/kohlbrecher/flor_repo/catkin_ws/devel/lib/libmoveit_planning_request_adapter.so

No symbol table info available.

#26 0x00007ffc1fe8bd7c in planning_request_adapter::PlanningRequestAdapterChain::adaptAndPlan(boost::shared_ptr<planning_interface::PlannerManager> const&, boost::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&, std::vector<unsigned long, std::allocator<unsigned long> >&) const () from /home/kohlbrecher/flor_repo/catkin_ws/devel/lib/libmoveit_planning_request_adapter.so

No symbol table info available.

#27 0x00007ffc24494e30 in planning_pipeline::PlanningPipeline::generatePlan(boost::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&, std::vector<unsigned long, std::allocator<unsigned long> >&) const () from /home/kohlbrecher/flor_repo/catkin_ws/devel/lib/libmoveit_planning_pipeline.so

No symbol table info available.

#28 0x00007ffc244969a7 in planning_pipeline::PlanningPipeline::generatePlan(boost::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&) const () from /home/kohlbrecher/flor_repo/catkin_ws/devel/lib/libmoveit_planning_pipeline.so

No symbol table info available.

Am Dienstag, 26. November 2013 16:36:43 UTC-5 schrieb Ioan Alexandru Sucan:

[Ioan Sucan]

Stefan,

This is very useful. I think this confirms the bug is indeed in setting up the sampler. I will take a look at this next week. In the meantime, you can disable the fix start states plugin by removing its name from move_group.launch. Sorry this is causing you trouble!

Ioan

[Dave Coleman]

Fixed in 

https://github.com/ros-planning/moveit_core/pull/151