add python API to set reference end effector position

idl/hpp/corbaserver/rbprm/rbprmbuilder.idl

@@ -106,6 +106,10 @@ module hpp
     void setReferenceConfig(in floatSeq referenceConfig)
       raises (Error);
 
+    /// set a reference position of the end effector for the given ROM
+    void setReferenceEndEffector(in string romName, in floatSeq ref)
+      raises (Error);
+
     /// Set Rom constraints for the configuration shooter
     /// a configuration will only be valid if all roms indicated
     /// are colliding with the environment.

src/hpp/corbaserver/rbprm/rbprmbuilder.py

@@ -95,3 +95,11 @@ class Builder (Robot):
   # \param filename name of the output file where to save the output
   def exportPath (self, viewer, problem, pathId, stepsize, filename):
     em.exportPath(viewer, self.client.robot, problem, pathId, stepsize, filename)
+
+  ## set a reference position of the end effector for the given ROM
+  # This reference will be used in the heuristic that choose the "best" contact surface
+  # and approximate the contact points in the kinodynamic planner
+  # \param romName the name of the rom
+  # \param ref the 3D reference position of the end effector, expressed in the root frame
+  def setReferenceEndEffector(self,romName,ref):
+    return self.clientRbprm.rbprm.setReferenceEndEffector(romName,ref)

src/rbprmbuilder.impl.cc

@@ -633,6 +633,17 @@ namespace hpp {
       fullBody()->referenceConfig(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 ());
+      if(!device)
+        throw Error ("No rbprmDevice in problemSolver");
+      if(device->robotRoms_.find(name) == device->robotRoms_.end())
+        throw Error("Device do not contain this rom ");
+      Configuration_t config(dofArrayToConfig (3, ref));
+      device->setEffectorReference(name,config);
+    }
+
 
 
     void RbprmBuilder::setFilter(const hpp::Names_t& roms) throw (hpp::Error)

src/rbprmbuilder.impl.hh

@@ -196,7 +196,7 @@ namespace hpp {
         void setStaticStability(const bool staticStability) throw (hpp::Error);
 
         void setReferenceConfig(const hpp::floatSeq &referenceConfig) 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);
 				virtual void setAffordanceFilter(const char* romName, const hpp::Names_t& affordances) throw (hpp::Error);