rbprm now working m...

CMakeLists.txt

@@ -64,19 +64,26 @@ install(FILES
   data/urdf/box_rom.urdf
   data/urdf/box.urdf
   data/urdf/scene.urdf
+  data/urdf/scene2.urdf
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf
   )
 install(FILES
   data/srdf/box_rom.srdf
   data/srdf/box.srdf
   data/srdf/scene.srdf
+  data/srdf/scene2.srdf
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/srdf
   )
 install(FILES
 	data/meshes/car.stl
+	data/meshes/car_simple.stl
 	data/meshes/box.stl
 	data/meshes/box_rom.stl
 	data/meshes/robot_box.stl
+	data/meshes/chair_simple.stl
+	data/meshes/ground.stl
+	#~ data/meshes/pedal.stl
+	#~ data/meshes/board.stl
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes
   )
   

data/srdf/scene2.srdf

+<?xml version="1.0"?>
+<robot name="scene">
+  <handle name="handle">
+    <position> 0 0 0 1 0 0 0 </position>
+    <link name="base_link"/>
+  </handle>
+  <handle name="handle2">
+    <position> 0 0 0
+      0 0 0.7071067811865476 0.7071067811865476 </position>
+    <link name="base_link"/>
+  </handle>
+  <contact name="box_surface">
+    <link name="base_link"/>
+    <point>-0.025 -0.025 -0.025   -0.025 0.025 -0.025   -0.025 -0.025 0.025   -0.025 0.025 0.025
+           +0.025 -0.025 -0.025   +0.025 0.025 -0.025   +0.025 -0.025 0.025   +0.025 0.025 0.025 </point>
+    <triangle> 0 2 1 4 5 6</triangle>
+  </contact>
+</robot>

data/urdf/scene.urdf

@@ -3,7 +3,7 @@
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh filename="package:///../hpp-rbprm-corba/meshes/car.stl"/>
+        <mesh filename="package:///../hpp-rbprm-corba/meshes/ground.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>
-        <mesh filename="package://hpp-rbprm-corba/meshes/car.stl"/>
+        <mesh filename="package://hpp-rbprm-corba/meshes/ground.stl"/>
       </geometry>
     </collision>
   </link>

data/urdf/scene2.urdf

+<robot name="scene">
+	
+  <!-- This is the top level joint /tf frame. -->
+  <link name="base_link">
+    <!--<sphere_inertia radius="0.001" mass="1"/>-->
+  </link>
+  <joint name="ground_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="base_link"/>
+    <child link="ground_link"/>
+  </joint>
+  <link name="ground_link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package:///../hpp-rbprm-corba/meshes/ground.stl"/>
+      </geometry>
+      <material name="white">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://hpp-rbprm-corba/meshes/ground.stl"/>
+      </geometry>
+    </collision>
+  </link> 
+  
+  <joint name="board_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="base_link"/>
+    <child link="board_link"/>
+  </joint>
+  <link name="board_link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package:///../hpp-rbprm-corba/meshes/board.stl"/>
+      </geometry>
+      <material name="white">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://hpp-rbprm-corba/meshes/board.stl"/>
+      </geometry>
+    </collision>
+  </link>
+   
+  
+  <joint name="chair_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="base_link"/>
+    <child link="chair_link"/>
+  </joint>
+  <link name="chair_link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package:///../hpp-rbprm-corba/meshes/chair_simple.stl"/>
+      </geometry>
+      <material name="white">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://hpp-rbprm-corba/meshes/chair_simple.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  
+  <joint name="pedal_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="base_link"/>
+    <child link="pedal_link"/>
+  </joint>
+  <link name="pedal_link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package:///../hpp-rbprm-corba/meshes/pedal.stl"/>
+      </geometry>
+      <material name="white">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://hpp-rbprm-corba/meshes/pedal.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  
+</robot>

script/load.py

@@ -12,7 +12,7 @@ srdfSuffix = ""
 rbprmBuilder = Builder ()
 
 rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
-rbprmBuilder.setJointBounds ("base_joint_xyz", [0, 2, -2, 0, 0, 1.5])
+rbprmBuilder.setJointBounds ("base_joint_xyz", [0, 2, -2, 0, -1, 1.5])
 
 #~ from hpp.corbaserver.rbprm. import ProblemSolver
 from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
@@ -23,22 +23,22 @@ r = Viewer (ps)
 
 q_init = rbprmBuilder.getCurrentConfig ()
 q_goal = q_init [::]
-q_init [0:3] = [0, -0.5, 0]
-q_goal [0:3] = [1.1, -0.5, 1]
+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)
 
-r.loadObstacleModel (packageName, "scene", "car")
 
 ps.setInitialConfig (q_init)
 ps.addGoalConfig (q_goal)
 
 ps.client.problem.selectConFigurationShooter("RbprmShooter")
 ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
+r.loadObstacleModel (packageName, "scene", "car")
 ps.solve ()
 
 
 from hpp.gepetto import PathPlayer
 pp = PathPlayer (rbprmBuilder.client.basic, r)
-
+#~ 
 pp (0)
 

src/hpp/corbaserver/rbprm/rbprmbuilder.py

@@ -50,8 +50,8 @@ class Builder (object):
         
     ## Virtual function to load the robot model
     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(urdfNamerom, rootJointType, packageName, urdfName, urdfSuffix, srdfSuffix)		
+		self.client.rbprm.rbprm.loadRobotRomModel(urdfNamerom, rootJointType, packageName, urdfNamerom, urdfSuffix, srdfSuffix)
+		self.client.rbprm.rbprm.loadRobotCompleteModel(urdfName, rootJointType, packageName, urdfName, urdfSuffix, srdfSuffix)		
 		self.name = urdfName
 		self.displayName = urdfName
 		self.tf_root = "base_link"

src/rbprmbuilder.impl.hh

@@ -32,7 +32,7 @@ namespace hpp {
     struct BindShooter
     {
         BindShooter(const std::size_t shootLimit = 10000,
-                    const std::size_t displacementLimit = 10)
+                    const std::size_t displacementLimit = 1000)
             : shootLimit_(shootLimit)
             , displacementLimit_(displacementLimit) {}