generate one contact

idl/hpp/corbaserver/rbprm/rbprmbuilder.idl

@@ -255,6 +255,13 @@ module hpp
     floatSeqSeq getContactSamplesProjected(in string name, in floatSeq dofArray, in floatSeq direction,
     in unsigned short numSamples) raises (Error);
 
+    /// Given a contact state and a limb, tries to generate a new contact, and returns the id of the new State if successful
+    /// \param currentState Id of the considered state
+    /// \param name name of the limb used to create a contact
+    /// \param direction desired direction of motion for the robot
+    /// \return transformed configuration for which all possible contacts have been created
+    short generateContactState(in unsigned short currentState, in string name, in floatSeq direction) raises (Error);
+
     /// get Ids of limb in an octree cell
     /// \param name name of the considered limb
     /// \param octreeNodeId considered configuration of the robot

script/scenarios/sandbox/ANYmal/anymal_flatGround.py

@@ -75,10 +75,10 @@ v(q_goal)
 
 v.addLandmark('anymal/base',0.3)
 v(q_init)
-
+z = [0.,0.,1]
 # specify the full body configurations as start and goal state of the problem
-fullBody.setStartState(q_init,fullBody.limbs_names)
-fullBody.setEndState(q_goal,fullBody.limbs_names)
+fullBody.setStartState(q_init,fullBody.limbs_names, [z for _ in range(4)])
+fullBody.setEndState(q_goal,fullBody.limbs_names, [z for _ in range(4)])
 
 
 print "Generate contact plan ..."

script/scenarios/sandbox/run.sh

 #!/bin/bash         
 
-gepetto-viewer-server & 
+gepetto-gui &
 hpp-rbprm-server &
 ipython -i --no-confirm-exit ./$1
 
-pkill -f  'gepetto-viewer-server'
+pkill -f  'gepetto-gui'
 pkill -f  'hpp-rbprm-server'

src/hpp/corbaserver/rbprm/rbprmfullbody.py

@@ -224,7 +224,7 @@ class FullBody (Robot):
           num_max_sample = 1
           for (limbName, normal) in  zip(contacts, normals):
                p = cl.getEffectorPosition(limbName,configuration)[0]
-               cl.addNewContact(sId, limbName, p, normal, num_max_sample)
+               cl.addNewContact(sId, limbName, p, normal, num_max_sample, False)
           return sId
           
      ## Retrieves the contact candidates configurations given a configuration and a limb
@@ -285,7 +285,7 @@ class FullBody (Robot):
           num_max_sample = 1
           for (limbName, normal) in  zip(contacts, normals):
                p = cl.getEffectorPosition(limbName,configuration)[0]
-               cl.addNewContact(sId, limbName, p, normal, num_max_sample, True)
+               cl.addNewContact(sId, limbName, p, normal, num_max_sample, False)
           return cl.setStartStateId(sId)
           
           
@@ -303,7 +303,7 @@ class FullBody (Robot):
           num_max_sample = 1
           for (limbName, normal) in  zip(contacts, normals):
                p = cl.getEffectorPosition(limbName,configuration)[0]
-               cl.addNewContact(sId, limbName, p, normal, num_max_sample, True)
+               cl.addNewContact(sId, limbName, p, normal, num_max_sample, False)
           return cl.setEndStateId(sId)
 
      ## Initialize the first state of the path interpolation

src/hpp/corbaserver/rbprm/rbprmstate.py

@@ -202,3 +202,16 @@ class StateHelper(object):
         sId = fullBody.generateStateInContact(q,direction,acceleration)
         s = State(fullBody,sId=sId)
         return s
+        
+    @staticmethod
+    def moveAndContact(state,rootOffset,limbName):
+        s, success = StateHelper.removeContact(state, limbName)
+        assert success
+        success = s.projectToRoot((array(s.q()[:3]) + array(rootOffset)).tolist()+s.q()[3:7])
+        if not success:
+            return state, False
+        sId = s.fullBody.clientRbprm.rbprm.generateContactState(s.sId, limbName,rootOffset)
+        if sId < 0:
+            return state, False
+        s = State(s.fullBody,sId=sId)
+        return s, True

src/hpp/corbaserver/rbprm/tools/affordance_centroids.py

+from numpy import array, cross
+from numpy.linalg import norm
+
+
+## tools to retrive obstacles of a given affordance, indexed by the centroid of their surface.
+## method to import is computeAffordanceCentroids
+
+def flat(pts):
+    return [item for sublist in pts for item in sublist]
+
+__EPS = 1e-5
+
+def __filter_points(points):
+    res = []
+    for el in points:
+        el_arr = array(el)
+        add = True
+        for al in res:
+            if(norm(al - el_arr) < __EPS):
+                add = False
+                break
+        if add:
+            res += [array(el)]
+    return res
+
+def __normal(points):
+    p1 = array(points[0])
+    p2 = array(points[1])
+    p3 = array(points[2])
+    normal = cross((p2 - p1),(p3 - p1))
+    normal /= norm(normal)
+    return normal.tolist()
+    
+def __centroid(points):
+    return sum(points) / len(points)
+
+def __centroid_list(list_points):
+    return [[__centroid(__filter_points(flat(pts))).tolist(), __normal(pts[0]) ]  for pts in list_points]
+
+def computeAffordanceCentroids(afftool, affordances=["Support","Lean"]):
+    all_points = []
+    for _, el in enumerate(affordances):
+        all_points += afftool.getAffordancePoints(el)
+    return __centroid_list(all_points)
+
+b_id = 0
+
+def draw_centroid(gui, winId, pt, scene="n_name", color = [1,1,1,0.3]):
+    p = pt[0]
+    n = array(pt[0]) + 0.03 * array(pt[1])
+    resolution = 0.01
+    global b_id
+    boxname = scene+"/"+str(b_id)
+    boxnameN = scene+"/"+str(b_id)+"n"
+    b_id += 1
+    gui.addBox(boxname,resolution,resolution,resolution, color)
+    gui.addBox(boxnameN,resolution,resolution,resolution, color)
+    gui.applyConfiguration(boxname,[p[0],p[1],p[2],1,0,0,0])
+    gui.applyConfiguration(boxnameN,[n[0],n[1],n[2],1,0,0,0])
+    gui.addSceneToWindow(scene,winId)
+    gui.refresh()
+
+def draw_centroids(gui, winId, pts_lists, scene="n_name", color = [1,0,0,1]):
+    gui.createScene(scene)
+    for _, pt in enumerate(pts_lists):
+        draw_centroid(gui, winId, pt, scene=scene, color = color)
+    

src/rbprmbuilder.impl.cc

@@ -1065,6 +1065,83 @@ namespace hpp {
         }
     }
 
+    short RbprmBuilder::generateContactState(::CORBA::UShort  cId, const char*  name,  const ::hpp::floatSeq& direction)
+    throw (hpp::Error)
+    {
+        try
+        {
+        if(lastStatesComputed_.size() <= (std::size_t)(cId))
+        {
+            throw std::runtime_error ("Unexisting state " + std::string(""+(cId+1)));
+        }
+        State& state = lastStatesComputed_[cId];
+        std::string limbname(name);
+        fcl::Vec3f dir;
+        for(std::size_t i =0; i <3; ++i)
+        {
+            dir[i] = direction[(_CORBA_ULong)i];
+        }
+        pinocchio::Configuration_t config = state.configuration_;
+        fullBody()->device_->currentConfiguration(config);
+
+        sampling::T_OctreeReport finalSet;
+        rbprm::T_Limb::const_iterator lit = fullBody()->GetLimbs().find(limbname);
+        if(lit == fullBody()->GetLimbs().end())
+        {
+            throw std::runtime_error ("Impossible to find limb for joint "
+                                      + std::string(limbname) + " to robot; limb not defined.");
+        }
+        const RbPrmLimbPtr_t& limb = lit->second;
+        pinocchio::Transform3f transformPino = limb->octreeRoot(); // get root transform from configuration
+        fcl::Transform3f transform(transformPino.rotation(),transformPino.translation());
+                    // TODO fix as in rbprm-fullbody.cc!!
+        const affMap_t &affMap = problemSolver()->affordanceObjects;
+        if (affMap.map.empty ())
+        {
+            throw hpp::Error ("No affordances found. Unable to interpolate.");
+        }
+        const std::vector<pinocchio::CollisionObjectPtr_t> objects = contact::getAffObjectsForLimb(std::string(limbname), affMap,
+                                                                   bindShooter_.affFilter_);
+
+        std::vector<sampling::T_OctreeReport> reports(objects.size());
+        std::size_t i (0);
+        //#pragma omp parallel for
+        for(std::vector<pinocchio::CollisionObjectPtr_t>::const_iterator oit = objects.begin();
+            oit != objects.end(); ++oit, ++i)
+        {
+            sampling::GetCandidates(limb->sampleContainer_, transform, *oit, dir, reports[i], sampling::HeuristicParam());
+        }
+        for(std::vector<sampling::T_OctreeReport>::const_iterator cit = reports.begin();
+            cit != reports.end(); ++cit)
+        {
+            finalSet.insert(cit->begin(), cit->end());
+        }
+        // randomize samples
+        std::random_shuffle(reports.begin(), reports.end());
+        unsigned short num_samples_ok (0);
+        pinocchio::Configuration_t sampleConfig = config;
+        sampling::T_OctreeReport::const_iterator candCit = finalSet.begin();
+        for(std::size_t i=0; i< _CORBA_ULong(finalSet.size()) && num_samples_ok < 100; ++i, ++candCit)
+        {
+            const sampling::OctreeReport& report = *candCit;
+            State state;
+            state.configuration_ = config;
+            hpp::rbprm::projection::ProjectionReport rep =
+            hpp::rbprm::projection::projectSampleToObstacle(fullBody(),std::string(limbname), limb, report, fullBody()->GetCollisionValidation(), sampleConfig, state);
+            if(rep.success_)
+            {
+
+                lastStatesComputed_.push_back(rep.result_);
+                lastStatesComputedTime_.push_back(std::make_pair(-1.,  rep.result_));
+                return lastStatesComputed_.size() -1;
+            }
+        }
+        return -1;
+    } catch (const std::exception& exc) {
+    throw hpp::Error (exc.what ());
+    }
+    }
+
     hpp::floatSeqSeq* RbprmBuilder::getContactSamplesProjected(const char* limbname,
                                         const hpp::floatSeq& configuration,
                                         const hpp::floatSeq& direction,

src/rbprmbuilder.impl.hh

@@ -237,6 +237,9 @@ namespace hpp {
                                                    const hpp::floatSeq& direction,
                                                    unsigned short numSamples) throw (hpp::Error);
 
+        virtual short generateContactState(::CORBA::UShort  currentState, const char*  name,  const ::hpp::floatSeq& direction)  throw (hpp::Error);
+
+
         virtual hpp::floatSeq* getSamplesIdsInOctreeNode(const char* limb,
                                                    double octreeNodeId) throw (hpp::Error);