# OMPL: How could I use OMPL with Cartesian Coordenates

Hi everyone!!!!

I need work with OMPL, but in a special way, The Input is a set of cartesian coordinates and I need the output in cartesian coordinates.

Can anyone help me?

PD: The tutorial of ompl in the wiki runs in diamondback but it is not working in electric

Thanks!

edit retag close merge delete

Sort by » oldest newest most voted

Hello Ivan,

What robot are you using this for? Do you have a collision checker? It seems to me what you can do is just use OMPL directly. Define a SE3StateSpace (which represents a position in space + orientation) or RealVectorStateSpace(3) (to represent just a 3D point), set the bounds for the space and plan there directly. You do need to specify your collision checker directly however. Check http://ompl.info for tutorials on how to do this, or post questions here again.

Ioan

more

( 2012-04-03 22:50:54 -0500 )edit

Ivan,

Are you using OMPL directly? The start trees cannot be initialized by a planner when there are no valid starting states. Is your start state in collision? Or at a state that does not satisfy the state validity constraints? If you are using ROS, make sure you turn on DEBUG and see if you get more information.

Ioan

more

Ivan,

Are you using OMPL directly? The start trees cannot be initialized by a planner when there are no valid starting states. Is your start state in collision? Or at a state that does not satisfy the state validity constraints? If you are using ROS, make sure you turn on DEBUG and see if you get more information.

Ioan

more

Hello Ioan!

Thanks for your answer!. This question is about experimental motion which requires the use of cartesian coordinates in the WAM robot.

I made the source code for this, but It returns this error: "SBL: Motion planning start tree could not be initialized", I have tried to solve this problem for a long time but I couldn't get it to work.

Do you know reason for this error? Thanks!!

more

Hi Ioan! I using OMPL and ROS, but I using a source code write for me.

With what you have said, I think the problem is with the constraints.

In any case, this is the code, Can you help me with this? This is my first time with planners.

How initialize the ompl strucutres

        /**
* Initialize OMPL Data Structures
*/
void initOmplStructures()
{
space_.reset(new ompl_base::SE3StateSpace());
ompl_base::RealVectorBounds vb(3);
vb.setLow(-2);
vb.setHigh(2);
bounds_=&vb;
space_->as<ompl_base::SE3StateSpace>()->setBounds(vb);
space_->setLongestValidSegmentFraction(0.0001);
simple_setup_=new ompl_geometric::SimpleSetup (space_);
current_pose_ompl_= new ompl::base::ScopedState<ompl::base::SE3StateSpace>(space_);
goal_pose_ompl_= new ompl::base::ScopedState<ompl::base::SE3StateSpace>(space_);
}


How initialize the planners:

    /**
* Initialize Ompl Planners
*/
void initPlanners()
{
ompl_base::PlannerPtr planner(new ompl_geometric::SBL(simple_setup_->getSpaceInformation()));
planner->as<ompl_geometric::SBL>()->setRange(0.001);
simple_setup_->setPlanner(planner);
}


How calculate the trajectory:

    /**
* This method calculate a trajectory between two points in cartesian coordinates.
* The trajectory contain points in cartesian coordenates.
*/
{
geometry_msgs::PoseStamped msg=req.pose_stamped;
(*goal_pose_ompl_)->setXYZ(msg.pose.position.x,msg.pose.position.y,msg.pose.position.z);
(*goal_pose_ompl_)->rotation().setAxisAngle(msg.pose.orientation.x,msg.pose.orientation.y,msg.pose.orientation.z,msg.pose.orientation.w);
simple_setup_->setStartAndGoalStates(*current_pose_ompl_, *goal_pose_ompl_);
simple_setup_->setup();
simple_setup_->print();
bool solved = simple_setup_->solve(2.0);
if (solved & pose_recieve_)
{
std::cout << "Found solution:" << std::endl;
// print the path to screen
simple_setup_->simplifySolution();
OmplPathToRosPath(simple_setup_->getSolutionPath().states,resp);
}
else if(not solved)  ROS_INFO_STREAM("No solution found" << std::endl);
else
{
ROS_FATAL("NO POSE RECIEVE FROM ROBOT");
exit(1);
}
return true;
}

more

PD: In this experimental motion is without IK (inverse kinematics)

( 2012-04-04 00:37:18 -0500 )edit

Thanks for all Ioan!!.

To answer your question,my problem is a throwing exception."SBL: Motion planning start tree could not be initialized". But I have shown you my code to eliminate the possibility of an error in the code.

And thanks for your comments, I will apply this change and tell you the results later

See you!!

more

Hello Ivan,

The code you show looks almost ok. When you initialize OMPL, you do this: bounds_=&vb; which is a problem, because vb will go out of scope and bounds_ will end up pointing to an invalid location. Just make bounds_ a copy of vb, not a pointer to it.

The way you set goal_pose_ompl_ is not correct. The message you use as input uses a quaternion as the representation of the rotation, not as axis-angle. You should just set the x,y,z and w individually, instead of using setAxisAngle().

The last thing I see is that you are missing the definition of a StateValidityChecker. You can follow this tutorial: http://ompl.kavrakilab.org/core/stateValidation.html

What is the problem you see exactly? Is the code crashing, or throwing an exception?

Ioan

more