[contact generation] add method to state api to get the center of the contacts

idl/hpp/corbaserver/rbprm/rbprmbuilder.idl

@@ -322,6 +322,12 @@ module hpp
     /// \return list of 6d vectors [pox, poy, posz, nprmalx, normaly, normalz]
     floatSeqSeq computeContactPointsAtStateForLimb(in unsigned short stateId, in unsigned short isIntermediate, in string limbname) raises (Error);
 
+    /// Provided a discrete contact sequence has already been computed, computes
+    /// the position of the contact center and the normal
+    /// \param stateId normalized step for generation along the path (ie the path has a length of 1).
+    /// \return 2 3D vector : the first is the position the second is the normal
+    floatSeqSeq computeCenterOfContactAtStateForLimb(in unsigned short cId,in  unsigned short isIntermediate, in string limbName) raises(Error);
+
     /// Provided a discrete contact sequence has already been computed, computes
     /// all the contact positions and normals for a given state
     /// \param stateId iD of the considered state

src/hpp/corbaserver/rbprm/rbprmstate.py

@@ -107,6 +107,9 @@ class State (object):
             rawdata = self.cl.computeContactPointsAtStateForLimb(self.sId,0, limbName) 
         return [[b[i:i+3] for i in range(0, len(b), 6)] for b in rawdata], [[b[i+3:i+6] for i in range(0, len(b), 6)] for b in rawdata]
         
+    def getCenterOfContactForLimb(self,limbName):
+        assert self.isLimbInContact(limbName), "in getContactPosAndNormals: limb " + limbName +  "is not in contact at  state" + str(stateId)
+        return self.cl.computeCenterOfContactAtStateForLimb(self.sId,self.isIntermediate, limbName)
     
     ## Get position of center of mass
     def getCenterOfMass (self):

src/rbprmbuilder.impl.cc

@@ -1980,6 +1980,59 @@ namespace hpp {
         return res;
     }
 
+    floatSeqSeq* RbprmBuilder::computeCenterOfContactAtStateForLimb(unsigned short cId, unsigned short isIntermediate, const char *limbName) throw (hpp::Error)
+    {
+        if(lastStatesComputed_.size() <= cId + isIntermediate)
+        {
+            throw std::runtime_error ("Unexisting state " + std::string(""+(cId)));
+        }
+        std::string limb(limbName);
+        State state = lastStatesComputed_[cId];
+        if(isIntermediate > 0)
+        {
+            const State&  thirdState = lastStatesComputed_[cId+1];
+            bool success(false);
+            State interm = intermediary(state, thirdState, cId, success);
+            if(success)
+                state = interm;
+        }
+        if(state.contacts_.find(limb) == state.contacts_.end()){
+            throw std::runtime_error ("Limb : "+limb+" is not in contact at state "+std::string(""+(cId)));
+        }
+
+
+        hpp::floatSeqSeq *res;
+        res = new hpp::floatSeqSeq ();
+
+
+        res->length ((_CORBA_ULong)2);
+
+        fcl::Vec3f& position = state.contactPositions_.at(limbName);
+        position += fullBody()->GetLimb(limb)->offset_;
+        _CORBA_ULong size = (_CORBA_ULong) 3;
+        double* dofArray = hpp::floatSeq::allocbuf(size);
+        hpp::floatSeq floats (size, size, dofArray, true);
+        //convert the config in dofseq
+        for(std::size_t k =0; k<3; ++k)
+        {
+            dofArray[k] = position[k];
+        }
+        (*res) [(_CORBA_ULong)0] = floats;
+
+        const fcl::Vec3f& normal = state.contactNormals_.at(limbName);
+        double* dofArray_n = hpp::floatSeq::allocbuf(size);
+        hpp::floatSeq floats_n (size, size, dofArray_n, true);
+        //convert the config in dofseq
+        for(std::size_t k =0; k<3; ++k)
+        {
+            dofArray_n[k] = normal[k];
+        }
+        (*res) [(_CORBA_ULong)1] = floats_n;
+
+        return res;
+    }
+
+
     floatSeqSeq* RbprmBuilder::interpolate(double timestep, double path, double robustnessTreshold, unsigned short filterStates) throw (hpp::Error)
     {
         hppDout(notice,"### Begin interpolate");

src/rbprmbuilder.impl.hh

@@ -259,6 +259,7 @@ namespace hpp {
         virtual hpp::floatSeqSeq* computeContactPointsAtState(unsigned short cId, unsigned short isIntermediate) throw (hpp::Error);
         virtual hpp::floatSeqSeq* computeContactPointsForLimb(unsigned short cId, const char* limbName) throw (hpp::Error);
         virtual hpp::floatSeqSeq* computeContactPointsAtStateForLimb(unsigned short cId, unsigned short isIntermediate, const char* limbName) throw (hpp::Error);
+        virtual hpp::floatSeqSeq* computeCenterOfContactAtStateForLimb(unsigned short cId, unsigned short isIntermediate, const char *limbName) throw (hpp::Error);
         virtual hpp::floatSeqSeq* interpolate(double timestep, double path, double robustnessTreshold, unsigned short filterStates) throw (hpp::Error);
         virtual hpp::floatSeqSeq* interpolateConfigs(const hpp::floatSeqSeq& configs, double robustnessTreshold, unsigned short filterStates) throw (hpp::Error);
         virtual hpp::floatSeqSeq* getContactCone(unsigned short stateId, double friction) throw (hpp::Error);