Skip to content

Added support for baked collision checking #67

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 15 commits into from
Aug 5, 2016
Merged

Added support for baked collision checking #67

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
15 commits
Select commit Hold shift + click to select a range
a17d587
initial implementation of baked checker support
cdellin Apr 13, 2016
6db365b
Merge branch 'master' into feature/baked_checkers
cdellin Apr 13, 2016
5806a50
remove accidental debug message
cdellin Apr 13, 2016
4ccb53a
Merge branch 'master' into feature/baked_checkers
cdellin Apr 20, 2016
9d11508
Merge branch 'master' into feature/baked_checkers
cdellin Apr 21, 2016
00e53f3
accommodate new or_fcl baking interface
cdellin Apr 30, 2016
d99d6e7
Merge branch 'master' into feature/baked_checkers
cdellin Jul 21, 2016
f61f9f9
Merge branch 'master' into feature/baked_checkers
cdellin Jul 26, 2016
6c3d478
Merge branch 'master' into feature/baked_checkers
cdellin Jul 27, 2016
8d6ca4e
Merge branch 'master' into feature/baked_checkers
mkoval Jul 31, 2016
9efdd85
Removed ill-fated stop() function.
Jul 31, 2016
3712445
Merge branch 'feature/baked_checkers' of https://github.com/personalr…
Jul 31, 2016
c475b3b
Don't collision-check invalid states.
Jul 31, 2016
6df1d0a
Revert "Removed ill-fated stop() function." pending re-implementation…
cdellin Aug 1, 2016
ba7539e
insert missing baking call erroneously removed in merge d99d6e779a9e6…
cdellin Aug 1, 2016
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
20 changes: 18 additions & 2 deletions include/or_ompl/OMPLPlannerParameters.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -53,12 +53,15 @@ class OMPLPlannerParameters : public OpenRAVE::PlannerBase::PlannerParameters
, m_timeLimit(10)
, m_isProcessingOMPL(false)
, m_dat_filename("")
, m_trajs_fileformat("") {
, m_trajs_fileformat("")
, m_doBaked(false)
{
_vXMLParameters.push_back("seed");
_vXMLParameters.push_back("time_limit");
_vXMLParameters.push_back("dat_filename");
_vXMLParameters.push_back("trajs_fileformat");
_vXMLParameters.push_back("tsr_chain");
_vXMLParameters.push_back("do_baked");
}

unsigned int m_seed;
Expand All @@ -67,6 +70,7 @@ class OMPLPlannerParameters : public OpenRAVE::PlannerBase::PlannerParameters
std::string m_dat_filename;
std::string m_trajs_fileformat;
std::vector<TSRChain::Ptr> m_tsrchains;
bool m_doBaked;

protected:

Expand All @@ -89,6 +93,7 @@ class OMPLPlannerParameters : public OpenRAVE::PlannerBase::PlannerParameters
BOOST_FOREACH(TSRChain::Ptr chain, m_tsrchains) {
O << "<tsr_chain>" << chain << "</tsr_chain>" << std::endl;
}
O << "<do_baked>" << m_doBaked << "</do_baked>" << std::endl;

return !!O;
}
Expand All @@ -113,7 +118,8 @@ class OMPLPlannerParameters : public OpenRAVE::PlannerBase::PlannerParameters
|| name == "time_limit"
|| name == "dat_filename"
|| name == "trajs_fileformat"
|| name == "tsr_chain";
|| name == "tsr_chain"
|| name == "do_baked";

return m_isProcessingOMPL ? PE_Support : PE_Pass;
}
Expand All @@ -136,6 +142,16 @@ class OMPLPlannerParameters : public OpenRAVE::PlannerBase::PlannerParameters
}else{
m_tsrchains.push_back(chain);
}
} else if (name == "do_baked") {
std::string strbool;
_ss >> strbool;
if (strbool=="on" || strbool=="yes" || strbool=="1" || strbool=="true") {
m_doBaked = true;
} else if (strbool=="off" || strbool=="no" || strbool=="0" || strbool=="false") {
m_doBaked = false;
} else {
RAVELOG_WARN(str(boost::format("unknown boolean %s, ignoring\n") % strbool));
}
} else {
RAVELOG_WARN(str(boost::format("unknown tag %s\n") % name));
}
Expand Down
14 changes: 12 additions & 2 deletions include/or_ompl/StateSpaces.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -93,6 +93,7 @@ class ContinuousJointsProjectionEvaluator : public ompl::base::ProjectionEvaluat
ompl::base::ProjectionMatrix _projectionMatrix;
};


