onto path validation rbprm

CMakeLists.txt

@@ -53,5 +53,31 @@ ADD_SUBDIRECTORY(src)
 
 CONFIG_FILES (include/hpp/corbaserver/rbprm/doc.hh)
 
+SET(CATKIN_PACKAGE_SHARE_DESTINATION
+  ${CMAKE_INSTALL_DATAROOTDIR}/${PROJECT_NAME})
+
+install(FILES
+  data/package.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  )
+install(FILES
+  data/urdf/box_rom.urdf
+  data/urdf/box.urdf
+  data/urdf/scene.urdf
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf
+  )
+install(FILES
+  data/srdf/box_rom.srdf
+  data/srdf/box.srdf
+  data/srdf/scene.srdf
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/srdf
+  )
+install(FILES
+  data/meshes/car.stl
+	data/meshes/box.stl
+	data/meshes/box_rom.stl
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes
+  )
+  
 SETUP_PROJECT_FINALIZE()
 

data/urdf/box.urdf

@@ -3,7 +3,7 @@
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <box size="0.05 0.05 0.05"/>
+        <mesh filename="package://hpp-rbprm-corba/meshes/robot_box.stl"/>
       </geometry>
       <material name="white">
         <color rgba="1 1 1 1"/>
@@ -12,7 +12,7 @@
     <collision>
       <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <box size="0.05 0.05 0.05"/>
+        <mesh filename="package://hpp-rbprm-corba/meshes/box.stl"/>
       </geometry>
     </collision>
   </link>

data/urdf/box_rom.urdf

@@ -3,7 +3,7 @@
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <box size="0.1 0.1 0.1"/>
+        <mesh filename="package://hpp-rbprm-corba/meshes/box_rom.stl"/>
       </geometry>
       <material name="white">
         <color rgba="1 1 1 1"/>
@@ -12,7 +12,7 @@
     <collision>
       <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <box size="0.1 0.1 0.1"/>
+        <mesh filename="package://hpp-rbprm-corba/meshes/box_rom.stl"/>
       </geometry>
     </collision>
   </link>

script/load.py

@@ -23,22 +23,8 @@ r = Viewer (ps)
 
 q_init = rbprmBuilder.getCurrentConfig ()
 q_goal = q_init [::]
-q_init [0:3] = [0, -0.5, 0.4]
-
-#~ rank = rbprmBuilder.rankInConfiguration ['torso_lift_joint']
-#~ q_init [rank] = 0.2
-r (q_init)
-
-q_goal [0:3] = [1, -0.5, 1]
-#~ q_goal [0:3] = [-3.2, 0, 3]
-#~ rank = rbprmBuilder.rankInConfiguration ['l_shoulder_lift_joint']
-#~ q_goal [rank] = 0.5
-#~ rank = rbprmBuilder.rankInConfiguration ['l_elbow_flex_joint']
-#~ q_goal [rank] = -0.5
-#~ rank = rbprmBuilder.rankInConfiguration ['r_shoulder_lift_joint']
-#~ q_goal [rank] = 0.5
-#~ rank = rbprmBuilder.rankInConfiguration ['r_elbow_flex_joint']
-#~ q_goal [rank] = -0.5
+q_init [0:3] = [0, -0.5, 0]
+q_goal [0:3] = [1.1, -0.5, 1]
 r (q_goal)
 
 r.loadObstacleModel (packageName, "scene", "car")
@@ -47,7 +33,7 @@ ps.setInitialConfig (q_init)
 ps.addGoalConfig (q_goal)
 
 ps.client.problem.selectConFigurationShooter("RbprmShooter")
-
+ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
 ps.solve ()
 
 

script/load2.py

+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.gepetto import Viewer
+
+rootJointType = 'freeflyer'
+packageName = 'hpp-rbprm-corba'
+meshPackageName = 'hpp-rbprm-corba'
+urdfName = 'box'
+urdfNameRom = 'box_rom'
+urdfSuffix = ""
+srdfSuffix = ""
+
+rbprmBuilder = Builder ()
+
+rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+rbprmBuilder.setJointBounds ("base_joint_xyz", [0, 2, -2, 0, 0, 1.5])
+
+#~ from hpp.corbaserver.rbprm. import ProblemSolver
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+
+ps = ProblemSolver( rbprmBuilder )
+
+r = Viewer (ps)
+
+q_init = rbprmBuilder.getCurrentConfig ()
+q_goal = q_init [::]
+q_init [0:3] = [0, -0.5, 0.4]
+
+#~ rank = rbprmBuilder.rankInConfiguration ['torso_lift_joint']
+#~ q_init [rank] = 0.2
+r (q_init)
+
+q_goal [0:3] = [1, -0.5, 1]
+#~ q_goal [0:3] = [-3.2, 0, 3]
+#~ rank = rbprmBuilder.rankInConfiguration ['l_shoulder_lift_joint']
+#~ q_goal [rank] = 0.5
+#~ rank = rbprmBuilder.rankInConfiguration ['l_elbow_flex_joint']
+#~ q_goal [rank] = -0.5
+#~ rank = rbprmBuilder.rankInConfiguration ['r_shoulder_lift_joint']
+#~ q_goal [rank] = 0.5
+#~ rank = rbprmBuilder.rankInConfiguration ['r_elbow_flex_joint']
+#~ q_goal [rank] = -0.5
+r (q_goal)
+
+r.loadObstacleModel (packageName, "scene", "car")
+
+ps.setInitialConfig (q_init)
+ps.addGoalConfig (q_goal)
+
+ps.client.problem.selectConFigurationShooter("RbprmShooter")
+
+ps.solve ()
+
+
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (rbprmBuilder.client.basic, r)
+
+pp (0)
+

