Merge pull request #28 from pFernbach/topic/posture_weights

add API for posture weights

idl/hpp/corbaserver/rbprm/rbprmbuilder.idl

@@ -106,6 +106,10 @@ module hpp
     void setReferenceConfig(in floatSeq referenceConfig)
       raises (Error);
 
+    /// set the weights used when computing a postural task
+    void setPostureWeights(in floatSeq postureWeights)
+      raises (Error);
+
     /// set a reference position of the end effector for the given ROM
     void setReferenceEndEffector(in string romName, in floatSeq ref)
       raises (Error);

src/hpp/corbaserver/rbprm/rbprmfullbody.py

@@ -843,6 +843,11 @@ def setStaticStability(self,staticStability):
      def setReferenceConfig(self,referenceConfig):
           return self.clientRbprm.rbprm.setReferenceConfig(referenceConfig)
 
+     ## set the weights used when computing a postural task
+     # \param postureWeights dofArray, must be of same size as device->numberDof
+     def setPostureWeights(self,postureWeights):
+          return self.clientRbprm.rbprm.setPostureWeights(postureWeights)
+
       ## return the time at the given state index (in the path computed during the first phase)
       # \param stateId : index of the state
      def getTimeAtState(self,stateId):

src/rbprmbuilder.impl.cc

@@ -633,6 +633,13 @@ namespace hpp {
       fullBody()->referenceConfig(config);
     }
 
+    void RbprmBuilder::setPostureWeights(const hpp::floatSeq& postureWeights) throw (hpp::Error){
+      if(!fullBodyLoaded_)
+        throw Error ("No full body robot was loaded");
+      Configuration_t config(dofArrayToConfig (fullBody()->device_->numberDof(), postureWeights));
+      fullBody()->postureWeights(config);
+    }
+
     void RbprmBuilder::setReferenceEndEffector(const char* romName, const hpp::floatSeq &ref) throw(hpp::Error){
       std::string name (romName);
       hpp::pinocchio::RbPrmDevicePtr_t device = boost::dynamic_pointer_cast<hpp::pinocchio::RbPrmDevice>(problemSolver()->robot ());

src/rbprmbuilder.impl.hh

@@ -196,6 +196,7 @@ namespace hpp {
         void setStaticStability(const bool staticStability) throw (hpp::Error);
 
         void setReferenceConfig(const hpp::floatSeq &referenceConfig) throw (hpp::Error);
+        void setPostureWeights(const hpp::floatSeq &postureWeights) throw (hpp::Error);
         void setReferenceEndEffector(const char* romName, const hpp::floatSeq &ref) throw(hpp::Error);
 
         virtual void setFilter(const hpp::Names_t& roms) throw (hpp::Error);