Menu
▾
▴
[ompl-users] Not sure about correctness of my code
|
From: Yongqiang H. <dra...@gm...> - 2015-03-03 15:06:32
|
Dear OMPL developper,
Hi. I just started using OMPL a week ago, so I am very not proficient with
it. I am trying to use OMPL control-based planner to generate a trajectory.
I consider a 4DOF joint space and I used the real vector space with bounds,
and in the state validity checker I defined an obstacle region.
I pass the start point, goal point, and obstacle bounds through files.
The trajectory I get is very zigzagged and inneficient. I am not sure if I
am using OMPL correctly. I would greatly appreciate it if someone could
check my code.
I am attaching a picture result and the code.
Here is the picture:
Any help is appreciated! Thank you!
Yongqiang Huang
Here is the code, I modified it from RigidBodyMotionPlanningWithControl.cpp
(one of the demos) :
>
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Rice University
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Rice University nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
> /* Author: Ioan Sucan */
> /* Modified: Yongqiang Huang */
> #include <ompl/control/SpaceInformation.h>
#include <ompl/base/goals/GoalState.h>
#include <ompl/control/spaces/RealVectorControlSpace.h>
#include <ompl/control/SimpleSetup.h>
#include <ompl/base/SpaceInformation.h>
#include <ompl/base/spaces/RealVectorStateSpace.h>
#include <ompl/base/spaces/RealVectorBounds.h>
#include <ompl/geometric/SimpleSetup.h>
#include <ompl/control/planners/rrt/RRT.h>
#include <ompl/control/planners/est/EST.h>
#include <ompl/geometric/PathSimplifier.h>
> #include <ompl/config.h>
#include <iostream>
#include <fstream>
#include <sstream>
> namespace ob = ompl::base;
namespace oc = ompl::control;
namespace og = ompl::geometric;
> bool isStateValid(std::string obstacle_filename, const
> ob::SpaceInformation *si, const ob::State *state)
{
> // cast data type
const ob::RealVectorStateSpace::StateType *realvectorstate =
> state->as<ob::RealVectorStateSpace::StateType>();
std::ifstream obstacle_file (obstacle_filename.c_str());
float obstacle_degree_one_lower, obstacle_degree_one_upper;
float obstacle_degree_two_lower, obstacle_degree_two_upper;
float obstacle_degree_three_lower, obstacle_degree_three_upper;
float obstacle_degree_four_lower, obstacle_degree_four_upper;
> obstacle_file >> obstacle_degree_one_lower;
obstacle_file >> obstacle_degree_one_upper;
obstacle_file >> obstacle_degree_two_lower;
obstacle_file >> obstacle_degree_two_upper;
obstacle_file >> obstacle_degree_three_lower;
obstacle_file >> obstacle_degree_three_upper;
obstacle_file >> obstacle_degree_four_lower;
obstacle_file >> obstacle_degree_four_upper;
> obstacle_file.close();
> bool hit_obstacle;
hit_obstacle = realvectorstate->values[0] >=
> obstacle_degree_one_lower
&& realvectorstate->values[0] <=
> obstacle_degree_one_upper
&& realvectorstate->values[1] >=
> obstacle_degree_two_lower
&& realvectorstate->values[1] <=
> obstacle_degree_two_upper
&& realvectorstate->values[2] >=
> obstacle_degree_three_lower
&& realvectorstate->values[2] <=
> obstacle_degree_three_upper
&& realvectorstate->values[3] >=
> obstacle_degree_four_lower
&& realvectorstate->values[3] <=
> obstacle_degree_four_upper;
> return si->satisfiesBounds(state) && (!hit_obstacle) ;
> }
> void propagate(const ob::State *start, const oc::Control *control, const
> double duration, ob::State *result)
{
const ob::RealVectorStateSpace::StateType *realvectorstate =
> start->as<ob::RealVectorStateSpace::StateType>();
const double* pos =
> realvectorstate->as<ob::RealVectorStateSpace::StateType>()->values;
const double* ctrl =
> control->as<oc::RealVectorControlSpace::ControlType>()->values;
> result->as<ob::RealVectorStateSpace::StateType>()->values[0] = pos[0]
> + ctrl[0] * duration;
result->as<ob::RealVectorStateSpace::StateType>()->values[1] = pos[1] +
> ctrl[1] * duration;
result->as<ob::RealVectorStateSpace::StateType>()->values[2] = pos[2] +
> ctrl[2] * duration;
result->as<ob::RealVectorStateSpace::StateType>()->values[3] = pos[3] +
> ctrl[3] * duration;
}
>
> void plan(std::string start_filename, std::string end_filename,
> std::string obstacle_filename, std::string path_filename)
{
// construct the state space we are planning in
ob::StateSpacePtr space(new ob::RealVectorStateSpace(4));
> // set the bounds for real vector space
ob::RealVectorBounds bounds(4);
bounds.setLow(0, -2.0857 );
bounds.setLow(1, -1.3265);
bounds.setLow(2, -2.0857);
bounds.setLow(3, 0.0349);
bounds.setHigh(0, 2.0857);
bounds.setHigh(1, 0.3142);
bounds.setHigh(2, 2.0857);
bounds.setHigh(3, 1.5446);
> space->as<ob::RealVectorStateSpace>()->setBounds(bounds);
> // create a control space
oc::ControlSpacePtr cspace(new oc::RealVectorControlSpace(space, 4));
> // set the bounds for the control space
ob::RealVectorBounds cbounds(4);
cbounds.setLow(-0.1);
cbounds.setHigh(0.1);
> cspace->as<oc::RealVectorControlSpace>()->setBounds(cbounds);
> // construct space information
oc::SpaceInformationPtr si(new oc::SpaceInformation(space, cspace));
>
> // set the state propagation routine
si->setStatePropagator(boost::bind(&propagate, _1, _2, _3, _4));
> // set state validity checking for this space
si->setStateValidityChecker(boost::bind(&isStateValid,
> obstacle_filename, si.get(), _1));
> // create a random start state
ob::ScopedState<> start(space);
std::ifstream start_file (start_filename.c_str());
start_file >> start[0];
start_file >> start[1];
start_file >> start[2];
start_file >> start[3];
> start_file.close();
> // create a random goal state
ob::ScopedState<> goal(space);
std::ifstream end_file (end_filename.c_str());
end_file >> goal[0];
end_file >> goal[1];
end_file >> goal[2];
end_file >> goal[3];
> end_file.close();
> // create problem definition
ob::ProblemDefinitionPtr pdef(new ob::ProblemDefinition(si));
> // set the start and goal states
pdef->setStartAndGoalStates(start, goal, 0.01);
> // create planner
ob::PlannerPtr planner(new oc::RRT(si));
> // set problem to planner
planner->setProblemDefinition(pdef);
> // this call is optional, but we put it in to get more output
> information
planner->setup();
//ss.print();
> // attempt to solve the problem within ten seconds of planning time
ob::PlannerStatus solved = planner->solve(10);
> if (solved)
{
std::cout << "Found solution:" << std::endl;
> std::ofstream matrix_file;
matrix_file.open(path_filename.c_str());
> boost::static_pointer_cast<oc::PathControl>(pdef->getSolutionPath())->print(std::cout);
> boost::static_pointer_cast<oc::PathControl>(pdef->getSolutionPath())->printAsMatrix(matrix_file);
> matrix_file.close();
}
else
std::cout << "No solution found" << std::endl;
}
> int main(int argc, char * argv[])
{
if (argc != 2)
{
std::cout << "Usage: ./pathgen num_repetitions" << std::endl;
return 1;
}
> std::istringstream istr(argv[1]);
int num_repetitions;
istr >> num_repetitions;
> std::ostringstream ostr;
std::string start_filename, end_filename, obstacle_filename,
> path_filename;
std::string foldername =
> "/home/a/try_motion_planning/start_end_obstacle/"; // hardwired
for (int i = 1; i <= num_repetitions; i++)
{
ostr.str("");
ostr << i;
start_filename = foldername + "start_" + ostr.str();
end_filename = foldername + "end_" + ostr.str();
obstacle_filename = foldername + "obstacle_range_" + ostr.str();
path_filename = foldername + "control_path_" + ostr.str();
> std::ifstream start_file, end_file, obstacle_file;
start_file.clear();
start_file.open(start_filename.c_str());
if (start_file.is_open() != true){
std::cout << start_filename << " " << " cannot open." <<
> std::endl;
return 1;
}
start_file.close();
> end_file.clear();
end_file.open(end_filename.c_str());
if (end_file.fail() == true){
std::cout << end_filename << " " << " cannot open." <<
> std::endl;
return 1;
}
end_file.close();
> obstacle_file.clear();
obstacle_file.open(obstacle_filename.c_str());
if (obstacle_file.fail() == true){
std::cout << obstacle_filename << " " << " cannot open." <<
> std::endl;
return 1;
}
obstacle_file.close();
> plan(start_filename, end_filename, obstacle_filename,
> path_filename);
> std::cout << "path saved to " << path_filename << std::endl;
> }
> return 0;
}
|