/**
* This is like ompl::base::StateValidityChecker,
* except it also knows how to compute forward kinematics
Expand All @@ -104,7 +105,10 @@ class OrStateValidityChecker: public ompl::base::StateValidityChecker
{
public:
OrStateValidityChecker(const ompl::base::SpaceInformationPtr &si,
OpenRAVE::RobotBasePtr robot, std::vector<int> const &indices);
OpenRAVE::RobotBasePtr robot, std::vector<int> const &indices,
bool do_baked);
void start();
void stop();
virtual bool computeFk(const ompl::base::State *state, uint32_t checklimits) const;
virtual bool isValid(const ompl::base::State *state) const;
void resetStatistics() { m_numCollisionChecks = 0; m_totalCollisionTime = 0.0; }
Expand All @@ -118,6 +122,11 @@ class OrStateValidityChecker: public ompl::base::StateValidityChecker
std::vector<int> const m_indices;
mutable int m_numCollisionChecks;
mutable double m_totalCollisionTime;
// optional baked stuff
const bool m_do_baked;
OpenRAVE::CollisionCheckerBasePtr m_baked_checker;
std::string m_baked_kinbody_type;
OpenRAVE::KinBodyPtr m_baked_kinbody;
};

/**
Expand All @@ -127,7 +136,8 @@ class RealVectorOrStateValidityChecker: public OrStateValidityChecker
{
public:
RealVectorOrStateValidityChecker(const ompl::base::SpaceInformationPtr &si,
OpenRAVE::RobotBasePtr robot, std::vector<int> const &indices);
OpenRAVE::RobotBasePtr robot, std::vector<int> const &indices,
bool do_baked);
virtual bool computeFk(const ompl::base::State *state, uint32_t checklimits) const;
private:
const std::size_t m_num_dof;
Expand Down
17 changes: 15 additions & 2 deletions src/OMPLPlanner.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <boost/chrono.hpp>
#include <boost/foreach.hpp>
#include <boost/make_shared.hpp>
#include <boost/scope_exit.hpp>
#include <ompl/config.h>
#include <ompl/base/goals/GoalStates.h>
#include <ompl/base/StateSpaceTypes.h>
Expand Down Expand Up @@ -118,10 +119,10 @@ bool OMPLPlanner::InitPlan(OpenRAVE::RobotBasePtr robot,
RAVELOG_DEBUG("Setting state validity checker.\n");
if (m_state_space->isCompound()) {
m_or_validity_checker.reset(new OrStateValidityChecker(
m_simple_setup->getSpaceInformation(), m_robot, dof_indices));
m_simple_setup->getSpaceInformation(), m_robot, dof_indices, m_parameters->m_doBaked));
} else {
m_or_validity_checker.reset(new RealVectorOrStateValidityChecker(
m_simple_setup->getSpaceInformation(), m_robot, dof_indices));
m_simple_setup->getSpaceInformation(), m_robot, dof_indices, m_parameters->m_doBaked));
}
#ifdef OR_OMPL_HAS_BOOSTSMARTPTRS
m_simple_setup->setStateValidityChecker(
Expand All @@ -131,6 +132,12 @@ bool OMPLPlanner::InitPlan(OpenRAVE::RobotBasePtr robot,
std::static_pointer_cast<ompl::base::StateValidityChecker>(m_or_validity_checker));
#endif

// start validity checker
m_or_validity_checker->start();
BOOST_SCOPE_EXIT((m_or_validity_checker)) {
m_or_validity_checker->stop();
} BOOST_SCOPE_EXIT_END

RAVELOG_DEBUG("Setting initial configuration.\n");
if (m_parameters->vinitialconfig.size() % num_dof != 0) {
RAVELOG_ERROR("Start configuration has incorrect DOF;"
Expand Down Expand Up @@ -344,6 +351,12 @@ OpenRAVE::PlannerStatus OMPLPlanner::PlanPath(OpenRAVE::TrajectoryBasePtr ptraj)
// TODO: Configure anytime algorithms to keep planning.
//m_simpleSetup->getGoal()->setMaximumPathLength(0.0);

// start validity checker
m_or_validity_checker->start();
BOOST_SCOPE_EXIT((m_or_validity_checker)) {
m_or_validity_checker->stop();
} BOOST_SCOPE_EXIT_END

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This needlessly bakes the collision checker twice.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is delicate -- what if the user has changed the environment between calls to InitPlan and PlanPath? For example, what if they want to use different padding/models for validating starts/goals than for doing the actual planning (not sure if anyone actually does this, but they could). I currently get around this by baking in each call to a function, since I can rely on the environment lock.

ompl::base::PlannerStatus ompl_status;
ompl_status = m_simple_setup->solve(m_parameters->m_timeLimit);

Expand Down
11 changes: 9 additions & 2 deletions src/OMPLSimplifier.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*************************************************************************/

#include <boost/make_shared.hpp>
#include <boost/scope_exit.hpp>
#include <ompl/base/ScopedState.h>
#include <ompl/util/Time.h>

