From bb0e36cfd404050e353bb7a7bac81e7bbe7a6fce Mon Sep 17 00:00:00 2001
From: Steve Tonneau <stonneau@axle.laas.fr>
Date: Fri, 21 Aug 2015 14:43:38 +0200
Subject: [PATCH] added setNormalFilter method to python bindings
---
idl/hpp/corbaserver/rbprm/rbprmbuilder.idl | 10 ++++++
src/hpp/corbaserver/rbprm/rbprmbuilder.py | 36 ++++++++++++++++------
src/rbprmbuilder.impl.cc | 25 ++++++++-------
src/rbprmbuilder.impl.hh | 6 ++--
4 files changed, 55 insertions(+), 22 deletions(-)
diff --git a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
index 9bcc3b2..4993973 100755
--- a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
+++ b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
@@ -96,6 +96,16 @@ module hpp
void setFilter (in Names_t roms)
raises (Error);
+ /// 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);
+
/// Get Sample configuration by its id
/// \param sampleName name of the limb from which to retrieve a sample
/// \param sampleId id of the desired samples
diff --git a/src/hpp/corbaserver/rbprm/rbprmbuilder.py b/src/hpp/corbaserver/rbprm/rbprmbuilder.py
index cff501d..72c9cc1 100755
--- a/src/hpp/corbaserver/rbprm/rbprmbuilder.py
+++ b/src/hpp/corbaserver/rbprm/rbprmbuilder.py
@@ -48,7 +48,16 @@ class Builder (object):
self.client = CorbaClient ()
self.load = load
- ## Virtual function to load the robot model
+ ## Virtual function to load the robot model.
+ #
+ # \param urdfName urdf description of the robot trunk,
+ # \param urdfNameroms either a string, or an array of strings, indicating the urdf of the different roms to add.
+ # \param rootJointType type of root joint among ("freeflyer", "planar",
+ # "anchor"),
+ # \param meshPackageName name of the meshpackage from where the robot mesh will be loaded
+ # \param packageName name of the package from where the robot will be loaded
+ # \param urdfSuffix optional suffix for the urdf of the robot package
+ # \param srdfSuffix optional suffix for the srdf of the robot package
def loadModel (self, urdfName, urdfNameroms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix):
if(isinstance(urdfNameroms, list)):
for urdfNamerom in urdfNameroms:
@@ -81,19 +90,28 @@ class Builder (object):
## Init RbprmShooter
#
- # \param jointName name of the joint,
- # \return name of the link.
def initshooter (self):
return self.client.rbprm.rbprm.initshooter ()
- ## \}
- ## Init RbprmShooter
- #
- # \param jointName name of the joint,
- # \return name of the link.
+ ## Specifies a preferred normal direction 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)
+
+ ## Specifies a rom constraint for the planner.
+ # A configuration will be valid if and only if the considered rom collides
+ # with the environment.
+ #
+ # \param romFilter array of roms indicated by name, which determine the constraint.
def setFilter (self, romFilter):
return self.client.rbprm.rbprm.setFilter (romFilter)
- ## \}
+
## \name Degrees of freedom
# \{
diff --git a/src/rbprmbuilder.impl.cc b/src/rbprmbuilder.impl.cc
index 4726183..97ef3c2 100755
--- a/src/rbprmbuilder.impl.cc
+++ b/src/rbprmbuilder.impl.cc
@@ -226,6 +226,20 @@ namespace hpp {
bindShooter_.romFilter_ = stringConversion(roms);
}
+
+ void RbprmBuilder::setNormalFilter(const char* romName, const hpp::floatSeq& normal, double range) 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));
+ }
+
hpp::floatSeq* RbprmBuilder::generateContacts(const hpp::floatSeq& configuration, const hpp::floatSeq& direction) throw (hpp::Error)
{
if(!fullBodyLoaded_)
@@ -455,17 +469,6 @@ namespace hpp {
}
}
- /*namespace
- {
- 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));
- hpp::core::CollisionPathValidationReport defaultValidation;
- return hpp::core::DiscretizedCollisionChecking::createWithValidation(robot,val ,defaultValidation, validation);
- }
- }*/
-
void RbprmBuilder::SetProblemSolver (hpp::core::ProblemSolverPtr_t problemSolver)
{
problemSolver_ = problemSolver;
diff --git a/src/rbprmbuilder.impl.hh b/src/rbprmbuilder.impl.hh
index 14a7b2f..22358db 100755
--- a/src/rbprmbuilder.impl.hh
+++ b/src/rbprmbuilder.impl.hh
@@ -45,19 +45,20 @@ namespace hpp {
{
hpp::model::RbPrmDevicePtr_t robotcast = boost::static_pointer_cast<hpp::model::RbPrmDevice>(robot);
return hpp::rbprm::RbPrmShooter::create
- (robotcast,problemSolver_->problem ()->collisionObstacles(),romFilter_,shootLimit_,displacementLimit_);
+ (robotcast,problemSolver_->problem ()->collisionObstacles(),romFilter_,normalFilter_,shootLimit_,displacementLimit_);
}
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_));
+ hpp::rbprm::RbPrmValidationPtr_t validation(hpp::rbprm::RbPrmValidation::create(robotcast, romFilter_, normalFilter_));
hpp::core::CollisionPathValidationReport defaultValidation;
return hpp::core::DiscretizedCollisionChecking::createWithValidation(robot,val ,defaultValidation, validation);
}
hpp::core::ProblemSolverPtr_t problemSolver_;
std::vector<std::string> romFilter_;
+ std::map<std::string, NormalFilter> normalFilter_;
std::size_t shootLimit_;
std::size_t displacementLimit_;
};
@@ -90,6 +91,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 hpp::floatSeq* getSampleConfig(const char* limb, unsigned short sampleId) throw (hpp::Error);
virtual hpp::floatSeq* getSamplePosition(const char* limb, unsigned short sampleId) throw (hpp::Error);
--
GitLab