Ask Your Question
2

Visualizing search space for OMPL planner

asked 2012-04-15 11:55:25 -0500

clee2693 gravatar image

Hi,
I am using the RRT-Connect planner in OMPL (through the ompl_ros_interface) to do planning for a pr2 arm. I was wondering if there was any way to obtain all of the states that the planner created while doing planning so I could see where the planner is searching in the environment that I created.
Thank you

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
2

answered 2012-04-19 03:56:19 -0500

isucan gravatar image

Hello,

OMPL itself does support the functionality you would like to test (In fact, we are just about to push some changes to the OMPL repository that allow outputting GraphML structures for the trees OMPL planners generate).

However, that functionality is not exposed through the ompl_ros_interface package in ROS Electric. This functionality is implemented for Fuerte, part of the upcoming MoveIt project (not yet released).

So.. the way to do this with ROS Electric will require adding some local hacks on your side: In the ompl_ros_interface package, the file src/ompl_ros_planning_group.cpp contains the call to the solve() function of an OMPL planner:

{{{ bool solved = planner_->solve(request.motion_plan_request.allowed_planning_time.toSec()); }}}

This is Line 446 for me.

After that line, you can do something like this:

{{{

const ompl::base::PlannerData &pd = planner_->getPlannerData();

planning_models::KinematicState kstate(*collision_models_interface_->getPlanningSceneState());

for (std::size_t i = 0 ; i < pd.states.size() ; ++i) {

omplStateToKinematicStateGroup(pd.states[i], ompl_state_to_kinematic_state_mapping_ kstate->getJointStateGroup(physical_joint_state_group_));

kstate->getJointStateGroup(physical_joint_state_group_)->updateKinematicLinks();

btTransform bt = kstate->getLinkState("some link")->getGlobalLinkTransform();

// do whatever with this; a NodeHandle is available in the class, so this can be published.

}

}}}

The only catch with the above code is that ompl_state_to_kinematic_state_mapping_ is not available in that class, so you have to change the planner class(es), e.g. ompl_ros_joint_planner.cpp, to pass that variable to the planning group.

I'm sorry this is so complicated right now, but for the next version things should be easier.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2012-04-15 11:55:25 -0500

Seen: 511 times

Last updated: Apr 19 '12