Commit c807c6ba authored by Akseppal's avatar Akseppal
Browse files

add typedef and use bindShooter_.affMap_ in fullBody functions that need affordance objects

parent 58317570
...@@ -270,7 +270,8 @@ namespace hpp { ...@@ -270,7 +270,8 @@ namespace hpp {
} }
hpp::floatSeq* RbprmBuilder::generateContacts(const hpp::floatSeq& configuration, const hpp::floatSeq& direction) throw (hpp::Error) hpp::floatSeq* RbprmBuilder::generateContacts(const hpp::floatSeq& configuration,
const hpp::floatSeq& direction) throw (hpp::Error)
{ {
if(!fullBodyLoaded_) if(!fullBodyLoaded_)
throw Error ("No full body robot was loaded"); throw Error ("No full body robot was loaded");
...@@ -281,8 +282,11 @@ namespace hpp { ...@@ -281,8 +282,11 @@ namespace hpp {
{ {
dir[i] = direction[i]; dir[i] = direction[i];
} }
if (bindShooter_.affMap_.empty ()) {
throw hpp::Error ("No affordances found. Unable to generate Contacts.");
}
model::Configuration_t config = dofArrayToConfig (fullBody_->device_, configuration); model::Configuration_t config = dofArrayToConfig (fullBody_->device_, configuration);
rbprm::State state = rbprm::ComputeContacts(fullBody_,config, rbprm::State state = rbprm::ComputeContacts(fullBody_,config, bindShooter_.affMap_,
problemSolver_->collisionObstacles(), dir); problemSolver_->collisionObstacles(), dir);
hpp::floatSeq* dofArray = new hpp::floatSeq(); hpp::floatSeq* dofArray = new hpp::floatSeq();
dofArray->length(_CORBA_ULong(state.configuration_.rows())); dofArray->length(_CORBA_ULong(state.configuration_.rows()));
...@@ -347,8 +351,10 @@ namespace hpp { ...@@ -347,8 +351,10 @@ namespace hpp {
} }
} }
void RbprmBuilder::addLimb(const char* id, const char* limb, const char* effector, const hpp::floatSeq& offset, const hpp::floatSeq& normal, double x, double y, void RbprmBuilder::addLimb(const char* id, const char* limb,
unsigned short samples, const char* heuristicName, double resolution, const char *contactType) throw (hpp::Error) const char* effector, const hpp::floatSeq& offset, const hpp::floatSeq& normal,
double x, double y, unsigned short samples, const char* heuristicName,
double resolution, const char *contactType) throw (hpp::Error)
{ {
if(!fullBodyLoaded_) if(!fullBodyLoaded_)
throw Error ("No full body robot was loaded"); throw Error ("No full body robot was loaded");
...@@ -365,7 +371,9 @@ namespace hpp { ...@@ -365,7 +371,9 @@ namespace hpp {
{ {
cType = hpp::rbprm::_3_DOF; cType = hpp::rbprm::_3_DOF;
} }
fullBody_->AddLimb(std::string(id), std::string(limb), std::string(effector), off, norm, x, y, problemSolver_->collisionObstacles(), samples,heuristicName,resolution,cType); fullBody_->AddLimb(std::string(id), std::string(limb),
std::string(effector), off, norm, x, y, problemSolver_->collisionObstacles(),
samples,heuristicName,resolution,cType);
} }
catch(std::runtime_error& e) catch(std::runtime_error& e)
{ {
...@@ -373,7 +381,9 @@ namespace hpp { ...@@ -373,7 +381,9 @@ namespace hpp {
} }
} }
void SetPositionAndNormal(rbprm::State& state, hpp::rbprm::RbPrmFullBodyPtr_t fullBody, const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) void SetPositionAndNormal(rbprm::State& state,
hpp::rbprm::RbPrmFullBodyPtr_t fullBody, const hpp::floatSeq& configuration,
const hpp::Names_t& contactLimbs)
{ {
core::Configuration_t old = fullBody->device_->currentConfiguration(); core::Configuration_t old = fullBody->device_->currentConfiguration();
model::Configuration_t config = dofArrayToConfig (fullBody->device_, configuration); model::Configuration_t config = dofArrayToConfig (fullBody->device_, configuration);
...@@ -428,7 +438,8 @@ namespace hpp { ...@@ -428,7 +438,8 @@ namespace hpp {
} }
} }
floatSeqSeq* RbprmBuilder::interpolateConfigs(const hpp::floatSeqSeq& configs, double robustnessTreshold) throw (hpp::Error) floatSeqSeq* RbprmBuilder::interpolateConfigs(const hpp::floatSeqSeq& configs,
double robustnessTreshold) throw (hpp::Error)
{ {
try try
{ {
...@@ -440,16 +451,23 @@ namespace hpp { ...@@ -440,16 +451,23 @@ namespace hpp {
{ {
throw std::runtime_error ("End state not initialized, can not interpolate "); throw std::runtime_error ("End state not initialized, can not interpolate ");
} }
hpp::rbprm::RbPrmInterpolationPtr_t interpolator = rbprm::RbPrmInterpolation::create(fullBody_,startState_,endState_); if (bindShooter_.affMap_.empty ()) {
throw hpp::Error ("No affordances found. Unable to interpolate.");
}
hpp::rbprm::RbPrmInterpolationPtr_t interpolator =
rbprm::RbPrmInterpolation::create(fullBody_,startState_,endState_);
std::vector<model::Configuration_t> configurations = doubleDofArrayToConfig(fullBody_->device_, configs); std::vector<model::Configuration_t> configurations = doubleDofArrayToConfig(fullBody_->device_, configs);
lastStatesComputed_ = interpolator->Interpolate(problemSolver_->collisionObstacles(),configurations,robustnessTreshold); lastStatesComputed_ = interpolator->Interpolate(bindShooter_.affMap_,
problemSolver_->collisionObstacles(),configurations,robustnessTreshold);
hpp::floatSeqSeq *res; hpp::floatSeqSeq *res;
res = new hpp::floatSeqSeq (); res = new hpp::floatSeqSeq ();
res->length (lastStatesComputed_.size ()); res->length (lastStatesComputed_.size ());
std::size_t i=0; std::size_t i=0;
std::size_t id = 0; std::size_t id = 0;
for(std::vector<State>::const_iterator cit = lastStatesComputed_.begin(); cit != lastStatesComputed_.end(); ++cit, ++id) for(std::vector<State>::const_iterator cit = lastStatesComputed_.begin();
cit != lastStatesComputed_.end(); ++cit, ++id)
{ {
std::cout << "ID " << id; std::cout << "ID " << id;
cit->print(); cit->print();
...@@ -479,20 +497,24 @@ namespace hpp { ...@@ -479,20 +497,24 @@ namespace hpp {
int pathId = int(path); int pathId = int(path);
if(startState_.configuration_.rows() == 0) if(startState_.configuration_.rows() == 0)
{ {
throw std::runtime_error ("Start state not initialized, can not interpolate "); throw std::runtime_error ("Start state not initialized, cannot interpolate ");
} }
if(endState_.configuration_.rows() == 0) if(endState_.configuration_.rows() == 0)
{ {
throw std::runtime_error ("End state not initialized, can not interpolate "); throw std::runtime_error ("End state not initialized, cannot interpolate ");
} }
if(problemSolver_->paths().size() <= pathId) if(problemSolver_->paths().size() <= pathId)
{ {
throw std::runtime_error ("No path computed, cannot interpolate "); throw std::runtime_error ("No path computed, cannot interpolate ");
} }
if (bindShooter_.affMap_.empty ()) {
throw hpp::Error ("No affordances found. Unable to interpolate.");
}
hpp::rbprm::RbPrmInterpolationPtr_t interpolator = rbprm::RbPrmInterpolation::create(fullBody_,startState_,endState_,problemSolver_->paths()[pathId]); hpp::rbprm::RbPrmInterpolationPtr_t interpolator =
lastStatesComputed_ = interpolator->Interpolate(problemSolver_->collisionObstacles(),timestep,robustnessTreshold); rbprm::RbPrmInterpolation::create(fullBody_,startState_,endState_,problemSolver_->paths()[pathId]);
lastStatesComputed_ = interpolator->Interpolate(bindShooter_.affMap_,
problemSolver_->collisionObstacles(),timestep,robustnessTreshold);
hpp::floatSeqSeq *res; hpp::floatSeqSeq *res;
res = new hpp::floatSeqSeq (); res = new hpp::floatSeqSeq ();
...@@ -500,7 +522,8 @@ namespace hpp { ...@@ -500,7 +522,8 @@ namespace hpp {
res->length (lastStatesComputed_.size ()); res->length (lastStatesComputed_.size ());
std::size_t i=0; std::size_t i=0;
std::size_t id = 0; std::size_t id = 0;
for(std::vector<State>::const_iterator cit = lastStatesComputed_.begin(); cit != lastStatesComputed_.end(); ++cit, ++id) for(std::vector<State>::const_iterator cit = lastStatesComputed_.begin();
cit != lastStatesComputed_.end(); ++cit, ++id)
{ {
std::cout << "ID " << id; std::cout << "ID " << id;
cit->print(); cit->print();
...@@ -547,7 +570,7 @@ namespace hpp { ...@@ -547,7 +570,7 @@ namespace hpp {
} }
else else
{ {
std::string error("Can not open outfile " + std::string(outfilename)); std::string error("Cannot open outfile " + std::string(outfilename));
throw Error(error.c_str()); throw Error(error.c_str());
} }
} }
......
...@@ -35,6 +35,7 @@ namespace hpp { ...@@ -35,6 +35,7 @@ namespace hpp {
namespace rbprm { namespace rbprm {
namespace impl { namespace impl {
using CORBA::Short; using CORBA::Short;
typedef std::map<std::string, std::vector<boost::shared_ptr<model::CollisionObject> > > affMap_t;
struct BindShooter struct BindShooter
{ {
...@@ -78,7 +79,7 @@ namespace hpp { ...@@ -78,7 +79,7 @@ namespace hpp {
std::size_t shootLimit_; std::size_t shootLimit_;
std::size_t displacementLimit_; std::size_t displacementLimit_;
std::vector<double> so3Bounds_; std::vector<double> so3Bounds_;
std::map<std::string, std::vector<boost::shared_ptr<model::CollisionObject> > > affMap_; affMap_t affMap_;
}; };
class RbprmBuilder : public virtual POA_hpp::corbaserver::rbprm::RbprmBuilder class RbprmBuilder : public virtual POA_hpp::corbaserver::rbprm::RbprmBuilder
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment