search

moveit / moveit_commander

THIS REPO HAS MOVED TO https://github.com/ros-planning/moveit
11 stars 42 forks source link

Added the calls necessary to manage path constraints. This commit #19

Closed e1000i closed 10 years ago

e1000i commented 10 years ago

This is to close an issue that I open before. https://github.com/ros-planning/moveit_commander/issues/18

I put my hands on and implemented it.

To work depends on the changes commited to moveit_ros package to have access to this functionalities.

If the pull request is accepted, this one should work fine also. https://github.com/ros-planning/moveit_ros/pull/438

Bassically now is possible to set the constraints as a moviet_msgs Constraint message and retrieve an instance of the current path constraints for a group.

sachinchitta commented 10 years ago

Thanks!

pirobot commented 10 years ago

Has this made it into debs yet? I'm running the latest updates under Ubuntu 12.04 and ROS Hydro and if I attempt to run the command

    right_arm.set_path_constraints(constraints)

I get the error:

  File "/opt/ros/hydro/lib/python2.7/dist-packages/moveit_commander/move_group.py", line 347, in set_path_constraints
    if not self._g.set_path_constraints(value):
Boost.Python.ArgumentError: Python argument types in
    MoveGroup.set_path_constraints(MoveGroup, Constraints)
did not match C++ signature:
    set_path_constraints(moveit::planning_interface::MoveGroupWrapper {lvalue}, std::string)
