Ask Your Question
1

OMPL: How could I use OMPL with Cartesian Coordenates

asked 2012-03-28 02:59:02 -0500

updated 2012-03-28 08:04:42 -0500

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 flag offensive close merge delete

7 Answers

Sort by ยป oldest newest most voted
1

answered 2012-03-28 21:41:23 -0500

isucan gravatar image

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

edit flag offensive delete link more

Comments

Thanks for your answer,

Ivan Rojas Jofre gravatar image Ivan Rojas Jofre  ( 2012-04-03 22:50:54 -0500 )edit
1

answered 2012-04-03 23:01:59 -0500

isucan gravatar image

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

edit flag offensive delete link more
0

answered 2012-04-03 23:02:39 -0500

isucan gravatar image

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

edit flag offensive delete link more
0

answered 2012-04-03 22:51:08 -0500

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!!

edit flag offensive delete link more
0

answered 2012-04-04 00:04:46 -0500

updated 2012-04-04 00:12:07 -0500

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.

Thanks for your help

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.
 */ 
bool computeCB(iri_wam_arm_navigation::PosePath::Request& req,iri_wam_arm_navigation::PosePath::Response& resp)
{
    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;
}
edit flag offensive delete link more

Comments

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

Ivan Rojas Jofre gravatar image Ivan Rojas Jofre  ( 2012-04-04 00:37:18 -0500 )edit
0

answered 2012-04-04 04:07:24 -0500

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!!

edit flag offensive delete link more
0

answered 2012-04-04 01:11:07 -0500

isucan gravatar image

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

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

3 followers

Stats

Asked: 2012-03-28 02:59:02 -0500

Seen: 1,406 times

Last updated: Apr 04 '12