Shows how to use a planner two plan for a robot opening a door. The configuration space consists of the robot's manipulator joints and the door joint. A generic planner is just passed in this configuration space inside its planner parameters.
#include <vector>
#include <sstream>
#include <boost/format.hpp>
using namespace OpenRAVE;
using namespace std;
namespace cppexamples {
class DoorConfiguration : public boost::enable_shared_from_this<DoorConfiguration>
{
{
dReal facos = min((t1.rot-t2.rot).lengthsqr4(),(t1.rot+t2.rot).
lengthsqr4());
return (t1.trans-t2.trans).lengthsqr3() + frotweight*facos;
}
public:
_probot = pmanip->GetRobot();
_ptarget = RaveInterfaceCast<RobotBase>(pdoorjoint->GetParent());
_pdoorlink = _pdoorjoint->GetHierarchyChildLink();
_tgrasp =
Transform(
Vector(0.65839364, 0.68616871, -0.22320624, -0.21417118),
Vector(0.23126595, -0.01218956, 0.1084143));
BOOST_ASSERT(_pdoorjoint->GetDOF()==1);
}
virtual ~DoorConfiguration() {
}
int GetDOF() {
return (int)_pmanip->GetArmIndices().size()+1;
}
dReal ComputeDistance(
const std::vector<dReal>& c0,
const std::vector<dReal>& c1) {
dReal d1 = _robotdistmetric->Eval(c0,c1);
std::vector<dReal> door0(1), door1(1);
door0[0] = c0.back();
door1[0] = c1.back();
dReal d2 = _doordistmetric->Eval(door0,door1);
}
bool Sample(std::vector<dReal>&v) {
for(int i = 0; i < 50; ++i) {
_robotsamplefn->Sample(v);
vector<dReal> vdoor;
_doorsamplefn->Sample(vdoor);
v.resize(GetDOF());
v.back() = vdoor.at(0);
if( _SetState(v) ) {
return true;
}
}
return false;
}
vector<dReal> vdoor(1); vdoor[0] = v.back();
_ptarget->SetActiveDOFValues(vdoor);
_probot->SetActiveDOFValues(v);
vector<dReal> vsolution;
bool bsuccess = _pmanip->FindIKSolution(_pdoorlink->GetTransform() * _tgrasp, vsolution, filteroptions);
if( bsuccess ) {
savestate1.reset();
savestate2.reset();
_probot->SetActiveDOFValues(vsolution);
std::copy(vsolution.begin(),vsolution.end(),v.begin());
_ptarget->SetActiveDOFValues(vdoor);
}
return bsuccess;
}
void SetState(const std::vector<dReal>& v)
{
vector<dReal> vtemp = v;
if( !_SetState(vtemp) ) {
}
}
void GetState(std::vector<dReal>& v)
{
_probot->GetActiveDOFValues(v);
v.resize(GetDOF());
v.back() = _pdoorjoint->GetValue(0);
}
void DiffState(std::vector<dReal>& v1,const std::vector<dReal>& v2)
{
_probot->SubtractActiveDOFValues(v1,v2);
v1.back() -= v2.back();
}
bool NeightState(std::vector<dReal>& v,const std::vector<dReal>& vdelta, int fromgoal)
{
_vprevsolution = v;
_tmanipprev = _pmanip->GetTransform();
Transform tdoorprev = _pdoorlink->GetTransform();
BOOST_ASSERT( TransformDistance2(tdoorprev*_tgrasp,_tmanipprev) <=
g_fEpsilon );
{
vector<dReal> vdoor(1);
vdoor[0] = v.back()+0.5*vdelta.back();
_ptarget->SetActiveDOFValues(vdoor);
_tmanipmidreal = _pdoorlink->GetTransform()*_tgrasp;
}
for(int i = 0; i < GetDOF(); ++i) {
v.at(i) += vdelta.at(i);
}
}
{
std::vector<dReal> vmidsolution(_probot->GetActiveDOF());
dReal realdist2 = TransformDistance2(_tmanipprev, tmanipnew);
const dReal ikmidpointmaxdist2mult = 0.5;
for(int i = 0; i < _probot->GetActiveDOF(); ++i) {
vmidsolution.at(i) = 0.5*(_vprevsolution.at(i)+vsolution.at(i));
}
_probot->SetActiveDOFValues(vmidsolution);
Transform tmanipmid = _pmanip->GetTransform();
dReal middist2 = TransformDistance2(tmanipmid, _tmanipmidreal);
if( middist2 >
g_fEpsilon && middist2 > ikmidpointmaxdist2mult*realdist2 ) {
RAVELOG_VERBOSE(str(boost::format(
"rejected due to discontinuity at mid-point %e > %e")%middist2%(ikmidpointmaxdist2mult*realdist2)));
}
}
{
_probot->SetActiveDOFs(_pmanip->GetArmIndices());
vector<int> v(1); v[0] = _pdoorjoint->GetDOFIndex();
_ptarget->SetActiveDOFs(v);
params->_configurationspecification = _probot->GetActiveConfigurationSpecification() + _ptarget->GetActiveConfigurationSpecification();
_probot->GetActiveDOFLimits(params->_vConfigLowerLimit,params->_vConfigUpperLimit);
_pdoorjoint->GetLimits(params->_vConfigLowerLimit,params->_vConfigUpperLimit,true);
_probot->GetActiveDOFVelocityLimits(params->_vConfigVelocityLimit);
params->_vConfigVelocityLimit.push_back(100);
_probot->GetActiveDOFAccelerationLimits(params->_vConfigAccelerationLimit);
params->_vConfigAccelerationLimit.push_back(100);
_probot->GetActiveDOFResolutions(params->_vConfigResolution);
params->_vConfigResolution.push_back(0.05);
params->_distmetricfn = boost::bind(&DoorConfiguration::ComputeDistance,shared_from_this(),_1,_2);
params->_samplefn = boost::bind(&DoorConfiguration::Sample,shared_from_this(),_1);
params->_sampleneighfn.clear();
params->_setstatefn = boost::bind(&DoorConfiguration::SetState,shared_from_this(),_1);
params->_getstatefn = boost::bind(&DoorConfiguration::GetState,shared_from_this(),_1);
params->_diffstatefn = boost::bind(&DoorConfiguration::DiffState,shared_from_this(),_1,_2);
params->_neighstatefn = boost::bind(&DoorConfiguration::NeightState,shared_from_this(),_1,_2,_3);
std::list<KinBodyPtr> listCheckCollisions;
listCheckCollisions.push_back(_probot);
_ikfilter = _pmanip->GetIkSolver()->RegisterCustomFilter(0, boost::bind(&DoorConfiguration::_CheckContinuityFilter, shared_from_this(), _1, _2, _3));
}
vector<dReal> _vprevsolution;
boost::shared_ptr<planningutils::SimpleDistanceMetric> _robotdistmetric, _doordistmetric;
boost::shared_ptr<planningutils::SimpleNeighborhoodSampler> _robotsamplefn, _doorsamplefn;
boost::shared_ptr<planningutils::LineCollisionConstraint> _collision;
};
class PlanningDoorExample : public OpenRAVEExample
{
public:
virtual void demothread(int argc, char ** argv) {
string scenefilename = "data/wam_cabinet.env.xml";
penv->Load(scenefilename);
for(size_t i = 1; i < probot->GetManipulators().size(); ++i) {
if( pmanip->GetArmIndices().size() < probot->GetManipulators()[i]->GetArmIndices().size() ) {
pmanip = probot->GetManipulators()[i];
}
}
RAVELOG_INFO(str(boost::format(
"planning with manipulator %s\n")%pmanip->GetName()));
std::vector<dReal> vpreshape(4);
vpreshape[0] = 2.3; vpreshape[1] = 2.3; vpreshape[2] = 0.8; vpreshape[3] = 0;
{
EnvironmentMutex::scoped_lock lock(penv->GetMutex());
probot->SetActiveDOFs(pmanip->GetGripperIndices());
probot->SetActiveDOFValues(vpreshape);
doorconfig->SetPlannerParameters(params);
params->_nMaxIterations = 150;
trobotorig = probot->GetTransform();
}
int iter = 0;
while(IsOk()) {
iter += 1;
{
EnvironmentMutex::scoped_lock lock(penv->GetMutex());
if( (iter%5) == 0 ) {
for(int i = 0; i < 100; ++i) {
probot->SetTransform(tnew);
try {
params->_getstatefn(params->vinitialconfig);
params->_setstatefn(params->vinitialconfig);
params->_getstatefn(params->vinitialconfig);
params->vgoalconfig = params->vinitialconfig;
params->_setstatefn(params->vgoalconfig);
params->_getstatefn(params->vgoalconfig);
break;
}
probot->SetTransform(trobotorig);
}
}
}
else {
params->_getstatefn(params->vinitialconfig);
params->_setstatefn(params->vinitialconfig);
params->_getstatefn(params->vinitialconfig);
params->vgoalconfig = params->vinitialconfig;
params->_setstatefn(params->vgoalconfig);
params->_getstatefn(params->vgoalconfig);
}
if( !planner->InitPlan(probot,params) ) {
continue;
}
if( !planner->PlanPath(ptraj) ) {
continue;
}
{
vector<RaveVector<float> > vpoints;
vector<dReal> vtrajdata;
for(
dReal ftime = 0; ftime <= ptraj->GetDuration(); ftime += 0.01) {
ptraj->Sample(vtrajdata,ftime,probot->GetActiveConfigurationSpecification());
probot->SetActiveDOFValues(vtrajdata);
vpoints.push_back(pmanip->GetEndEffectorTransform().trans);
}
pgraph = penv->drawlinestrip(&vpoints[0].x,vpoints.size(),sizeof(vpoints[0]),1.0f);
}
probot->GetController()->SetPath(ptraj);
target->GetController()->SetPath(ptraj);
}
while(!probot->GetController()->IsDone() && IsOk()) {
boost::this_thread::sleep(boost::posix_time::milliseconds(1));
}
}
}
};
}
int main(
int argc,
char ** argv)
{
return example.
main(argc,argv);
}