Skip to content
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

Jump to bottom

Added baked checking support #9

Merged
merged 1 commit into from
Jul 31, 2016
Merged

Added baked checking support #9

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
59 changes: 59 additions & 0 deletions cbirrt2/cbirrt.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -98,6 +98,8 @@ OpenRAVE::PlannerStatus CBirrtPlanner::CleanUpReturn(bool retval)
else
return PS_Failed;

bakedkinbody.reset();
bakedchecker.reset();
}


Expand Down Expand Up @@ -308,6 +310,42 @@ bool CBirrtPlanner::InitPlan(RobotBasePtr pbase, PlannerParametersConstPtr ppar

bSmoothPath = _parameters->bsmoothpath;
bdofresl2norm = _parameters->bdofresl2norm;
bbakedcheckers = _parameters->bbakedcheckers;

if (bbakedcheckers) {
bakedchecker = GetEnv()->GetCollisionChecker();
std::string bakedkinbody_type;
// get baked kinbody type (and verify that checker supports baking)
{
std::stringstream sinput("BakeGetType"), soutput;
try
{
if (!bakedchecker->SendCommand(soutput, sinput))
{
_outputstream << "Error: collision checker doesn't support baked checks\n";
return false;
}
}
catch (const OpenRAVE::openrave_exception & exc)
{
_outputstream << "Error: collision checker doesn't support baked checks\n";
return false;
}
bakedkinbody_type = soutput.str();
}
// do baking
{
std::stringstream sinput("BakeBegin BakeEnd"), soutput;
bakedchecker->SendCommand(soutput, sinput); // BakeBegin
bakedkinbody = OpenRAVE::RaveCreateKinBody(GetEnv(), bakedkinbody_type);
GetEnv()->CheckCollision(KinBodyConstPtr(_pRobot), sIgnoredBodies, sIgnoredLinks);
for(int i = 0; i < vMimicBodies.size(); i++) {
GetEnv()->CheckCollision(KinBodyConstPtr(vMimicBodies[i]), sIgnoredBodies, sIgnoredLinks);
}
_pRobot->CheckSelfCollision();
bakedchecker->SendCommand(soutput, sinput); // BakeEnd
}
}

_pIkSolver = RaveCreateIkSolver(GetEnv(),"GeneralIK");
_pIkSolver->Init(_pRobot->GetActiveManipulator());
Expand Down Expand Up @@ -336,6 +374,8 @@ bool CBirrtPlanner::InitPlan(RobotBasePtr pbase, PlannerParametersConstPtr ppar
{
RAVELOG_INFO("CBirrtPlanner::InitPlan - Error: Starts are improperly specified.\n");
_outputstream << "Error: Starts are improperly specified\n";
bakedkinbody.reset();
bakedchecker.reset();
return false;
}

Expand Down Expand Up @@ -368,6 +408,8 @@ bool CBirrtPlanner::InitPlan(RobotBasePtr pbase, PlannerParametersConstPtr ppar
_outputstream << "Error: Start configuration "<< num_starts << " is not in balance\n";
}

bakedkinbody.reset();
bakedchecker.reset();
return false;
}

Expand All @@ -380,6 +422,8 @@ bool CBirrtPlanner::InitPlan(RobotBasePtr pbase, PlannerParametersConstPtr ppar
s << endl;
RAVELOG_INFO(s.str().c_str());
_outputstream << "Error: Start configuration in collision\n";
bakedkinbody.reset();
bakedchecker.reset();
return false;
}

Expand Down Expand Up @@ -447,6 +491,8 @@ bool CBirrtPlanner::InitPlan(RobotBasePtr pbase, PlannerParametersConstPtr ppar
{
RAVELOG_INFO("CBirrtPlanner::InitPlan - Error: goals are improperly specified.\n");
_outputstream << "Error: goals are improperly specified\n";
bakedkinbody.reset();
bakedchecker.reset();
return false;
}
goal_index++;
Expand Down Expand Up @@ -476,6 +522,8 @@ bool CBirrtPlanner::InitPlan(RobotBasePtr pbase, PlannerParametersConstPtr ppar
_outputstream << "Error: Goal configuration "<< num_goals << " is not in balance\n";
}

bakedkinbody.reset();
bakedchecker.reset();
return false;
}

Expand Down Expand Up @@ -516,6 +564,8 @@ bool CBirrtPlanner::InitPlan(RobotBasePtr pbase, PlannerParametersConstPtr ppar
{
RAVELOG_DEBUG("Could not initialize Goal Configuration\n");
_outputstream << "Could not initialize Goal Configuration\n";
bakedkinbody.reset();
bakedchecker.reset();
return false;
}
}
Expand Down Expand Up @@ -959,6 +1009,15 @@ bool CBirrtPlanner::_CheckCollision(std::vector<dReal>& pConfig)
#endif

SetDOF(pConfig);

if (bbakedcheckers)
{
bool collided = bakedchecker->CheckStandaloneSelfCollision(bakedkinbody);
#ifdef RECORD_TIMES
collisionchecktime += timeGetThreadTime() - starttime;
#endif
return collided;
}

CollisionReportPtr preport(new CollisionReport());

Expand Down
8 changes: 6 additions & 2 deletions cbirrt2/cbirrt.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -92,8 +92,8 @@ class CBirrtPlanner : public PlannerBase
__description = ":Interface Author: Dmitry Berenson\nRRT-based algorithm for planning with end-effector pose constraints described by Task Space Region Chains.\n\n`C++ Documentation <http://automation.berkeley.edu/~berenson/docs/cbirrt/index.html>`_";
_pActiveNode = NULL;
_pConnectNode = NULL;
bdofresl2norm = false;

bdofresl2norm = false;
//these shouldn't be de-allocated so that multiple calls of this planner are faster
_pForwardTree = new NodeTree(false);
_pBackwardTree = new NodeTree(true);
Expand Down Expand Up @@ -305,6 +305,10 @@ class CBirrtPlanner : public PlannerBase
bool bSmoothPath;
bool bdofresl2norm;

bool bbakedcheckers;
OpenRAVE::CollisionCheckerBasePtr bakedchecker;
OpenRAVE::KinBodyPtr bakedkinbody;

dReal P_SAMPLE_IK;


Expand Down
12 changes: 10 additions & 2 deletions cbirrt2/cbirrtparameters.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,7 @@ class CBirrtParameters : public PlannerBase::PlannerParameters
smoothingitrs(-1), bsamplingstart(false), bsamplinggoal(false),
timelimit(-1.0), bProcessing(false),
bikfastsinglesolution(true), pplannerstate(0),
steplength(0.05), bdofresl2norm(false)
steplength(0.05), bdofresl2norm(false), bbakedcheckers(false)
{
_vXMLParameters.push_back("tsrchain");
_vXMLParameters.push_back("grabbed");
Expand All @@ -59,6 +59,7 @@ class CBirrtParameters : public PlannerBase::PlannerParameters
_vXMLParameters.push_back("pplannerstate");
_vXMLParameters.push_back("steplength");
_vXMLParameters.push_back("bdofresl2norm");
_vXMLParameters.push_back("bbakedcheckers");

}
bool bgrabbed; ///< are we grabbing an object?
Expand Down Expand Up @@ -86,6 +87,7 @@ class CBirrtParameters : public PlannerBase::PlannerParameters

dReal steplength;
bool bdofresl2norm; ///< use the L2 norm (instead of the default L-infinity norm) for dof resolutions
bool bbakedcheckers; ///< use the baked collision checking interface (e.g. from or_fcl)

protected:
bool bProcessing;
Expand Down Expand Up @@ -164,6 +166,7 @@ class CBirrtParameters : public PlannerBase::PlannerParameters

O << "<steplength>" << steplength << "</steplength>" << endl;
O << "<bdofresl2norm>" << bdofresl2norm << "</bdofresl2norm>" << endl;
O << "<bbakedcheckers>" << bbakedcheckers << "</bbakedcheckers>" << endl;

O.precision(old_precision);
return !!O;
Expand Down Expand Up @@ -196,7 +199,8 @@ class CBirrtParameters : public PlannerBase::PlannerParameters
name == "bikfastsinglesolution" ||
name == "pplannerstate" ||
name == "steplength" ||
name == "bdofresl2norm");
name == "bdofresl2norm" ||
name == "bbakedcheckers");

return bProcessing ? PE_Support : PE_Pass;
}
Expand Down Expand Up @@ -310,6 +314,10 @@ class CBirrtParameters : public PlannerBase::PlannerParameters
{
_ss >> bdofresl2norm;
}
else if( stricmp(name.c_str(), "bbakedcheckers") == 0 )
{
_ss >> bbakedcheckers;
}
else
{
RAVELOG_WARN(str(boost::format("unknown tag %s\n")%name));
Expand Down
4 changes: 4 additions & 0 deletions cbirrt2/cbirrtproblem.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -820,6 +820,10 @@ int CBirrtProblem::RunCBirrt(ostream& sout, istream& sinput)
{
sinput >> params->bdofresl2norm;
}
else if( stricmp(cmd.c_str(), "bbakedcheckers") == 0)
{
sinput >> params->bbakedcheckers;
}
else if( stricmp(cmd.c_str(), "planinnewthread") == 0)
{
sinput >> bPlanInNewThread;
Expand Down