Expand Down Expand Up @@ -79,10 +80,10 @@ bool OMPLSimplifier::InitPlan(OpenRAVE::RobotBasePtr robot,
m_space_info.reset(new SpaceInformation(m_state_space));
if (m_state_space->isCompound()) {
m_or_validity_checker.reset(new OrStateValidityChecker(
m_space_info, m_robot, dof_indices));
m_space_info, m_robot, dof_indices, m_parameters->m_doBaked));
} else {
m_or_validity_checker.reset(new RealVectorOrStateValidityChecker(
m_space_info, m_robot, dof_indices));
m_space_info, m_robot, dof_indices, m_parameters->m_doBaked));
}
#ifdef OR_OMPL_HAS_BOOSTSMARTPTRS
m_space_info->setStateValidityChecker(
Expand Down Expand Up @@ -161,6 +162,12 @@ OpenRAVE::PlannerStatus OMPLSimplifier::PlanPath(OpenRAVE::TrajectoryBasePtr ptr

RAVELOG_DEBUG("Running path simplification for %f seconds.\n",
m_parameters->m_timeLimit);

// start validity checker
m_or_validity_checker->start();
BOOST_SCOPE_EXIT((m_or_validity_checker)) {
m_or_validity_checker->stop();
} BOOST_SCOPE_EXIT_END

do {
// Run one iteration of shortcutting. This gives us fine control over
Expand Down
67 changes: 55 additions & 12 deletions src/StateSpaces.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -158,13 +158,49 @@ void ContinuousJointsProjectionEvaluator::project(const ompl::base::State *state

or_ompl::OrStateValidityChecker::OrStateValidityChecker(
const ompl::base::SpaceInformationPtr &si,
OpenRAVE::RobotBasePtr robot, std::vector<int> const &indices):
OpenRAVE::RobotBasePtr robot, std::vector<int> const &indices,
bool do_baked):
ompl::base::StateValidityChecker(si),
m_stateSpace(si->getStateSpace().get()),
m_env(robot->GetEnv()), m_robot(robot), m_indices(indices) {
m_env(robot->GetEnv()), m_robot(robot), m_indices(indices),
m_do_baked(do_baked)
{
if (m_do_baked)
{
m_baked_checker = m_env->GetCollisionChecker();
std::stringstream sinput("BakeGetType"), soutput;
try
{
if (!m_baked_checker->SendCommand(soutput, sinput))
throw std::runtime_error("collision checker doesn't support baked checks!");
}
catch (const OpenRAVE::openrave_exception & exc)
{
throw std::runtime_error("collision checker doesn't support baked checks!");
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Replace these runtime_errors with openrave_exception.

}
m_baked_kinbody_type = soutput.str();
}

resetStatistics();
}

void or_ompl::OrStateValidityChecker::start() {
if (m_do_baked)
{
// start baking
std::stringstream sinput("BakeBegin BakeEnd"), soutput;
m_baked_checker->SendCommand(soutput, sinput); // BakeBegin
m_baked_kinbody = OpenRAVE::RaveCreateKinBody(m_env, m_baked_kinbody_type);
m_env->CheckCollision(m_robot);
m_robot->CheckSelfCollision();
m_baked_checker->SendCommand(soutput, sinput); // BakeEnd
}
}

void or_ompl::OrStateValidityChecker::stop() {
m_baked_kinbody.reset();
}

bool or_ompl::OrStateValidityChecker::computeFk(const ompl::base::State *state, uint32_t checklimits) const {
std::vector<double> values;
m_stateSpace->copyToReals(values, state);
Expand All @@ -184,23 +220,30 @@ bool or_ompl::OrStateValidityChecker::isValid(const ompl::base::State *state) co
boost::chrono::steady_clock::time_point const tic
= boost::chrono::steady_clock::now();

bool const collided = !computeFk(state, OpenRAVE::KinBody::CLA_Nothing)
|| m_env->CheckCollision(m_robot)
|| m_robot->CheckSelfCollision();
bool collided = !computeFk(state, OpenRAVE::KinBody::CLA_Nothing);

boost::chrono::steady_clock::time_point const toc
= boost::chrono::steady_clock::now();
m_totalCollisionTime += boost::chrono::duration_cast<
boost::chrono::duration<double> >(toc - tic).count();
m_numCollisionChecks++;
if (!collided)
{
if (m_do_baked)
collided = collided || m_baked_checker->CheckStandaloneSelfCollision(m_baked_kinbody);
else
collided = collided || m_env->CheckCollision(m_robot) || m_robot->CheckSelfCollision();

boost::chrono::steady_clock::time_point const toc
= boost::chrono::steady_clock::now();
m_totalCollisionTime += boost::chrono::duration_cast<
boost::chrono::duration<double> >(toc - tic).count();
m_numCollisionChecks++;
}

return !collided;
}

or_ompl::RealVectorOrStateValidityChecker::RealVectorOrStateValidityChecker(
const ompl::base::SpaceInformationPtr &si,
OpenRAVE::RobotBasePtr robot, std::vector<int> const &indices):
or_ompl::OrStateValidityChecker(si,robot,indices),
OpenRAVE::RobotBasePtr robot, std::vector<int> const &indices,
bool do_baked):
or_ompl::OrStateValidityChecker(si,robot,indices,do_baked),
m_num_dof(si->getStateDimension()) {
}

Expand Down