Skip to content
Snippets Groups Projects
Commit 7e83fa93 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

python binding to bound so3

parent 378a1b0e
No related branches found
No related tags found
No related merge requests found
......@@ -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.
......
......@@ -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_)
......
......@@ -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);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment