Commit c22dce8a authored by Akseppal's avatar Akseppal
Browse files

replace normalFilter with affFilters

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