diff --git a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
index 5baed51b0af5d58eea245c5ae41c639acd0c6618..42ddf5ec84b5a3d5ec064f55b05645873e27a853 100644
--- a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
+++ b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
@@ -737,11 +737,16 @@ module hpp
Names_t getContactsVariations(in unsigned short stateIdFrom,in unsigned short stateIdTo) raises (Error);
- /// For a given limb, returns all the contact surfaces in collision with the limb's reachable workspace
+ /// For a given limb, returns the names of all the contact surfaces in collisions with the limb's reachable workspace
/// \param configuration : root configuration
/// \param limbName : name of the considered limb
Names_t getCollidingObstacleAtConfig(in floatSeq configuration,in string limbName) raises (Error);
+ /// For a given limb, return all the intersections between the limb reachable workspace and a contact surface
+ /// \param configuration : root configuration
+ /// \param limbName : name of the considered limb
+ floatSeqSeq getContactSurfacesAtConfig(in floatSeq configuration,in string limbName) raises (Error);
+
/// return a list of all the limbs names
Names_t getAllLimbsNames() raises (Error);
/// tries to add a new contact to the state
diff --git a/src/hpp/corbaserver/rbprm/rbprmbuilder.py b/src/hpp/corbaserver/rbprm/rbprmbuilder.py
index 9b6308ffca4604dbc22b7bd0711cd99eabb9bf84..1984763e48297dd6a6b3afe7a6570e4898d67399 100755
--- a/src/hpp/corbaserver/rbprm/rbprmbuilder.py
+++ b/src/hpp/corbaserver/rbprm/rbprmbuilder.py
@@ -103,3 +103,14 @@ class Builder (Robot):
# \param ref the 3D reference position of the end effector, expressed in the root frame
def setReferenceEndEffector(self,romName,ref):
return self.clientRbprm.rbprm.setReferenceEndEffector(romName,ref)
+
+ ## For a given limb, return all the intersections between the limb reachable workspace and a contact surface
+ # \param configuration the root configuration
+ # \param limbName name of the considered limb
+ # \return a 3D list : first id is the different surfaces, second id is the different vertex of a surface, last id is the 3 coordinate of each vertex
+ def getContactSurfacesAtConfig(self,configuration,limbName):
+ surfaces = self.clientRbprm.rbprm.getContactSurfacesAtConfig(configuration,limbName)
+ res = []
+ for surface in surfaces:
+ res += [[surface[i:i+3] for i in range(0, len(surface),3)]] # split list of coordinate every 3 points (3D points)
+ return res
diff --git a/src/rbprmbuilder.impl.cc b/src/rbprmbuilder.impl.cc
index b08d542c199a0269ca3f347ea6a48a331fad7ff3..314c953875afd701112627d7f22903bb171ad4dd 100644
--- a/src/rbprmbuilder.impl.cc
+++ b/src/rbprmbuilder.impl.cc
@@ -50,6 +50,8 @@
#include <hpp/rbprm/sampling/heuristic-tools.hh>
#include <hpp/rbprm/contact_generation/reachability.hh>
#include <hpp/pinocchio/urdf/util.hh>
+#include "hpp/rbprm/utils/algorithms.h"
+
#ifdef PROFILE
#include "hpp/rbprm/rbprm-profiler.hh"
#endif
@@ -1508,6 +1510,78 @@ namespace hpp {
}
}
+ floatSeqSeq* RbprmBuilder::getContactSurfacesAtConfig(const ::hpp::floatSeq& configuration,const char* limbName)throw (hpp::Error){
+ try
+ {
+ hppDout(notice,"begin getContactSurfacesAtConfig");
+ std::string name (limbName);
+ //hpp::pinocchio::RbPrmDevicePtr_t rbprmDevice = boost::dynamic_pointer_cast<hpp::pinocchio::RbPrmDevice>(problemSolver()->robot ());
+ const hpp::pinocchio::DevicePtr_t romDevice = romDevices_[name];
+ pinocchio::Configuration_t q = dofArrayToConfig(romDevice, configuration);
+ romDevice->currentConfiguration(q);
+ RbPrmPathValidationPtr_t rbprmPathValidation_ (boost::dynamic_pointer_cast<hpp::rbprm::RbPrmPathValidation>(problemSolver()->problem()->pathValidation()));
+ rbprmPathValidation_->getValidator()->computeAllContacts(true);
+ core::ValidationReportPtr_t report;
+ hppDout(notice,"begin collision check");
+ problemSolver()->problem()->configValidations()->validate(q,report);
+ hppDout(notice,"done.");
+ core::RbprmValidationReportPtr_t rbReport = boost::dynamic_pointer_cast<hpp::core::RbprmValidationReport> (report);
+ hppDout(notice,"try to find rom name");
+ if(rbReport->ROMReports.find(name) == rbReport->ROMReports.end()){
+ throw std::runtime_error ("The given ROM name is not in collision in the given configuration.");
+ }
+ hppDout(notice,"try to cast report");
+ core::AllCollisionsValidationReportPtr_t romReports = boost::dynamic_pointer_cast<core::AllCollisionsValidationReport>(rbReport->ROMReports.at(name));
+ if(!romReports){
+ throw std::runtime_error ("Error while retrieving collision reports.");
+ }
+ hppDout(notice,"try deviceSync");
+ pinocchio::DeviceSync deviceSync (romDevice);
+ hppDout(notice,"done.");
+ hpp::floatSeqSeq *res;
+ res = new hpp::floatSeqSeq ();
+ res->length ((_CORBA_ULong)romReports->collisionReports.size());
+ std::size_t idSurface=0;
+ geom::Point pn;
+ hppDout(notice,"Number of collision reports for the rom : "<<romReports->collisionReports.size());
+ // for all collision report of the given ROM, compute the intersection surface between the affordance object and the rom :
+ for(std::vector<CollisionValidationReportPtr_t>::const_iterator itReport = romReports->collisionReports.begin() ; itReport != romReports->collisionReports.end() ; ++itReport){
+ // compute the intersection for itReport :
+ core::CollisionObjectConstPtr_t obj_rom = (*itReport)->object1;
+ core::CollisionObjectConstPtr_t obj_env = (*itReport)->object2;
+ // convert the two objects :
+ geom::BVHModelOBConst_Ptr_t model_rom = geom::GetModel(obj_rom,deviceSync.d());
+ geom::BVHModelOBConst_Ptr_t model_env = geom::GetModel(obj_env,deviceSync.d());
+ geom::T_Point plane = geom::intersectPolygonePlane(model_rom,model_env,pn);
+ // plane contains a list of points : the intersections between model_rom and the infinite plane defined by model_env.
+ // but they may not be contained inside the shape defined by model_env
+ if(plane.size() > 0){
+ geom::T_Point inter = geom::compute3DIntersection(plane,geom::convertBVH(model_env)); // hull contain only points inside the model_env shape
+ if(inter.size() > 0){
+ hppDout(notice,"Number of points for the intersection rom/surface : "<<inter.size());
+ // add inter points to res list:
+ _CORBA_ULong size = (_CORBA_ULong) (inter.size()*3);
+ double* dofArray = hpp::floatSeq::allocbuf(size);
+ hpp::floatSeq floats (size, size, dofArray, true);
+ //convert the config in dofseq
+ for (pinocchio::size_type j=0 ; j < (pinocchio::size_type)inter.size() ; ++j) {
+ dofArray[3*j] = inter[j][0];
+ dofArray[3*j+1] = inter[j][1];
+ dofArray[3*j+2] = inter[j][2];
+ }
+ (*res) [(_CORBA_ULong)idSurface] = floats;
+ ++idSurface;
+ }
+ }
+ }
+ return res;
+ }
+ catch(std::runtime_error& e)
+ {
+ throw Error(e.what());
+ }
+ }
+
std::vector<State> TimeStatesToStates(const T_StateFrame& ref)
{
std::vector<State> res;
diff --git a/src/rbprmbuilder.impl.hh b/src/rbprmbuilder.impl.hh
index 2116a55e9d9005fe9e1de0b038c73069e0102f41..a10049d3fed11f8ffd1bb7ff0e133d89d31f4eb3 100644
--- a/src/rbprmbuilder.impl.hh
+++ b/src/rbprmbuilder.impl.hh
@@ -348,6 +348,8 @@ namespace hpp {
virtual double getTimeAtState(unsigned short stateId)throw (hpp::Error);
virtual Names_t* getContactsVariations(unsigned short stateIdFrom,unsigned short stateIdTo )throw (hpp::Error);
virtual Names_t* getCollidingObstacleAtConfig(const ::hpp::floatSeq& configuration,const char* limbName)throw (hpp::Error);
+ virtual floatSeqSeq* getContactSurfacesAtConfig(const ::hpp::floatSeq& configuration,const char* limbName)throw (hpp::Error);
+
virtual Names_t* getAllLimbsNames()throw (hpp::Error);
virtual CORBA::Short addNewContact(unsigned short stateId, const char* limbName,
const hpp::floatSeq& position, const hpp::floatSeq& normal, unsigned short max_num_sample, bool lockOtherJoints) throw (hpp::Error);