corba connexion to sample generation

script/loadfullbody.py

 from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
 from hpp.gepetto import Viewer
 
 rootJointType = 'freeflyer'
@@ -9,23 +10,23 @@ urdfNameRom = 'box_rom'
 urdfSuffix = ""
 srdfSuffix = ""
 
-rbprmBuilder = Builder ()
+fullBody = FullBody ()
 
-rbprmBuilder.loadFullBodyModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
-rbprmBuilder.setJointBounds ("base_joint_xyz", [0, 2, -2, 0, -1, 1.5])
+fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+fullBody.setJointBounds ("base_joint_xyz", [0, 2, -2, 0, -1, 1.5])
+fullBody.addLimb("base_joint_SO3", 100, 0.1)
 
 #~ from hpp.corbaserver.rbprm. import ProblemSolver
 from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
 
-ps = ProblemSolver( rbprmBuilder )
+ps = ProblemSolver( fullBody )
 
 r = Viewer (ps)
 
-q_init = rbprmBuilder.getCurrentConfig ()
-q_goal = q_init [::]
-q_init [0:3] = [0, -0.5, 0.3]
-q_goal = [0, -0.5, -0.2, -0.501544,0.431183, 0.662926, -0.350804]
-r (q_goal)
+fullBody.getSample('base_joint_SO3', 1)
+fullBody.client.rbprm.rbprm.getSampleConfig('base_joint_SO3', 1)
+q_init = fullBody.client.rbprm.rbprm.getSampleConfig('base_joint_SO3', 1)
+r (q_init)
 
 
 ps.setInitialConfig (q_init)

src/hpp/corbaserver/rbprm/rbprmbuilder.py

@@ -72,29 +72,6 @@ class Builder (object):
 			rankInConfiguration += self.client.basic.robot.getJointConfigSize (j)
 			self.rankInVelocity [j] = rankInVelocity
 			rankInVelocity += self.client.basic.robot.getJointNumberDof (j)
-			
-	def loadFullBodyModel (self, urdfName, rootJointType, packageName, meshPackageName, urdfSuffix, srdfSuffix):
-		self.client.rbprm.rbprm.loadloadFullBodyRobot(urdfName, rootJointType, packageName, urdfNamerom, urdfSuffix, srdfSuffix)
-		self.name = urdfName
-		self.displayName = urdfName
-		self.tf_root = "base_link"
-		self.rootJointType = rootJointType
-		self.jointNames = self.client.basic.robot.getJointNames ()
-		self.allJointNames = self.client.basic.robot.getAllJointNames ()
-		self.client.basic.robot.meshPackageName = meshPackageName
-		self.meshPackageName = meshPackageName
-		self.rankInConfiguration = dict ()
-		self.rankInVelocity = dict ()
-		self.packageName = packageName
-		self.urdfName = urdfName
-		self.urdfSuffix = urdfSuffix
-		self.srdfSuffix = srdfSuffix
-		rankInConfiguration = rankInVelocity = 0
-		for j in self.jointNames:
-			self.rankInConfiguration [j] = rankInConfiguration
-			rankInConfiguration += self.client.basic.robot.getJointConfigSize (j)
-			self.rankInVelocity [j] = rankInVelocity
-			rankInVelocity += self.client.basic.robot.getJointNumberDof (j)
 
 	## Init RbprmShooter
     #

src/hpp/corbaserver/rbprm/rbprmfullbody.py

@@ -29,11 +29,11 @@ class CorbaClient:
         self.basic = BasicClient ()
         self.rbprm = RbprmClient ()
 
-## Load and handle a RbprmDevice robot for rbprm planning
+## Load and handle a RbprmFullbody robot for rbprm planning
 #
 #  A RbprmDevice robot is a set of two robots. One for the 
 #  trunk of the robot, one for the range of motion
-class Builder (object):
+class FullBody (object):
     ## Constructor
     # \param trunkName name of the first robot that is loaded now,
     # \param romName name of the first robot that is loaded now,
@@ -48,8 +48,8 @@ class Builder (object):
         self.client = CorbaClient ()
         self.load = load
 			
-	def loadFullBodyModel (self, urdfName, rootJointType, packageName, meshPackageName, urdfSuffix, srdfSuffix):
-		self.client.rbprm.rbprm.loadloadFullBodyRobot(urdfName, rootJointType, packageName, urdfNamerom, urdfSuffix, srdfSuffix)
+    def loadFullBodyModel (self, urdfName, rootJointType, packageName, meshPackageName, urdfSuffix, srdfSuffix):
+		self.client.rbprm.rbprm.loadFullBodyRobot(urdfName, rootJointType, packageName, urdfName, urdfSuffix, srdfSuffix)
 		self.name = urdfName
 		self.displayName = urdfName
 		self.tf_root = "base_link"
@@ -71,6 +71,12 @@ class Builder (object):
 			self.rankInVelocity [j] = rankInVelocity
 			rankInVelocity += self.client.basic.robot.getJointNumberDof (j)
 
+    def addLimb(self, name, samples, resolution):
+		self.client.rbprm.rbprm.addLimb(name, samples, resolution)
+
+    def getSample(self, name, idsample):
+		return self.client.rbprm.rbprm.getSampleConfig(name,idsample)
+
    ## \name Degrees of freedom
     #  \{
 

src/rbprmbuilder.impl.cc

@@ -109,13 +109,16 @@ namespace hpp {
         try
         {
             model::DevicePtr_t device = model::Device::create (robotName);
-            hpp::model::urdf::loadRobotModel (romDevice_,
+            hpp::model::urdf::loadRobotModel (device,
                     std::string (rootJointType),
                     std::string (packageName),
                     std::string (modelName),
                     std::string (urdfSuffix),
                     std::string (srdfSuffix));
             fullBody_ = rbprm::RbPrmFullBody::create(device);
+            problemSolver_->robot (fullBody_->device_);
+            problemSolver_->robot ()->controlComputation
+            (model::Device::JOINT_POSITION);
         }
         catch (const std::exception& exc)
         {
@@ -147,7 +150,7 @@ namespace hpp {
         }
         const sampling::Sample& sample = limbPtr->sampleContainer_.samples_[sampleId];
         size_t ric = sample.startRank_;
-        config.tail(ric) = sample.configuration_;
+        config.tail(config.rows() - ric) = sample.configuration_;
         dofArray = new hpp::floatSeq();
         dofArray->length(_CORBA_ULong(config.rows()));
         for(std::size_t i=0; i< _CORBA_ULong(config.rows()); i++)