finished integration of multiple roms

.gitignore

@@ -27,3 +27,5 @@
 *.exe
 *.out
 *.app
+
+build/

script/stepladder_path_hrp2.py

@@ -5,14 +5,15 @@ rootJointType = 'freeflyer'
 packageName = 'hpp-rbprm-corba'
 meshPackageName = 'hpp-rbprm-corba'
 urdfName = 'hrp2_trunk'
-urdfNameRom = 'hrp2_rom'
+urdfNameRoms = ['hrp2_larm_rom','hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom']
 urdfSuffix = ""
 srdfSuffix = ""
 
 rbprmBuilder = Builder ()
 
-rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+rbprmBuilder.loadModel(urdfName, urdfNameRoms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
 rbprmBuilder.setJointBounds ("base_joint_xyz", [-1, 1, -0.5, 0.5, 0, 5])
+rbprmBuilder.setFilter(['hrp2_lleg_rom','hrp2_rleg_rom'])
 
 #~ from hpp.corbaserver.rbprm. import ProblemSolver
 from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
@@ -27,7 +28,7 @@ q_goal =  rbprmBuilder.getCurrentConfig ();
 
 q_init = rbprmBuilder.getCurrentConfig ();
 #~ q_init = [-0.15, -0.2, 0.6, 0.99500416527802582, 0.0, 0.099833416646828155, 0.0]
-q_init = [-0.3, -0.2, 0.6, 1, 0.0, 0., 0.0]; r (q_init)
+q_init = [-1, -0.2, 0.6, 1, 0.0, 0., 0.0]; r (q_init)
 rbprmBuilder.setCurrentConfig (q_init); r (q_init)
 
 #~ q_goal = [0, 0, 2, 0.99500416527802582, 0.0, 0.099833416646828155, 0.0]; r (q_goal)
@@ -39,7 +40,7 @@ ps.setInitialConfig (q_init)
 ps.addGoalConfig (q_goal)
 
 ps.client.problem.selectConFigurationShooter("RbprmShooter")
-ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
+ps.client.problem.selectPathValidation("RbprmPathValidation",0.5)
 r.loadObstacleModel (packageName, "stepladder", "planning")
 ps.solve ()
 

src/hpp/corbaserver/rbprm/rbprmbuilder.py

@@ -49,8 +49,12 @@ class Builder (object):
         self.load = load
         
     ## Virtual function to load the robot model
-    def loadModel (self, urdfName, urdfNamerom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix):
-		self.client.rbprm.rbprm.loadRobotRomModel(urdfNamerom, rootJointType, packageName, urdfNamerom, urdfSuffix, srdfSuffix)
+    def loadModel (self, urdfName, urdfNameroms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix):
+		if(isinstance(urdfNameroms, list)):		
+			for urdfNamerom in urdfNameroms:
+				self.client.rbprm.rbprm.loadRobotRomModel(urdfNamerom, rootJointType, packageName, urdfNamerom, urdfSuffix, srdfSuffix)
+		else:
+			self.client.rbprm.rbprm.loadRobotRomModel(urdfNameroms, rootJointType, packageName, urdfNameroms, urdfSuffix, srdfSuffix)
 		self.client.rbprm.rbprm.loadRobotCompleteModel(urdfName, rootJointType, packageName, urdfName, urdfSuffix, srdfSuffix)		
 		self.name = urdfName
 		self.displayName = urdfName
@@ -73,6 +77,8 @@ class Builder (object):
 			self.rankInVelocity [j] = rankInVelocity
 			rankInVelocity += self.client.basic.robot.getJointNumberDof (j)
 
+
+
 	## Init RbprmShooter
     #
     # \param jointName name of the joint,

src/rbprmbuilder.impl.cc

@@ -22,10 +22,6 @@
 #include "hpp/rbprm/rbprm-validation.hh"
 #include "hpp/rbprm/rbprm-path-interpolation.hh"
 #include "hpp/model/urdf/util.hh"
-#include <hpp/core/collision-path-validation-report.hh>
-#include <hpp/core/problem-solver.hh>
-#include <hpp/core/discretized-collision-checking.hh>
-#include <hpp/core/straight-path.hh>
 #include <fstream>
 
 
@@ -459,7 +455,7 @@ namespace hpp {
         }
     }
 
-    namespace
+    /*namespace
     {
         hpp::core::PathValidationPtr_t createPathValidation (const hpp::model::DevicePtr_t& robot, const hpp::model::value_type& val)
         {
@@ -468,7 +464,7 @@ namespace hpp {
             hpp::core::CollisionPathValidationReport defaultValidation;
             return hpp::core::DiscretizedCollisionChecking::createWithValidation(robot,val ,defaultValidation, validation);
         }
-    }
+    }*/
 
     void RbprmBuilder::SetProblemSolver (hpp::core::ProblemSolverPtr_t problemSolver)
     {
@@ -479,7 +475,8 @@ namespace hpp {
         // add rbprmshooter
         problemSolver->addConfigurationShooterType("RbprmShooter",
                                                    boost::bind(&BindShooter::create, boost::ref(bindShooter_), _1));
-        problemSolver->addPathValidationType("RbprmPathValidation", &createPathValidation);
+        problemSolver->addPathValidationType("RbprmPathValidation",
+                                                   boost::bind(&BindShooter::createPathValidation, boost::ref(bindShooter_), _1, _2));
     }
 
     } // namespace impl

src/rbprmbuilder.impl.hh

@@ -24,6 +24,10 @@
 # include <hpp/rbprm/rbprm-fullbody.hh>
 # include <hpp/rbprm/rbprm-shooter.hh>
 # include <hpp/rbprm/rbprm-validation.hh>
+# include <hpp/core/collision-path-validation-report.hh>
+# include <hpp/core/problem-solver.hh>
+# include <hpp/core/discretized-collision-checking.hh>
+# include <hpp/core/straight-path.hh>
 
 namespace hpp {
   namespace rbprm {
@@ -43,6 +47,15 @@ namespace hpp {
             return hpp::rbprm::RbPrmShooter::create
                     (robotcast,problemSolver_->problem ()->collisionObstacles(),romFilter_,shootLimit_,displacementLimit_);
         }
+
+        hpp::core::PathValidationPtr_t createPathValidation (const hpp::model::DevicePtr_t& robot, const hpp::model::value_type& val)
+        {
+            hpp::model::RbPrmDevicePtr_t robotcast = boost::static_pointer_cast<hpp::model::RbPrmDevice>(robot);
+            hpp::rbprm::RbPrmValidationPtr_t validation(hpp::rbprm::RbPrmValidation::create(robotcast, romFilter_));
+            hpp::core::CollisionPathValidationReport defaultValidation;
+            return hpp::core::DiscretizedCollisionChecking::createWithValidation(robot,val ,defaultValidation, validation);
+        }
+
         hpp::core::ProblemSolverPtr_t problemSolver_;
         std::vector<std::string> romFilter_;
         std::size_t shootLimit_;