early attempt at computing contact points

idl/hpp/corbaserver/rbprm/rbprmbuilder.idl

@@ -204,6 +204,12 @@ module hpp
     /// \param contactLimbs ids of the limb in contact for the state
     void setEndState(in floatSeq dofArray, in Names_t contactLimbs) raises (Error);
 
+    /// Provided a discrete contact sequence has already been computed, computes
+    /// all the contact positions and normals for a given state, the next one, and the intermediate between them.
+    /// \param stateId normalized step for generation along the path (ie the path has a length of 1).
+    /// \return list of 2 or 3 lists of 6d vectors [pox, poy, posz, nprmalx, normaly, normalz]
+    floatSeqSeq computeContactPoints(in unsigned short stateId) raises (Error);
+
     /// Provided a path has already been computed, interpolates it and generates the statically stable
     /// constact configurations along it. setStartState and setEndState must have been called prior
     /// to this function. If these conditions are not met an error is raised.

src/hpp/corbaserver/rbprm/rbprmfullbody.py

@@ -233,6 +233,16 @@ class FullBody (object):
     # \param robustnessTreshold minimum value of the static equilibrium robustness criterion required to accept the configuration (0 by default).
     def interpolate(self, stepsize, pathId = 1, robustnessTreshold = 0):
 		return self.client.rbprm.rbprm.interpolate(stepsize, pathId, robustnessTreshold)
+	
+	## Discretizes a path return by a motion planner into a discrete
+	# sequence of balanced, contact configurations and returns
+	# the sequence as an array of configurations
+	#
+    # \param stateId id of the first state
+    # \param pathId Id of the path to compute from
+    # \param robustnessTreshold minimum value of the static equilibrium robustness criterion required to accept the configuration (0 by default).
+    def computeContactPoints(self, stateId):
+		return self.client.rbprm.rbprm.computeContactPoints(stateId)
 		
 	## Given start and goal states
 	#  generate a contact sequence over a list of configurations

src/rbprmbuilder.impl.cc

@@ -616,6 +616,94 @@ namespace hpp {
         }
     }
 
+
+    typedef Eigen::Matrix <value_type, 4, 3, Eigen::RowMajor> Matrix43;
+    typedef Eigen::Matrix <value_type, 4, 3, Eigen::RowMajor> Rotation;
+    typedef Eigen::Ref<Matrix43> Ref_matrix43;
+
+    std::vector<fcl::Vec3f> computeRectangleContact(const rbprm::RbPrmFullBodyPtr_t device,
+                                                    const rbprm::State& state)
+    {
+        std::vector<fcl::Vec3f> res;
+        const rbprm::T_Limb& limbs = device->GetLimbs();
+        rbprm::RbPrmLimbPtr_t limb;
+        Matrix43 p; Eigen::Matrix3d R;
+        for(std::map<std::string, fcl::Vec3f>::const_iterator cit = state.contactPositions_.begin();
+            cit != state.contactPositions_.end(); ++cit)
+        {
+            const std::string& name = cit->first;
+            const fcl::Vec3f& position = cit->second;
+            limb = limbs.at(name);
+            const double& lx = limb->x_, ly = limb->y_;
+            p << lx,  ly, 0,
+                 lx, -ly, 0,
+                -lx, -ly, 0,
+                -lx,  ly, 0;
+            const fcl::Matrix3f& fclRotation = state.contactRotation_.at(name);
+            for(int i =0; i< 3; ++i)
+                for(int j =0; j<3;++j)
+                    R(i,j) = fclRotation(i,j);
+            for(std::size_t i =0; i<4; ++i)
+            {
+                res.push_back(position + (R*p.row(i).transpose()));
+                res.push_back(state.contactNormals_.at(name));
+            }
+        }
+        return res;
+    }
+
+    floatSeqSeq* RbprmBuilder::computeContactPoints(unsigned short cId) throw (hpp::Error)
+    {
+        if(lastStatesComputed_.size() <= cId + 1)
+        {
+            throw std::runtime_error ("Unexisting state " + std::string(""+(cId+1)));
+        }
+        const State& firstState = lastStatesComputed_[cId], thirdState = lastStatesComputed_[cId+1];
+        std::vector<std::vector<fcl::Vec3f> > allStates;
+        allStates.push_back(computeRectangleContact(fullBody_, firstState));
+        std::vector<std::string> variations =thirdState.contactVariations(firstState);
+        if(variations.size() >1)
+        {
+            throw std::runtime_error ("too many state variation between states" + std::string(""+cId) +
+                                      "and " + std::string(""+(cId + 1)));
+        }
+        if(!variations.empty())
+        {
+std::cout << "variation " << variations[0] << std::endl;
+            State intermediary(firstState);
+            intermediary.RemoveContact(*variations.begin());
+            allStates.push_back(computeRectangleContact(fullBody_, intermediary));
+        }
+        allStates.push_back(computeRectangleContact(fullBody_, thirdState));
+
+        hpp::floatSeqSeq *res;
+        res = new hpp::floatSeqSeq ();
+
+        // compute array of contact positions
+
+        res->length ((_CORBA_ULong)allStates.size());
+        std::size_t i=0;
+        for(std::vector<std::vector<fcl::Vec3f> >::const_iterator cit = allStates.begin();
+                    cit != allStates.end(); ++cit, ++i)
+        {
+            const std::vector<fcl::Vec3f>& positions = *cit;
+            _CORBA_ULong size = (_CORBA_ULong) positions.size () * 3;
+            double* dofArray = hpp::floatSeq::allocbuf(size);
+            hpp::floatSeq floats (size, size, dofArray, true);
+            //convert the config in dofseq
+            for(std::size_t h = 0; h<positions.size(); ++h)
+            {
+                for(std::size_t k =0; k<3; ++k)
+                {
+                    model::size_type j (h*3 + k);
+                    dofArray[j] = positions[h][k];
+                }
+            }
+            (*res) [(_CORBA_ULong)i] = floats;
+        }
+        return res;
+    }
+
     floatSeqSeq* RbprmBuilder::interpolate(double timestep, double path, double robustnessTreshold) throw (hpp::Error)
     {
         try

src/rbprmbuilder.impl.hh

@@ -139,6 +139,7 @@ namespace hpp {
 
         virtual void setStartState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error);
         virtual void setEndState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error);
+        virtual hpp::floatSeqSeq* computeContactPoints(unsigned short cId) throw (hpp::Error);
         virtual hpp::floatSeqSeq* interpolate(double timestep, double path, double robustnessTreshold) throw (hpp::Error);
         virtual hpp::floatSeqSeq* interpolateConfigs(const hpp::floatSeqSeq& configs, double robustnessTreshold) throw (hpp::Error);
         virtual void interpolateBetweenStates(double state1, double state2, unsigned short numOptimizations) throw (hpp::Error);