Commit 1d29f658 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

add method getContactSurfacesAtConfig to python API

parent bfb15076
......@@ -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
......
......@@ -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
......@@ -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;
......
......@@ -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);
......
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