python: /usr/include/boost/thread/pthread/pthread_mutex_scoped_lock.hpp:26: boost::pthread::pthread_mutex_scoped_lock::pthread_mutex_scoped_lock(pthread_mutex_t*): Assertion `!pthread_mutex_lock(m)' failed.
Aborted (core dumped)

My contraints object is defined as follows:

    constraints = Constraints()
    constraints.name = "Upright"
    orientation_constraint = OrientationConstraint()
    orientation_constraint.header = start_pose.header
    orientation_constraint.link_name = right_arm.get_end_effector_link()
    orientation_constraint.orientation = start_pose.pose.orientation
    orientation_constraint.absolute_x_axis_tolerance = 1.0
    orientation_constraint.absolute_y_axis_tolerance = 1.0
    orientation_constraint.absolute_z_axis_tolerance = 6.28 #ignore this axis
    orientation_constraint.weight = 1.0
mikeferguson commented 10 years ago

@pirobot looks like moveit_commander is only 0.5.5 in hydro (http://www.ros.org/debbuild/hydro.html), so no, not in hydro yet.

pirobot commented 10 years ago

@mikeferguson - OK thanks for the clarification.

fivef commented 10 years ago

@pirobot I wasn't sure in which repo to put this problem so I put it here because constraints seem to be the problem. I use the latest MoveIt hydro_devel from source. As soon as I add path constraints MoveIt stops with a segfault while planning. Without constraints everything works as expected. I have been debugging for several days now but wasn't able to find a solution yet. Moveit crashes in the KinematicsBase::initialize() function (see frame 3 below). The same function is called two times when move_group is started by the Kinematics plugin loader. Everytime with correct parameters. Only when I start a move group commander instance, add path constraints and start planning I get the backtrace below. Initialize is called with completely wrong addresses. The strange thing is that I can't find out where initialize is called from. Frame 4 says moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp:175 put there I only find a call to getTipFrame(). Can someone tell me who calls initialize? Could this be a concurrence problem? Does anyone have a running moveit (latest hydro_devel from source) with path constraints enabled? 

#0 0x00007ffff55503b1 in _IO_vfprintf_internal (s=<optimized out>, format=<optimized out>, ap=<optimized out>) at vfprintf.c:1630
#1 0x00007ffff560e4c0 in ___vsnprintf_chk (s=0x7fffb2afc0e0 "moveit.kinematics_base: group_name: 18446744051160973318", maxlen=<optimized out>, flags=1, slen=<optimized out>, format=
0x7fffeb563ea8 "moveit.kinematics_base: group_name: %s", args=0x7fffb2afc0a8) at vsnprintf_chk.c:65
#2 0x00007ffff5de07bb in console_bridge::log(char const*, int, console_bridge::LogLevel, char const*, ...) () from /opt/ros/hydro/lib/libconsole_bridge.so
#3 0x00007fffeb561938 in kinematics::KinematicsBase::initialize (this=0x7fffa8032740, robot_description=<error reading variable: Cannot access memory at address 0x7fffb2afc6afe8>, group_name=
<error reading variable: Cannot access memory at address 0xec834853e589483d>, base_frame="", tip_frames=std::vector of length -22548578297, capacity -22548578303 = {...}, search_discretization=
0) at /home/iki/catkin_ws/src/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h:336
#4 0x00007fffc17df7de in ompl_interface::PoseModelStateSpace::PoseComponent::PoseComponent (this=0x7fffb2afc6d0, subgroup=0x7af4e0, k=...)
at /home/iki/catkin_ws/src/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp:175
#5 0x00007fffc17deb33 in ompl_interface::PoseModelStateSpace::PoseModelStateSpace (this=0x7fffa80302a0, spec=...)
at /home/iki/catkin_ws/src/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp:48
#6 0x00007fffc17e637d in ompl_interface::PoseModelStateSpaceFactory::allocStateSpace (this=0x3777f10, space_spec=...)
at /home/iki/catkin_ws/src/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space_factory.cpp:87
#7 0x00007fffc17de374 in ompl_interface::ModelBasedStateSpaceFactory::getNewStateSpace (this=0x3777f10, space_spec=...)
at /home/iki/catkin_ws/src/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space_factory.cpp:41
#8 0x00007fffc176b1aa in ompl_interface::PlanningContextManager::getPlanningContext(planning_interface::PlannerConfigurationSettings const&, boost::function<boost::shared_ptr<ompl_interface::ModelBasedStateSpaceFactory> const& (std::string const&)> const&) const (this=0x2d0ce50, config=..., factory_selector=...)
at /home/iki/catkin_ws/src/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp:222
#9 0x00007fffc176c25b in ompl_interface::PlanningContextManager::getPlanningContext (this=0x2d0ce50, planning_scene=
(boost::shared_ptr<planning_scene::PlanningScene const>) (count 5, weak count 2) 0x2d16710, req=..., error_code=...)
at /home/iki/catkin_ws/src/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp:349
#10 0x00007fffc175fe9f in ompl_interface::OMPLInterface::getPlanningContext (this=0x2d0cda0, planning_scene=
(boost::shared_ptr<planning_scene::PlanningScene const>) (count 5, weak count 2) 0x2d16710, req=..., error_code=...)
at /home/iki/catkin_ws/src/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp:108
#11 0x00007fffc841dfa5 in ompl_interface::OMPLPlannerManager::getPlanningContext (this=0x3776fc0, planning_scene=
(boost::shared_ptr<planning_scene::PlanningScene const>) (count 5, weak count 2) 0x2d16710, req=..., error_code=...)
at /home/iki/catkin_ws/src/moveit_planners/ompl/ompl_interface/src/ompl_plugin.cpp:106
#12 0x00007fffefe317a4 in planning_request_adapter::(anonymous namespace)::callPlannerInterfaceSolve (planner=0x3776fc0, planning_scene=
(boost::shared_ptr<planning_scene::PlanningScene const>) (count 5, weak count 2) 0x2d16710, req=..., res=...)
at /home/iki/catkin_ws/src/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp:52
pirobot commented 10 years ago

I am also seeing this bug when compiling moveit-core, moveit-ros and moveit-commander from source and using the Python interface to MoveIt Commander. When using the KDL solver and an orientation constraint, the moveit_group node barfs with the error:

[move_group-1] process has died [pid 2654, exit code -11, cmd /home/patrick/catkin_ws/devel/lib/moveit_ros_move_group/move_group __name:=move_group __log:=/home/patrick/.ros/log/a1584ebe-0eaa-11e4-82ed-f8b156b5e743/move_group-1.log]. log file: /home/patrick/.ros/log/a1584ebe-0eaa-11e4-82ed-f8b156b5e743/move_group-1*.log

When using an IKFast solver for the same arm and the same orientation constraint, I get the error:

[ERROR] [1405708943.503712694]: This kinematic solver does not support initialization with more than one tip frames [move_group-1] process has died [pid 1935, exit code -11, cmd /home/patrick/catkin_ws/devel/lib/moveit_ros_move_group/move_group __name:=move_group __log:=/home/patrick/.ros/log/a1584ebe-0eaa-11e4-82ed-f8b156b5e743/move_group-1.log]. log file: /home/patrick/.ros/log/a1584ebe-0eaa-11e4-82ed-f8b156b5e743/move_group-1*.log

NOTE: This bug does not exist in shadow fixed. And in the latest binary release Debs, a different bug exists (but is fixed in shadow fixed):

Traceback (most recent call last): File "/home/patrick/catkin_ws/src/rbx2/rbx2_arm_nav/scripts/moveit_constraints_demo.py", line 170, in MoveItDemo() File "/home/patrick/catkin_ws/src/rbx2/rbx2_arm_nav/scripts/moveit_constraints_demo.py", line 131, in init right_arm.set_path_constraints(constraints) File "/opt/ros/hydro/lib/python2.7/dist-packages/moveit_commander/move_group.py", line 347, in set_path_constraints if not self._g.set_path_constraints(value): Boost.Python.ArgumentError: Python argument types in MoveGroup.set_path_constraints(MoveGroup, Constraints) did not match C++ signature: set_path_constraints(moveit::planning_interface::MoveGroupWrapper {lvalue}, std::string) python: /usr/include/boost/thread/pthread/pthread_mutex_scoped_lock.hpp:26: boost::pthread::pthread_mutex_scoped_lock::pthread_mutex_scoped_lock(pthread_mutex_t*): Assertion `!pthread_mutex_lock(m)' failed. Aborted (core dumped)

