Unverified Commit e7bbf3db authored by stonneau's avatar stonneau Committed by GitHub
Browse files

Merge pull request #45 from stonneau/python_get_colliding_obstacles

Get colliding obstacles
parents fa088318 46d89f7c
......@@ -754,6 +754,17 @@ module hpp
/// \param stateIdTo : index of the second state
Names_t getContactsVariations(in unsigned short stateIdFrom,in unsigned short stateIdTo) raises (Error);
/// 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
......
......@@ -37,4 +37,16 @@ def addVector(viewer,rbprmBuilder,color,v,name=None):
gui.applyConfiguration(name,v)
gui.refresh()
def displaySurfaceFromPoints(viewer,p_list,color=[0,0,0,1],name=None):
gui = viewer.client.gui
if name==None:
i=0
name='surface_'+str(i)
while name in gui.getNodeList():
i=i+1
name='surface_'+str(i)
gui.addCurve(name,p_list,color)
gui.addToGroup(name,viewer.sceneName)
gui.refresh()
......@@ -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
......@@ -1553,6 +1555,156 @@ namespace hpp {
}
}
Names_t* RbprmBuilder::getCollidingObstacleAtConfig(const ::hpp::floatSeq& configuration,const char* limbName)throw (hpp::Error){
try
{
std::vector<std::string> res;
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);
hpp::pinocchio::RbPrmDevicePtr_t rbprmDevice = boost::dynamic_pointer_cast<hpp::pinocchio::RbPrmDevice>(romDevice);
RbPrmPathValidationPtr_t rbprmPathValidation_ (boost::dynamic_pointer_cast<hpp::rbprm::RbPrmPathValidation>(problemSolver()->problem()->pathValidation()));
rbprmPathValidation_->getValidator()->computeAllContacts(true);
core::ValidationReportPtr_t report;
problemSolver()->problem()->configValidations()->validate(q,report);
core::RbprmValidationReportPtr_t rbReport = boost::dynamic_pointer_cast<hpp::core::RbprmValidationReport> (report);
for(std::map<std::string,core::CollisionValidationReportPtr_t>::const_iterator it = rbReport->ROMReports.begin() ; it != rbReport->ROMReports.end() ; ++it){
if (name == it->first)
//if (true)
{
core::AllCollisionsValidationReportPtr_t romReports = boost::dynamic_pointer_cast<core::AllCollisionsValidationReport>(it->second);
if(!romReports){
hppDout(warning,"For rom : "<<it->first<<" unable to cast in a AllCollisionsValidationReport, did you correctly call computeAllContacts(true) before generating the report ? ");
//return;
}
if(romReports->collisionReports.size()> 1){
for(std::vector<CollisionValidationReportPtr_t>::const_iterator itAff = romReports->collisionReports.begin() ; itAff != romReports->collisionReports.end() ; ++itAff){
res.push_back((*itAff)->object2->name ());
}
}
}
}
CORBA::ULong size = (CORBA::ULong) res.size ();
char** nameList = Names_t::allocbuf(size);
Names_t *variations = new Names_t (size,size,nameList);
for (std::size_t i = 0 ; i < res.size() ; ++i){
nameList[i] = (char*) malloc (sizeof(char)*(res[i].length ()+1));
strcpy (nameList [i], res[i].c_str ());
}
return variations;
}
catch(std::runtime_error& e)
{
throw Error(e.what());
}
}
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.");
// Compute the referencePoint for the given configuration : heuristic used to select a 'best' contact surface:
hpp::pinocchio::RbPrmDevicePtr_t rbprmDevice = boost::dynamic_pointer_cast<hpp::pinocchio::RbPrmDevice>(problemSolver()->robot ());
fcl::Vec3f reference = rbprmDevice->getEffectorReference(name);
hppDout(notice,"Reference position for rom"<<name<<" = "<<reference);
//apply transform from currernt config :
fcl::Transform3f tRoot;
tRoot.setTranslation(fcl::Vec3f(q[0],q[1],q[2]));
fcl::Quaternion3f quat(q[6],q[3],q[4],q[5]);
tRoot.setRotation(quat.matrix());
reference = (tRoot*reference).getTranslation();
geom::Point refPoint(reference);
hppDout(notice,"Reference after root transform = "<<reference);
geom::Point normal,proj;
double minDistance = std::numeric_limits<double>::max();
double distance;
_CORBA_ULong bestSurface(0);
// init floatSeqSeq to store results
hpp::floatSeqSeq *res;
res = new hpp::floatSeqSeq ();
res->length ((_CORBA_ULong)romReports->collisionReports.size());
_CORBA_ULong 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());
// compute heuristic score :
distance = geom::projectPointInsidePlan(inter,refPoint,normal,inter.front(),proj);
hppDout(notice,"Distance found : "<<distance);
if(distance < minDistance){
minDistance = distance;
bestSurface = idSurface;
}
// 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) [idSurface] = floats;
++idSurface;
}
}
}
// swap res[0] and res[bestSurface]:
if(bestSurface>0){
hpp::floatSeq tmp = (*res)[0];
(*res)[0] = (*res)[bestSurface];
(*res)[bestSurface] = tmp;
}
return res;
}
catch(std::runtime_error& e)
{
throw Error(e.what());
}
}
std::vector<State> TimeStatesToStates(const T_StateFrame& ref)
{
std::vector<State> res;
......
......@@ -357,7 +357,10 @@ namespace hpp {
virtual hpp::floatSeq* evaluateConfig(const hpp::floatSeq& configuration, const hpp::floatSeq &direction) throw (hpp::Error);
virtual void dumpProfile(const char* logFile) throw (hpp::Error);
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* 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