src/hpp/corbaserver/rbprm/rbprmbuilder.py

@@ -49,9 +49,9 @@ class Builder (object):
         self.load = load
         
     ## Virtual function to load the robot model
-    def loadModel (self, urdfName, rootJointType, packageName, meshPackageName, urdfSuffix, srdfSuffix):
+    def loadModel (self, urdfName, urdfNamerom, rootJointType, packageName, meshPackageName, urdfSuffix, srdfSuffix):
 		self.client.rbprm.rbprm.loadRobotRomModel(urdfName, rootJointType, packageName, urdfName, urdfSuffix, srdfSuffix)
-		self.client.rbprm.rbprm.loadRobotCompleteModel(urdfName, rootJointType, packageName, urdfName, urdfSuffix, srdfSuffix)		
+		self.client.rbprm.rbprm.loadRobotCompleteModel(urdfNamerom, rootJointType, packageName, urdfName, urdfSuffix, srdfSuffix)		
 		self.name = urdfName
 		self.displayName = urdfName
 		self.tf_root = "base_link"

src/rbprmbuilder.impl.cc

@@ -19,8 +19,11 @@
 #include <hpp/util/debug.hh>
 #include "rbprmbuilder.impl.hh"
 #include "hpp/rbprm/rbprm-device.hh"
+#include "hpp/rbprm/rbprm-validation.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>
 
 
 
@@ -95,6 +98,17 @@ namespace hpp {
         }
     }
 
+    namespace
+    {
+        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));
+            hpp::core::CollisionPathValidationReport defaultValidation;
+            return hpp::core::DiscretizedCollisionChecking::createWithValidation(robot,val ,defaultValidation, validation);
+        }
+    }
+
     void RbprmBuilder::SetProblemSolver (hpp::core::ProblemSolverPtr_t problemSolver)
     {
         problemSolver_ = problemSolver;
@@ -104,6 +118,7 @@ namespace hpp {
         // add rbprmshooter
         problemSolver->addConfigurationShooterType("RbprmShooter",
                                                    boost::bind(&BindShooter::create, boost::ref(bindShooter_), _1));
+        problemSolver->addPathValidationType("RbprmPathValidation", &createPathValidation);
     }
 
     } // namespace impl

src/rbprmbuilder.impl.hh

@@ -33,23 +33,15 @@ namespace hpp {
     struct BindShooter
     {
         BindShooter(const std::size_t shootLimit = 10000,
-                    const std::size_t displacementLimit = 100)
+                    const std::size_t displacementLimit = 10)
             : shootLimit_(shootLimit)
             , displacementLimit_(displacementLimit) {}
 
         hpp::rbprm::RbPrmShooterPtr_t create (const hpp::model::DevicePtr_t& robot)
         {
-            hpp::model::DevicePtr_t pRobot = robot;
             hpp::model::RbPrmDevicePtr_t robotcast = boost::static_pointer_cast<hpp::model::RbPrmDevice>(robot);
-            hpp::rbprm::RbPrmValidationPtr_t validator = hpp::rbprm::RbPrmValidation::create(robotcast);
-            const hpp::core::ObjectVector_t& obstacles = problemSolver_->collisionObstacles();
-            for(hpp::core::ObjectVector_t::const_iterator cit = obstacles.begin();
-                cit != obstacles.end(); ++cit)
-            {
-                validator->addObstacle(*cit);
-            }
             return hpp::rbprm::RbPrmShooter::create
-                    (robotcast,problemSolver_->collisionObstacles(),validator,shootLimit_,displacementLimit_);
+                    (robotcast,problemSolver_->problem ()->collisionObstacles(),shootLimit_,displacementLimit_);
         }
         hpp::core::ProblemSolverPtr_t problemSolver_;
         std::size_t shootLimit_;