fivef commented 10 years ago

Ok, so at least I am not alone. @davetcoleman could you help us concerning the last two posts? You seem to be quite into Kinematics Base.

davetcoleman commented 10 years ago

I'm confused by the statement

NOTE: This bug does not exist in shadow fixed. And in the latest binary release Debs, a different bug exists (but is fixed in shadow fixed):

and

I am also seeing this bug when compiling moveit-core, moveit-ros and moveit-commander from source and using the Python interface to MoveIt Commander.

I never use MoveIt commander and avoid python as much as possible, but I could take a look if you give me exact instructions to reproduce the bug with a particular robot's URDF and moveit_config package

pirobot commented 10 years ago

Thanks Dave. I just saw there was an update to the Hydro Precise 64-bit Debs for MoveIt (which I have now installed). The update fixes the constraints bug. What's more, a source install also seems to work now with constraints. So at least on my machine, life is good again.

fivef commented 10 years ago

Sounds great! Thanks Dave for the offer! As it works now for pirobot I will try this asap. @pirobot perhaps the problem somehow wasn't related to the packages we had installed from source. This would explain why it works now also from source. Another question: Which ik and which planner do you use? Are orientation constraints useable for you? I had only tried it with RRTConnect and KDL with very bad results. Actually I was only able to really generate a feasible trajectory once. Now I wanted to give it a second shot with Levenberg-Marquard ik when this problem occurred.

2014-07-22 16:46 GMT+02:00 pirobot notifications@github.com:

Thanks Dave. I just saw there was an update to the Hydro Precise 64-bit Debs for MoveIt (which I have now installed). The update fixes the constraints bug. What's more, a source install also seems to work now with constraints. So at least on my machine, life is good again.

Reply to this email directly or view it on GitHub https://github.com/ros-planning/moveit_commander/pull/19#issuecomment-49748844 .

pirobot commented 10 years ago

@fivef I am using the default planner and I can get orientation constraints to work with both KDL and IKFast although IKFast is much faster (of course!)