From 7e83fa93f5801f4307616fe34b46cc947e1d6c2f Mon Sep 17 00:00:00 2001
From: Steve Tonneau <stonneau@axle.laas.fr>
Date: Mon, 12 Oct 2015 16:10:42 +0200
Subject: [PATCH] python binding to bound so3
---
src/hpp/corbaserver/rbprm/rbprmbuilder.py | 6 ++++++
src/rbprmbuilder.impl.cc | 22 ++++++++++++++++++++++
src/rbprmbuilder.impl.hh | 8 +++++++-
3 files changed, 35 insertions(+), 1 deletion(-)
diff --git a/src/hpp/corbaserver/rbprm/rbprmbuilder.py b/src/hpp/corbaserver/rbprm/rbprmbuilder.py
index 4d53db18..b68ca7f0 100755
--- a/src/hpp/corbaserver/rbprm/rbprmbuilder.py
+++ b/src/hpp/corbaserver/rbprm/rbprmbuilder.py
@@ -86,6 +86,12 @@ class Builder (object):
def initshooter (self):
return self.client.rbprm.rbprm.initshooter ()
+ ## Sets limits on robot orientation, described according to Euler's ZYX rotation order
+ #
+ # \param bounds 6D vector with the lower and upperBound for each rotation axis in sequence
+ def boundSO3 (self, bounds):
+ return self.client.rbprm.rbprm.boundSO3 (bounds)
+
## 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.
diff --git a/src/rbprmbuilder.impl.cc b/src/rbprmbuilder.impl.cc
index dc40646b..0ac4cdc1 100755
--- a/src/rbprmbuilder.impl.cc
+++ b/src/rbprmbuilder.impl.cc
@@ -221,6 +221,17 @@ namespace hpp {
return res;
}
+ std::vector<double> doubleConversion(const hpp::floatSeq& dofArray)
+ {
+ std::vector<double> res;
+ std::size_t dim = (std::size_t)dofArray.length();
+ for (std::size_t iDof = 0; iDof < dim; iDof++)
+ {
+ res.push_back(dofArray[iDof]);
+ }
+ return res;
+ }
+
void RbprmBuilder::setFilter(const hpp::Names_t& roms) throw (hpp::Error)
{
@@ -241,6 +252,17 @@ namespace hpp {
bindShooter_.normalFilter_.insert(std::make_pair(name,filter));
}
+ void RbprmBuilder::boundSO3(const hpp::floatSeq& limitszyx) throw (hpp::Error)
+ {
+ std::vector<double> limits = doubleConversion(limitszyx);
+ if(limits.size() !=6)
+ {
+ throw Error ("Can not bound SO3, array of 6 double required");
+ }
+ bindShooter_.so3Bounds_ = limits;
+
+ }
+
hpp::floatSeq* RbprmBuilder::generateContacts(const hpp::floatSeq& configuration, const hpp::floatSeq& direction) throw (hpp::Error)
{
if(!fullBodyLoaded_)
diff --git a/src/rbprmbuilder.impl.hh b/src/rbprmbuilder.impl.hh
index b1e70722..1b440bae 100755
--- a/src/rbprmbuilder.impl.hh
+++ b/src/rbprmbuilder.impl.hh
@@ -44,8 +44,11 @@ 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);
- return hpp::rbprm::RbPrmShooter::create
+ rbprm::RbPrmShooterPtr_t shooter = hpp::rbprm::RbPrmShooter::create
(robotcast,problemSolver_->problem ()->collisionObstacles(),romFilter_,normalFilter_,shootLimit_,displacementLimit_);
+ if(!so3Bounds_.empty())
+ shooter->BoundSO3(so3Bounds_);
+ return shooter;
}
hpp::core::PathValidationPtr_t createPathValidation (const hpp::model::DevicePtr_t& robot, const hpp::model::value_type& val)
@@ -61,6 +64,7 @@ namespace hpp {
std::map<std::string, NormalFilter> normalFilter_;
std::size_t shootLimit_;
std::size_t displacementLimit_;
+ std::vector<double> so3Bounds_;
};
class RbprmBuilder : public virtual POA_hpp::corbaserver::rbprm::RbprmBuilder
@@ -92,6 +96,8 @@ namespace hpp {
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 boundSO3(const hpp::floatSeq& limitszyx) 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