Commit c22dce8a authored by Akseppal's avatar Akseppal
Browse files

replace normalFilter with affFilters

parent 2f459a0f
...@@ -98,13 +98,11 @@ module hpp ...@@ -98,13 +98,11 @@ module hpp
/// Set Rom surface constraints for the configuration shooter /// Set Rom surface constraints for the configuration shooter
/// a Rom configuration will only be valid it collides with a surface /// a Rom configuration will only be valid it collides with a surface
/// the normal of which is colinear to the indicated normal, /// that forms a given affordance (support, lean, etc.)
/// modulated by a range value. /// \param affordances a list of affordances accepted for
/// \param normal prefered contact surface normal direction /// validation of given Rom
/// \param range tolerance between surface normal and desired surface normal. If the dot ///
/// product of the normal is greater than range then the surface will be accepted. void setAffordanceFilter(in string romName, in Names_t affordances) raises (Error);
///
void setNormalFilter(in string romName, in floatSeq normal, in double range) raises (Error);
/// Sets limits on robot orientation, described according to Euler's ZYX rotation order /// Sets limits on robot orientation, described according to Euler's ZYX rotation order
/// ///
......
...@@ -93,16 +93,14 @@ class Builder (object): ...@@ -93,16 +93,14 @@ class Builder (object):
def boundSO3 (self, bounds): def boundSO3 (self, bounds):
return self.client.rbprm.rbprm.boundSO3 (bounds) return self.client.rbprm.rbprm.boundSO3 (bounds)
## Specifies a preferred normal direction for a given rom. ## Specifies a preferred affordance for a given rom.
# This constrains the planner to accept a rom configuration only if # This constrains the planner to accept a rom configuration only if
# it collides with a surface the normal of which has these properties. # it collides with a surface the normal of which has these properties.
# #
# \param rom name of the rome, # \param rom name of the rome,
# \param normal 3d vector specifying the normal, # \param affordances list of affordance names
# \param tolerance expressed as the dot product between the considered obstacle and the ideal normal. def setAffordanceFilter (self, rom, affordances):
# if the dot product is greater than the tolerance the surface will be considered valid. return self.client.rbprm.rbprm.setAffordanceFilter (rom, affordances)
def setNormalFilter (self, rom, normal, tolerance):
return self.client.rbprm.rbprm.setNormalFilter (rom, normal, tolerance)
## Specifies a rom constraint for the planner. ## Specifies a rom constraint for the planner.
# A configuration will be valid if and only if the considered rom collides # A configuration will be valid if and only if the considered rom collides
......
...@@ -251,17 +251,12 @@ namespace hpp { ...@@ -251,17 +251,12 @@ namespace hpp {
} }
void RbprmBuilder::setNormalFilter(const char* romName, const hpp::floatSeq& normal, double range) throw (hpp::Error) void RbprmBuilder::setAffordanceFilter(const char* romName, const hpp::Names_t& affordances) throw (hpp::Error)
{ {
std::string name(romName); std::string name (romName);
bindShooter_.normalFilter_.erase(name); std::vector<std::string> affNames = stringConversion(affordances);
fcl::Vec3f dir; bindShooter_.affFilter_.erase(name);
for(std::size_t i =0; i <3; ++i) bindShooter_.affFilter_.insert(std::make_pair(name,affNames));
{
dir[i] = normal[i];
}
NormalFilter filter(dir,range);
bindShooter_.normalFilter_.insert(std::make_pair(name,filter));
} }
void RbprmBuilder::boundSO3(const hpp::floatSeq& limitszyx) throw (hpp::Error) void RbprmBuilder::boundSO3(const hpp::floatSeq& limitszyx) throw (hpp::Error)
......
...@@ -29,6 +29,8 @@ ...@@ -29,6 +29,8 @@
# include <hpp/core/discretized-collision-checking.hh> # include <hpp/core/discretized-collision-checking.hh>
# include <hpp/core/straight-path.hh> # include <hpp/core/straight-path.hh>
#include <hpp/fcl/BVH/BVH_model.h>
namespace hpp { namespace hpp {
namespace rbprm { namespace rbprm {
namespace impl { namespace impl {
...@@ -44,8 +46,15 @@ namespace hpp { ...@@ -44,8 +46,15 @@ namespace hpp {
hpp::rbprm::RbPrmShooterPtr_t create (const hpp::model::DevicePtr_t& robot) hpp::rbprm::RbPrmShooterPtr_t create (const hpp::model::DevicePtr_t& robot)
{ {
hpp::model::RbPrmDevicePtr_t robotcast = boost::static_pointer_cast<hpp::model::RbPrmDevice>(robot); hpp::model::RbPrmDevicePtr_t robotcast = boost::static_pointer_cast<hpp::model::RbPrmDevice>(robot);
std::map<std::string, std::vector<boost::shared_ptr<model::CollisionObject> > > affMap =
problemSolver_->map
<std::vector<boost::shared_ptr<model::CollisionObject> > > ();
if (affMap.empty ()) {
throw hpp::Error ("No affordances found. Unable to create shooter object.");
}
rbprm::RbPrmShooterPtr_t shooter = hpp::rbprm::RbPrmShooter::create rbprm::RbPrmShooterPtr_t shooter = hpp::rbprm::RbPrmShooter::create
(robotcast,problemSolver_->problem ()->collisionObstacles(),romFilter_,normalFilter_,shootLimit_,displacementLimit_); (robotcast, problemSolver_->problem ()->collisionObstacles(), affMap,
romFilter_,affFilter_,shootLimit_,displacementLimit_);
if(!so3Bounds_.empty()) if(!so3Bounds_.empty())
shooter->BoundSO3(so3Bounds_); shooter->BoundSO3(so3Bounds_);
return shooter; return shooter;
...@@ -54,7 +63,7 @@ namespace hpp { ...@@ -54,7 +63,7 @@ namespace hpp {
hpp::core::PathValidationPtr_t createPathValidation (const hpp::model::DevicePtr_t& robot, const hpp::model::value_type& val) hpp::core::PathValidationPtr_t createPathValidation (const hpp::model::DevicePtr_t& robot, const hpp::model::value_type& val)
{ {
hpp::model::RbPrmDevicePtr_t robotcast = boost::static_pointer_cast<hpp::model::RbPrmDevice>(robot); hpp::model::RbPrmDevicePtr_t robotcast = boost::static_pointer_cast<hpp::model::RbPrmDevice>(robot);
hpp::rbprm::RbPrmValidationPtr_t validation(hpp::rbprm::RbPrmValidation::create(robotcast, romFilter_, normalFilter_)); hpp::rbprm::RbPrmValidationPtr_t validation(hpp::rbprm::RbPrmValidation::create(robotcast, romFilter_, affFilter_));
hpp::core::DiscretizedCollisionCheckingPtr_t collisionChecking = hpp::core::DiscretizedCollisionChecking::create(robot,val); hpp::core::DiscretizedCollisionCheckingPtr_t collisionChecking = hpp::core::DiscretizedCollisionChecking::create(robot,val);
collisionChecking->add (validation); collisionChecking->add (validation);
return collisionChecking; return collisionChecking;
...@@ -62,7 +71,7 @@ namespace hpp { ...@@ -62,7 +71,7 @@ namespace hpp {
hpp::core::ProblemSolverPtr_t problemSolver_; hpp::core::ProblemSolverPtr_t problemSolver_;
std::vector<std::string> romFilter_; std::vector<std::string> romFilter_;
std::map<std::string, NormalFilter> normalFilter_; std::map<std::string, std::vector<std::string> > affFilter_;
std::size_t shootLimit_; std::size_t shootLimit_;
std::size_t displacementLimit_; std::size_t displacementLimit_;
std::vector<double> so3Bounds_; std::vector<double> so3Bounds_;
...@@ -96,7 +105,7 @@ namespace hpp { ...@@ -96,7 +105,7 @@ namespace hpp {
const char* srdfSuffix) throw (hpp::Error); const char* srdfSuffix) throw (hpp::Error);
virtual void setFilter(const hpp::Names_t& roms) throw (hpp::Error); virtual void setFilter(const hpp::Names_t& roms) throw (hpp::Error);
virtual void setNormalFilter(const char* romName, const hpp::floatSeq& normal, double range) throw (hpp::Error); virtual void setAffordanceFilter(const char* romName, const hpp::Names_t& affordances) throw (hpp::Error);
virtual void boundSO3(const hpp::floatSeq& limitszyx) throw (hpp::Error); virtual void boundSO3(const hpp::floatSeq& limitszyx) throw (hpp::Error);
......
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