Hi Konrad,
I think I might have an answer now. Could you please follow these two
steps and let me know if things work for you:
(1) Add the following lines to your urdf
<link name="world">
<inertial>
<mass value="0.05" />
<origin xyz="-0.0 0.0 -10.0" />
<inertia ixx="1.0" ixy="0.0" ixz="0.0"
iyy="1.0" iyz="0.0"
izz="1.0" />
</inertial>
<collision>
<origin xyz="0 0 0.15" rpy="0 0 0" />
<geometry>
<box size="0.4 0.4 0.3" />
</geometry>
</collision>
</link>
<joint name="base_joint" type="fixed">
<parent link="world"/>
<child link="base_link"/>
</joint>
This anchors your robot in the world with a dummy fixed link.
(2) Apply the attached patch to a cturtle version of the
motion_planners stack directory. (I am assuming you are working with
cturtle).
Try again with these changes and let me know - if they work for you, I
will push them out into cturtle. For your reference, I have also
attached a tarred version of the planning files that I used to
configure the planner for your robot. Some of these issues are
currently fixed in trunk but it might take us a while to push out a
release from trunk into cturtle, hence the patch.
Regards,
Sachin
On Mon, Oct 18, 2010 at 11:58 PM, Konrad Banachowicz <
konradb3@gmail.com> wrote:
> Here is my urdf.
> Pozdrawiam
> Konrad Banachowicz
>
>
> 2010/10/19 Konrad Banachowicz <konradb3@gmail.com>
>>
>> Pozdrawiam
>> Konrad Banachowicz
>>
>>
>> 2010/10/19 Sachin Chitta <sachinc@willowgarage.com>
>>>
>>> Hi Konrad,
>>>
>>> Could you please send me a urdf for your manipulator and I will try to
>>> reproduce your problem.
>>>
>>> Regards,
>>> Sachin
>>>
>>> On Sun, Oct 17, 2010 at 9:23 AM, Konrad Banachowicz <konradb3@gmail.com>
>>> wrote:
>>> > Hi all,
>>> > I have problem with running ompl motion planner on our irb-6
>>> > manipulator.
>>> > When i send a goal to move arm ompl-planning node ends with segfault.
>>> > This is very strange to me because stomp-motion-planner works for me.
>>> > Here is backtrace :
>>> > GNU gdb (GDB) 7.1-ubuntu
>>> > Copyright (C) 2010 Free Software Foundation, Inc.
>>> > License GPLv3+: GNU GPL version 3 or later
>>> > <http://gnu.org/licenses/gpl.html>
>>> > This is free software: you are free to change and redistribute it.
>>> > There is NO WARRANTY, to the extent permitted by law. Type "show
>>> > copying"
>>> > and "show warranty" for details.
>>> > This GDB was configured as "x86_64-linux-gnu".
>>> > For bug reporting instructions, please see:
>>> > <http://www.gnu.org/software/gdb/bugs/>...
>>> > Reading symbols from
>>> >
>>> > /opt/ros/cturtle/stacks/motion_planners/ompl_planning/motion_planner...done.
>>> > (gdb) r
>>> > Starting program:
>>> > /opt/ros/cturtle/stacks/motion_planners/ompl_planning/motion_planner
>>> > __name:=motion_planner
>>> >
>>> > __log:=/home/konrad/.ros/log/479ff7be-d9de-11df-87a9-485b397ce758/motion_planner-1.log
>>> > [Thread debugging using libthread_db enabled]
>>> > [New Thread 0x7fffebbbf710 (LWP 9194)]
>>> > [New Thread 0x7fffeb3be710 (LWP 9195)]
>>> > [New Thread 0x7fffeabbd710 (LWP 9196)]
>>> > [New Thread 0x7fffea3bc710 (LWP 9201)]
>>> > [New Thread 0x7fffe9bbb710 (LWP 9204)]
>>> > [ WARN] [1287330043.490047007]: No default collision operations
>>> > specified
>>> > [ INFO] [1287330043.493481452]: Robot frame is 'base_link'
>>> > [New Thread 0x7fffe93ba710 (LWP 9218)]
>>> > [ WARN] [1287330043.669888723]: Unknown planner type:
>>> > [ WARN] [1287330043.671902945]: Unknown planner type:
>>> > [ INFO] [1287330043.676405613]: Known models:
>>> > [ INFO] [1287330043.676785969]: * irp6p
>>> > [ INFO] [1287330043.676924813]: Motion planning running in frame
>>> > 'base_link'
>>> > [ WARN] [1287330078.760853844]: Message from [/addCylinder] has a
>>> > non-fully-qualified frame_id [base_link]. Resolved locally to
>>> > [/base_link].
>>> > This is will likely not work in multi-robot systems. This message
>>> > will
>>> > only print once.
>>> > [ INFO] [1287330078.761141310]: Added 1 object to namespace pole in
>>> > collision space
>>> > [New Thread 0x7fffe8bb9710 (LWP 9331)]
>>> > [ INFO] [1287330290.291668304]: Received request for planning
>>> > [ INFO] [1287330290.293103682]: Selected motion planner:
>>> > 'kinematic::SBL[SBLkConfig2]', with priority 10
>>> > [ INFO] [1287330290.304334439]: ompl planning for group irp6p
>>> > Program received signal SIGSEGV, Segmentation fault.
>>> > planning_models::KinematicModel::computeTransforms (this=0x0,
>>> > params=0x7a0900)
>>> > at
>>> >
>>> > /tmp/buildd/ros-cturtle-motion-planning-common-0.2.4/debian/ros-cturtle-motion-planning-common/opt/ros/cturtle/stacks/motion_planning_common/planning_models/src/kinematic_model.cpp:461
>>> > 461
>>> >
>>> > /tmp/buildd/ros-cturtle-motion-planning-common-0.2.4/debian/ros-cturtle-motion-planning-common/opt/ros/cturtle/stacks/motion_planning_common/planning_models/src/kinematic_model.cpp:
>>> > No such file or directory.
>>> > in
>>> >
>>> > /tmp/buildd/ros-cturtle-motion-planning-common-0.2.4/debian/ros-cturtle-motion-planning-common/opt/ros/cturtle/stacks/motion_planning_common/planning_models/src/kinematic_model.cpp
>>> > (gdb) bt
>>> > #0 planning_models::KinematicModel::computeTransforms (this=0x0,
>>> > params=0x7a0900)
>>> > at
>>> >
>>> > /tmp/buildd/ros-cturtle-motion-planning-common-0.2.4/debian/ros-cturtle-motion-planning-common/opt/ros/cturtle/stacks/motion_planning_common/planning_models/src/kinematic_model.cpp:461
>>> > #1 0x000000000043b96e in ompl_planning::RequestHandler::configure (
>>> > this=<value optimized out>, startState=<value optimized out>,
>>> > req=...,
>>> > psetup=0x6adf40, distance_metric=...)
>>> > at
>>> >
>>> > /tmp/buildd/ros-cturtle-motion-planners-0.2.4/debian/ros-cturtle-motion-planners/opt/ros/cturtle/stacks/motion_planners/ompl_planning/src/request_handler/RequestHandler.cpp:218
>>> > #2 0x000000000043c252 in ompl_planning::RequestHandler::computePlan (
>>> > this=0x7fffffffdf80, models=<value optimized out>,
>>> > start=<value optimized out>, stateDelay=<value optimized out>,
>>> > req=...,
>>> > res=<value optimized out>, distance_metric=<value optimized out>)
>>> > at
>>> >
>>> > /tmp/buildd/ros-cturtle-motion-planners-0.2.4/debian/ros-cturtle-motion-planners/opt/ros/cturtle/stacks/motion_planners/ompl_planning/src/request_handler/RequestHandler.cpp:302
>>> > #3 0x000000000042be62 in OMPLPlanning::planToGoal
>>> > (this=0x7fffffffdb60,
>>> > req=<value optimized out>, res=...)
>>> > at
>>> >
>>> > /tmp/buildd/ros-cturtle-motion-planners-0.2.4/debian/ros-cturtle-motion-planners/opt/ros/cturtle/stacks/motion_planners/ompl_planning/src/motion_planner.cpp:178
>>> > #4 0x0000000000435e94 in boost::function2<bool,
>>> > motion_planning_msgs::GetMotionPlanRequest_<std::allocator<void> >&,
>>> > motion_planning_msgs::GetMotionPlanResponse_<std::allocator<void>
>>> >>&>::operator() (this=0x6b6390, params=...)
>>> > at /usr/include/boost/function/function_template.hpp:1013
>>> > #5
>>> >
>>> > ros::ServiceSpec<motion_planning_msgs::GetMotionPlanRequest_<std::allocator<void>
>>> >>, motion_planning_msgs::GetMotionPlanResponse_<std::allocator<void> >
>>> >>::call(boost::function<bool
>>> > ()(motion_planning_msgs::GetMotionPlanRequest_<std::allocator<void> >&,
>>> > motion_planning_msgs::GetMotionPlanResponse_<std::allocator<void> >&)>
>>> > const&,
>>> >
>>> > ros::ServiceSpecCallParams<motion_planning_msgs::GetMotionPlanRequest_<std::allocator<void>
>>> >>, motion_planning_msgs::GetMotionPlanResponse_<std::allocator<void> >
>>> >> >&)
>>> > (this=0x6b6390, params=...)
>>> > at
>>> >
>>> > /opt/ros/cturtle/ros/core/roscpp/include/ros/service_callback_helper.h:136
>>> > #6
>>> >
>>> > ros::ServiceCallbackHelperT<ros::ServiceSpec<motion_planning_msgs::GetMotionPlanRequest_<std::allocator<void>
>>> >>, motion_planning_msgs::GetMotionPlanResponse_<std::allocator<void> > >
>>> >>::call (this=0x6b6390, params=...)
>>> > at
>>> >
>>> > /opt/ros/cturtle/ros/core/roscpp/include/ros/service_callback_helper.h:188
>>> > #7 0x00007ffff4ad8488 in ros::ServiceCallback::call
>>> > (this=0x7fffe417a910)
>>> > at
>>> >
>>> > /tmp/buildd/ros-cturtle-ros-1.2.3/debian/ros-cturtle-ros/opt/ros/cturtle/ros/core/roscpp/src/libros/service_publication.cpp:123
>>> > #8 0x00007ffff4aea631 in ros::CallbackQueue::callOneCB (this=0x693220,
>>> > tls=0x7fffe4003140)
>>> > at
>>> >
>>> > /tmp/buildd/ros-cturtle-ros-1.2.3/debian/ros-cturtle-ros/opt/ros/cturtle/ros/core/roscpp/src/libros/callback_queue.cpp:381
>>> > #9 0x00007ffff4aeb72b in ros::CallbackQueue::callAvailable
>>> > (this=0x693220,
>>> > timeout=<value optimized out>)
>>> > at
>>> >
>>> > /tmp/buildd/ros-cturtle-ros-1.2.3/debian/ros-cturtle-ros/opt/ros/cturtle/ros/core/roscpp/src/libros/callback_queue.cpp:333
>>> > #10 0x00007ffff4af0b09 in ros::SingleThreadedSpinner::spin (
>>> > this=<value optimized out>, queue=0x693220)
>>> > at
>>> >
>>> > /tmp/buildd/ros-cturtle-ros-1.2.3/debian/ros-cturtle-ros/opt/ros/cturtle/ros/core/roscpp/src/libros/spinner.cpp:49
>>> > #11 0x00007ffff4a79abb in ros::spin ()
>>> > at
>>> >
>>> > /tmp/buildd/ros-cturtle-ros-1.2.3/debian/ros-cturtle-ros/opt/ros/cturtle/ros/core/roscpp/src/libros/init.cpp:488
>>> > #12 0x000000000041c248 in OMPLPlanning::run (this=0x7fffffffdb60)
>>> > at
>>> >
>>> > /tmp/buildd/ros-cturtle-motion-planners-0.2.4/debian/ros-cturtle-motion-planners/opt/ros/cturtle/stacks/motion_planners/ompl_planning/src/motion_planner.cpp:105
>>> > #13 0x0000000000419a39 in main (argc=1, argv=0x7fffffffe0a8)
>>> > at
>>> >
>>> > /tmp/buildd/ros-cturtle-motion-planners-0.2.4/debian/ros-cturtle-motion-planners/opt/ros/cturtle/stacks/motion_planners/ompl_planning/src/motion_planner.cpp:210
>>> >
>>> > Pozdrawiam
>>> > Konrad Banachowicz
>>> >
>>> > _______________________________________________
>>> > ros-users mailing list
>>> > ros-users@code.ros.org
>>> > https://code.ros.org/mailman/listinfo/ros-users
>>> >
>>> >
>>>
>>>
>>>
>>> --
>>> Sachin Chitta
>>> Research Scientist
>>> Willow Garage
>>> _______________________________________________
>>> ros-users mailing list
>>> ros-users@code.ros.org
>>> https://code.ros.org/mailman/listinfo/ros-users
>>
>
>
> _______________________________________________
> ros-users mailing list
> ros-users@code.ros.org
> https://code.ros.org/mailman/listinfo/ros-users
>
>
--
Sachin Chitta
Research Scientist
Willow Garage
Index: ompl_ros/src/ModelBase.cpp
===================================================================
--- ompl_ros/src/ModelBase.cpp (revision 45896)
+++ ompl_ros/src/ModelBase.cpp (working copy)
@@ -78,13 +78,13 @@
{
result = new EnvironmentDescription();
result->collisionSpace = planningMonitor->getEnvironmentModel();
- result->kmodel = result->collisionSpace->getRobotModel().get();
+ result->kmodel = planningMonitor->getKinematicModel();
result->constraintEvaluator = &constraintEvaluator;
result->group = group;
}
else
{
- ROS_DEBUG("Cloning collision environment (%d total)", (int)ENVS.size() + 1);
+ ROS_INFO("Cloning collision environment (%d total)", (int)ENVS.size() + 1);
result = new EnvironmentDescription();
result->collisionSpace = planningMonitor->getEnvironmentModel()->clone();
result->kmodel = result->collisionSpace->getRobotModel().get();
Index: ompl_planning/src/request_handler/RequestHandler.cpp
===================================================================
--- ompl_planning/src/request_handler/RequestHandler.cpp (revision 45896)
+++ ompl_planning/src/request_handler/RequestHandler.cpp (working copy)
@@ -212,6 +212,7 @@
/* set the starting state */
const unsigned int dim = psetup->ompl_model->si->getStateDimension();
ompl::base::State *start = new ompl::base::State(dim);
+
/* set the pose of the whole robot */
ompl_ros::EnvironmentDescription *ed = psetup->ompl_model->getEnvironmentDescription();
@@ -317,6 +318,7 @@
/* copy the solution to the result */
if (sol.path)
{
+ ROS_INFO("Found path");
fillResult(psetup, start, stateDelay, res, sol);
delete sol.path;
if (!sol.approximate)
@@ -325,7 +327,13 @@
if (psetup->priority > (int)m->planners.size())
psetup->priority = m->planners.size();
}
+ res.error_code.val = res.error_code.SUCCESS;
}
+ else
+ {
+ ROS_ERROR("No path found");
+ res.error_code.val = res.error_code.PLANNING_FAILED;
+ }
if (!sol.path || sol.approximate)
{
(text/plain)