OMPL: How could I use OMPL with Cartesian Coordenates

edit

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

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

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;
}