added state helper class (finally)

idl/hpp/corbaserver/rbprm/rbprmbuilder.idl

@@ -275,6 +275,19 @@ module hpp
     /// \return list of 2 or 3 lists of 6d vectors [pox, poy, posz, nprmalx, normaly, normalz]
     floatSeqSeq computeContactPointsForLimb(in unsigned short stateId, 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 normalized step for generation along the path (ie the path has a length of 1).
+    /// \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
+    /// all the contact positions and normals for a given state
+    /// \param stateId iD of the considered state
+    /// \param isIntermediate whether the intermediate state should be considerred rather than this one
+    /// \return list of 6d vectors [pox, poy, posz, nprmalx, normaly, normalz]
+    floatSeqSeq computeContactPointsAtState(in unsigned short stateId, in unsigned short isIntermediate) 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/CMakeLists.txt

@@ -101,6 +101,7 @@ INSTALL(
 	${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/client.py
 	${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/rbprmbuilder.py
         ${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/rbprmfullbody.py
+        ${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/rbprmstate.py
         ${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/problem_solver.py
   DESTINATION ${PYTHON_SITELIB}/hpp/corbaserver/rbprm
   )

src/hpp/corbaserver/rbprm/rbprmstate.py

+#!/usr/bin/env python
+# Copyright (c) 2014 CNRS
+# Author: Steve Tonneau
+#
+# This file is part of hpp-rbprm-corba.
+# hpp-rbprm-corba is free software: you can redistribute it
+# and/or modify it under the terms of the GNU Lesser General Public
+# License as published by the Free Software Foundation, either version
+# 3 of the License, or (at your option) any later version.
+#
+# hpp-manipulation-corba is distributed in the hope that it will be
+# useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+# of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+# General Lesser Public License for more details.  You should have
+# received a copy of the GNU Lesser General Public License along with
+# hpp-manipulation-corba.  If not, see
+# <http://www.gnu.org/licenses/>.
+
+from hpp.corbaserver.rbprm import Client as RbprmClient
+from hpp.corbaserver import Client as BasicClient
+from numpy import array
+
+
+## Creates a state given an Id pointing to an existing c++ state
+#
+#  A RbprmDevice robot is a set of two robots. One for the 
+#  trunk of the robot, one for the range of motion
+class State (object):
+    ## Constructor
+    def __init__ (self, fullBody, sId, isIntermediate = False):
+        self.cl = fullBody.client.rbprm.rbprm
+        self.sId = sId
+        self.isIntermediate = isIntermediate    
+        self.fullBody = fullBody
+    
+    ## assert for case where functions can't be used with intermediate state
+    def _cni(self):
+        assert not self.isIntermediate, "method can't be called with intermediate state"
+            
+    ## Get the state configuration
+    def q(self):
+        self._cni()
+        return self.cl.getConfigAtState(self.sId)
+        
+    ## Set the state configuration
+    # \param q configuration of the robot
+    # \return whether or not the configuration was successfully set
+    def setQ(self, q):
+        self._cni()
+        return self.cl.setConfigAtState(self.sId, q)    > 0
+        
+    # \param state1 current state considered
+    # \param limb name of the limb for which the request aplies
+    # \return whether the limb is in contact at this state
+    def isLimbInContact(self, limbname):
+        if(self.isIntermediate):
+            return self.cl.isLimbInContactIntermediary(limbname, self.sId) >0
+        else:            
+            return self.cl.isLimbInContact(limbname, self.sId) >0
+                
+    # 
+    # \param state1 current state considered
+    # \param limb name of the limb for which the request aplies
+    # \return all limbs in contact at this state
+    def getLimbsInContact(self):
+        return [limbName for limbName in self.fullBody.limbNames if self.isLimbInContact(limbName)]
+        
+    ## Get the end effector position for a given configuration, assuming z normal
+    # \param limbName name of the limb from which to retrieve a sample
+    # \param configuration configuration of the robot
+    # \return world position of the limb end effector given the current robot configuration.
+    # array of size 4, where each entry is the position of a corner of the contact surface
+    def getEffectorPosition(self, limbName):
+        self._cni()
+        return self.cl.getEffectorPosition(limbName,self.q())
+     
+    ## Get the end effector position for a given configuration, assuming z normal
+    # \param limbName name of the limb from which to retrieve a sample
+    # \param configuration configuration of the robot
+    # \return world position of the limb end effector given the current robot configuration.
+    # array of size 4, where each entry is the position of a corner of the contact surface
+    def getContactPosAndNormals(self):
+        if(self.isIntermediate):  
+            rawdata = self.cl.computeContactPointsAtState(self.sId, 1)
+        else:            
+            rawdata = self.cl.computeContactPointsAtState(self.sId, 0) 
+        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]
+     
+    ## Get the end effector position for a given configuration, assuming z normal
+    # \param limbName name of the limb from which to retrieve a sample
+    # \param configuration configuration of the robot
+    # \return world position of the limb end effector given the current robot configuration.
+    # array of size 4, where each entry is the position of a corner of the contact surface
+    def getContactPosAndNormalsForLimb(self, limbName):
+        assert self.isLimbInContact(limbname), "in getContactPosAndNormals: limb " + limbName +  "is not in contact at  state" + str(stateId) 
+        if(self.isIntermediate):  
+            rawdata = cl.computeContactPointsAtStateForLimb(self.sId,1)
+        else:            
+            rawdata = cl.computeContactPointsAtStateForLimb(self.sId,0) 
+        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]
+        
+    
+    ## Get position of center of mass
+    def getCenterOfMass (self):
+        q0 = fullBody.client.basic.robot.getCurrentConfig()
+        fullBody.client.basic.robot.setCurrentConfig(self.q())
+        c = fullBody.client.basic.robot.getComPosition()
+        fullBody.client.basic.robot.setCurrentConfig(q0)
+        return c
+    
+    
+    ## Get the end effector position for a given configuration, assuming z normal
+    # \param limbName name of the limb from which to retrieve a sample
+    # \param configuration configuration of the robot
+    # \return world position of the limb end effector given the current robot configuration.
+    # array of size 4, where each entry is the position of a corner of the contact surface
+    def getContactCone(self, friction):
+        if(self.isIntermediate):  
+            rawdata = self.cl.getContactIntermediateCone(self.sId,friction)
+        else:            
+            rawdata = self.cl.getContactCone(self.sId,friction) 
+        H_h =  array(rawdata)
+        return H_h[:,:-1], H_h[:, -1]

src/rbprmbuilder.impl.cc

@@ -1473,6 +1473,53 @@ namespace hpp {
         return res;
     }
 
+    floatSeqSeq* RbprmBuilder::computeContactPointsAtState(unsigned short cId, unsigned short isIntermediate) throw (hpp::Error)
+    {
+        if(lastStatesComputed_.size() <= cId + isIntermediate)
+        {
+            throw std::runtime_error ("Unexisting state " + std::string(""+(cId)));
+        }
+        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;
+        }
+        std::vector<std::vector<fcl::Vec3f> > allStates;
+        allStates.push_back(computeRectangleContact(fullBody_, state));
+
+        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::computeContactPointsForLimb(unsigned short cId, const char *limbName) throw (hpp::Error)
     {
         if(lastStatesComputed_.size() <= cId + 1)
@@ -1528,6 +1575,53 @@ namespace hpp {
         return res;
     }
 
+    floatSeqSeq* RbprmBuilder::computeContactPointsAtStateForLimb(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;
+        }
+        std::vector<std::vector<fcl::Vec3f> > allStates;
+        allStates.push_back(computeRectangleContact(fullBody_, state,limb));
+
+        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, unsigned short filterStates) throw (hpp::Error)
     {
         try

src/rbprmbuilder.impl.hh

@@ -154,7 +154,9 @@ namespace hpp {
         virtual void setEndState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error);
         virtual hpp::floatSeq*  computeContactForConfig(const hpp::floatSeq& configuration, const char* limbNam) throw (hpp::Error);
         virtual hpp::floatSeqSeq* computeContactPoints(unsigned short cId) throw (hpp::Error);
+        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* 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);